From 94ead5f496d6c97c6feec6356a475f395b88c713 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 10 Aug 2017 20:38:26 -0400 Subject: [PATCH 01/39] fix MP gps and add status --- buzz_scripts/flock.bzz | 1 + buzz_scripts/include/utilities.bzz | 152 ++++++++ buzz_scripts/tree_centroid.bzz | 537 +++++++++++++++++++++++++++++ src/buzz_utility.cpp | 2 +- 4 files changed, 691 insertions(+), 1 deletion(-) create mode 100644 buzz_scripts/include/utilities.bzz create mode 100644 buzz_scripts/tree_centroid.bzz diff --git a/buzz_scripts/flock.bzz b/buzz_scripts/flock.bzz index c4246ab..3db9d25 100644 --- a/buzz_scripts/flock.bzz +++ b/buzz_scripts/flock.bzz @@ -65,6 +65,7 @@ function action() { function init() { uav_initswarm() uav_initstig() + TARGET_ALTITUDE = 2.5 + id * 5 } # Executed at each time step. diff --git a/buzz_scripts/include/utilities.bzz b/buzz_scripts/include/utilities.bzz new file mode 100644 index 0000000..6c4dab6 --- /dev/null +++ b/buzz_scripts/include/utilities.bzz @@ -0,0 +1,152 @@ +# Utilities + +# Rads to degrees +function rtod(r) { + return (r*(180.0/math.pi)) +} + +# Copy a table +function table_deep_copy(new_t, old_t, depth) { + depth = depth - 1 + if (old_t != nil) { + foreach(old_t, function(key, value) { + new_t[key] = value + if(depth != 0) { + new_t[key] = {} + table_deep_copy(new_t[key], value, depth) + } + }) + } +} + +function table_copy(old_t, depth) { + var t = {}; + table_deep_copy(t, old_t, depth); + return t; +} + +# Print the contents of a table +function table_print(t, depth) { + depth = depth - 1 + if (t != nil) { + foreach(t, function(key, value) { + log(key, " -> ", value) + if(depth != 0) { + table_print(t[key], depth) + } + }) + } +} + +# Write a table as if it was a vector +function write_vector(k, index, val) { + var key = string.tostring(index) + k[key] = val +} + +# Read a table as if it was a vector +function read_vector(k, index) { + var key = string.tostring(index) + if (k[key] == nil) { + return -1 + } else { + return k[key] + } +} + +# Write a table as if it was a matrix +function write_matrix(k, row, col, val) { + var key = string.concat(string.tostring(row),"-",string.tostring(col)) + k[key] = val +} + +# Read a table as if it was a matrix +function read_matrix(k, row, col) { + var key = string.concat(string.tostring(row),"-",string.tostring(col)) + if (k[key] == nil) { + return -1 + } else { + return k[key] + } +} + +# Int to String +function itos(i) { + + log("Use 'string.tostring(OJB)' instead") + + if (i==0) { return "0" } + if (i==1) { return "1" } + if (i==2) { return "2" } + if (i==3) { return "3" } + if (i==4) { return "4" } + if (i==5) { return "5" } + if (i==6) { return "6" } + if (i==7) { return "7" } + if (i==8) { return "8" } + if (i==9) { return "9" } + + log("Function 'itos' out of bounds, returning the answer (42)") + return "42" +} + +# String to Int +function stoi(s) { + if (s=='0') { return 0 } + if (s=='1') { return 1 } + if (s=='2') { return 2 } + if (s=='3') { return 3 } + if (s=='4') { return 4 } + if (s=='5') { return 5 } + if (s=='6') { return 6 } + if (s=='7') { return 7 } + if (s=='8') { return 8 } + if (s=='9') { return 9 } + + log("Function 'stoi' out of bounds, returning the answer (42)") + return 42 + +} + +# Force angles in the (-pi,pi) interval +function radians_interval(a) { + var temp = a + while ((temp>2.0*math.pi) or (temp<0.0)) { + if (temp > 2.0*math.pi) { + temp = temp - 2.0*math.pi + } else if (temp < 0.0){ + temp = temp + 2.0*math.pi + } + } + if (temp > math.pi) { + temp = temp - 2.0*math.pi + } + return temp +} + +############################################ + +#base = {} + +#base.create = function() { +# return { +# .method = function(a,b) { +# return a + b +# } +# } +# } + +#x = base.create() +#x.method(3,4) # returns 7 + +#derived = {} + +#derived.create = function() { +# b = base.create() +# b.method = function(a,b) { +# return a * b +# } +#} + +#x = derived.create() +#x.method(3,4) # returns 12 diff --git a/buzz_scripts/tree_centroid.bzz b/buzz_scripts/tree_centroid.bzz new file mode 100644 index 0000000..d14277f --- /dev/null +++ b/buzz_scripts/tree_centroid.bzz @@ -0,0 +1,537 @@ +# Include files +include "string.bzz" +include "vec2.bzz" +include "utilities.bzz" +include "barrier.bzz" + +############################################ + +# Global variables + +RANGE = 200 # rab range in cm, get from argos file + +N_SONS = 10 # Maximum number of sons + +TRIGGER_VSTIG = 101 # ID of trigger virtual stigmergy +BARRIER_VSTIG = 102 # ID of barrier virtual stigmergy + +################################################################ +################################################################ + +# Tree utility functions + +function parent_select() { + + + # Selects potential parents + var candidates = {} + candidates = neighbors.filter(function(rid, data) { + return ((knowledge.level[rid] > 0) and (data.distance < (RANGE - 10.0)) and (knowledge.free[rid] == 1))}) + # and (data.distance > 10.0) + + # Selects closest parent candidate as parent + var temp = {} + temp = candidates.reduce(function(rid, data, accum) { + accum.min = math.min(accum.min, data.distance) + return accum + }, {.min = RANGE}) + var min = temp.min + + + var flag = 0 + foreach(knowledge.distance, function(key, value) { + if ((flag == 0) and (candidates.data[key] != nil)) { + if (value == min) { + tree.parent.id = key + tree.parent.distance = value + tree.parent.azimuth = knowledge.azimuth[key] + flag = 1 + } + } + #break (when implemented) + }) + +} + +#################################### + +function count() { + + if (nb_sons == 0) { + eq_level = level + } + + else if (nb_sons >= 1) { + var temp = {} + temp = sons.reduce(function(rid, data, accum) { + accum.sum = accum.sum + tree.sons[rid].eq_level + return accum + }, {.sum = 0}) + eq_level = temp.sum - (nb_sons - 1) * level + } + +} + +#################################### + +function change_frame(p01, p1, theta) { + + var p0 = { + .x = math.cos(theta) * p1.x + math.sin(theta) * p1.y + p01.x, + .y = -math.sin(theta) * p1.x + math.cos(theta) * p1.y + p01.y + } + return p0 + +} + +#################################### + +transform_accum = function(rid, data) { + + # Son contribution in frame F1 + var p1 = { + .x = tree.sons[rid].accum_x, + .y = tree.sons[rid].accum_y + } + + # Rotation angle between F0 and F1 + var theta = tree.sons[rid].angle_parent - data.azimuth - math.pi + + # Translation vector from F0 to F1 + var p01 = { + .x = 0, + .y = 0 + } + + var p0 = {} + + if (tree.sons[rid].angle_parent != nil) { + var rot_angle = radians_interval(theta) + p0 = change_frame(p01, p1, rot_angle) + } + else { + p0.x = p1.x + p0.y = p1.y + } + + return p0 +} + +#################################### + +function centroid() { + + # If the robot has a parent + if ((tree.parent != nil) or (root == 1)) { + var sum_F1 = { .x = 0, .y = 0} + + # If the robot has at least one son + if (nb_sons > 0) { + var temp = {} + # Expresses son contrib (in F1) in its own reference frame (F0) + temp = sons.map(transform_accum) + # Sums son contributions expressed in F0 frame + sum_F1 = temp.reduce(function(rid, data, accum) { + accum.x = accum.x + data.x + accum.y = accum.y + data.y + #log("id ", rid, " sum_x ", accum.x, " sum_y ", accum.y) + return accum + }, {.x = 0, .y = 0}) + } + + # If the robot is not the root + if ((root == 0) and (tree.parent.id != nil)) { + #var nb_descendants = eq_level - level + var p0 = knowledge.distance[tree.parent.id]#tree.parent.distance + var theta = knowledge.azimuth[tree.parent.id]#tree.parent.azimuth + # Adds current robot contribution to centroid sum + accum_x = sum_F1.x - (nb_descendants + 1) * p0 * math.cos(theta) + accum_y = sum_F1.y - (nb_descendants + 1) * p0 * math.sin(theta) + } + # If the robot is the root + else if ((root == 1) and (robot_count != 0)) { + # Centroid coordinates in root ref frame + accum_x = sum_F1.x / robot_count + accum_y = sum_F1.y / robot_count + } + } +} + + +################################################################ +################################################################ + +# Tree reconfiguration functions + +function tree_config() { + statef() +} + +function end_fun() { + if (root == 1) { + log("centroid X: ", accum_x, " Y ", accum_y) + } +} + +#################################### + +function root_select() { + + log(id," root_select") + if (tree.parent.id != nil) { + if(neighbors.data[tree.parent.id] != nil) { + angle_parent = neighbors.data[tree.parent.id].azimuth + } + } + + if (root == 1) { + + # Listens for new root acknowledgment + + foreach(knowledge.ackn, function(key, value) { + if (value == better_root) { + #log(id, " got it") + root = 0 + level = 0 + setleds(0,0,0) + } + }) + + if (ack == 1) { + # Triggers transition to new state + trigger.put("a", 1) + } + else if ((root == 1) and (better_root != id) and (trigger.get("a") != 1)) { + setleds(255,0,0) + better_root = id + + # Centroid position in root reference frame (F0) + var c0 = math.vec2.new(accum_x, accum_y) + + # Distance from current root to centroid + var dist_rc = math.vec2.length(c0) + #log("root ", id, " dist_centr ", dist_rc) + + # Distances from neighbors to centroid + var dist_centroid = {} + dist_centroid = neighbors.map(function(rid, data) { + # Neighbour position in frame F0 + var p0 = math.vec2.newp(data.distance, data.azimuth) + # Difference vector between p0 and c0 + var v = math.vec2.sub(p0, c0) + # Distance between robot and centroid + return math.vec2.length(v) + }) + + # Minimal distance to centroid + var temp = {} + temp = dist_centroid.reduce(function(rid, data, accum) { + accum.min = math.min(accum.min, data) + return accum + }, {.min = dist_rc}) + var min = temp.min + #log("min ", min) + + # If the current root is the closest to the centroid + if(dist_rc == min) { + ack = 1 + } + # Otherwise + else { + var flag = 0 + # Selects closest robot to centroid as better root + foreach(dist_centroid.data, function(key, value) { + if(flag == 0) { + if(value == min) { + better_root = key + flag = 1 + } + # break (when implemented) + } + }) + + + #log(id, " better root : ", better_root) + #log("X : ", accum_x, "Y : ", accum_y) + var angle = neighbors.data[better_root].azimuth + # Broadcasts + var message = { + .better_root = better_root, + .centroid_x = accum_x, + .centroid_y = accum_y, + .angle_old_root = angle + } + neighbors.broadcast("msg1", message) + } + } + } + + else if (better_root == nil) { + # Listen for old root message + foreach(knowledge.better_root, function(rid, value) { + if(value == id) { + + var theta = neighbors.data[rid].azimuth + + var p1 = { + .x = knowledge.centroid_x[rid], + .y = knowledge.centroid_y[rid] + } + + var p01 = { + .x = neighbors.data[rid].distance * math.cos(theta), + .y = neighbors.data[rid].distance * math.sin(theta) + } + + var p0 = {} + + if (knowledge.angle_old_root[rid] != nil) { + var rot_angle = radians_interval(knowledge.angle_old_root[rid] - theta - math.pi) + p0 = change_frame(p01, p1, rot_angle) + } + else { + p0.x = p1.x + p0.y = p1.y + } + + accum_x = p0.x + accum_y = p0.y + + centroid_x = accum_x + centroid_y = accum_y + + root = 1 + neighbors.broadcast("got_it", id) + } + }) + } + + # Transitions to new state when all robots are ready + if (trigger.get("a") == 1) { + barrier_set(ROBOTS, end_fun) + barrier_ready() + } + +} + +#################################### + +function tree_select() { + + log(id, " tree_select") + + neighbors.map(function(rid, data) { + knowledge.distance[rid] = data.distance + knowledge.azimuth[rid] = data.azimuth + return 1 + }) + + if (level == 0) { + # Finds a parent + parent_select() + # Updates robot level + if (tree.parent.id != nil) { + #log(" ") + #log("selected") + #log("son ", id) + #log("parent ", tree.parent.id) + #log(" ") + + level = knowledge.level[tree.parent.id] + 1 + angle_parent = neighbors.data[tree.parent.id].azimuth + } + } + else { + # Updates list of sons + foreach(knowledge.parent, function(key, value) { + if(value == id) { + #log(value) + if(tree.sons[key] == nil) { + # Updates robot nb_sons + nb_sons = nb_sons + 1 + # Updates robot free status + if (nb_sons >= N_SONS) { + free = 0 + } + } + tree.sons[key] = { + .distance = knowledge.distance[key], + .azimuth = knowledge.azimuth[key], + .eq_level = knowledge.eq_level[key], + .accum_x = knowledge.accum_x[key], + .accum_y = knowledge.accum_y[key], + .angle_parent = knowledge.angle_parent[key] + } + } + }) + } + + # Creates a subset of neighbors to get the sons + # and parent + sons = {} + sons = neighbors.filter(function(rid, data) { + return (tree.sons[rid] != nil) + }) + parent = {} + parent = neighbors.filter(function(rid, data) { + return (tree.parent.id == rid) + }) + + # Counts robots (updates eq_level) + count() + + # Updates count of robots in the tree + if (root == 1) { + robot_count = eq_level + } + + nb_descendants = eq_level - level + + + # Computes centroid (updates accum_x, accum_y) + centroid() + + # Broadcast information to other robots + var message = { + .level = level, + .parent = tree.parent.id, + .eq_level = eq_level, + .free = free, + .accum_x = accum_x, + .accum_y = accum_y, + .angle_parent = angle_parent + } + neighbors.broadcast("msg2", message) + + # Triggers transition to new state if root count = ROBOTS + if (root == 1) { + if(robot_count == ROBOTS) { + log("centroid X: ", accum_x, " Y ", accum_y) + trigger.put("b", 1) + } + } + + # Transitions to new state when all robots are ready + if (trigger.get("b") == 1) { + barrier_set(ROBOTS, root_select) + barrier_ready() + } +} + +################################################################ +################################################################ + + +# This function is executed every time you press the 'execute' button +function init() { + + trigger = stigmergy.create(TRIGGER_VSTIG) + barrier = stigmergy.create(BARRIER_VSTIG) + + # Trees + old_tree = {} + tree = old_tree + old_tree.parent = {} + old_tree.sons = {} + + # Boolean variables + root = 0 # Root status + free = 1 # Node status (true if accepts sons) + ack = 0 + + # Tree variables + level = 0 + eq_level = 0 + nb_sons = 0 + nb_descendants = 0 + + # Update root status + if (id == 0) { + root = 1 # True if robot is the ro ot + level = 1 + robot_count = 0 + } + + statef = tree_select + + knowledge = { + .level = {}, + .parent = {}, + .eq_level = {}, + .free = {}, + .accum_x = {}, + .accum_y = {}, + .angle_parent = {}, + .distance = {}, + .azimuth = {}, + .better_root = {}, + .centroid_x = {}, + .centroid_y = {}, + .angle_old_root = {}, + .ackn = {} + } + + # Broadcast information to other robots + var message = { + .level = level, + .parent = old_tree.parent.id, + .eq_level = eq_level, + .free = free, + .accum_x = accum_x, + .accum_y = accum_y, + .angle_parent = 0.0 + } + neighbors.broadcast("msg2", message) + + # Listen to information from other robots for root_select + neighbors.listen("msg1", function(vid, value, rid) { + knowledge.better_root[rid] = value.better_root + knowledge.centroid_x[rid] = value.centroid_x + knowledge.centroid_y[rid] = value.centroid_y + knowledge.angle_old_root[rid] = value.angle_old_root + knowledge.angle_parent[rid] = value.angle_parent + }) + + # Listen to information from other robots for tree_select + neighbors.listen("msg2", function(vid, value, rid) { + knowledge.level[rid] = value.level + knowledge.parent[rid] = value.parent + knowledge.eq_level[rid] = value.eq_level + knowledge.free[rid] = value.free + knowledge.accum_x[rid] = value.accum_x + knowledge.accum_y[rid] = value.accum_y + knowledge.angle_parent[rid] = value.angle_parent + }) + + neighbors.listen("got_it", function(vid, value, rid) { + knowledge.ackn[rid] = value + }) + +} + +############################################ + +# This function is executed at each time step +function step() { + + tree_config() + goal = math.vec2.new(0.0, 0.0) + +} +############################################ + +# This function is executed every time you press the 'reset' +# button in the GUI. +function reset() { + # put your code here +} + +############################################ + +# This function is executed only once, when the robot is removed +# from the simulation +function destroy() { + # put your code here +} + +################ \ No newline at end of file diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 6039dc7..1d296c9 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -17,7 +17,7 @@ namespace buzz_utility{ static char* BO_FNAME = 0; static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; - static uint32_t MSG_SIZE = 500; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size) + static uint32_t MSG_SIZE = 250; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size) static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets static uint8_t Robot_id = 0; static std::vector IN_MSG; From 97e0a2c4b2db1eb4e0422ab09db9bd92a8c524b9 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 11 Aug 2017 17:59:28 -0400 Subject: [PATCH 02/39] test solo gazebo and clean rcclient --- launch/launch_config/solo.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index 6ad2ec8..f9ca5d2 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -6,7 +6,7 @@ topics: setpoint: mavros/setpoint_position/local armclient: mavros/cmd/arming modeclient: mavros/set_mode - localpos: /mavros/local_position/pose + localpos: mavros/local_position/pose stream: mavros/set_stream_rate altitude: mavros/global_position/rel_alt type: From 5804aace94fad0be764723a295cea756cd13328d Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 18 Aug 2017 17:13:57 -0400 Subject: [PATCH 03/39] integrate PY comments and start waypoints control --- include/buzzuav_closures.h | 4 +- include/roscontroller.h | 1 + src/buzzuav_closures.cpp | 32 +++++++++++----- src/roscontroller.cpp | 76 +++++++++++++++++++------------------- 4 files changed, 65 insertions(+), 48 deletions(-) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index c205c47..d1f7619 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -42,7 +42,9 @@ int getcmd(); /*Sets goto position */ void set_goto(double pos[]); /*Sets goto position from rc client*/ -void rc_set_goto(double pos[]); +void rc_set_goto(int id, double latitude, double longitude, double altitude); +/*Sets gimbal orientation from rc client*/ +void rc_set_gimbal(int id, float yaw, float pitch); /*sets rc requested command */ void rc_call(int rc_cmd); /* sets the battery state */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 31e7537..ae0b7d0 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -39,6 +39,7 @@ #define XBEE_STOP_TRANSMISSION 4355356352 #define TIMEOUT 60 #define BUZZRATE 10 +#define CMD_REQUEST_UPDATE 666 using namespace std; diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 019fe35..da8d9b4 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -16,20 +16,22 @@ namespace buzzuav_closures{ //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin); static double goto_pos[3]; static double rc_goto_pos[3]; + static float rc_gimbal[2]; static float batt[3]; static float obst[5]={0,0,0,0,0}; static double cur_pos[3]; static uint8_t status; static int cur_cmd = 0; static int rc_cmd=0; + static int rc_id=-1; static int buzz_cmd=0; static float height=0; - static bool deque_full = false; - static int rssi = 0; - static int message_number = 0; - static float raw_packet_loss = 0.0; - static int filtered_packet_loss = 0; - static float api_rssi = 0.0; + static bool deque_full = false; + static int rssi = 0; + static int message_number = 0; + static float raw_packet_loss = 0.0; + static int filtered_packet_loss = 0; + static float api_rssi = 0.0; std::map< int, buzz_utility::RB_struct> targets_map; std::map< int, buzz_utility::Pos_struct> neighbors_map; @@ -344,12 +346,22 @@ namespace buzzuav_closures{ return cmd; } - void rc_set_goto(double pos[]) { - rc_goto_pos[0] = pos[0]; - rc_goto_pos[1] = pos[1]; - rc_goto_pos[2] = pos[2]; + void rc_set_goto(int id, double longitude, double latitude, double altitude) { + rc_id = id; + rc_goto_pos[0] = latitude; + rc_goto_pos[1] = longitude; + rc_goto_pos[2] = altitude; } + + void rc_set_gimbal(int id, float yaw, float pitch) { + + rc_id = id; + rc_gimbal[0] = yaw; + rc_gimbal[1] = pitch; + + } + void rc_call(int rc_cmd_in) { rc_cmd = rc_cmd_in; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ce77de7..3a19b05 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -658,37 +658,37 @@ void roscontroller::flight_controller_service_call() // SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND // mode switch (buzzuav_closures::getcmd()) { - case mavros_msgs::CommandCode::NAV_LAND: - if (current_mode != "LAND") { - SetMode("LAND", 0); + case mavros_msgs::CommandCode::NAV_LAND: + if (current_mode != "LAND") { + SetMode("LAND", 0); + armstate = 0; + Arm(); + } + if (mav_client.call(cmd_srv)) { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } else { + ROS_ERROR("Failed to call service from flight controller"); + } armstate = 0; - Arm(); - } - if (mav_client.call(cmd_srv)) { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else { - ROS_ERROR("Failed to call service from flight controller"); - } - armstate = 0; - break; - case mavros_msgs::CommandCode::NAV_TAKEOFF: - if (!armstate) { + break; + case mavros_msgs::CommandCode::NAV_TAKEOFF: + if (!armstate) { - SetMode("LOITER", 0); - armstate = 1; - Arm(); - ros::Duration(0.5).sleep(); - // Registering HOME POINT. - home = cur_pos; - } - if (current_mode != "GUIDED") - SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it - // should always be in loiter after arm/disarm) - if (mav_client.call(cmd_srv)) { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else - ROS_ERROR("Failed to call service from flight controller"); - break; + SetMode("LOITER", 0); + armstate = 1; + Arm(); + ros::Duration(0.5).sleep(); + // Registering HOME POINT. + home = cur_pos; + } + if (current_mode != "GUIDED") + SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it + // should always be in loiter after arm/disarm) + if (mav_client.call(cmd_srv)) { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } else + ROS_ERROR("Failed to call service from flight controller"); + break; } } else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/ @@ -1121,18 +1121,20 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, res.success = true; break; case mavros_msgs::CommandCode::NAV_WAYPOINT: - ROS_INFO("RC_Call: GO TO!!!! --- Doing this! "); - double rc_goto[3]; - // testing PositionTarget - rc_goto[0] = req.param5; - rc_goto[1] = req.param6; - rc_goto[2] = req.param7; - buzzuav_closures::rc_set_goto(rc_goto); + ROS_INFO("RC_Call: GO TO!!!! "); + buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7); rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; - case 666: + case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: + ROS_INFO("RC_Call: Gimbal!!!! "); + buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3); + rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case CMD_REQUEST_UPDATE: ROS_INFO("RC_Call: Update Fleet Status!!!!"); rc_cmd = 666; buzzuav_closures::rc_call(rc_cmd); From 13267a9e97dce849ed0807453d8e93b627667d5b Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 25 Aug 2017 17:05:40 -0400 Subject: [PATCH 04/39] add gps goal control, re-structured launch dir and add height to buzz moveto closure --- buzz_scripts/flock.bzz | 6 +- buzz_scripts/graphform.bzz | 20 +-- buzz_scripts/include/uavstates.bzz | 73 +++++++--- buzz_scripts/testalone.bzz | 9 +- include/buzzuav_closures.h | 2 +- launch/husky.launch | 59 --------- launch/launch_config/topics.yaml | 12 ++ launch/rosbuzz-solo.launch | 57 -------- launch/rosbuzz-testing-sitl.launch | 51 ------- launch/rosbuzz-testing.launch | 43 ------ launch/rosbuzz.launch | 32 +++-- launch/rosbuzz_2_parallel.launch | 27 ---- launch/rosbuzzm100.launch | 16 --- src/buzz_utility.cpp | 6 +- src/buzzuav_closures.cpp | 95 ++++++------- src/roscontroller.cpp | 205 ++++++++++++++--------------- 16 files changed, 261 insertions(+), 452 deletions(-) delete mode 100644 launch/husky.launch create mode 100644 launch/launch_config/topics.yaml delete mode 100644 launch/rosbuzz-solo.launch delete mode 100644 launch/rosbuzz-testing-sitl.launch delete mode 100644 launch/rosbuzz-testing.launch delete mode 100644 launch/rosbuzz_2_parallel.launch delete mode 100644 launch/rosbuzzm100.launch diff --git a/buzz_scripts/flock.bzz b/buzz_scripts/flock.bzz index 3db9d25..bb2da81 100644 --- a/buzz_scripts/flock.bzz +++ b/buzz_scripts/flock.bzz @@ -38,7 +38,7 @@ function action() { # Move according to vector print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum)) - uav_moveto(accum.x, accum.y) + uav_moveto(accum.x, accum.y, 0.0) UAVSTATE = "LENNARDJONES" # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS @@ -47,11 +47,11 @@ function action() { # } else if(timeW>=WAIT_TIMEOUT/2) { # UAVSTATE ="GOEAST" # timeW = timeW+1 -# uav_moveto(0.0,5.0) +# uav_moveto(0.0, 5.0, 0.0) # } else { # UAVSTATE ="GONORTH" # timeW = timeW+1 -# uav_moveto(5.0,0.0) +# uav_moveto(5.0, 0.0, 0.0) # } } diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index adb8139..ae0bd5b 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -446,7 +446,7 @@ function TransitionToJoined(){ m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } # @@ -471,7 +471,7 @@ while(iROBOT_MAXVEL*2) m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL*2/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) }else{ #no joined robots in sight i=0 var tempvec={.x=0.0,.y=0.0} @@ -545,7 +545,7 @@ function DoFree() { i=i+1 } m_navigation=math.vec2.scale(tempvec,1.0/i) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } @@ -554,7 +554,7 @@ function DoFree() { if(step_cunt<=1){ m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } #set message m_selfMessage.State=s2i(m_eState) @@ -614,7 +614,7 @@ function DoAsking(){ } m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } # @@ -662,7 +662,7 @@ function DoJoining(){ S2Target = math.vec2.scale(S2Target, ROBOT_MAXVEL/math.vec2.length(S2Target)) m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) @@ -760,7 +760,7 @@ function DoJoined(){ m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) #check if should to transists to lock @@ -789,7 +789,7 @@ if(m_nLabel!=0){ m_navigation=motion_vector() } #move -uav_moveto(m_navigation.x,m_navigation.y) +uav_moveto(m_navigation.x, m_navigation.y, 0.0) } function action(){ @@ -912,7 +912,7 @@ function destroy() { #clear neighbour message m_navigation.x=0.0 m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) m_vecNodes={} #stop listening neighbors.ignore("m") diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 4ecac31..10d7118 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -5,8 +5,10 @@ ######################################## TARGET_ALTITUDE = 5.0 UAVSTATE = "TURNEDOFF" +GOTO_MAXVEL = 2 +goal = {.range=0, .bearing=0} -function uav_initswarm(){ +function uav_initswarm() { s = swarm.create(1) s.join() statef=turnedoff @@ -26,13 +28,10 @@ function idle() { function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff - #log("TakeOff: ", flight.status) - #log("Relative position: ", position.altitude) if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS,action,land) barrier_ready() - #statef=hexagon } else { log("Altitude: ", TARGET_ALTITUDE) @@ -44,7 +43,7 @@ function takeoff() { function land() { UAVSTATE = "LAND" statef=land - #log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ neighbors.broadcast("cmd", 21) uav_land() @@ -58,6 +57,28 @@ function land() { } } +function goto() { + UAVSTATE = "GOTO" + statef=goto + # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) + if(rc_goto.id==id){ + s.leave() + rb_from_gps(rc_goto.latitude, rc_goto.longitude) + print(id, " has to move ", goal.range) + m_navigation = math.vec2.newp(goal.range, goal.bearing) + if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { + m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + } else if(math.vec2.length(m_navigation)>0.25) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + else + statef = idle + } else { + neighbors.broadcast("cmd", 16) + neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) + } +} + function follow() { if(size(targets)>0) { UAVSTATE = "FOLLOW" @@ -67,7 +88,7 @@ function follow() { force=(0.05)*(tab.range)^4 attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) }) - uav_moveto(attractor.x, attractor.y) + uav_moveto(attractor.x, attractor.y, 0.0) } else { log("No target in local table!") #statef=idle @@ -90,11 +111,8 @@ function uav_rccmd() { neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 - UAVSTATE = "FOLLOW" - log(rc_goto.latitude, " ", rc_goto.longitude) - add_targetrb(0,rc_goto.latitude,rc_goto.longitude) - statef = follow - #uav_goto() + UAVSTATE = "GOTO" + statef = goto } else if(flight.rc_cmd==400) { flight.rc_cmd=0 uav_arm() @@ -114,13 +132,38 @@ function uav_neicmd() { function(vid, value, rid) { print("Got (", vid, ",", value, ") from robot #", rid) if(value==22 and UAVSTATE!="TAKEOFF") { - statef=takeoff + statef=takeoff } else if(value==21) { - statef=land + statef=land } else if(value==400 and UAVSTATE=="IDLE") { - uav_arm() + uav_arm() } else if(value==401 and UAVSTATE=="IDLE"){ - uav_disarm() + uav_disarm() + } else if(value==16){ + neighbors.listen("gt",function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + # uav_storegoal(lat, lon, alt) + }) + statef=goto } }) +} + +function LimitAngle(angle){ + if(angle>2*math.pi) + return angle-2*math.pi + else if (angle<0) + return angle+2*math.pi + else + return angle +} + +function rb_from_gps(lat, lon) { + print("rb dbg: ",lat,lon,position.latitude,position.longitude) + d_lon = lon - position.longitude + d_lat = lat - position.latitude + ned_x = d_lat/180*math.pi * 6371000.0; + ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); + goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + goal.bearing = LimitAngle(math.atan(ned_y,ned_x)); } \ No newline at end of file diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testalone.bzz index 17b66a1..e607d4e 100644 --- a/buzz_scripts/testalone.bzz +++ b/buzz_scripts/testalone.bzz @@ -7,17 +7,22 @@ include "vstigenv.bzz" function action() { statef=action # test moveto cmd dx, dy -# uav_moveto(0.5, 0.5) +# uav_moveto(0.5, 0.5, 0.0) } # Executed once at init time. function init() { + uav_initswarm() + uav_initstig() } # Executed at each time step. function step() { - log("Altitude: ", position.altitude) uav_rccmd() + + statef() + + log("Current state: ", UAVSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index d1f7619..4723269 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -36,7 +36,7 @@ int buzzros_print(buzzvm_t vm); * commands the UAV to go to a position supplied */ int buzzuav_moveto(buzzvm_t vm); -int buzzuav_goto(buzzvm_t vm); +int buzzuav_storegoal(buzzvm_t vm); /* Returns the current command from local variable*/ int getcmd(); /*Sets goto position */ diff --git a/launch/husky.launch b/launch/husky.launch deleted file mode 100644 index bb883c5..0000000 --- a/launch/husky.launch +++ /dev/null @@ -1,59 +0,0 @@ - - - - - - - - - - - - - - - - magnetic_declination_radians: 0 - roll_offset: 0 - pitch_offset: 0 - yaw_offset: 0 - zero_altitude: false - broadcast_utm_transform: false - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml new file mode 100644 index 0000000..8f1295b --- /dev/null +++ b/launch/launch_config/topics.yaml @@ -0,0 +1,12 @@ +topics: + gps : global_position/global + battery : battery + status : state + estatus : extended_state + fcclient: cmd/command + setpoint: setpoint_position/local + armclient: cmd/arming + modeclient: set_mode + localpos: local_position/pose + stream: set_stream_rate + altitude: global_position/rel_alt diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch deleted file mode 100644 index fce3b00..0000000 --- a/launch/rosbuzz-solo.launch +++ /dev/null @@ -1,57 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch deleted file mode 100644 index c09d2c5..0000000 --- a/launch/rosbuzz-testing-sitl.launch +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzz-testing.launch b/launch/rosbuzz-testing.launch deleted file mode 100644 index d955758..0000000 --- a/launch/rosbuzz-testing.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index d79e016..de7857d 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -1,14 +1,22 @@ - - + + + + - - - - - - - - - - + + + + + + + + + + + + + + + + diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch deleted file mode 100644 index 5bc52f4..0000000 --- a/launch/rosbuzz_2_parallel.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/rosbuzzm100.launch b/launch/rosbuzzm100.launch deleted file mode 100644 index 69da82f..0000000 --- a/launch/rosbuzzm100.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 1d296c9..8b883e9 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -304,8 +304,8 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); @@ -349,7 +349,7 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index da8d9b4..95d2656 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -123,27 +123,27 @@ namespace buzzuav_closures{ / Buzz closure to move following a 2D vector /----------------------------------------*/ int buzzuav_moveto(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 2); - buzzvm_lload(vm, 1); /* dx */ - buzzvm_lload(vm, 2); /* dy */ - //buzzvm_lload(vm, 3); /* Latitude */ - //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - float dy = buzzvm_stack_at(vm, 1)->f.value; - float dx = buzzvm_stack_at(vm, 2)->f.value; - double d = sqrt(dx*dx+dy*dy); //range - goto_pos[0]=dx; - goto_pos[1]=dy; - goto_pos[2]=height; - /*double b = atan2(dy,dx); //bearing - printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); - gps_from_rb(d, b, goto_pos); - cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ - //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); - //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]); - buzz_cmd= COMMAND_MOVETO; // TO DO what should we use - return buzzvm_ret0(vm); + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); /* dx */ + buzzvm_lload(vm, 2); /* dy */ + buzzvm_lload(vm, 3); /* dheight */ + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + float dh = buzzvm_stack_at(vm, 1)->f.value; + float dy = buzzvm_stack_at(vm, 2)->f.value; + float dx = buzzvm_stack_at(vm, 3)->f.value; + goto_pos[0]=dx; + goto_pos[1]=dy; + goto_pos[2]=height+dh; + /*double b = atan2(dy,dx); //bearing + printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); + gps_from_rb(d, b, goto_pos); + cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ + //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); + //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]); + buzz_cmd= COMMAND_MOVETO; // TO DO what should we use + return buzzvm_ret0(vm); } int buzzuav_update_targets(buzzvm_t vm) { @@ -224,23 +224,23 @@ namespace buzzuav_closures{ int buzzuav_addNeiStatus(buzzvm_t vm){ buzzvm_lnum_assert(vm, 5); - buzzvm_lload(vm, 1); // fc - buzzvm_lload(vm, 2); // xbee - buzzvm_lload(vm, 3); // batt - buzzvm_lload(vm, 4); // gps - buzzvm_lload(vm, 5); // id - buzzvm_type_assert(vm, 5, BUZZTYPE_INT); - buzzvm_type_assert(vm, 4, BUZZTYPE_INT); - buzzvm_type_assert(vm, 3, BUZZTYPE_INT); - buzzvm_type_assert(vm, 2, BUZZTYPE_INT); - buzzvm_type_assert(vm, 1, BUZZTYPE_INT); + buzzvm_lload(vm, 1); // fc + buzzvm_lload(vm, 2); // xbee + buzzvm_lload(vm, 3); // batt + buzzvm_lload(vm, 4); // gps + buzzvm_lload(vm, 5); // id + buzzvm_type_assert(vm, 5, BUZZTYPE_INT); + buzzvm_type_assert(vm, 4, BUZZTYPE_INT); + buzzvm_type_assert(vm, 3, BUZZTYPE_INT); + buzzvm_type_assert(vm, 2, BUZZTYPE_INT); + buzzvm_type_assert(vm, 1, BUZZTYPE_INT); buzz_utility::neighbors_status newRS; uint8_t id = buzzvm_stack_at(vm, 5)->i.value; - newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value; - newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value; - newRS.xbee= buzzvm_stack_at(vm, 2)->i.value; - newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value; - map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id); + newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value; + newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value; + newRS.xbee= buzzvm_stack_at(vm, 2)->i.value; + newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value; + map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id); if(it!=neighbors_status_map.end()) neighbors_status_map.erase(it); neighbors_status_map.insert(make_pair(id, newRS)); @@ -265,14 +265,17 @@ namespace buzzuav_closures{ return payload_out; } /*----------------------------------------/ - / Buzz closure to go directly to a GPS destination from the Mission Planner + / Buzz closure to store locally a GPS destination from the fleet /----------------------------------------*/ - int buzzuav_goto(buzzvm_t vm) { - rc_goto_pos[2]=height; - set_goto(rc_goto_pos); - cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; - printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); - buzz_cmd=COMMAND_GOTO; + int buzzuav_storegoal(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); // latitude + buzzvm_lload(vm, 2); // longitude + buzzvm_lload(vm, 3); // altitude + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + rc_set_goto(buzzvm_stack_at(vm, 1)->f.value, buzzvm_stack_at(vm, 2)->f.value, buzzvm_stack_at(vm, 3)->f.value, (int)buzz_utility::get_robotid()); return buzzvm_ret0(vm); } @@ -346,7 +349,7 @@ namespace buzzuav_closures{ return cmd; } - void rc_set_goto(int id, double longitude, double latitude, double altitude) { + void rc_set_goto(int id, double latitude, double longitude, double altitude) { rc_id = id; rc_goto_pos[0] = latitude; rc_goto_pos[1] = longitude; @@ -529,6 +532,10 @@ namespace buzzuav_closures{ buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); buzzvm_pusht(vm); buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1)); + buzzvm_pushi(vm, rc_id); + buzzvm_tput(vm); + buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); buzzvm_pushf(vm, rc_goto_pos[0]); buzzvm_tput(vm); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3a19b05..aa04afa 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -266,7 +266,8 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) /*Obtain .bzz file name from parameter server*/ if (n_c.getParam("bzzfile_name", bzzfile_name)) ; - else { + else + { ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node"); } @@ -284,13 +285,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) { ROS_INFO("RC service is disabled"); } - } else { + } else + { ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node"); } - /*Obtain robot_id from parameter server*/ - // n_c.getParam("robot_id", robot_id); - // robot_id=(int)buzz_utility::get_robotid(); /*Obtain out payload name*/ n_c.getParam("out_payload", out_payload); /*Obtain in payload name*/ @@ -300,22 +299,22 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) if (n_c.getParam("xbee_plugged", xbeeplugged)) ; - else { + else + { ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node"); } if (!xbeeplugged) { if (n_c.getParam("name", robot_name)) ; - else { + else + { ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node"); } } else n_c.getParam("xbee_status_srv", xbeesrv_name); - std::cout << "////////////////// " << xbeesrv_name; - GetSubscriptionParameters(n_c); // initialize topics to null? } @@ -325,70 +324,71 @@ used /-----------------------------------------------------------------------------------*/ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle) { - // todo: make it as an array in yaml? m_sMySubscriptions.clear(); std::string gps_topic, gps_type; if (node_handle.getParam("topics/gps", gps_topic)) ; - else { + else + { ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node"); } - node_handle.getParam("type/gps", gps_type); - m_smTopic_infos.insert(pair(gps_topic, gps_type)); + m_smTopic_infos.insert(pair(gps_topic, "sensor_msgs/NavSatFix")); - std::string battery_topic, battery_type; + std::string battery_topic; node_handle.getParam("topics/battery", battery_topic); - node_handle.getParam("type/battery", battery_type); - m_smTopic_infos.insert( - pair(battery_topic, battery_type)); + m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryState")); - std::string status_topic, status_type; + std::string status_topic; node_handle.getParam("topics/status", status_topic); - node_handle.getParam("type/status", status_type); - m_smTopic_infos.insert( - pair(status_topic, status_type)); + m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/State")); + node_handle.getParam("topics/estatus", status_topic); + m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/ExtendedState")); - std::string altitude_topic, altitude_type; + std::string altitude_topic; node_handle.getParam("topics/altitude", altitude_topic); - node_handle.getParam("type/altitude", altitude_type); - m_smTopic_infos.insert( - pair(altitude_topic, altitude_type)); + m_smTopic_infos.insert(pair(altitude_topic, "std_msgs/Float64")); - /*Obtain fc client name from parameter server*/ + // Obtain required topic and service names from the parameter server if (node_handle.getParam("topics/fcclient", fcclient_name)) ; - else { + else + { ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/setpoint", setpoint_name)) ; - else { + else + { ROS_ERROR("Provide a Set Point name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/armclient", armclient)) ; - else { + else + { ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/modeclient", modeclient)) ; - else { + else + { ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/stream", stream_client_name)) ; - else { + else + { ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node"); } if (node_handle.getParam("topics/localpos", local_pos_sub_name)) ; - else { + else + { ROS_ERROR("Provide a localpos name in YAML file"); system("rosnode kill rosbuzz_node"); } @@ -405,33 +405,28 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c) Subscribe(n_c); - payload_sub = - n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); + payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); - obstacle_sub = n_c.subscribe(obstacles_topic, 5, - &roscontroller::obstacle_dist, this); + obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this); /*publishers*/ payload_pub = n_c.advertise(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); neigh_pos_pub = n_c.advertise("neighbours_pos", 5); - localsetpoint_nonraw_pub = - n_c.advertise(setpoint_name, 5); + localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); + /* Service Clients*/ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); mav_client = n_c.serviceClient(fcclient_name); if (rcclient == true) - service = - n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); + service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); ROS_INFO("Ready to receive Mav Commands from RC client"); xbeestatus_srv = n_c.serviceClient(xbeesrv_name); - stream_client = - n_c.serviceClient(stream_client_name); + stream_client = n_c.serviceClient(stream_client_name); users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this); - local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, - &roscontroller::local_pos_callback, this); + local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this); multi_msg = true; } @@ -444,20 +439,15 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c) m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it) { if (it->second == "mavros_msgs/ExtendedState") { - flight_status_sub = n_c.subscribe( - it->first, 100, &roscontroller::flight_extended_status_update, this); + flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this); } else if (it->second == "mavros_msgs/State") { - flight_status_sub = n_c.subscribe( - it->first, 100, &roscontroller::flight_status_update, this); + flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this); } else if (it->second == "mavros_msgs/BatteryStatus") { - battery_sub = - n_c.subscribe(it->first, 5, &roscontroller::battery, this); + battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); } else if (it->second == "sensor_msgs/NavSatFix") { - current_position_sub = - n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); + current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); } else if (it->second == "std_msgs/Float64") { - relative_altitude_sub = - n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); + relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); } std::cout << "Subscribed to: " << it->first << endl; @@ -1059,13 +1049,10 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.altitude = neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; - // cout<<"Got" << neighbours_pos_payload[0] <<", " << - //neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; gps_rb(nei_pos, cvt_neighbours_pos_payload); /*Extract robot id of the neighbour*/ uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); - ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], - cvt_neighbours_pos_payload[1]); + //ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); /*pass neighbour position to local maintaner*/ buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1], @@ -1089,60 +1076,60 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res) { int rc_cmd; switch (req.command) { - case mavros_msgs::CommandCode::NAV_TAKEOFF: - ROS_INFO("RC_call: TAKE OFF!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::NAV_LAND: - ROS_INFO("RC_Call: LAND!!!! sending land"); - rc_cmd = mavros_msgs::CommandCode::NAV_LAND; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: - rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; - armstate = req.param1; - if (armstate) { - ROS_INFO("RC_Call: ARM!!!!"); + case mavros_msgs::CommandCode::NAV_TAKEOFF: + ROS_INFO("RC_call: TAKE OFF!!!!"); + rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; buzzuav_closures::rc_call(rc_cmd); res.success = true; - } else { - ROS_INFO("RC_Call: DISARM!!!!"); - buzzuav_closures::rc_call(rc_cmd + 1); + break; + case mavros_msgs::CommandCode::NAV_LAND: + ROS_INFO("RC_Call: LAND!!!!"); + rc_cmd = mavros_msgs::CommandCode::NAV_LAND; + buzzuav_closures::rc_call(rc_cmd); res.success = true; - } - break; - case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: - ROS_INFO("RC_Call: GO HOME!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::NAV_WAYPOINT: - ROS_INFO("RC_Call: GO TO!!!! "); - buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7); - rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: - ROS_INFO("RC_Call: Gimbal!!!! "); - buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3); - rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case CMD_REQUEST_UPDATE: - ROS_INFO("RC_Call: Update Fleet Status!!!!"); - rc_cmd = 666; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - default: - res.success = false; - break; + break; + case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: + rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; + armstate = req.param1; + if (armstate) { + ROS_INFO("RC_Call: ARM!!!!"); + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + } else { + ROS_INFO("RC_Call: DISARM!!!!"); + buzzuav_closures::rc_call(rc_cmd + 1); + res.success = true; + } + break; + case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: + ROS_INFO("RC_Call: GO HOME!!!!"); + rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case mavros_msgs::CommandCode::NAV_WAYPOINT: + ROS_INFO("RC_Call: GO TO!!!! "); + buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7); + rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: + ROS_INFO("RC_Call: Gimbal!!!! "); + buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3); + rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case CMD_REQUEST_UPDATE: + //ROS_INFO("RC_Call: Update Fleet Status!!!!"); + rc_cmd = CMD_REQUEST_UPDATE; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + default: + res.success = false; + break; } return true; } From 32c10dbc42497d12f7b3dc57a2aab0f601d280b5 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 30 Aug 2017 14:58:44 -0400 Subject: [PATCH 05/39] fix topic naming and rosbuzz launch --- buzz_scripts/flock.bzz | 7 +- buzz_scripts/graphform.bzz | 29 +- buzz_scripts/graphform.bzz_old | 826 ----------------------------- buzz_scripts/include/barrier.bzz | 12 +- buzz_scripts/include/uavstates.bzz | 39 +- buzz_scripts/testalone.bzz | 3 +- include/roscontroller.h | 1 + launch/launch_config/husky.yaml | 17 - launch/launch_config/m100.yaml | 17 - launch/launch_config/solo.yaml | 21 - launch/launch_config/topics.yaml | 2 +- launch/rosbuzz.launch | 2 +- launch/rosbuzzd.launch | 22 + src/buzz_utility.cpp | 10 +- src/roscontroller.cpp | 26 +- 15 files changed, 87 insertions(+), 947 deletions(-) delete mode 100644 buzz_scripts/graphform.bzz_old delete mode 100644 launch/launch_config/husky.yaml delete mode 100644 launch/launch_config/m100.yaml delete mode 100644 launch/launch_config/solo.yaml create mode 100644 launch/rosbuzzd.launch diff --git a/buzz_scripts/flock.bzz b/buzz_scripts/flock.bzz index bb2da81..b8de0cf 100644 --- a/buzz_scripts/flock.bzz +++ b/buzz_scripts/flock.bzz @@ -65,16 +65,19 @@ function action() { function init() { uav_initswarm() uav_initstig() - TARGET_ALTITUDE = 2.5 + id * 5 + # TARGET_ALTITUDE = 2.5 + id * 5 + statef=turnedoff + UAVSTATE = "TURNEDOFF" } # Executed at each time step. function step() { uav_rccmd() uav_neicmd() + uav_updatestig() statef() - uav_updatestig() + log("Current state: ", UAVSTATE) log("Swarm size: ",ROBOTS) if(id==0) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index ae0bd5b..7382af9 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -10,9 +10,10 @@ include "vstigenv.bzz" include "graphs/shapes_Y.bzz" -ROBOT_RADIUS=50 -ROBOT_DIAMETER=2.0*ROBOT_RADIUS -ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER +ROBOT_RADIUS = 50 +ROBOT_DIAMETER = 2.0*ROBOT_RADIUS +ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER +ROOT_ID = 2 # max velocity in cm/step ROBOT_MAXVEL = 250.0 @@ -802,7 +803,7 @@ function action(){ # function init() { # - #Adjust parameters here + # Global parameters for graph formation # m_unResponseTimeThreshold=10 m_unLabelSearchWaitTime=10 @@ -816,23 +817,32 @@ function init() { uav_initswarm() v_tag = stigmergy.create(m_lockstig) uav_initstig() - Reset() + # go to diff. height since no collision avoidance implemented yet TARGET_ALTITUDE = 2.5 + id * 1.5 + statef=turnedoff + UAVSTATE = "TURNEDOFF" + + # reset the graph + Reset() } # # Executed every step # function step(){ + # listen to potential RC uav_rccmd() + # get the swarm commands uav_neicmd() + # update the vstig (status/net/batt) uav_updatestig() + #update the graph UpdateNodeInfo() #reset message package to be sent m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} # - #act according to current state + # graph state machine # if(UAVSTATE=="GRAPH"){ if(m_eState=="STATE_FREE") @@ -849,17 +859,16 @@ function step(){ DoLock() } + # high level UAV state machine statef() debug(m_eState,m_nLabel) log("Current state: ", UAVSTATE) log("Swarm size: ", ROBOTS) -# if(id==0) -# stattab_print() #navigation - #broadcast messag + #broadcast message neighbors.broadcast("m",packmessage(m_selfMessage)) # @@ -895,7 +904,7 @@ function Reset(){ #set initial state, only one robot choose [A], while the rest choose [B] # #[A]The robot used to triger the formation process is defined as joined, - if(id==0){ + if(id==ROOT_ID){ m_nLabel=0 TransitionToJoined() } diff --git a/buzz_scripts/graphform.bzz_old b/buzz_scripts/graphform.bzz_old deleted file mode 100644 index 937d69c..0000000 --- a/buzz_scripts/graphform.bzz_old +++ /dev/null @@ -1,826 +0,0 @@ -# -# Include files -# -include "string.bzz" -include "vec2.bzz" -include "update.bzz" -include "barrier.bzz" # don't use a stigmergy id=11 with this header. -include "uavstates.bzz" # require an 'action' function to be defined here. - -ROBOT_RADIUS=50 -ROBOT_DIAMETER=2.0*ROBOT_RADIUS -ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER - -# -# Global variables -# - -# -#Save message from all neighours -#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot -m_MessageState={}#store received neighbour message -m_MessageLable={}#store received neighbour message -m_MessageReqLable={}#store received neighbour message -m_MessageReqID={}#store received neighbour message -m_MessageSide={}#store received neighbour message -m_MessageResponse={}#store received neighbour message -m_MessageRange={}#store received neighbour message -m_MessageBearing={}#store received neighbour message -m_neighbourCunt=0#used to cunt neighbours -#Save message from one neighbour -#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Side,Response,Range,Bearing -m_receivedMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE",.Range=0,.Bearing=0} -# -#Save the message to send -#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Side(side),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) -m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} - -#Current robot state -m_eState="STATE_FREE" - -#navigation vector -m_navigation={.x=0,.y=0} - -#Debug message to be displayed in qt-opengl -#m_ossDebugMsg - -#Debug vector to draw -#CVector2 m_cDebugVector - -#Table of the nodes in the graph -m_vecNodes={} -m_vecNodes_fixed={} - -#Current label being requested or chosen (-1 when none) -m_nLabel=-1 - -#Label request id -m_unRequestId=0 - -#Side -m_unbSide=0 - -#Global bias, used to map local coordinate to global coordinate -m_bias=0 - -#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate -m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} - -#Counter to wait for something to happen -m_unWaitCount=0 - -#Number of steps to wait before looking for a free label -m_unLabelSearchWaitTime=0 - -#Number of steps to wait for an answer to be received -m_unResponseTimeThreshold=0 - -#Number of steps to wait until giving up joining -m_unJoiningLostPeriod=0 - -#Tolerance distance to a target location -m_fTargetDistanceTolerance=0 - -#step cunt -step_cunt=0 - -#virtual stigmergy -v_tag = stigmergy.create(1) - -# Lennard-Jones parameters -EPSILON = 13.5 #3.5 - -# Lennard-Jones interaction magnitude - -function FlockInteraction(dist,target,epsilon){ - var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) - return mag -} - -function LimitAngle(angle){ - if(angle>2*math.pi) - return angle-2*math.pi - else if (angle<0) - return angle+2*math.pi - else - return angle -} - -# -# Calculates the angle of the given vector2. -# PARAM v: The vector2. -# RETURN: The angle of the vector. -# -Angle = function(v) { - return math.atan(v.y, v.x) -} - -# -#return the number of value in table -# -function count(table,value){ - var number=0 - var i=0 - while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ - m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 - if(m_vecNodes[i].StateAge==0) - m_vecNodes[i].State="UNASSIGNED" - } - i=i+1 - } -} - -# -#Transistion to state free -# -function TransitionToFree(){ - m_eState="STATE_FREE" - m_unWaitCount=m_unLabelSearchWaitTime - m_selfMessage.State=m_eState -} - -# -#Transistion to state asking -# -function TransitionToAsking(un_label){ - m_eState="STATE_ASKING" - m_nLabel=un_label - m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different - m_selfMessage.State=m_eState - m_selfMessage.ReqLable=m_nLabel - m_selfMessage.ReqID=m_unRequestId - - m_unWaitCount=m_unResponseTimeThreshold -} - -# -#Transistion to state joining -# -function TransitionToJoining(){ - m_eState="STATE_JOINING" - m_selfMessage.State=m_eState - m_selfMessage.Lable=m_nLabel - m_unWaitCount=m_unJoiningLostPeriod - - neighbors.listen("reply", - function(vid,value,rid){ - #store the received message - if(value.Lable==m_nLabel){ - m_cMeToPred.GlobalBearing=value.GlobalBearing - - } - }) - -} - -# -#Transistion to state joined -# -function TransitionToJoined(){ - m_eState="STATE_JOINED" - m_selfMessage.State=m_eState - m_selfMessage.Lable=m_nLabel - m_vecNodes[m_nLabel].State="ASSIGNED" - neighbors.ignore("reply") - - #write statues - v_tag.put(m_nLabel, 1) - - m_navigation.x=0.0 - m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) -} - -# -#Transistion to state Lock, lock the current formation -# -function TransitionToLock(){ - m_eState="STATE_LOCK" - m_selfMessage.State=m_eState - m_selfMessage.Lable=m_nLabel - m_vecNodes[m_nLabel].State="ASSIGNED" - - m_navigation.x=0.0 - m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) -} - -# -# Do free -# -function DoFree() { - m_selfMessage.State=m_eState - #wait for a while before looking for a lable - if(m_unWaitCount>0) - m_unWaitCount=m_unWaitCount-1 - - #find a set of joined robots - var setJoinedLables={} - var setJoinedIndexes={} - var i=0 - var j=0 - while(i0){ - TransitionToAsking(unFoundLable) - return - } - - #navigation - #if there is a joined robot within sight, move around joined robots - #else, gather with other free robots - if(size(setJoinedIndexes)>0){ - var tempvec_P={.x=0.0,.y=0.0} - var tempvec_N={.x=0.0,.y=0.0} - i=0 - while(imath.pi) - S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi - else - S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi - - var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing) - - #the vector from self to target in global coordinate - var S2Target=math.vec2.add(S2Pred,P2Target) - #change the vector to local coordinate of self - var S2Target_bearing=Angle(S2Target) - m_bias=m_cMeToPred.Bearing-S2PGlobalBearing - #S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17 - m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) - - - - #test if is already in desired position - if(math.abs(S2Target.x)m_MessageRange[mapRequests[i]]) - ReqIndex=i - i=i+1 - } - #get the best index, whose Reqlable and Reqid are - ReqLable=m_MessageReqLable[mapRequests[ReqIndex]] - var ReqID=m_MessageReqID[mapRequests[ReqIndex]] - m_selfMessage.ReqLable=ReqLable - m_selfMessage.ReqID=ReqID - m_selfMessage.Response="REQ_GRANTED" - } - - #lost pred, wait for some time and transit to free - #if(seenPred==0){ - #m_unWaitCount=m_unWaitCount-1 - #if(m_unWaitCount==0){ - #TransitionToFree() - #return - #} - #} - - - m_navigation.x=0.0 - m_navigation.y=0.0 - uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) - - - #check if should to transists to lock - - - if(v_tag.size()==ROBOTS){ - TransitionToLock() - } -} - -# -#Do Lock -# -function DoLock(){ -m_selfMessage.State=m_eState -m_selfMessage.Lable=m_nLabel - -m_navigation.x=0.0 -m_navigation.y=0.0 - -#collect preds information -var i=0 -var mypred1={.range=0,.bearing=0} -var mypred2={.range=0,.bearing=0} - -while(i1){ - var cDir={.x=0.0,.y=0.0} - var cDir1={.x=0.0,.y=0.0} - var cDir2={.x=0.0,.y=0.0} - cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing) - cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing) - #cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing) - #cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing) - cDir=math.vec2.add(cDir1,cDir2) - - cDir=math.vec2.scale(cDir,100) - m_navigation.x=cDir.x - m_navigation.y=cDir.y - #log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2) - log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1) -} -#move -uav_moveto(m_navigation.x,m_navigation.y) -} - -function action(){ - statef=action - UAVSTATE="GRAPH" -} - -# -# Executed at init -# -function init() { - # - #Adjust parameters here - # - m_unResponseTimeThreshold=10 - m_unLabelSearchWaitTime=10 - m_fTargetDistanceTolerance=10 - m_unJoiningLostPeriod=100 - - # - # Join Swarm - # - uav_initswarm() - Reset(); -} - -# -# Executed every step -# -function step(){ - uav_rccmd() - uav_neicmd() - #update the graph - UpdateNodeInfo() - #reset message package to be sent - m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} - # - #act according to current state - # - if(UAVSTATE=="GRAPH"){ - if(m_eState=="STATE_FREE") - DoFree() - else if(m_eState=="STATE_ESCAPE") - DoEscape() - else if(m_eState=="STATE_ASKING") - DoAsking() - else if(m_eState=="STATE_JOINING") - DoJoining() - else if(m_eState=="STATE_JOINED") - DoJoined() - else if(m_eState=="STATE_LOCK") - DoLock() - } - - statef() - - - debug(m_eState,m_nLabel) - log("Current state: ", UAVSTATE) - log("Swarm size: ", ROBOTS) - #navigation - - #broadcast messag - neighbors.broadcast("msg",m_selfMessage) - - # - #clean message storage - m_MessageState={}#store received neighbour message - m_MessageLable={}#store received neighbour message - m_MessageReqLable={}#store received neighbour message - m_MessageReqID={}#store received neighbour message - m_MessageSide={}#store received neighbour message - m_MessageResponse={}#store received neighbour message - m_MessageRange={}#store received neighbour message - m_MessageBearing={}#store received neighbour message - m_neighbourCunt=0 - - - #step cunt+1 - step_cunt=step_cunt+1 -} - -# -# Executed when reset -# -function Reset(){ - m_vecNodes={} - m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary - m_vecNodes_fixed={} - m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") - m_nLabel=-1 - - #start listening - start_listen() - # - #set initial state, only one robot choose [A], while the rest choose [B] - # - #[A]The robot used to triger the formation process is defined as joined, - if(id==0){ - m_nLabel=0 - TransitionToJoined() - } - #[B]Other robots are defined as free. - else{ - TransitionToFree() - } -} - -# -# Executed upon destroy -# -function destroy() { - #clear neighbour message - uav_moveto(0.0,0.0) - m_vecNodes={} - #stop listening - neighbors.ignore("msg") -} diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/barrier.bzz index 6192b65..8c55806 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -12,9 +12,9 @@ BARRIER_VSTIG = 11 # # Sets a barrier # -function barrier_set(threshold, transf, resumef) { +function barrier_set(threshold, transf, resumef, bdt) { statef = function() { - barrier_wait(threshold, transf, resumef); + barrier_wait(threshold, transf, resumef, bdt); } barrier = stigmergy.create(BARRIER_VSTIG) } @@ -31,14 +31,16 @@ function barrier_ready() { # BARRIER_TIMEOUT = 200 timeW=0 -function barrier_wait(threshold, transf, resumef) { - barrier.get(id) +function barrier_wait(threshold, transf, resumef, bdt) { + #barrier.get(id) barrier.put(id, 1) UAVSTATE = "BARRIERWAIT" + if(bdt!=-1) + neighbors.broadcast("cmd", brd) if(barrier.size() >= threshold) { # getlowest() transf() - } else if(timeW>=BARRIER_TIMEOUT) { + } else if(timeW >= BARRIER_TIMEOUT) { barrier = nil resumef() timeW=0 diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 10d7118..180c6cc 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -11,8 +11,6 @@ goal = {.range=0, .bearing=0} function uav_initswarm() { s = swarm.create(1) s.join() - statef=turnedoff - UAVSTATE = "TURNEDOFF" } function turnedoff() { @@ -30,7 +28,7 @@ function takeoff() { statef=takeoff if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,action,land) + barrier_set(ROBOTS,action,land,22) barrier_ready() } else { @@ -49,7 +47,7 @@ function land() { uav_land() } else { - barrier_set(ROBOTS,turnedoff,land) + barrier_set(ROBOTS,turnedoff,land,21) barrier_ready() timeW=0 #barrier = nil @@ -129,23 +127,22 @@ function uav_rccmd() { function uav_neicmd() { neighbors.listen("cmd", - function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and UAVSTATE!="TAKEOFF") { - statef=takeoff - } else if(value==21) { - statef=land - } else if(value==400 and UAVSTATE=="IDLE") { - uav_arm() - } else if(value==401 and UAVSTATE=="IDLE"){ - uav_disarm() - } else if(value==16){ - neighbors.listen("gt",function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - # uav_storegoal(lat, lon, alt) - }) - statef=goto - } + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and UAVSTATE!="TAKEOFF") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and UAVSTATE=="IDLE") { + uav_arm() + } else if(value==401 and UAVSTATE=="IDLE"){ + uav_disarm() + } else if(value==16){ + # neighbors.listen("gt",function(vid, value, rid) { + # print("Got (", vid, ",", value, ") from robot #", rid) + # # if(gt.id == id) statef=goto + # }) + } }) } diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testalone.bzz index e607d4e..7c987d6 100644 --- a/buzz_scripts/testalone.bzz +++ b/buzz_scripts/testalone.bzz @@ -12,7 +12,8 @@ function action() { # Executed once at init time. function init() { - uav_initswarm() + statef=turnedoff + UAVSTATE = "TURNEDOFF" uav_initstig() } diff --git a/include/roscontroller.h b/include/roscontroller.h index ae0b7d0..bc5e117 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -111,6 +111,7 @@ private: ros::Subscriber users_sub; ros::Subscriber battery_sub; ros::Subscriber payload_sub; + ros::Subscriber flight_estatus_sub; ros::Subscriber flight_status_sub; ros::Subscriber obstacle_sub; ros::Subscriber Robot_id_sub; diff --git a/launch/launch_config/husky.yaml b/launch/launch_config/husky.yaml deleted file mode 100644 index c878cc4..0000000 --- a/launch/launch_config/husky.yaml +++ /dev/null @@ -1,17 +0,0 @@ -topics: - gps : global_position - localpos : local_position - battery : power_status - status : flight_status - fcclient : dji_mavcmd - setpoint : setpoint_position/local - armclient: dji_mavarm - modeclient: dji_mavmode - altitude: rel_alt - stream: set_stream_rate - -type: - gps : sensor_msgs/NavSatFix - battery : mavros_msgs/BatteryStatus - status : mavros_msgs/ExtendedState - altitude: std_msgs/Float64 diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml deleted file mode 100644 index c878cc4..0000000 --- a/launch/launch_config/m100.yaml +++ /dev/null @@ -1,17 +0,0 @@ -topics: - gps : global_position - localpos : local_position - battery : power_status - status : flight_status - fcclient : dji_mavcmd - setpoint : setpoint_position/local - armclient: dji_mavarm - modeclient: dji_mavmode - altitude: rel_alt - stream: set_stream_rate - -type: - gps : sensor_msgs/NavSatFix - battery : mavros_msgs/BatteryStatus - status : mavros_msgs/ExtendedState - altitude: std_msgs/Float64 diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml deleted file mode 100644 index f9ca5d2..0000000 --- a/launch/launch_config/solo.yaml +++ /dev/null @@ -1,21 +0,0 @@ -topics: - gps : mavros/global_position/global - battery : mavros/battery - status : mavros/state - fcclient: mavros/cmd/command - setpoint: mavros/setpoint_position/local - armclient: mavros/cmd/arming - modeclient: mavros/set_mode - localpos: mavros/local_position/pose - stream: mavros/set_stream_rate - altitude: mavros/global_position/rel_alt -type: - gps : sensor_msgs/NavSatFix - # for SITL Solo - battery : mavros_msgs/BatteryState - # for solo - #battery : mavros_msgs/BatteryStatus - status : mavros_msgs/State - altitude: std_msgs/Float64 -environment : - environment : solo-simulator diff --git a/launch/launch_config/topics.yaml b/launch/launch_config/topics.yaml index 8f1295b..e7358cd 100644 --- a/launch/launch_config/topics.yaml +++ b/launch/launch_config/topics.yaml @@ -2,7 +2,7 @@ topics: gps : global_position/global battery : battery status : state - estatus : extended_state + estatus : extended_state fcclient: cmd/command setpoint: setpoint_position/local armclient: cmd/arming diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index de7857d..c2531f4 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -7,7 +7,7 @@ - + diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch new file mode 100644 index 0000000..de7857d --- /dev/null +++ b/launch/rosbuzzd.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 8b883e9..3d5531d 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -487,7 +487,7 @@ int create_stig_tables() { if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("%s: Error loading Buzz script", bo_filename); + ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename); return 0; } /* Register hook functions */ @@ -550,14 +550,14 @@ int create_stig_tables() { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id); return 0; } // Register hook functions if(buzz_register_hooks() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); + ROS_ERROR("[%i] Error registering hooks (update)", Robot_id); return 0; } /* Create vstig tables @@ -606,14 +606,14 @@ int create_stig_tables() { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id); return 0; } // Register hook functions if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); - fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); + ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id); return 0; } /* Create vstig tables diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index aa04afa..639c9ea 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -337,7 +337,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle) std::string battery_topic; node_handle.getParam("topics/battery", battery_topic); - m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryState")); + m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryStatus")); std::string status_topic; node_handle.getParam("topics/status", status_topic); @@ -439,9 +439,9 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c) m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it) { if (it->second == "mavros_msgs/ExtendedState") { - flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this); + flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this); } else if (it->second == "mavros_msgs/State") { - flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this); + flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); } else if (it->second == "mavros_msgs/BatteryStatus") { battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); } else if (it->second == "sensor_msgs/NavSatFix") { @@ -459,31 +459,17 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c) /-------------------------------------------------------*/ std::string roscontroller::Compile_bzz(std::string bzzfile_name) { - /*TODO: change to bzzc instead of bzzparse and also add -I for includes*/ /*Compile the buzz code .bzz to .bo*/ stringstream bzzfile_in_compile; std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - // bzzfile_in_compile << path << "/"; - // path = bzzfile_in_compile.str(); - // bzzfile_in_compile.str(""); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0, name.find_last_of(".")); bzzfile_in_compile << "bzzc -I " << path - << "include/"; //<<" "< Date: Wed, 30 Aug 2017 17:22:37 -0400 Subject: [PATCH 06/39] fix graph bzz bug --- buzz_scripts/graphform.bzz | 41 ++++-------------------------- buzz_scripts/include/barrier.bzz | 17 +++++++------ buzz_scripts/include/uavstates.bzz | 2 +- buzz_scripts/include/vstigenv.bzz | 27 +++++++++++++++----- 4 files changed, 35 insertions(+), 52 deletions(-) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 7382af9..2bcdd30 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -4,9 +4,10 @@ include "string.bzz" include "vec2.bzz" include "update.bzz" -include "barrier.bzz" # don't use a stigmergy id=11 with this header. + +include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. +include "barrier.bzz" # reserve stigmergy id=11 for this header. include "uavstates.bzz" # require an 'action' function to be defined here. -include "vstigenv.bzz" include "graphs/shapes_Y.bzz" @@ -88,7 +89,7 @@ m_fTargetDistanceTolerance=0 #step cunt step_cunt=0 -#virtual stigmergy +# virtual stigmergy for the LOCK barrier. m_lockstig = 1 # Lennard-Jones parameters, may need change @@ -101,24 +102,6 @@ function FlockInteraction(dist,target,epsilon){ return mag } -function LimitAngle(angle){ - if(angle>2*math.pi) - return angle-2*math.pi - else if (angle<0) - return angle+2*math.pi - else - return angle -} - -# -# Calculates the angle of the given vector2. -# PARAM v: The vector2. -# RETURN: The angle of the vector. -# -Angle = function(v) { - return math.atan(v.y, v.x) -} - # #return the number of value in table # @@ -205,20 +188,6 @@ function find(table,value){ return index } -function pow(base,exponent){ - var i=0 - var renturn_val=1 - if(exponent==0) - return 1 - else{ - while(i= threshold) { # getlowest() transf() @@ -44,14 +43,16 @@ function barrier_wait(threshold, transf, resumef, bdt) { barrier = nil resumef() timeW=0 - } + } else if(bdt!=-1) + neighbors.broadcast("cmd", bdt) + timeW = timeW+1 } -# get the lowest id of the fleet, but requires too much bandwidth +# get the lowest id of the fleet, but requires too much bandwidth... function getlowest(){ - Lid = 20; - u=20 + Lid = 15; + u=15 while(u>=0){ tab = barrier.get(u) if(tab!=nil){ diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 180c6cc..8b5a9dd 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -28,7 +28,7 @@ function takeoff() { statef=takeoff if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,action,land,22) + barrier_set(ROBOTS,action,land,-1) barrier_ready() } else { diff --git a/buzz_scripts/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz index c983fcc..7fd8e42 100644 --- a/buzz_scripts/include/vstigenv.bzz +++ b/buzz_scripts/include/vstigenv.bzz @@ -1,16 +1,29 @@ -STATUS_VSTIG = 10 -GROUND_VSTIG = 11 +######################################## +# +# FLEET V.STIGMERGY-RELATED FUNCTIONS +# +######################################## +# +# Constants +# +STATUS_VSTIG = 20 +GROUND_VSTIG = 21 +HIGHEST_ROBOTID = 14 WAIT4STEP = 10 -v_status = {} -v_ground = {} + +# +# Init var +# +var v_status = {} +var v_ground = {} b_updating = 0 +counter=WAIT4STEP function uav_initstig() { v_status = stigmergy.create(STATUS_VSTIG) v_ground = stigmergy.create(GROUND_VSTIG) } -counter=WAIT4STEP function uav_updatestig() { # TODO: Push values on update only. if(counter<=0) { @@ -78,7 +91,7 @@ function stattab_print() { if(v_status.size()>0) { if(b_updating==0) { u=0 - while(u<8){ + while(u0) { if(b_updating==0) { u=0 - while(u<8){ + while(u Date: Wed, 30 Aug 2017 20:29:10 -0400 Subject: [PATCH 07/39] launch file change --- misc/cmdlinectr.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 82b7a0b..b519148 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -31,5 +31,5 @@ function stoprobot { } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch - rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch + rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch } From 397a6ccda9ff8fbdb34b0bd446274d62419eb02e Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 30 Aug 2017 21:02:37 -0400 Subject: [PATCH 08/39] logger addition --- .gitignore | 1 + buzz_scripts/graphform.bzz | 41 +++--------------- include/buzz_utility.h | 2 + src/buzz_utility.cpp | 4 ++ src/roscontroller.cpp | 87 ++++++++++++++++++++++++++++++++++---- 5 files changed, 90 insertions(+), 45 deletions(-) diff --git a/.gitignore b/.gitignore index 64f22eb..f426dc9 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ src/test* *.bo *.bdb *.bdbg +buzz_scripts/log* diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 7382af9..2bcdd30 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -4,9 +4,10 @@ include "string.bzz" include "vec2.bzz" include "update.bzz" -include "barrier.bzz" # don't use a stigmergy id=11 with this header. + +include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. +include "barrier.bzz" # reserve stigmergy id=11 for this header. include "uavstates.bzz" # require an 'action' function to be defined here. -include "vstigenv.bzz" include "graphs/shapes_Y.bzz" @@ -88,7 +89,7 @@ m_fTargetDistanceTolerance=0 #step cunt step_cunt=0 -#virtual stigmergy +# virtual stigmergy for the LOCK barrier. m_lockstig = 1 # Lennard-Jones parameters, may need change @@ -101,24 +102,6 @@ function FlockInteraction(dist,target,epsilon){ return mag } -function LimitAngle(angle){ - if(angle>2*math.pi) - return angle-2*math.pi - else if (angle<0) - return angle+2*math.pi - else - return angle -} - -# -# Calculates the angle of the given vector2. -# PARAM v: The vector2. -# RETURN: The angle of the vector. -# -Angle = function(v) { - return math.atan(v.y, v.x) -} - # #return the number of value in table # @@ -205,20 +188,6 @@ function find(table,value){ return index } -function pow(base,exponent){ - var i=0 - var renturn_val=1 - if(exponent==0) - return 1 - else{ - while(i::iterator it = + neighbours_pos_map.begin(); + log << neighbours_pos_map.size()<< ","; + for (; it != neighbours_pos_map.end(); ++it) + { + log << (double)it->second.x << "," << (double)it->second.y + << "," << (double)it->second.z << ","; + } + const uint8_t shrt_id= 0xFF; + float result; + if ( GetAPIRssi(shrt_id, result) ) + log<second.x << "," << (double)it->second.y << "," << (double)it->second.z; } - log << std::endl; + log << std::endl;*/ } /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ // no_of_robots=get_number_of_robots(); get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); + /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF + SCRIPT IS NOT graphform.bzz*/ + static buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); + buzzvm_gload(VM); + buzzobj_t graph_state = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + std::stringstream state_buff; + state_buff<< graph_state->s.value.str<<","; + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); + buzzvm_gload(VM); + buzzobj_t uav_state = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + state_buff<< uav_state->s.value.str; + log<0) no_of_robots // =neighbours_pos_map.size()+1; // buzz_utility::set_robot_var(no_of_robots); @@ -337,7 +389,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle) std::string battery_topic; node_handle.getParam("topics/battery", battery_topic); - m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryStatus")); + m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryState")); std::string status_topic; node_handle.getParam("topics/status", status_topic); @@ -439,9 +491,9 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c) m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it) { if (it->second == "mavros_msgs/ExtendedState") { - flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this); + flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this); } else if (it->second == "mavros_msgs/State") { - flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); + flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this); } else if (it->second == "mavros_msgs/BatteryStatus") { battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); } else if (it->second == "sensor_msgs/NavSatFix") { @@ -459,17 +511,31 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c) /-------------------------------------------------------*/ std::string roscontroller::Compile_bzz(std::string bzzfile_name) { + /*TODO: change to bzzc instead of bzzparse and also add -I for includes*/ /*Compile the buzz code .bzz to .bo*/ stringstream bzzfile_in_compile; std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + // bzzfile_in_compile << path << "/"; + // path = bzzfile_in_compile.str(); + // bzzfile_in_compile.str(""); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0, name.find_last_of(".")); bzzfile_in_compile << "bzzc -I " << path - << "include/"; + << "include/"; //<<" "< Date: Thu, 31 Aug 2017 16:17:27 -0400 Subject: [PATCH 09/39] graph tuning and sdk topic fix --- buzz_scripts/graphform.bzz | 6 +++--- src/roscontroller.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 2bcdd30..c395c11 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -17,7 +17,7 @@ ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER ROOT_ID = 2 # max velocity in cm/step -ROBOT_MAXVEL = 250.0 +ROBOT_MAXVEL = 150.0 # # Global variables @@ -776,8 +776,8 @@ function init() { # m_unResponseTimeThreshold=10 m_unLabelSearchWaitTime=10 - m_fTargetDistanceTolerance=50 - m_fTargetAngleTolerance=0.1 + m_fTargetDistanceTolerance=150 + m_fTargetAngleTolerance=0.2 m_unJoiningLostPeriod=100 # diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 0ddfd74..55ac7c1 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1092,7 +1092,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { gps_rb(nei_pos, cvt_neighbours_pos_payload); /*Extract robot id of the neighbour*/ uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); - //ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); + ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); /*pass neighbour position to local maintaner*/ buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1], From 64f4ec41a58ea97d1ebbbe38e18041a5149073b2 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Thu, 31 Aug 2017 17:25:34 -0400 Subject: [PATCH 10/39] log prints in graphform --- buzz_scripts/graphform.bzz | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index c395c11..a857309 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -546,8 +546,10 @@ function DoAsking(){ #the request Label be the same as requesed #get a respond if(m_MessageState[i]=="STATE_JOINED"){ + log("received label = ",m_MessageReqLabel[i]) if(m_MessageReqLabel[i]==m_nLabel) if(m_MessageResponse[i]!="REQ_NONE"){ + log("get response") psResponse=i }} i=i+1 @@ -565,6 +567,7 @@ function DoAsking(){ } } else{ + log("respond id=",m_MessageReqID[psResponse]) if(m_MessageReqID[psResponse]!=m_unRequestId){ m_vecNodes[m_nLabel].State="ASSIGNING" m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod @@ -675,10 +678,13 @@ function DoJoined(){ while(i0) @@ -527,7 +527,7 @@ function DoFree() { uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } #set message - m_selfMessage.State=s2i(m_eState) + m_selfMessage.State=s2i(UAVSTATE) } @@ -558,7 +558,7 @@ function DoAsking(){ if(psResponse==-1){ #no response, wait m_unWaitCount=m_unWaitCount-1 - m_selfMessage.State=s2i(m_eState) + m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqID=m_unRequestId if(m_unWaitCount==0){ @@ -576,7 +576,6 @@ function DoAsking(){ if(m_MessageReqID[psResponse]==m_unRequestId){ if(m_MessageResponse[psResponse]=="REQ_GRANTED"){ TransitionToJoining() - #TransitionToJoined() return } else{ @@ -656,7 +655,7 @@ function DoJoining(){ } #pack the communication package - m_selfMessage.State=s2i(m_eState) + m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel } @@ -665,7 +664,7 @@ function DoJoining(){ #Do joined # function DoJoined(){ - m_selfMessage.State=s2i(m_eState) + m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel #collect all requests @@ -754,7 +753,7 @@ function DoJoined(){ #Do Lock # function DoLock(){ -m_selfMessage.State=s2i(m_eState) +m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel m_navigation.x=0.0 m_navigation.y=0.0 @@ -772,7 +771,10 @@ uav_moveto(m_navigation.x, m_navigation.y, 0.0) function action(){ statef=action - UAVSTATE="GRAPH" + UAVSTATE="STATE_FREE" + + # reset the graph + Reset() } # @@ -792,27 +794,24 @@ function init() { # Join Swarm # uav_initswarm() - v_tag = stigmergy.create(m_lockstig) - uav_initstig() + v_tag = stigmergy.create(m_lockstig) + uav_initstig() # go to diff. height since no collision avoidance implemented yet TARGET_ALTITUDE = 2.5 + id * 1.5 statef=turnedoff UAVSTATE = "TURNEDOFF" - - # reset the graph - Reset() } # # Executed every step # -function step(){ +function step() { # listen to potential RC uav_rccmd() # get the swarm commands uav_neicmd() # update the vstig (status/net/batt) - uav_updatestig() + uav_updatestig() #update the graph UpdateNodeInfo() @@ -821,27 +820,23 @@ function step(){ # # graph state machine # - if(UAVSTATE=="GRAPH"){ - if(m_eState=="STATE_FREE") - DoFree() - else if(m_eState=="STATE_ESCAPE") - DoEscape() - else if(m_eState=="STATE_ASKING") - DoAsking() - else if(m_eState=="STATE_JOINING") - DoJoining() - else if(m_eState=="STATE_JOINED") - DoJoined() - else if(m_eState=="STATE_LOCK") - DoLock() - } + if(UAVSTATE=="STATE_FREE") + statef=DoFree + else if(UAVSTATE=="STATE_ESCAPE") + statef=DoEscape + else if(UAVSTATE=="STATE_ASKING") + statef=DoAsking + else if(UAVSTATE=="STATE_JOINING") + statef=DoJoining + else if(UAVSTATE=="STATE_JOINED") + statef=DoJoined + else if(UAVSTATE=="STATE_LOCK") + statef=DoLock # high level UAV state machine statef() - - debug(m_eState,m_nLabel) - log("Current state: ", UAVSTATE) + log("Current state: ", UAVSTATE, " and label: ", m_nLabel) log("Swarm size: ", ROBOTS) #navigation @@ -868,12 +863,8 @@ function step(){ # Executed when reset # function Reset(){ -# m_vecNodes={} -# m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary -# m_vecNodes_fixed={} -# m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") Read_Graph() - m_nLabel=-1 + m_nLabel=-1 #start listening start_listen() diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz new file mode 100644 index 0000000..78caa0d --- /dev/null +++ b/buzz_scripts/graphformGPS.bzz @@ -0,0 +1,829 @@ +# +# Include files +# +include "string.bzz" +include "vec2.bzz" +include "update.bzz" + +include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. +include "barrier.bzz" # reserve stigmergy id=11 for this header. +include "uavstates.bzz" # require an 'action' function to be defined here. + +include "graphs/shapes_Y.bzz" + +ROBOT_RADIUS = 50 +ROBOT_DIAMETER = 2.0*ROBOT_RADIUS +ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER +ROOT_ID = 2 + +# max velocity in cm/step +ROBOT_MAXVEL = 150.0 + +# +# Global variables +# + +# +# Save message from all neighours +#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot +m_MessageState={}#store received neighbour message +m_MessageLabel={}#store received neighbour message +m_MessageReqLabel={}#store received neighbour message +m_MessageReqID={}#store received neighbour message +m_MessageResponse={}#store received neighbour message +m_MessageRange={}#store received neighbour message +m_MessageBearing={}#store received neighbour message +m_neighbourCount=0#used to cunt neighbours +#Save message from one neighbour +#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing +m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} + +# +#Save the message to send +#The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) +m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + +#Current robot state +# m_eState="STATE_FREE" # replace with default UAVSTATE + +#navigation vector +m_navigation={.x=0,.y=0} + +#Debug message to be displayed in qt-opengl +#m_ossDebugMsg + +#Debug vector to draw +#CVector2 m_cDebugVector + +#Current label being requested or chosen (-1 when none) +m_nLabel=-1 +m_messageID={} +#neighbor distance to lock the current pattern +lock_neighbor_id={} +lock_neighbor_dis={} + +#Label request id +m_unRequestId=0 + +#Global bias, used to map local coordinate to global coordinate +m_bias=0 + +#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate +m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + +#Counter to wait for something to happen +m_unWaitCount=0 + +#Number of steps to wait before looking for a free label +m_unLabelSearchWaitTime=0 + +#Number of steps to wait for an answer to be received +m_unResponseTimeThreshold=0 + +#Number of steps to wait until giving up joining +m_unJoiningLostPeriod=0 + +#Tolerance distance to a target location +m_fTargetDistanceTolerance=0 + +#step cunt +step_cunt=0 + +# virtual stigmergy for the LOCK barrier. +m_lockstig = 1 + +# Lennard-Jones parameters, may need change +EPSILON = 1800 #13.5 the LJ parameter for other robots + +# Lennard-Jones interaction magnitude + +function FlockInteraction(dist,target,epsilon){ + var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + return mag +} + +# +#return the number of value in table +# +function count(table,value){ + var number=0 + var i=0 + while(i=0){ + pon=0 + } + else{ + pon=1 + } + var b=math.abs(send_table.Bearing) + send_value=r_id*1000+pon*100+b + return send_value +} +# +#unpack message +# +function unpackmessage(recv_value){ + var wan=(recv_value-recv_value%100000)/100000 + recv_value=recv_value-wan*100000 + var qian=(recv_value-recv_value%10000)/10000 + recv_value=recv_value-qian*10000 + var bai=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-bai*1000 + var shi=(recv_value-recv_value%10)/10 + recv_value=recv_value-shi*10 + var ge=recv_value + var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0} + return_table.State=wan + return_table.Label=qian + return_table.ReqLabel=bai + return_table.ReqID=shi + return_table.Response=ge + return return_table +} +# +#unpack guide message +# +function unpack_guide_msg(recv_value){ +log(id,"I pass value=",recv_value) + var qian=(recv_value-recv_value%1000)/1000 + recv_value=recv_value-qian*1000 + var bai=(recv_value-recv_value%100)/100 + recv_value=recv_value-bai*100 + var b=recv_value + var return_table={.Label=0.0,.Bearing=0.0} + return_table.Label=qian + if(bai==1){ + b=b*-1.0 + } + return_table.Bearing=b + return return_table +} + +#get the target distance to neighbr nei_id +function target4label(nei_id){ + var return_val="miss" + var i=0 + while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ + m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 + if(m_vecNodes[i].StateAge==0) + m_vecNodes[i].State="UNASSIGNED" + } + i=i+1 + } +} + +# +#Transistion to state free +# +function TransitionToFree(){ + UAVSTATE="STATE_FREE" + m_unWaitCount=m_unLabelSearchWaitTime + m_selfMessage.State=s2i(UAVSTATE) +} + +# +#Transistion to state asking +# +function TransitionToAsking(un_label){ + UAVSTATE="STATE_ASKING" + m_nLabel=un_label + m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different + m_selfMessage.State=s2i(UAVSTATE) + m_selfMessage.ReqLabel=m_nLabel + m_selfMessage.ReqID=m_unRequestId + + m_unWaitCount=m_unResponseTimeThreshold +} + +# +#Transistion to state joining +# +function set_rc_goto() { + #get information of pred + var i=0 + var IDofPred=-1 + while(imath.pi) + S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi + else + S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi + + var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing) + + #the vector from self to target in global coordinate + var S2Target=math.vec2.add(S2Pred,P2Target) + #change the vector to local coordinate of self + var S2Target_bearing=math.atan(S2Target.y, S2Target.x) + m_bias=m_cMeToPred.Bearing-S2PGlobalBearing + S2Target_bearing=S2Target_bearing+m_bias + + gps_from_rb(math.vec2.length(S2Target)/100.0, S2Target_bearing) + uav_storegoal(goal.latitude, goal.longitude, position.altitude) + } +} + +function TransitionToJoining(){ + UAVSTATE="STATE_JOINING" + m_selfMessage.State=s2i(UAVSTATE) + m_selfMessage.Label=m_nLabel + m_unWaitCount=m_unJoiningLostPeriod + + set_rc_goto() + + neighbors.listen("r", + function(vid,value,rid){ + var recv_table={.Label=0,.Bearing=0.0} + recv_table=unpack_guide_msg(value) + #store the received message + if(recv_table.Label==m_nLabel){ + m_cMeToPred.GlobalBearing=recv_table.Bearing + } + }) +} + +# +#Transistion to state joined +# +function TransitionToJoined(){ + UAVSTATE="STATE_JOINED" + m_selfMessage.State=s2i(UAVSTATE) + m_selfMessage.Label=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + neighbors.ignore("r") + + #write statues + #v_tag.put(m_nLabel, m_lockstig) + barrier_create(m_lockstig) + barrier_ready() + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) +} + +# +#Transistion to state Lock, lock the current formation +# +function TransitionToLock(){ + UAVSTATE="STATE_LOCK" + m_selfMessage.State=s2i(UAVSTATE) + m_selfMessage.Label=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + #record neighbor distance + lock_neighbor_id={} + lock_neighbor_dis={} + var i=0 + while(i0) + m_unWaitCount=m_unWaitCount-1 + + #find a set of joined robots + var setJoinedLabels={} + var setJoinedIndexes={} + var i=0 + var j=0 + while(i0){ + TransitionToAsking(unFoundLabel) + return + } + + #set message + m_selfMessage.State=s2i(UAVSTATE) + +} + +# +#Do asking +# +function DoAsking(){ + #look for response from predecessor + var i=0 + var psResponse=-1 + while(im_MessageRange[mapRequests[i]]) + ReqIndex=i + i=i+1 + } + #get the best index, whose ReqLabel and Reqid are + ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]] + var ReqID=m_MessageReqID[mapRequests[ReqIndex]] + m_selfMessage.ReqLabel=ReqLabel + m_selfMessage.ReqID=ReqID + m_selfMessage.Response=r2i("REQ_GRANTED") + m_vecNodes[ReqLabel].State="ASSIGNING" + log("Label=",ReqLabel) + log("ID=",ReqID) + m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod + } + + #lost pred, wait for some time and transit to free + if(seenPred==0){ + m_unWaitCount=m_unWaitCount-1 + if(m_unWaitCount==0){ + TransitionToFree() + return + } + } + + #check if should to transists to lock + #write statues + #v_tag.get(m_nLabel) + #log(v_tag.size(), " of ", ROBOTS, " ready to lock") + #if(v_tag.size()==ROBOTS){ + # TransitionToLock() + #} + barrier_wait(ROBOTS, TransitionToLock, DoJoined) +} + +# +#Do Lock +# +function DoLock(){ + m_selfMessage.State=s2i(UAVSTATE) + m_selfMessage.Label=m_nLabel + m_navigation.x=0.0 + m_navigation.y=0.0 + #calculate motion vection + if(m_nLabel==0){ + m_navigation.x=0.0 #change value so that robot 0 will move + m_navigation.y=0.0 + } + if(m_nLabel!=0){ + m_navigation=motion_vector() + } + #move + uav_moveto(m_navigation.x, m_navigation.y, 0.0) +} + +function action(){ + statef=action + UAVSTATE="STATE_FREE" + + # reset the graph + Reset() +} + +# +# Executed at init +# +function init() { + # + # Global parameters for graph formation + # + m_unResponseTimeThreshold=10 + m_unLabelSearchWaitTime=10 + m_fTargetDistanceTolerance=100 + m_fTargetAngleTolerance=0.1 + m_unJoiningLostPeriod=100 + + # + # Join Swarm + # + uav_initswarm() + #v_tag = stigmergy.create(m_lockstig) + uav_initstig() + # go to diff. height since no collision avoidance implemented yet + TARGET_ALTITUDE = 2.5 + id * 1.5 + statef=turnedoff + UAVSTATE = "TURNEDOFF" +} + +# +# Executed every step +# +function step() { + # listen to potential RC + uav_rccmd() + # get the swarm commands + uav_neicmd() + # update the vstig (status/net/batt) + uav_updatestig() + + #update the graph + UpdateNodeInfo() + #reset message package to be sent + m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + # + # graph state machine + # + if(UAVSTATE=="STATE_FREE") + statef=DoFree + else if(UAVSTATE=="STATE_ESCAPE") + statef=DoEscape + else if(UAVSTATE=="STATE_ASKING") + statef=DoAsking + else if(UAVSTATE=="STATE_JOINING") + statef=DoJoining + else if(UAVSTATE=="STATE_JOINED") + statef=DoJoined + else if(UAVSTATE=="STATE_LOCK") + statef=DoLock + + # high level UAV state machine + statef() + + log("Current state: ", UAVSTATE, " and label: ", m_nLabel) + log("Swarm size: ", ROBOTS) + + #navigation + #broadcast message + neighbors.broadcast("m",packmessage(m_selfMessage)) + + # + #clean message storage + m_MessageState={}#store received neighbour message + m_MessageLabel={}#store received neighbour message + m_MessageReqLabel={}#store received neighbour message + m_MessageReqID={}#store received neighbour message + m_MessageResponse={}#store received neighbour message + m_MessageRange={}#store received neighbour message + m_MessageBearing={}#store received neighbour message + m_neighbourCount=0 + + + #step cunt+1 + step_cunt=step_cunt+1 +} + +# +# Executed when reset +# +function Reset(){ + Read_Graph() + m_nLabel=-1 + + #start listening + start_listen() + # + #set initial state, only one robot choose [A], while the rest choose [B] + # + #[A]The robot used to triger the formation process is defined as joined, + if(id==ROOT_ID){ + m_nLabel=0 + TransitionToJoined() + } + #[B]Other robots are defined as free. + else{ + TransitionToFree() + } +} + +# +# Executed upon destroy +# +function destroy() { + #clear neighbour message + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) + m_vecNodes={} + #stop listening + neighbors.ignore("m") +} diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/barrier.bzz index dbf5978..9fe0f28 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -7,44 +7,53 @@ # # Constants # -BARRIER_VSTIG = 11 BARRIER_TIMEOUT = 200 # in steps +timeW = 0 # # Sets a barrier # -function barrier_set(threshold, transf, resumef, bdt) { +function barrier_create(vstig_nb) { + # reset + timeW = 0 + # create barrier vstig + barrier = stigmergy.create(vstig_nb) +} + +function barrier_set(threshold, transf, resumef, vstig_nb) { statef = function() { - barrier_wait(threshold, transf, resumef, bdt); + barrier_wait(threshold, transf, resumef); } - barrier = stigmergy.create(BARRIER_VSTIG) + UAVSTATE = "BARRIERWAIT" + barrier_create(vstig_nb) } # # Make yourself ready # function barrier_ready() { + log("BARRIER READY -------") barrier.put(id, 1) + barrier.put("d", 0) } # # Executes the barrier # -timeW=0 -function barrier_wait(threshold, transf, resumef, bdt) { +function barrier_wait(threshold, transf, resumef) { barrier.put(id, 1) - UAVSTATE = "BARRIERWAIT" barrier.get(id) - if(barrier.size() >= threshold) { -# getlowest() + if(barrier.size() >= threshold or barrier.get("d")==1) { + barrier.put("d", 1) +# barrier = nil + timeW = 0 transf() } else if(timeW >= BARRIER_TIMEOUT) { - barrier = nil + timeW = 0 +# barrier = nil resumef() - timeW=0 - } else if(bdt!=-1) - neighbors.broadcast("cmd", bdt) + } timeW = timeW+1 } diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 8b5a9dd..5d90034 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -6,7 +6,8 @@ TARGET_ALTITUDE = 5.0 UAVSTATE = "TURNEDOFF" GOTO_MAXVEL = 2 -goal = {.range=0, .bearing=0} +TAKEOFF_VSTIG = 11 +goal = {.range=0, .bearing=0, .latitude=0, .longitude=0} function uav_initswarm() { s = swarm.create(1) @@ -23,60 +24,76 @@ function idle() { UAVSTATE = "IDLE" } +firstpassT = 1 function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff + + if(firstpassT) { + barrier_create(TAKEOFF_VSTIG) + firstpassT = 0 + } if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,action,land,-1) - barrier_ready() - } - else { - log("Altitude: ", TARGET_ALTITUDE) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } + barrier_wait(ROBOTS, action, land) + } else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } } +firstpassL = 1 function land() { UAVSTATE = "LAND" statef=land - if(flight.status == 2 or flight.status == 3){ - neighbors.broadcast("cmd", 21) - uav_land() - } - else { - barrier_set(ROBOTS,turnedoff,land,21) - barrier_ready() - timeW=0 - #barrier = nil - #statef=idle + if(firstpassL) { + barrier_create(TAKEOFF_VSTIG+1) + firstpassL = 0 + } + + neighbors.broadcast("cmd", 21) + uav_land() + + if(flight.status != 2 and flight.status != 3){ + barrier_wait(ROBOTS,turnedoff,land) } } function goto() { UAVSTATE = "GOTO" statef=goto - # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) + if(rc_goto.id==id){ s.leave() - rb_from_gps(rc_goto.latitude, rc_goto.longitude) - print(id, " has to move ", goal.range) - m_navigation = math.vec2.newp(goal.range, goal.bearing) - if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { - m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) - } else if(math.vec2.length(m_navigation)>0.25) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) - else - statef = idle + gotoWP(picture) } else { neighbors.broadcast("cmd", 16) neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) } } +function picture() { + #TODO: change gimbal orientation + uav_takepicture() + statef=idle +} + +function gotoWP(transf) { + # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) + rb_from_gps(rc_goto.latitude, rc_goto.longitude) + print(id, " has to move ", goal.range) + m_navigation = math.vec2.newp(goal.range, goal.bearing) + if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { + m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + } else if(math.vec2.length(m_navigation)>0.25) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + else + transf() +} + function follow() { if(size(targets)>0) { UAVSTATE = "FOLLOW" @@ -156,11 +173,21 @@ function LimitAngle(angle){ } function rb_from_gps(lat, lon) { - print("rb dbg: ",lat,lon,position.latitude,position.longitude) + print("gps2rb: ",lat,lon,position.latitude,position.longitude) d_lon = lon - position.longitude d_lat = lat - position.latitude ned_x = d_lat/180*math.pi * 6371000.0; ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); goal.bearing = LimitAngle(math.atan(ned_y,ned_x)); +} + +function gps_from_rb(range, bearing) { + print("rb2gps: ",range,bearing,position.latitude,position.longitude) + latR = position.latitude*math.pi/180.0; + lonR = position.longitude*math.pi/180.0; + target_lat = math.asin(math.sin(latR) * math.cos(range/6371000.0) + math.cos(latR) * math.sin(range/6371000.0) * math.cos(bearing)); + target_lon = lonR + math.atan(math.sin(bearing) * math.sin(range/6371000.0) * math.cos(latR), math.cos(range/6371000.0) - math.sin(latR) * math.sin(target_lat)); + goal.latitude = target_lat*180.0/math.pi; + goal.longitude = target_lon*180.0/math.pi; } \ No newline at end of file diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 4723269..61c7000 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -22,6 +22,7 @@ namespace buzzuav_closures{ COMMAND_DISARM, COMMAND_GOTO, COMMAND_MOVETO, + COMMAND_PICTURE, } Custom_CommandCode; /* @@ -37,6 +38,7 @@ int buzzros_print(buzzvm_t vm); */ int buzzuav_moveto(buzzvm_t vm); int buzzuav_storegoal(buzzvm_t vm); +int buzzuav_takepicture(buzzvm_t vm); /* Returns the current command from local variable*/ int getcmd(); /*Sets goto position */ diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 01d960a..b191b90 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -304,9 +304,12 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture)); + buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); buzzvm_gstore(VM); @@ -349,7 +352,7 @@ void in_message_process(){ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 95d2656..418b297 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -142,7 +142,7 @@ namespace buzzuav_closures{ cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]); - buzz_cmd= COMMAND_MOVETO; // TO DO what should we use + buzz_cmd = COMMAND_MOVETO; // TO DO what should we use return buzzvm_ret0(vm); } @@ -264,19 +264,32 @@ namespace buzzuav_closures{ message_number++;*/ return payload_out; } + /*----------------------------------------/ + / Buzz closure to take a picture here. + /----------------------------------------*/ + int buzzuav_takepicture(buzzvm_t vm) { + cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; + buzz_cmd = COMMAND_PICTURE; + return buzzvm_ret0(vm); + } + /*----------------------------------------/ / Buzz closure to store locally a GPS destination from the fleet /----------------------------------------*/ int buzzuav_storegoal(buzzvm_t vm) { buzzvm_lnum_assert(vm, 3); - buzzvm_lload(vm, 1); // latitude - buzzvm_lload(vm, 2); // longitude - buzzvm_lload(vm, 3); // altitude - buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - rc_set_goto(buzzvm_stack_at(vm, 1)->f.value, buzzvm_stack_at(vm, 2)->f.value, buzzvm_stack_at(vm, 3)->f.value, (int)buzz_utility::get_robotid()); - return buzzvm_ret0(vm); + buzzvm_lload(vm, 1); // altitude + buzzvm_lload(vm, 2); // longitude + buzzvm_lload(vm, 3); // latitude + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + float lat = buzzvm_stack_at(vm, 3)->f.value; + float lon = buzzvm_stack_at(vm, 2)->f.value; + float alt = buzzvm_stack_at(vm, 1)->f.value; + ROS_ERROR("DEBUG ---- %f %f %f",lat,lon,alt); + rc_set_goto((int)buzz_utility::get_robotid(), lat, lon, alt); + return buzzvm_ret0(vm); } /*----------------------------------------/ diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 55ac7c1..2711a73 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -750,9 +750,9 @@ void roscontroller::flight_controller_service_call() } } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); - - // roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], - // goto_pos[2], 0); + } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ + ROS_INFO("TAKING A PICTURE HERE!! --------------") + ; } } /*---------------------------------------------- From 1814c0976bc1a414748640ccb1ee729acc8c9de8 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 5 Sep 2017 11:52:24 -0400 Subject: [PATCH 17/39] Logger change to adapt m_estate var removal in graphform.bzz --- src/roscontroller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2711a73..924a249 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -267,12 +267,12 @@ void roscontroller::RosControllerRun() /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF SCRIPT IS NOT graphform.bzz*/ static buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); - buzzvm_gload(VM); - buzzobj_t graph_state = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - std::stringstream state_buff; - state_buff<< graph_state->s.value.str<<","; + std::stringstream state_buff; + //buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); + // buzzvm_gload(VM); + // buzzobj_t graph_state = buzzvm_stack_at(VM, 1); + // buzzvm_pop(VM); + // state_buff<< graph_state->s.value.str<<","; buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); buzzvm_gload(VM); buzzobj_t uav_state = buzzvm_stack_at(VM, 1); From 32b0e44e9fff6b19adb9f7b7f1848e723508c5f8 Mon Sep 17 00:00:00 2001 From: pyhs Date: Tue, 5 Sep 2017 15:39:03 -0400 Subject: [PATCH 18/39] Added capture image service --- include/roscontroller.h | 19 +++++++++++++++++-- src/roscontroller.cpp | 25 +++++++++++++++++-------- 2 files changed, 34 insertions(+), 10 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index bc5e117..cebee9d 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -55,6 +55,8 @@ public: //void RosControllerInit(); void RosControllerRun(); + static const string CAPTURE_SRV_DEFAULT_NAME; + private: struct num_robot_count { @@ -70,7 +72,7 @@ private: double latitude=0.0; float altitude=0.0; }; typedef struct gps GPS ; // not useful in cpp - + GPS target, home, cur_pos; double cur_rel_altitude; @@ -92,7 +94,19 @@ private: /*tmp to be corrected*/ uint8_t no_cnt=0; uint8_t old_val=0 ; - std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,obstacles_topic,stand_by,xbeesrv_name, setpoint_name; + std::string bzzfile_name; + std::string fcclient_name; + std::string armclient; + std::string modeclient; + std::string rcservice_name; + std::string bcfname,dbgfname; + std::string out_payload; + std::string in_payload; + std::string obstacles_topic; + std::string stand_by; + std::string xbeesrv_name; + std::string capture_srv_name; + std::string setpoint_name; std::string stream_client_name; std::string relative_altitude_sub_name; std::string setpoint_nonraw; @@ -102,6 +116,7 @@ private: Num_robot_count count_robots; ros::ServiceClient mav_client; ros::ServiceClient xbeestatus_srv; + ros::ServiceClient capture_srv; ros::Publisher payload_pub; ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2711a73..766b41b 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -2,6 +2,8 @@ #include namespace rosbzz_node { +const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; + /*--------------- /Constructor ---------------*/ @@ -200,7 +202,7 @@ void roscontroller::RosControllerRun() while (ros::ok() && !buzz_utility::buzz_script_done()) { /*Update neighbors position inside Buzz*/ // buzz_closure::neighbour_pos_callback(neighbours_pos_map); - + /*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh, neigh pos, RSSI val, Packet loss, filtered packet loss*/ log<second.x << "," << (double)it->second.y << "," << (double)it->second.z << ","; } - const uint8_t shrt_id= 0xFF; + const uint8_t shrt_id= 0xFF; float result; - if ( GetAPIRssi(shrt_id, result) ) + if ( GetAPIRssi(shrt_id, result) ) log<(xbeesrv_name); + capture_srv = n_c.serviceClient(capture_srv_name); stream_client = n_c.serviceClient(stream_client_name); users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this); @@ -751,8 +759,9 @@ void roscontroller::flight_controller_service_call() } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ - ROS_INFO("TAKING A PICTURE HERE!! --------------") - ; + ROS_INFO("TAKING A PICTURE HERE!! --------------"); + mavros_msgs::CommandBool capture_command; + capture_srv.call(capture_command); } } /*---------------------------------------------- From 0dfe6b1497a689696a12775ea9b4501325878715 Mon Sep 17 00:00:00 2001 From: pyhs Date: Tue, 5 Sep 2017 16:10:16 -0400 Subject: [PATCH 19/39] Fixing capture_image_srv name --- src/roscontroller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index bbbafdd..7927814 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -273,7 +273,7 @@ void roscontroller::RosControllerRun() //buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); // buzzvm_gload(VM); // buzzobj_t graph_state = buzzvm_stack_at(VM, 1); - // buzzvm_pop(VM); + // buzzvm_pop(VM); // state_buff<< graph_state->s.value.str<<","; buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); buzzvm_gload(VM); @@ -368,7 +368,7 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) } else n_c.getParam("xbee_status_srv", xbeesrv_name); - if(!n_c.getParam("xbee_status_srv", capture_srv_name)) + if(!n_c.getParam("capture_image_srv", capture_srv_name)) { capture_srv_name = CAPTURE_SRV_DEFAULT_NAME; } From f906be2d97e0f81f4d2ac9a3b91cdddcc0ae7d86 Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 5 Sep 2017 23:48:50 -0400 Subject: [PATCH 20/39] enhance barrier, simplify gps-graph and add multi-WP csv load --- buzz_scripts/graphformGPS.bzz | 104 ++++++++----------- buzz_scripts/include/barrier.bzz | 32 ++++-- buzz_scripts/include/graphs/waypointlist.csv | 89 ++++++++++++++++ buzz_scripts/include/uavstates.bzz | 94 ++++++++++------- buzz_scripts/testaloneWP.bzz | 35 +++++++ include/buzz_utility.h | 4 +- include/buzzuav_closures.h | 7 +- src/buzzuav_closures.cpp | 99 +++++++++++++++--- src/roscontroller.cpp | 7 +- 9 files changed, 334 insertions(+), 137 deletions(-) create mode 100644 buzz_scripts/include/graphs/waypointlist.csv create mode 100644 buzz_scripts/testaloneWP.bzz diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index 78caa0d..3ba5262 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -6,7 +6,7 @@ include "vec2.bzz" include "update.bzz" include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. -include "barrier.bzz" # reserve stigmergy id=11 for this header. +include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. include "uavstates.bzz" # require an 'action' function to be defined here. include "graphs/shapes_Y.bzz" @@ -268,14 +268,14 @@ function target4label(nei_id){ #calculate LJ vector for neibhor stored in i function LJ_vec(i){ var dis=m_MessageRange[i] - var bearing=m_MessageBearing[i] + var ljbearing=m_MessageBearing[i] var nei_id=m_messageID[i] var target=target4label(nei_id) var cDir={.x=0.0,.y=0.0} if(target!="miss"){ - cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),bearing) + cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),ljbearing) } - #log(id,"dis=",dis,"target=",target,"label=",nei_id) + #log(id,"dis=",dis,"target=",target,"label=",nei_id) #log("x=",cDir.x,"y=",cDir.y) return cDir } @@ -383,65 +383,11 @@ function TransitionToAsking(un_label){ # #Transistion to state joining # -function set_rc_goto() { - #get information of pred - var i=0 - var IDofPred=-1 - while(imath.pi) - S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi - else - S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi - - var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing) - - #the vector from self to target in global coordinate - var S2Target=math.vec2.add(S2Pred,P2Target) - #change the vector to local coordinate of self - var S2Target_bearing=math.atan(S2Target.y, S2Target.x) - m_bias=m_cMeToPred.Bearing-S2PGlobalBearing - S2Target_bearing=S2Target_bearing+m_bias - - gps_from_rb(math.vec2.length(S2Target)/100.0, S2Target_bearing) - uav_storegoal(goal.latitude, goal.longitude, position.altitude) - } -} - function TransitionToJoining(){ UAVSTATE="STATE_JOINING" m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel m_unWaitCount=m_unJoiningLostPeriod - - set_rc_goto() - - neighbors.listen("r", - function(vid,value,rid){ - var recv_table={.Label=0,.Bearing=0.0} - recv_table=unpack_guide_msg(value) - #store the received message - if(recv_table.Label==m_nLabel){ - m_cMeToPred.GlobalBearing=recv_table.Bearing - } - }) } # @@ -452,11 +398,10 @@ function TransitionToJoined(){ m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel m_vecNodes[m_nLabel].State="ASSIGNED" - neighbors.ignore("r") #write statues #v_tag.put(m_nLabel, m_lockstig) - barrier_create(m_lockstig) + barrier_create() barrier_ready() m_navigation.x=0.0 @@ -591,13 +536,47 @@ function DoAsking(){ # #Do joining # +m_gotjoinedparent = 0 function DoJoining(){ - gotoWP(TransitionToJoined) + + if(m_gotjoinedparent!=1) + set_rc_goto() + else + gotoWP(TransitionToJoined) + #pack the communication package m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel } +function set_rc_goto() { + #get information of pred + var i=0 + var IDofPred=-1 + while(i Prev. br. ", barrier, " ", BARRIER_VSTIG) + if(barrier!=nil) { + barrier=nil + BARRIER_VSTIG = BARRIER_VSTIG +1 + } + #log("---> New. br. ", barrier, " ", BARRIER_VSTIG) + barrier = stigmergy.create(BARRIER_VSTIG) } -function barrier_set(threshold, transf, resumef, vstig_nb) { +function barrier_set(threshold, transf, resumef, bdt) { statef = function() { - barrier_wait(threshold, transf, resumef); + barrier_wait(threshold, transf, resumef, bdt); } UAVSTATE = "BARRIERWAIT" - barrier_create(vstig_nb) + barrier_create() } # # Make yourself ready # function barrier_ready() { - log("BARRIER READY -------") barrier.put(id, 1) barrier.put("d", 0) } @@ -40,20 +47,23 @@ function barrier_ready() { # # Executes the barrier # -function barrier_wait(threshold, transf, resumef) { +function barrier_wait(threshold, transf, resumef, bdt) { barrier.put(id, 1) barrier.get(id) - if(barrier.size() >= threshold or barrier.get("d")==1) { + #log("----->BS: ", barrier.size()) + if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { barrier.put("d", 1) -# barrier = nil timeW = 0 transf() } else if(timeW >= BARRIER_TIMEOUT) { + log("------> Barrier Timeout !!!!") timeW = 0 -# barrier = nil resumef() - } + } + + if(bdt!=-1) + neighbors.broadcast("cmd", bdt) timeW = timeW+1 } diff --git a/buzz_scripts/include/graphs/waypointlist.csv b/buzz_scripts/include/graphs/waypointlist.csv new file mode 100644 index 0000000..e0a2c42 --- /dev/null +++ b/buzz_scripts/include/graphs/waypointlist.csv @@ -0,0 +1,89 @@ +0,-73.1531935978886,45.5084960903092,15,15 +1,-73.1530989420915,45.5085624255498,15,15 +2,-73.1530042862771,45.5086287608025,15,15 +3,-73.1529096304626,45.5086950960552,15,15 +4,-73.1529458247399,45.5087204611798,15,15 +5,-73.1530404805543,45.5086541259271,15,15 +6,-73.1531351363515,45.5085877906865,15,15 +7,-73.1532297921486,45.508521455446,15,15 +8,-73.1533244479457,45.5084551202054,15,15 +9,-73.1533606422273,45.508480485333,15,15 +10,-73.1532659864302,45.5085468205736,15,15 +11,-73.153171330633,45.5086131558142,15,15 +12,-73.1530766748359,45.5086794910548,15,15 +13,-73.1529820190215,45.5087458263075,15,15 +14,-73.1530182132901,45.5087711914261,15,15 +15,-73.1531128691045,45.5087048561733,15,15 +16,-73.1532075249016,45.5086385209327,15,15 +17,-73.1533021806988,45.5085721856922,15,15 +18,-73.1533968364959,45.5085058504516,15,15 +19,-73.1534330307645,45.5085312155701,15,15 +20,-73.1533383749674,45.5085975508107,15,15 +21,-73.1532437191702,45.5086638860513,15,15 +22,-73.1531490633731,45.5087302212919,15,15 +23,-73.1530544075587,45.5087965565446,15,15 +24,-73.1530906018273,45.5088219216632,15,15 +25,-73.1531852576417,45.5087555864105,15,15 +26,-73.1532799134389,45.5086892511699,15,15 +27,-73.153374569236,45.5086229159293,15,15 +28,-73.1534692250331,45.5085565806887,15,15 +29,-73.1535054193017,45.5085819458072,15,15 +30,-73.1534107635046,45.5086482810478,15,15 +31,-73.1533161077075,45.5087146162884,15,15 +32,-73.1532214519103,45.508780951529,15,15 +33,-73.1531267960959,45.5088472867817,15,15 +34,-73.1531629903645,45.5088726519003,15,15 +35,-73.1532576461789,45.5088063166476,15,15 +36,-73.1533523019761,45.508739981407,15,15 +37,-73.1534469577732,45.5086736461664,15,15 +38,-73.1535416135703,45.5086073109258,15,15 +39,-73.1535778078389,45.5086326760444,15,15 +40,-73.1534831520418,45.5086990112849,15,15 +41,-73.1533884962447,45.5087653465255,15,15 +42,-73.1532938404476,45.5088316817661,15,15 +43,-73.1531991846331,45.5088980170188,15,15 +44,-73.1532353789017,45.5089233821374,15,15 +45,-73.1533300347162,45.5088570468847,15,15 +46,-73.1534246905133,45.5087907116441,15,15 +47,-73.1535193463104,45.5087243764035,15,15 +48,-73.1536140021075,45.5086580411629,15,15 +49,-73.1536501963762,45.5086834062815,15,15 +50,-73.153555540579,45.508749741522,15,15 +51,-73.1534608847819,45.5088160767626,15,15 +52,-73.1533662289848,45.5088824120032,15,15 +53,-73.1532715731703,45.508948747256,15,15 +54,-73.1533077674389,45.5089741123745,15,15 +55,-73.1534024232534,45.5089077771218,15,15 +56,-73.1534970790505,45.5088414418812,15,15 +57,-73.1535917348476,45.5087751066406,15,15 +58,-73.1536863906448,45.5087087714,15,15 +59,-73.1537225849134,45.5087341365186,15,15 +60,-73.1536279291163,45.5088004717592,15,15 +61,-73.1535332733191,45.5088668069997,15,15 +62,-73.153438617522,45.5089331422403,15,15 +63,-73.1533439617076,45.5089994774931,15,15 +64,-73.1533801559762,45.5090248426116,15,15 +65,-73.1534748117906,45.5089585073589,15,15 +66,-73.1535694675877,45.5088921721183,15,15 +67,-73.1536641233849,45.5088258368777,15,15 +68,-73.153758779182,45.5087595016371,15,15 +69,-73.1537949734506,45.5087848667557,15,15 +70,-73.1537003176535,45.5088512019963,15,15 +71,-73.1536056618563,45.5089175372369,15,15 +72,-73.1535110060592,45.5089838724775,15,15 +73,-73.1534163502448,45.5090502077302,15,15 +74,-73.1534525445134,45.5090755728487,15,15 +75,-73.1535472003278,45.509009237596,15,15 +76,-73.1536418561249,45.5089429023554,15,15 +77,-73.1537365119221,45.5088765671148,15,15 +78,-73.1538311677192,45.5088102318742,15,15 +79,-73.1538673619878,45.5088355969928,15,15 +80,-73.1537727061907,45.5089019322334,15,15 +81,-73.1536780503936,45.508968267474,15,15 +82,-73.1535833945964,45.5090346027146,15,15 +83,-73.153488738782,45.5091009379673,15,15 +84,-73.1535249330852,45.5091263031101,15,15 +85,-73.1536195888996,45.5090599678574,15,15 +86,-73.1537142446968,45.5089936326168,15,15 +87,-73.1538089004939,45.5089272973762,15,15 +88,-73.153903556291,45.5088609621356,15,15 diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 5d90034..cdf9e3b 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -3,10 +3,13 @@ # FLIGHT-RELATED FUNCTIONS # ######################################## -TARGET_ALTITUDE = 5.0 +TARGET_ALTITUDE = 5.0 # m. UAVSTATE = "TURNEDOFF" -GOTO_MAXVEL = 2 -TAKEOFF_VSTIG = 11 +PICTURE_WAIT = 40 # steps +GOTO_MAXVEL = 2 # m/steps +GOTO_MAXDIST = 150 # m. +GOTODIST_TOL = 0.5 # m. +GOTOANG_TOL = 0.1 # rad. goal = {.range=0, .bearing=0, .latitude=0, .longitude=0} function uav_initswarm() { @@ -24,18 +27,13 @@ function idle() { UAVSTATE = "IDLE" } -firstpassT = 1 function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff - - if(firstpassT) { - barrier_create(TAKEOFF_VSTIG) - firstpassT = 0 - } if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_wait(ROBOTS, action, land) + barrier_set(ROBOTS, action, land, -1) + barrier_ready() } else { log("Altitude: ", TARGET_ALTITUDE) neighbors.broadcast("cmd", 22) @@ -43,55 +41,65 @@ function takeoff() { } } -firstpassL = 1 function land() { UAVSTATE = "LAND" statef=land - if(firstpassL) { - barrier_create(TAKEOFF_VSTIG+1) - firstpassL = 0 - } - neighbors.broadcast("cmd", 21) uav_land() if(flight.status != 2 and flight.status != 3){ - barrier_wait(ROBOTS,turnedoff,land) + barrier_set(ROBOTS,turnedoff,land, 21) + barrier_ready() } } -function goto() { +function set_goto(transf) { UAVSTATE = "GOTO" - statef=goto + statef=function() { + gotoWP(transf); + } if(rc_goto.id==id){ - s.leave() - gotoWP(picture) + if(s!=nil){ + if(s.in()) + s.leave() + } } else { neighbors.broadcast("cmd", 16) neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) } } +ptime=0 function picture() { + statef=picture + UAVSTATE="PICTURE" #TODO: change gimbal orientation - uav_takepicture() - statef=idle + if(ptime==PICTURE_WAIT/2) + uav_takepicture() + else if(ptime>=PICTURE_WAIT) { + statef=action + ptime=0 + } + ptime=ptime+1 } function gotoWP(transf) { # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) rb_from_gps(rc_goto.latitude, rc_goto.longitude) - print(id, " has to move ", goal.range) + print(" has to move ", goal.range) m_navigation = math.vec2.newp(goal.range, goal.bearing) - if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { + + if(math.vec2.length(m_navigation)>GOTO_MAXDIST) + log("Sorry this is too far.") + else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) - } else if(math.vec2.length(m_navigation)>0.25) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) - else + } else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination transf() + else + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) } function follow() { @@ -145,16 +153,18 @@ function uav_rccmd() { function uav_neicmd() { neighbors.listen("cmd", function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid) - if(value==22 and UAVSTATE!="TAKEOFF") { + print("Got (", vid, ",", value, ") from robot #", rid, "(", UAVSTATE, ")") + if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") { statef=takeoff - } else if(value==21) { + UAVSTATE = "TAKEOFF" + } else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") { statef=land - } else if(value==400 and UAVSTATE=="IDLE") { + UAVSTATE = "LAND" + } else if(value==400 and UAVSTATE=="TURNEDOFF") { uav_arm() - } else if(value==401 and UAVSTATE=="IDLE"){ + } else if(value==401 and UAVSTATE=="TURNEDOFF"){ uav_disarm() - } else if(value==16){ + } else if(value==16 and UAVSTATE=="IDLE"){ # neighbors.listen("gt",function(vid, value, rid) { # print("Got (", vid, ",", value, ") from robot #", rid) # # if(gt.id == id) statef=goto @@ -173,7 +183,7 @@ function LimitAngle(angle){ } function rb_from_gps(lat, lon) { - print("gps2rb: ",lat,lon,position.latitude,position.longitude) +# print("gps2rb: ",lat,lon,position.latitude,position.longitude) d_lon = lon - position.longitude d_lat = lat - position.latitude ned_x = d_lat/180*math.pi * 6371000.0; @@ -182,12 +192,18 @@ function rb_from_gps(lat, lon) { goal.bearing = LimitAngle(math.atan(ned_y,ned_x)); } -function gps_from_rb(range, bearing) { - print("rb2gps: ",range,bearing,position.latitude,position.longitude) +function gps_from_vec(vec) { + Vrange = math.vec2.length(vec) + Vbearing = LimitAngle(math.atan(vec.y, vec.x)) +# print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude) latR = position.latitude*math.pi/180.0; lonR = position.longitude*math.pi/180.0; - target_lat = math.asin(math.sin(latR) * math.cos(range/6371000.0) + math.cos(latR) * math.sin(range/6371000.0) * math.cos(bearing)); - target_lon = lonR + math.atan(math.sin(bearing) * math.sin(range/6371000.0) * math.cos(latR), math.cos(range/6371000.0) - math.sin(latR) * math.sin(target_lat)); + target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); + target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); goal.latitude = target_lat*180.0/math.pi; goal.longitude = target_lon*180.0/math.pi; + #d_lat = (vec.x / 6371000.0)*180.0/math.pi; + #goal.latitude = d_lat + position.latitude; + #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; + #goal.longitude = d_lon + position.longitude; } \ No newline at end of file diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz new file mode 100644 index 0000000..97b92bf --- /dev/null +++ b/buzz_scripts/testaloneWP.bzz @@ -0,0 +1,35 @@ +include "vec2.bzz" +include "update.bzz" +include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header. +include "uavstates.bzz" # require an 'action' function to be defined here. +include "vstigenv.bzz" + +function action() { + uav_storegoal(-1.0,-1.0,-1.0) + set_goto(picture) +} + +# Executed once at init time. +function init() { + statef=turnedoff + UAVSTATE = "TURNEDOFF" + uav_initstig() + TARGET_ALTITUDE = 15.0 +} + +# Executed at each time step. +function step() { + uav_rccmd() + + statef() + + log("Current state: ", UAVSTATE) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/include/buzz_utility.h b/include/buzz_utility.h index f3f9550..692de52 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -23,8 +23,8 @@ struct pos_struct typedef struct pos_struct Pos_struct; struct rb_struct { - double r,b,la,lo; - rb_struct(double la, double lo, double r,double b):la(la),lo(lo),r(r),b(b){}; + double r,b,latitude,longitude,altitude; + rb_struct(double la, double lo, double al, double r,double b):latitude(la),longitude(lo),altitude(al),r(r),b(b){}; rb_struct(){} }; typedef struct rb_struct RB_struct; diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 61c7000..47c5388 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -9,8 +9,8 @@ #include "ros/ros.h" #include "buzz_utility.h" - #define EARTH_RADIUS (double) 6371000.0 - #define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0))) +#define EARTH_RADIUS (double) 6371000.0 +#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0))) namespace buzzuav_closures{ typedef enum { @@ -31,13 +31,14 @@ namespace buzzuav_closures{ * The command to use in Buzz is buzzros_print takes any available datatype in Buzz */ int buzzros_print(buzzvm_t vm); - +void setWPlist(std::string path); /* * buzzuav_goto(latitude,longitude,altitude) function in Buzz * commands the UAV to go to a position supplied */ int buzzuav_moveto(buzzvm_t vm); int buzzuav_storegoal(buzzvm_t vm); +void parse_gpslist(); int buzzuav_takepicture(buzzvm_t vm); /* Returns the current command from local variable*/ int getcmd(); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 418b297..d1f6804 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -32,6 +32,7 @@ namespace buzzuav_closures{ static float raw_packet_loss = 0.0; static int filtered_packet_loss = 0; static float api_rssi = 0.0; + string WPlistname = ""; std::map< int, buzz_utility::RB_struct> targets_map; std::map< int, buzz_utility::Pos_struct> neighbors_map; @@ -83,6 +84,10 @@ namespace buzzuav_closures{ return buzzvm_ret0(vm); } + void setWPlist(string path){ + WPlistname = path + "include/graphs/waypointlist.csv"; + } + /*----------------------------------------/ / Compute GPS destination from current position and desired Range and Bearing /----------------------------------------*/ @@ -105,10 +110,10 @@ namespace buzzuav_closures{ } void rb_from_gps(double nei[], double out[], double cur[]){ - double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; - double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + double d_lon = nei[1] - cur[1]; + double d_lat = nei[0] - cur[0]; + double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); out[1] = constrainAngle(atan2(ned_y,ned_x)); out[2] = 0.0; @@ -119,6 +124,52 @@ namespace buzzuav_closures{ double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958}; double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677}; + void parse_gpslist() + { + // Open file: + ROS_INFO("WP list file: %s", WPlistname.c_str()); + std::ifstream fin(WPlistname.c_str()); // Open in text-mode. + + // Opening may fail, always check. + if (!fin) { + ROS_ERROR("GPS list parser, could not open file."); + return; + } + + // Prepare a C-string buffer to be used when reading lines. + const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need. + char buffer[MAX_LINE_LENGTH] = {}; + const char * DELIMS = "\t ,"; // Tab, space or comma. + + // Read the file and load the data: + double lat, lon; + int alt, tilt, tid; + buzz_utility::RB_struct RB_arr; + // Read one line at a time. + while ( fin.getline(buffer, MAX_LINE_LENGTH) ) { + // Extract the tokens: + tid = atoi(strtok( buffer, DELIMS )); + lon = atof(strtok( NULL, DELIMS )); + lat = atof(strtok( NULL, DELIMS )); + alt = atoi(strtok( NULL, DELIMS )); + tilt = atoi(strtok( NULL, DELIMS )); + //ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid); + RB_arr.latitude=lat; + RB_arr.longitude=lon; + RB_arr.altitude=alt; + // Insert elements. + map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(tid); + if(it!=targets_map.end()) + targets_map.erase(it); + targets_map.insert(make_pair(tid, RB_arr)); + } + + ROS_INFO("----->Saved %i waypoints.", targets_map.size()); + + // Close the file: + fin.close(); + } + /*----------------------------------------/ / Buzz closure to move following a 2D vector /----------------------------------------*/ @@ -158,9 +209,9 @@ namespace buzzuav_closures{ double rb[3], tmp[3]; map< int, buzz_utility::RB_struct >::iterator it; for (it=targets_map.begin(); it!=targets_map.end(); ++it){ - tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height; - rb_from_gps(tmp, rb, cur_pos); - ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]); + tmp[0]=(it->second).latitude; tmp[1]=(it->second).longitude; tmp[2]=height; + rb_from_gps(tmp, rb, cur_pos); + //ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]); buzzvm_push(vm, targettbl); /* When we get here, the "targets" table is on top of the stack */ //ROS_INFO("Buzz_utility will save user %i.", it->first); @@ -206,8 +257,9 @@ namespace buzzuav_closures{ if(fabs(rb[0])<100.0) { //printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]); buzz_utility::RB_struct RB_arr; - RB_arr.la=tmp[0]; - RB_arr.lo=tmp[1]; + RB_arr.latitude=tmp[0]; + RB_arr.longitude=tmp[1]; + RB_arr.altitude=tmp[2]; RB_arr.r=rb[0]; RB_arr.b=rb[1]; map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(uid); @@ -217,7 +269,7 @@ namespace buzzuav_closures{ //ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); return vm->state; } else - printf(" ---------- Target too far %f\n",rb[0]); + ROS_WARN(" ---------- Target too far %f",rb[0]); return 0; } @@ -249,7 +301,7 @@ namespace buzzuav_closures{ mavros_msgs::Mavlink get_status(){ mavros_msgs::Mavlink payload_out; - map< int, buzz_utility::neighbors_status >::iterator it; + map< int, buzz_utility::neighbors_status >::iterator it; for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++it){ payload_out.payload64.push_back(it->first); payload_out.payload64.push_back(it->second.gps_strenght); @@ -284,11 +336,26 @@ namespace buzzuav_closures{ buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - float lat = buzzvm_stack_at(vm, 3)->f.value; - float lon = buzzvm_stack_at(vm, 2)->f.value; - float alt = buzzvm_stack_at(vm, 1)->f.value; - ROS_ERROR("DEBUG ---- %f %f %f",lat,lon,alt); - rc_set_goto((int)buzz_utility::get_robotid(), lat, lon, alt); + double goal[3]; + goal[0] = buzzvm_stack_at(vm, 3)->f.value; + goal[1] = buzzvm_stack_at(vm, 2)->f.value; + goal[2] = buzzvm_stack_at(vm, 1)->f.value; + if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){ + if(targets_map.size()<=0) + parse_gpslist(); + goal[0] = targets_map.begin()->second.latitude; + goal[1] = targets_map.begin()->second.longitude; + goal[2] = targets_map.begin()->second.altitude; + targets_map.erase(targets_map.begin()->first); + } + + double rb[3]; + + rb_from_gps(goal, rb, cur_pos); + if(fabs(rb[0])<150.0) + rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); + + ROS_ERROR("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]); return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2711a73..6354584 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -17,6 +17,7 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv) bcfname = fname + ".bo"; dbgfname = fname + ".bdb"; set_bzz_file(bzzfile_name.c_str()); + buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); /*Initialize variables*/ // Solo things SetMode("LOITER", 0); @@ -751,8 +752,8 @@ void roscontroller::flight_controller_service_call() } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ - ROS_INFO("TAKING A PICTURE HERE!! --------------") - ; + ROS_INFO("TAKING A PICTURE HERE!! --------------"); + ros::Duration(0.2).sleep(); } } /*---------------------------------------------- @@ -1092,7 +1093,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { gps_rb(nei_pos, cvt_neighbours_pos_payload); /*Extract robot id of the neighbour*/ uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); - ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); +// ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); /*pass neighbour position to local maintaner*/ buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1], From fda672ac26a826b5cf3cdecbe5fcbe93751df28e Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 6 Sep 2017 17:08:10 -0400 Subject: [PATCH 21/39] merge dji_sdk_mistlab modification on the field --- buzz_scripts/graphform.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 339e6ce..3df20fd 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -795,7 +795,7 @@ function init() { v_tag = stigmergy.create(m_lockstig) uav_initstig() # go to diff. height since no collision avoidance implemented yet - TARGET_ALTITUDE = 2.5 + id * 1.5 + TARGET_ALTITUDE = 7.5 #2.5 + id * 1.5 statef=turnedoff UAVSTATE = "TURNEDOFF" From 796bb78dd8c864738b2fdd014630fb5990ec9ca2 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 6 Sep 2017 18:55:12 -0400 Subject: [PATCH 22/39] onboard fix and gimbal ctr --- buzz_scripts/include/uavstates.bzz | 50 +++++++++++++----------------- buzz_scripts/testaloneWP.bzz | 2 +- src/buzzuav_closures.cpp | 35 +++++++++++---------- src/roscontroller.cpp | 9 ++++++ 4 files changed, 50 insertions(+), 46 deletions(-) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index b2d4d03..2ae665a 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -27,27 +27,20 @@ function idle() { UAVSTATE = "IDLE" } -firstpassT = 1 function takeoff() { UAVSTATE = "TAKEOFF" statef=takeoff - if(firstpassT) { - barrier_create(TAKEOFF_VSTIG) - firstpassT = 0 - } - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { barrier_set(ROBOTS, action, land, -1) barrier_ready() } else { - log("Altitude: ", TARGET_ALTITUDE) - neighbors.broadcast("cmd", 22) - uav_takeoff(TARGET_ALTITUDE) - } + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } } -firstpassL = 1 function land() { UAVSTATE = "LAND" statef=land @@ -56,26 +49,26 @@ function land() { uav_land() if(flight.status != 2 and flight.status != 3){ - barrier_set(ROBOTS,turnedoff,land, 21) + barrier_set(ROBOTS,turnedoff,land, 21) barrier_ready() } } function set_goto(transf) { - UAVSTATE = "GOTO" + UAVSTATE = "GOTOGPS" statef=function() { - gotoWP(transf); - } + gotoWP(transf); + } - if(rc_goto.id==id){ - if(s!=nil){ - if(s.in()) - s.leave() - } - } else { - neighbors.broadcast("cmd", 16) - neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) - } + #if(rc_goto.id==id){ + # if(s!=nil){ + # if(s.in()) + # s.leave() + #} + #} else { + # neighbors.broadcast("cmd", 16) + # neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) + #} } ptime=0 @@ -93,7 +86,7 @@ function picture() { } function gotoWP(transf) { - # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) + print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) rb_from_gps(rc_goto.latitude, rc_goto.longitude) print(" has to move ", goal.range) m_navigation = math.vec2.newp(goal.range, goal.bearing) @@ -141,7 +134,7 @@ function uav_rccmd() { neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 - UAVSTATE = "GOTO" + UAVSTATE = "GOTOGPS" statef = goto } else if(flight.rc_cmd==400) { flight.rc_cmd=0 @@ -190,7 +183,8 @@ function LimitAngle(angle){ } function rb_from_gps(lat, lon) { -# print("gps2rb: ",lat,lon,position.latitude,position.longitude) +# print("gps2rb d: ",lat,lon) +# print("gps2rb h: ",position.latitude,position.longitude) d_lon = lon - position.longitude d_lat = lat - position.latitude ned_x = d_lat/180*math.pi * 6371000.0; @@ -213,4 +207,4 @@ function gps_from_vec(vec) { #goal.latitude = d_lat + position.latitude; #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; #goal.longitude = d_lon + position.longitude; -} \ No newline at end of file +} diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz index 97b92bf..892b5a6 100644 --- a/buzz_scripts/testaloneWP.bzz +++ b/buzz_scripts/testaloneWP.bzz @@ -23,7 +23,7 @@ function step() { statef() - log("Current state: ", UAVSTATE) +# log("Current state: ", UAVSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index d1f6804..d0ae31a 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -35,6 +35,7 @@ namespace buzzuav_closures{ string WPlistname = ""; std::map< int, buzz_utility::RB_struct> targets_map; + std::map< int, buzz_utility::RB_struct> wplist_map; std::map< int, buzz_utility::Pos_struct> neighbors_map; std::map< int, buzz_utility::neighbors_status> neighbors_status_map; @@ -158,13 +159,13 @@ namespace buzzuav_closures{ RB_arr.longitude=lon; RB_arr.altitude=alt; // Insert elements. - map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(tid); - if(it!=targets_map.end()) - targets_map.erase(it); - targets_map.insert(make_pair(tid, RB_arr)); + map< int, buzz_utility::RB_struct >::iterator it = wplist_map.find(tid); + if(it!=wplist_map.end()) + wplist_map.erase(it); + wplist_map.insert(make_pair(tid, RB_arr)); } - ROS_INFO("----->Saved %i waypoints.", targets_map.size()); + ROS_INFO("----->Saved %i waypoints.", wplist_map.size()); // Close the file: fin.close(); @@ -213,23 +214,23 @@ namespace buzzuav_closures{ rb_from_gps(tmp, rb, cur_pos); //ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]); buzzvm_push(vm, targettbl); - /* When we get here, the "targets" table is on top of the stack */ + // When we get here, the "targets" table is on top of the stack //ROS_INFO("Buzz_utility will save user %i.", it->first); - /* Push user id */ + // Push user id buzzvm_pushi(vm, it->first); - /* Create entry table */ + // Create entry table buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); - /* Insert range */ + // Insert range buzzvm_push(vm, entry); buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1)); buzzvm_pushf(vm, rb[0]); buzzvm_tput(vm); - /* Insert longitude */ + // Insert longitude buzzvm_push(vm, entry); buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1)); buzzvm_pushf(vm, rb[1]); buzzvm_tput(vm); - /* Save entry into data table */ + // Save entry into data table buzzvm_push(vm, entry); buzzvm_tput(vm); } @@ -341,12 +342,12 @@ namespace buzzuav_closures{ goal[1] = buzzvm_stack_at(vm, 2)->f.value; goal[2] = buzzvm_stack_at(vm, 1)->f.value; if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){ - if(targets_map.size()<=0) + if(wplist_map.size()<=0) parse_gpslist(); - goal[0] = targets_map.begin()->second.latitude; - goal[1] = targets_map.begin()->second.longitude; - goal[2] = targets_map.begin()->second.altitude; - targets_map.erase(targets_map.begin()->first); + goal[0] = wplist_map.begin()->second.latitude; + goal[1] = wplist_map.begin()->second.longitude; + goal[2] = wplist_map.begin()->second.altitude; + wplist_map.erase(wplist_map.begin()->first); } double rb[3]; @@ -355,7 +356,7 @@ namespace buzzuav_closures{ if(fabs(rb[0])<150.0) rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); - ROS_ERROR("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]); + ROS_WARN("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]); return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index f4e0162..3383569 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -761,6 +761,15 @@ void roscontroller::flight_controller_service_call() roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ ROS_INFO("TAKING A PICTURE HERE!! --------------"); + cmd_srv.request.param1 = 90; + cmd_srv.request.param2 = 0; + cmd_srv.request.param3 = 0; + cmd_srv.request.param4 = 0; + cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + if (mav_client.call(cmd_srv)) { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } else + ROS_ERROR("Failed to call service from flight controller"); mavros_msgs::CommandBool capture_command; capture_srv.call(capture_command); } From dc014841054a5b71695527f5fde5f26ec027a412 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 6 Sep 2017 21:53:00 -0400 Subject: [PATCH 23/39] add camera service check and fix ROSBuzz gimbal control --- buzz_scripts/include/uavstates.bzz | 28 ++++++++++++++-------------- buzz_scripts/testaloneWP.bzz | 2 +- src/buzzuav_closures.cpp | 2 +- src/roscontroller.cpp | 16 +++++++++------- 4 files changed, 25 insertions(+), 23 deletions(-) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 2ae665a..254d84d 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -35,7 +35,7 @@ function takeoff() { barrier_set(ROBOTS, action, land, -1) barrier_ready() } else { - log("Altitude: ", TARGET_ALTITUDE) + log("Altitude: ", position.altitude) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } @@ -60,15 +60,15 @@ function set_goto(transf) { gotoWP(transf); } - #if(rc_goto.id==id){ - # if(s!=nil){ - # if(s.in()) - # s.leave() - #} - #} else { - # neighbors.broadcast("cmd", 16) - # neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) - #} + if(rc_goto.id==id){ + if(s!=nil){ + if(s.in()) + s.leave() + } + } else { + neighbors.broadcast("cmd", 16) + neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) + } } ptime=0 @@ -86,14 +86,14 @@ function picture() { } function gotoWP(transf) { - print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) + # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) rb_from_gps(rc_goto.latitude, rc_goto.longitude) - print(" has to move ", goal.range) + print(" has to move ", goal.range, goal.bearing) m_navigation = math.vec2.newp(goal.range, goal.bearing) if(math.vec2.length(m_navigation)>GOTO_MAXDIST) - log("Sorry this is too far.") - else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity + log("Sorry this is too far.") + else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) } else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination diff --git a/buzz_scripts/testaloneWP.bzz b/buzz_scripts/testaloneWP.bzz index 892b5a6..97b92bf 100644 --- a/buzz_scripts/testaloneWP.bzz +++ b/buzz_scripts/testaloneWP.bzz @@ -23,7 +23,7 @@ function step() { statef() -# log("Current state: ", UAVSTATE) + log("Current state: ", UAVSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index ed9ef9b..23676d2 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -321,7 +321,7 @@ namespace buzzuav_closures{ / Buzz closure to take a picture here. /----------------------------------------*/ int buzzuav_takepicture(buzzvm_t vm) { - cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; + //cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; buzz_cmd = COMMAND_PICTURE; return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 3383569..1041ec7 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -761,17 +761,20 @@ void roscontroller::flight_controller_service_call() roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ ROS_INFO("TAKING A PICTURE HERE!! --------------"); - cmd_srv.request.param1 = 90; - cmd_srv.request.param2 = 0; - cmd_srv.request.param3 = 0; - cmd_srv.request.param4 = 0; + cmd_srv.request.param1 = 0.0; + cmd_srv.request.param2 = 0.0; + cmd_srv.request.param3 = -90.0; + cmd_srv.request.param4 = 0.0; cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } else ROS_ERROR("Failed to call service from flight controller"); mavros_msgs::CommandBool capture_command; - capture_srv.call(capture_command); + if (capture_srv.call(capture_command)) { + ROS_INFO("Reply: %ld", (long int)capture_command.response.success); + } else + ROS_ERROR("Failed to call service from camera streamer"); } } /*---------------------------------------------- @@ -993,8 +996,7 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) { // gps_ned_home(ned_x, ned_y); // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], // home[1]); - // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, - // local_pos[0], local_pos[1]); +// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos_new[0], local_pos_new[1]); /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD * (then converted to NED)*/ From 4deefe64d402e152e8e1a9e0a7bce8601bb35758 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Thu, 7 Sep 2017 18:03:53 -0400 Subject: [PATCH 24/39] Improved rosrate --- buzz_scripts/graphform.bzz | 2 +- src/roscontroller.cpp | 78 +++++++++++++++++++------------------- 2 files changed, 41 insertions(+), 39 deletions(-) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 339e6ce..722d186 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -795,7 +795,7 @@ function init() { v_tag = stigmergy.create(m_lockstig) uav_initstig() # go to diff. height since no collision avoidance implemented yet - TARGET_ALTITUDE = 2.5 + id * 1.5 + TARGET_ALTITUDE = 4.5 #2.5 + id * 1.5 statef=turnedoff UAVSTATE = "TURNEDOFF" diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 55ac7c1..55d5225 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -192,7 +192,9 @@ void roscontroller::RosControllerRun() robot_id)) { ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; - init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + /*loop rate of ros*/ + ros::Rate loop_rate(15); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -217,14 +219,14 @@ void roscontroller::RosControllerRun() } const uint8_t shrt_id= 0xFF; float result; - if ( GetAPIRssi(shrt_id, result) ) - log<::iterator it = neighbours_pos_map.begin(); log << "," << neighbours_pos_map.size(); @@ -259,7 +261,7 @@ void roscontroller::RosControllerRun() << "," << (double)it->second.z; } log << std::endl;*/ - } + //} /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ // no_of_robots=get_number_of_robots(); get_number_of_robots(); @@ -267,12 +269,12 @@ void roscontroller::RosControllerRun() /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF SCRIPT IS NOT graphform.bzz*/ static buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); - buzzvm_gload(VM); - buzzobj_t graph_state = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); + //buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); + // buzzvm_gload(VM); + // buzzobj_t graph_state = buzzvm_stack_at(VM, 1); + // buzzvm_pop(VM); std::stringstream state_buff; - state_buff<< graph_state->s.value.str<<","; + // state_buff<< graph_state->s.value.str<<","; buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); buzzvm_gload(VM); buzzobj_t uav_state = buzzvm_stack_at(VM, 1); @@ -284,7 +286,7 @@ void roscontroller::RosControllerRun() // buzz_utility::set_robot_var(no_of_robots); /*Set no of robots for updates TODO only when not updating*/ // if(multi_msg) - updates_set_robots(no_of_robots); + //updates_set_robots(no_of_robots); // ROS_INFO("ROBOTS: %i , acutal : // %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1); get_xbee_status(); @@ -624,7 +626,7 @@ void roscontroller::prepare_msg_and_publish() delete[] out; delete[] payload_out_ptr; /*Check for updater message if present send*/ - if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && + /* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && multi_msg) { uint8_t *buff_send = 0; @@ -666,7 +668,7 @@ void roscontroller::prepare_msg_and_publish() payload_pub.publish(update_packets); multi_msg = false; delete[] payload_64; - } + }*/ } /*--------------------------------------------------------------------------------- /Flight controller service call every step if there is a command set from bzz @@ -1239,22 +1241,22 @@ void roscontroller::get_xbee_status() bool result_bool; float result_float; const uint8_t all_ids = 0xFF; - if(GetDequeFull(result_bool)) - { - buzzuav_closures::set_deque_full(result_bool); - } - if(GetRssi(result_float)) - { - buzzuav_closures::set_rssi(result_float); - } - if(GetRawPacketLoss(all_ids, result_float)) - { - buzzuav_closures::set_raw_packet_loss(result_float); - } - if(GetFilteredPacketLoss(all_ids, result_float)) - { - buzzuav_closures::set_filtered_packet_loss(result_float); - } + //if(GetDequeFull(result_bool)) + //{ + // buzzuav_closures::set_deque_full(result_bool); + //} + //if(GetRssi(result_float)) + //{ + // buzzuav_closures::set_rssi(result_float); + //} + //if(GetRawPacketLoss(all_ids, result_float)) + //{ + // buzzuav_closures::set_raw_packet_loss(result_float); + //} + //if(GetFilteredPacketLoss(all_ids, result_float)) + //{ + // buzzuav_closures::set_filtered_packet_loss(result_float); + //} // This part needs testing since it can overload the xbee module /* * if(GetAPIRssi(all_ids, result_float)) From 7830235c78ead1d6e79df67ccaf6c2708935ca48 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 7 Sep 2017 18:17:03 -0400 Subject: [PATCH 25/39] 8 Hz improved loop rate --- src/roscontroller.cpp | 74 ++++++++++++++++++++++--------------------- 1 file changed, 38 insertions(+), 36 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1041ec7..6ecd042 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -195,7 +195,9 @@ void roscontroller::RosControllerRun() robot_id)) { ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; - init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + /*loop rate of ros*/ + ros::Rate loop_rate(15); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -220,15 +222,15 @@ void roscontroller::RosControllerRun() } const uint8_t shrt_id= 0xFF; float result; - if ( GetAPIRssi(shrt_id, result) ) - log<::iterator it = neighbours_pos_map.begin(); log << "," << neighbours_pos_map.size(); @@ -262,7 +264,7 @@ void roscontroller::RosControllerRun() << "," << (double)it->second.z; } log << std::endl;*/ - } + //} /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ // no_of_robots=get_number_of_robots(); get_number_of_robots(); @@ -270,12 +272,12 @@ void roscontroller::RosControllerRun() /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF SCRIPT IS NOT graphform.bzz*/ static buzzvm_t VM = buzz_utility::get_vm(); - std::stringstream state_buff; //buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); // buzzvm_gload(VM); // buzzobj_t graph_state = buzzvm_stack_at(VM, 1); // buzzvm_pop(VM); - // state_buff<< graph_state->s.value.str<<","; + std::stringstream state_buff; + // state_buff<< graph_state->s.value.str<<","; buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); buzzvm_gload(VM); buzzobj_t uav_state = buzzvm_stack_at(VM, 1); @@ -287,7 +289,7 @@ void roscontroller::RosControllerRun() // buzz_utility::set_robot_var(no_of_robots); /*Set no of robots for updates TODO only when not updating*/ // if(multi_msg) - updates_set_robots(no_of_robots); + //updates_set_robots(no_of_robots); // ROS_INFO("ROBOTS: %i , acutal : // %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1); get_xbee_status(); @@ -633,7 +635,7 @@ void roscontroller::prepare_msg_and_publish() delete[] out; delete[] payload_out_ptr; /*Check for updater message if present send*/ - if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && + /* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && multi_msg) { uint8_t *buff_send = 0; @@ -675,7 +677,7 @@ void roscontroller::prepare_msg_and_publish() payload_pub.publish(update_packets); multi_msg = false; delete[] payload_64; - } + }*/ } /*--------------------------------------------------------------------------------- /Flight controller service call every step if there is a command set from bzz @@ -1260,22 +1262,22 @@ void roscontroller::get_xbee_status() bool result_bool; float result_float; const uint8_t all_ids = 0xFF; - if(GetDequeFull(result_bool)) - { - buzzuav_closures::set_deque_full(result_bool); - } - if(GetRssi(result_float)) - { - buzzuav_closures::set_rssi(result_float); - } - if(GetRawPacketLoss(all_ids, result_float)) - { - buzzuav_closures::set_raw_packet_loss(result_float); - } - if(GetFilteredPacketLoss(all_ids, result_float)) - { - buzzuav_closures::set_filtered_packet_loss(result_float); - } + //if(GetDequeFull(result_bool)) + //{ + // buzzuav_closures::set_deque_full(result_bool); + //} + //if(GetRssi(result_float)) + //{ + // buzzuav_closures::set_rssi(result_float); + //} + //if(GetRawPacketLoss(all_ids, result_float)) + //{ + // buzzuav_closures::set_raw_packet_loss(result_float); + //} + //if(GetFilteredPacketLoss(all_ids, result_float)) + //{ + // buzzuav_closures::set_filtered_packet_loss(result_float); + //} // This part needs testing since it can overload the xbee module /* * if(GetAPIRssi(all_ids, result_float)) From b41a086ede24c213a8264588f36a4403a87b3efa Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 7 Sep 2017 18:42:26 -0400 Subject: [PATCH 26/39] Looprate setting bug fix. --- src/roscontroller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6ecd042..342b0ed 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -197,7 +197,7 @@ void roscontroller::RosControllerRun() std::string standby_bo = Compile_bzz(stand_by) + ".bo"; //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); /*loop rate of ros*/ - ros::Rate loop_rate(15); + ros::Rate loop_rate(BUZZRATE); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -295,8 +295,6 @@ void roscontroller::RosControllerRun() get_xbee_status(); /*run once*/ ros::spinOnce(); - /*loop rate of ros*/ - ros::Rate loop_rate(BUZZRATE); loop_rate.sleep(); if (fcu_timeout <= 0) buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); From f03b76c241437b6f458a16d6ce0d453d2455b996 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Fri, 8 Sep 2017 12:23:23 -0400 Subject: [PATCH 27/39] merge confilct manual resolution --- buzz_scripts/graphform.bzz | 4 ---- 1 file changed, 4 deletions(-) diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 1b397e9..ef03438 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -797,11 +797,7 @@ function init() { v_tag = stigmergy.create(m_lockstig) uav_initstig() # go to diff. height since no collision avoidance implemented yet -<<<<<<< HEAD - TARGET_ALTITUDE = 4.5 #2.5 + id * 1.5 -======= TARGET_ALTITUDE = 7.5 #2.5 + id * 1.5 ->>>>>>> d587cdba4387b65bcc583671d26ce82fa4e25720 statef=turnedoff UAVSTATE = "TURNEDOFF" } From 79d746d9ed6c5e95ad1c8bab5ebea540bf8673c2 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Fri, 8 Sep 2017 19:29:45 +0000 Subject: [PATCH 28/39] change TX launch --- misc/cmdlinectr.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 2de3326..31f2061 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -31,6 +31,6 @@ function stoprobot { } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch - rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch -# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch +# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch + rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch } From dafe876e15228af992134283254fb5afceb14cc6 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Sun, 10 Sep 2017 22:22:16 -0400 Subject: [PATCH 29/39] tklaunch filechange --- misc/cmdlinectr.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 31f2061..2de3326 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -31,6 +31,6 @@ function stoprobot { } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch -# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch - rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch + rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch +# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch } From ad883f8a798e71a2591937b4386f6a8854a7d5ed Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Sun, 10 Sep 2017 22:47:20 -0400 Subject: [PATCH 30/39] addition of 5 historic logs to preserve --- src/roscontroller.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index bed9e10..49ceb38 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -52,9 +52,11 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv) path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/"; std::string folder_check="mkdir -p "+path; system(folder_check.c_str()); - rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+".log").c_str(), - (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_old.log").c_str()); - path += "logger_"+std::to_string(robot_id)+".log"; + for(int i=5;i>0;i--){ + rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(), + (path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str()); + } + path += "logger_"+std::to_string(robot_id)+"_0.log"; log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); } From ded07aa5e881343223359fe60b6cd74b4249df1b Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Sun, 10 Sep 2017 23:30:57 -0400 Subject: [PATCH 31/39] addition of highlevel packet drop --- src/roscontroller.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 49ceb38..2e4a284 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -195,6 +195,8 @@ void roscontroller::RosControllerRun() /* Set the Buzz bytecode */ if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id)) { + int packet_loss_tmp,time_step=0; + double cur_packet_loss=0; ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); @@ -233,7 +235,19 @@ void roscontroller::RosControllerRun() neighbours_pos_publisher(); send_MPpayload(); /*Check updater state and step code*/ - // update_routine(bcfname.c_str(), dbgfname.c_str()); + // update_routine(bcfname.c_str(), dbgfname.c_str()); + if(time_step==BUZZRATE){ + time_step=0; + cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) ); + packet_loss_tmp=0; + if(cur_packet_loss<0) cur_packet_loss=0; + else if (cur_packet_loss>1) cur_packet_loss=1; + } + else{ + packet_loss_tmp+=(int)buzz_utility::get_inmsg_size(); + time_step++; + } + ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss); /*Log In Msg queue size*/ log<<(int)buzz_utility::get_inmsg_size()<<","; /*Step buzz script */ From 80fd376a8c1243d103aab4133ef2a6309dc71594 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 11 Sep 2017 18:01:56 -0400 Subject: [PATCH 32/39] field modifications 09/11 --- buzz_scripts/graphformGPS.bzz | 10 +++++----- buzz_scripts/include/barrier.bzz | 4 ++-- buzz_scripts/include/uavstates.bzz | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index 3588ed8..a85fcad 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -5,7 +5,7 @@ include "string.bzz" include "vec2.bzz" include "update.bzz" -include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. +#include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. include "uavstates.bzz" # require an 'action' function to be defined here. @@ -14,7 +14,7 @@ include "graphs/shapes_Y.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER -ROOT_ID = 2 +ROOT_ID = 13 # max velocity in cm/step ROBOT_MAXVEL = 150.0 @@ -704,9 +704,9 @@ function init() { # uav_initswarm() #v_tag = stigmergy.create(m_lockstig) - uav_initstig() + #uav_initstig() # go to diff. height since no collision avoidance implemented yet - TARGET_ALTITUDE = 2.5 + id * 1.5 + TARGET_ALTITUDE = 4.5 #2.5 + id * 1.5 statef=turnedoff UAVSTATE = "TURNEDOFF" } @@ -720,7 +720,7 @@ function step() { # get the swarm commands uav_neicmd() # update the vstig (status/net/batt) - uav_updatestig() + #uav_updatestig() #update the graph UpdateNodeInfo() diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/barrier.bzz index 3a029b0..9b1658e 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -7,7 +7,7 @@ # # Constants # -BARRIER_TIMEOUT = 200 # in steps +BARRIER_TIMEOUT = 1200 # in steps BARRIER_VSTIG = 80 timeW = 0 barrier = nil @@ -82,4 +82,4 @@ function getlowest(){ u=u-1 } log("--> LOWEST ID:",Lid) -} \ No newline at end of file +} diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 48a28a7..fa8f6d3 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -152,7 +152,7 @@ function uav_rccmd() { function uav_neicmd() { neighbors.listen("cmd", function(vid, value, rid) { - print("Got (", vid, ",", value, ") from robot #", rid, "(", UAVSTATE, ")") + print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")") if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") { statef=takeoff UAVSTATE = "TAKEOFF" From 0eb07f973d53d0cf7001f09b709c0618fa19d56f Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Mon, 11 Sep 2017 23:02:04 -0400 Subject: [PATCH 33/39] spinner sync --- src/roscontroller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2e4a284..c578ea8 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -227,10 +227,10 @@ void roscontroller::RosControllerRun() const uint8_t shrt_id= 0xFF; float result; - if ( GetFilteredPacketLoss(shrt_id, result) ) - log< Date: Fri, 15 Sep 2017 11:15:03 -0400 Subject: [PATCH 34/39] field improvement to graphformGPS --- buzz_scripts/graphformGPS.bzz | 55 +++++++++++++++++++++++++---------- 1 file changed, 39 insertions(+), 16 deletions(-) diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index a85fcad..8a30502 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -9,7 +9,7 @@ include "update.bzz" include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. include "uavstates.bzz" # require an 'action' function to be defined here. -include "graphs/shapes_Y.bzz" +include "graphs/shapes_L.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS @@ -93,7 +93,7 @@ step_cunt=0 m_lockstig = 1 # Lennard-Jones parameters, may need change -EPSILON = 1800 #13.5 the LJ parameter for other robots +EPSILON = 4000 #13.5 the LJ parameter for other robots # Lennard-Jones interaction magnitude @@ -311,6 +311,8 @@ neighbors.listen("m", m_MessageRange[m_neighbourCount]=m_receivedMessage.Range m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing m_messageID[m_neighbourCount]=rid +log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount]) + m_neighbourCount=m_neighbourCount+1 }) } @@ -496,19 +498,24 @@ function DoAsking(){ log("get response") psResponse=i }} + if(m_MessageState[i]=="STATE_JOINING" and m_MessageLabel[i]==m_nLabel){ + TransitionToFree() + return + } i=i+1 } #analyse response if(psResponse==-1){ #no response, wait + m_unWaitCount=m_unWaitCount-1 m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqID=m_unRequestId - if(m_unWaitCount==0){ - TransitionToFree() - return - } + #if(m_unWaitCount==0){ + #TransitionToFree() + #return + #} } else{ log("respond id=",m_MessageReqID[psResponse]) @@ -528,6 +535,8 @@ function DoAsking(){ } } } + m_selfMessage.Label=m_nLabel + m_navigation.x=0.0 m_navigation.y=0.0 uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) @@ -578,6 +587,9 @@ function set_rc_goto() { # #Do joined # +repeat_assign=0 +assign_label=-1 +assign_id=-1 function DoJoined(){ m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel @@ -609,6 +621,12 @@ function DoJoined(){ var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias} } } + + if(m_MessageState[i]=="STATE_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){ + repeat_assign=0 + } + + #if it is the pred if((m_MessageState[i]=="STATE_JOINED" or m_MessageState[i]=="STATE_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){ seenPred=1 @@ -627,16 +645,21 @@ function DoJoined(){ ReqIndex=i i=i+1 } - #get the best index, whose ReqLabel and Reqid are - ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]] - var ReqID=m_MessageReqID[mapRequests[ReqIndex]] - m_selfMessage.ReqLabel=ReqLabel - m_selfMessage.ReqID=ReqID + if(repeat_assign==0){ + #get the best index, whose ReqLabel and Reqid are + ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]] + var ReqID=m_MessageReqID[mapRequests[ReqIndex]] + assign_label=ReqLabel + assign_id=ReqID + repeat_assign=1 + } + m_selfMessage.ReqLabel=assign_label + m_selfMessage.ReqID=assign_id m_selfMessage.Response=r2i("REQ_GRANTED") - m_vecNodes[ReqLabel].State="ASSIGNING" - log("Label=",ReqLabel) - log("ID=",ReqID) - m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod + #m_vecNodes[ReqLabel].State="ASSIGNING" + log("Label=",assign_label) + log("ID=",assign_id) + m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod } #lost pred, wait for some time and transit to free @@ -706,7 +729,7 @@ function init() { #v_tag = stigmergy.create(m_lockstig) #uav_initstig() # go to diff. height since no collision avoidance implemented yet - TARGET_ALTITUDE = 4.5 #2.5 + id * 1.5 + TARGET_ALTITUDE = 6.0 #2.5 + id * 1.5 statef=turnedoff UAVSTATE = "TURNEDOFF" } From 37c3cf151b12a41e4190d0a7ec454909191ed656 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 28 Sep 2017 18:49:13 -0400 Subject: [PATCH 35/39] new rtt* algo --- buzz_scripts/graphformGPS.bzz | 2 +- buzz_scripts/include/rttstar.bzz | 233 +++++++++++++++++++++++++++++++ buzz_scripts/testalone.bzz | 3 + src/buzzuav_closures.cpp | 4 +- 4 files changed, 239 insertions(+), 3 deletions(-) create mode 100644 buzz_scripts/include/rttstar.bzz diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index 324c52e..f3b0bd5 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -729,7 +729,7 @@ function init() { #v_tag = stigmergy.create(m_lockstig) #uav_initstig() # go to diff. height since no collision avoidance implemented yet - TARGET_ALTITUDE = 3.0 + id * 1.25 + #TARGET_ALTITUDE = 3.0 + id * 1.25 statef=turnedoff UAVSTATE = "TURNEDOFF" } diff --git a/buzz_scripts/include/rttstar.bzz b/buzz_scripts/include/rttstar.bzz new file mode 100644 index 0000000..cfd4924 --- /dev/null +++ b/buzz_scripts/include/rttstar.bzz @@ -0,0 +1,233 @@ +##### +# RRT* Path Planing +# START, GOAL, TOL vec2 +# map table-based matrix +##### + +map = {.nb_col=100, .nb_row=100, .mat={}} + +function RRTSTAR(START,GOAL,TOL) { + # RRT_STAR + HEIGHT = map.nb_col + WIDTH = map.nb_row + RADIUS = 2 + + rng.setseed(11) + + goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} + arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=START.x,.1=START.y}} + numberOfPoints = 1 + Q = {.nb_col=5,.nb_row=1,.mat={.0=START.x,.1=START.y,.2=0,.3=1,.4=0}} + log("Created ",Q) + + # Open space or obstacle + # DISPLAY_patchwork(map); + + goalReached = 0; + while(goalReached == 0) { + # Point generation + x = -HEIGHT*rng.uniform(1.0)-1; + y = WIDTH*rng.uniform(1.0)+1; + log("First trial point (", rng.uniform(1.0), "): ", x, " ", y) + + pointList = findPointsInRadius(x,y,Q,RADIUS); + + log(pointList.nb_col,pointList.nb_row,pointList.mat) + + # Find connection that provides the least cost to come + nbCount = 0; + minCounted = inf; + minCounter = 0; + + if(pointList.nb_col!=0) { + i=0 + while(i 0) { + arrayOfPoints.nb_row=arrayOfPoints.nb_row+1 + arrayOfPoints.mat[arrayOfPoints.nb_row]=x + arrayOfPoints.mat[arrayOfPoints.nb_row+1]=y + numberOfPoints = numberOfPoints + 1; + + wmat(Q,numberOfPoints,0, x) + wmat(Q,numberOfPoints,1, y) + wmat(Q,numberOfPoints,2, rmat(pointList,minCounter,4)); + wmat(Q,numberOfPoints,3, numberOfPoints) + wmat(Q,numberOfPoints,4, minCounted) + + # Now check to see if any of the other points can be redirected + nbCount = 0; + i = 0 + while(i goalBoundary.xmin and y > goalBoundary.ymin and y < goalBoundary.ymax) + goalReached = 1; + } + } else { + # Associate with the closest point + pointNum = findClosestPoint(x,y,arrayOfPoints); + + # Follow the line to see if it intersects anything + intersects = doesItIntersect(x,y,math.vec2.new(rmat(arrayOfPoints,1,pointNum),rmat(arrayOfPoints,2,pointNum)),map); + + # If there is no intersection we need to add to the tree + if(intersects != 1) { + arrayOfPoints.nb_row=arrayOfPoints.nb_row+1 + arrayOfPoints.mat[arrayOfPoints.nb_row]=x + arrayOfPoints.mat[arrayOfPoints.nb_row+1]=y + numberOfPoints = numberOfPoints + 1; + + wmat(Q,numberOfPoints,0, x) + wmat(Q,numberOfPoints,1, y) + wmat(Q,numberOfPoints,2, pointNum); + wmat(Q,numberOfPoints,3, numberOfPoints) + wmat(Q,numberOfPoints,4, rmat(Q,pointNum,5)+math.vec2.length(rmat(Q,pointNum,1)-x,rmat(Q,pointNum,2)-y)) + + # Check to see if this new point is within the goal + if(x < goalBoundary.xmax and x > goalBoundary.xmin and y > goalBoundary.ymin and y < goalBoundary.ymax) + goalReached = 1; + } + } + if(numberOfPoints % 100 == 0) { + log(numberOfPoints, " points processed. Still looking for goal."); + } + } + log("Goal found!"); +} + +function findClosestPoint(x,y,arrayOfPoints) { + # Go through each points and find the distances between them and the + # target point + distance = inf; + i=1 + while(i 0 and yi > 0) { + if(rmap(xi,yi) == 0) + return 1; + } else + return 1; + } + i = i + 1 + } + return 0 +} + +# Write to matrix +function wmat(mat, row, col, val) { + var index = (row-1)*mat.nb_col + col + mat.mat[index] = val +} + +# Read from matrix +function rmat(mat, row, col) { + var index = (row-1)*mat.nb_col + col + if (mat.mat[index] == nil) { + return -1 + } else { + return mat.mat[index] + } +} + +# copy a full matrix row +function mat_copyrow(out,ro,in,ri){ + var indexI = (ri-1)*in.nb_col + var indexO = (ro-1)*out.nb_col + i=0 + while(ilsyms->syms); ++i) { + for(uint32_t i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { buzzvm_lload(vm, i); buzzobj_t o = buzzvm_stack_at(vm, 1); buzzvm_pop(vm); From b1197377400415205d20f210e943b9216b41fd10 Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 2 Oct 2017 12:47:02 -0400 Subject: [PATCH 36/39] RRT* bzz and rosbuzz floor fct --- buzz_scripts/graphformGPS.bzz | 10 +- buzz_scripts/include/mapmatrix.bzz | 93 +++++++++++ buzz_scripts/include/rrtstar.bzz | 248 +++++++++++++++++++++++++++++ buzz_scripts/include/rttstar.bzz | 233 --------------------------- buzz_scripts/include/uavstates.bzz | 61 ++++--- buzz_scripts/testalone.bzz | 8 +- include/buzzuav_closures.h | 1 + src/buzz_utility.cpp | 3 + src/buzzuav_closures.cpp | 9 ++ 9 files changed, 405 insertions(+), 261 deletions(-) create mode 100644 buzz_scripts/include/mapmatrix.bzz create mode 100644 buzz_scripts/include/rrtstar.bzz delete mode 100644 buzz_scripts/include/rttstar.bzz diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index f3b0bd5..01fc3f9 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -8,6 +8,7 @@ include "update.bzz" #include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. include "uavstates.bzz" # require an 'action' function to be defined here. +include "rrtstar.bzz" include "graphs/shapes_L.bzz" @@ -178,14 +179,14 @@ function r2i(value){ #return the index of value # function find(table,value){ - var index=nil + var ind=nil var i=0 while(i 0 and yi > 0){ + wmat(map,xi,yi,0) + wmat(map,xi+1,yi,0) + wmat(map,xi-1,yi,0) + wmat(map,xi,yi+1,0) + wmat(map,xi,yi-1,0) + } +} + +function table_print(t) { + foreach(t, function(key, value) { + log(key, " -> ", value) + }) +} +function print_pos(t) { + ir=1 + while(ir<=t.nb_row){ + log(ir, ": ", rmat(t,ir,1), rmat(t,ir,2)) + ir = ir + 1 + } +} \ No newline at end of file diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz new file mode 100644 index 0000000..f37fe10 --- /dev/null +++ b/buzz_scripts/include/rrtstar.bzz @@ -0,0 +1,248 @@ +##### +# RRT* Path Planing +# +# map table-based matrix +##### +include "mapmatrix.bzz" + +map = {} +cur_cell = {} + +function pathPlanner(m_navigation) { + # create a map big enough for the goal + init_map(2*floor(math.vec2.length(m_navigation))+2) + # create the goal in the map grid + mapgoal = math.vec2.add(m_navigation,cur_cell) + # add all neighbors as obstacles in the grid + neighbors.foreach(function(rid, data) { + add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell)) + }) + # TODO: add proximity sensor obstacles to the grid + # search for a path + return RRTSTAR(mapgoal,math.vec2.new(map.nb_col/20.0,map.nb_row/20.0)) +} + +function RRTSTAR(GOAL,TOL) { + HEIGHT = map.nb_col + WIDTH = map.nb_row + RADIUS = map.nb_col/10.0 # to consider 2 points consecutive + + rng.setseed(11) + + goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} + table_print(goalBoundary) + arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}} + Path = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}} + numberOfPoints = 1 + Q = {.nb_col=5,.nb_row=1,.mat={.0=cur_cell.x,.1=cur_cell.y,.2=0,.3=1,.4=0}} + + goalReached = 0; + timeout = 0 + ## + # main search loop + ## + while(goalReached == 0 and timeout < 200) { + # Point generation + pt = math.vec2.new(HEIGHT*rng.uniform(1.0)+1,WIDTH*rng.uniform(1.0)+1) + + pointList = findPointsInRadius(pt,Q,RADIUS); + + # Find connection that provides the least cost to come + nbCount = 0; + minCounted = 999; + minCounter = 0; + + if(pointList.nb_row!=0) { + #log("Found ", pointList.nb_row, " close point:", pointList.mat) + ipt=1 + while(ipt<=pointList.nb_row){ + pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}} + mat_copyrow(pointNumber,1,pointList,ipt) + #log("pointnumber: ", pointNumber.mat) + + # Follow the line to see if it intersects anything + intersects = doesItIntersect(pt,getvec(pointNumber,1)); + #log("intersects1: ", intersects) + + # If there is no intersection we need consider its connection + nbCount = nbCount + 1; + if(intersects != 1) { + #log(pointNumber, "do not intersect (",pointNumber.mat[3],")") + distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+rmat(Q,pointNumber.mat[3],5) + + if(distance < minCounted) { + minCounted = distance; + minCounter = nbCount; + } + } + ipt = ipt + 1 + } + if(minCounter > 0) { + numberOfPoints = numberOfPoints + 1; + wmat(arrayOfPoints,numberOfPoints,1,pt.x) + wmat(arrayOfPoints,numberOfPoints,2,pt.y) + + wmat(Q,numberOfPoints,1, pt.x) + wmat(Q,numberOfPoints,2, pt.y) + wmat(Q,numberOfPoints,3, rmat(pointList,minCounter,4)); + wmat(Q,numberOfPoints,4, numberOfPoints) + wmat(Q,numberOfPoints,5, minCounted) + + log("added point to Q(", Q.nb_row, "): ", pt.x, pt.y) + + # Now check to see if any of the other points can be redirected + nbCount = 0; + ipt = 1 + while(ipt goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax) + goalReached = 1; + } + } else { + # Associate with the closest point + pointNum = findClosestPoint(pt,arrayOfPoints); + + # Follow the line to see if it intersects anything + intersects = doesItIntersect(pt,getvec(arrayOfPoints,pointNum)); + #log("intersects3 (", pointNum, "): ", intersects) + + # If there is no intersection we need to add to the tree + if(intersects != 1) { + numberOfPoints = numberOfPoints + 1; + wmat(arrayOfPoints,numberOfPoints,1,pt.x) + wmat(arrayOfPoints,numberOfPoints,2,pt.y) + + wmat(Q,numberOfPoints,1, pt.x) + wmat(Q,numberOfPoints,2, pt.y) + wmat(Q,numberOfPoints,3, pointNum); + wmat(Q,numberOfPoints,4, numberOfPoints) + wmat(Q,numberOfPoints,5, rmat(Q,pointNum,5)+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt))) + + log("added point to Q(", Q.nb_row, "): ", pt.x, pt.y) + + # Check to see if this new point is within the goal + if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax) + goalReached = 1; + } + } + if(numberOfPoints % 100 == 0) { + log(numberOfPoints, " points processed. Still looking for goal."); + } + timeout = timeout + 1 + } + if(goalReached){ + log("Goal found(",numberOfPoints,")!") + Path = getPath(Q,numberOfPoints) + print_pos(Path) + } + return Path +} + +function findClosestPoint(point,aPt) { + # Go through each points and find the distances between them and the + # target point + distance = 999 + pointNumber = -1 + ifcp=1 + while(ifcp<=aPt.nb_row) { + range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) + + if(range < distance) { + distance = range; + pointNumber = ifcp; + } + ifcp = ifcp + 1 + } + return pointNumber +} + +function findPointsInRadius(point,q,r) { + counted = 0; + pointList = {.nb_col=q.nb_col, .nb_row=counted, .mat={}} + iir=1 + while(iir <= q.nb_row) { + distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) + + if(distance < r) { + counted = counted+1; + pointList.nb_row=counted + mat_copyrow(pointList,counted,q,iir) + } + + iir = iir + 1 + } + return pointList +} + +function doesItIntersect(point,vector) { + dif = math.vec2.sub(point,vector) + distance = math.vec2.length(dif) + vec = math.vec2.scale(dif,1/distance) + + idii = 1 + while(idii<=100) { + range = distance/100.0*idii + pos_chk = math.vec2.sub(pt,math.vec2.scale(vec,range)); + + # Find what block we're in right now + xi = floor(pos_chk.x)+1 + yi = floor(pos_chk.y)+1 + #log("Grid :", pos_chk.x, pos_chk.y, xi, yi) + + if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) { + if(rmat(map,xi,yi) == 0) { + #log("Obstacle (", rmat(map,xi,yi), ")!") + return 1; + } + } else { + #log("Outside the map(", x, y, ")!") + return 1; + } + idii = idii + 1 + } + #log("No intersect!") + return 0 +} + +function getPath(Q,nb){ + path={.nb_col=2, .nb_row=0, .mat={}} + npt=0 + # get the path from the point list + while(nb!=1){ + npt=npt+1 + path.nb_row=npt + path.mat[npt*2]=rmat(Q,nb,1) + path.mat[npt*2+1]=rmat(Q,nb,2) + nb = rmat(Q,nb,3); + } + + # re-order the list and make the path points absolute + pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}} + while(npt>0){ + tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell)) + pathR.mat[(path.nb_row-npt)*2]=tmpgoal.latitude + pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.longitude + npt = npt - 1 + } + return pathR +} \ No newline at end of file diff --git a/buzz_scripts/include/rttstar.bzz b/buzz_scripts/include/rttstar.bzz deleted file mode 100644 index cfd4924..0000000 --- a/buzz_scripts/include/rttstar.bzz +++ /dev/null @@ -1,233 +0,0 @@ -##### -# RRT* Path Planing -# START, GOAL, TOL vec2 -# map table-based matrix -##### - -map = {.nb_col=100, .nb_row=100, .mat={}} - -function RRTSTAR(START,GOAL,TOL) { - # RRT_STAR - HEIGHT = map.nb_col - WIDTH = map.nb_row - RADIUS = 2 - - rng.setseed(11) - - goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} - arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=START.x,.1=START.y}} - numberOfPoints = 1 - Q = {.nb_col=5,.nb_row=1,.mat={.0=START.x,.1=START.y,.2=0,.3=1,.4=0}} - log("Created ",Q) - - # Open space or obstacle - # DISPLAY_patchwork(map); - - goalReached = 0; - while(goalReached == 0) { - # Point generation - x = -HEIGHT*rng.uniform(1.0)-1; - y = WIDTH*rng.uniform(1.0)+1; - log("First trial point (", rng.uniform(1.0), "): ", x, " ", y) - - pointList = findPointsInRadius(x,y,Q,RADIUS); - - log(pointList.nb_col,pointList.nb_row,pointList.mat) - - # Find connection that provides the least cost to come - nbCount = 0; - minCounted = inf; - minCounter = 0; - - if(pointList.nb_col!=0) { - i=0 - while(i 0) { - arrayOfPoints.nb_row=arrayOfPoints.nb_row+1 - arrayOfPoints.mat[arrayOfPoints.nb_row]=x - arrayOfPoints.mat[arrayOfPoints.nb_row+1]=y - numberOfPoints = numberOfPoints + 1; - - wmat(Q,numberOfPoints,0, x) - wmat(Q,numberOfPoints,1, y) - wmat(Q,numberOfPoints,2, rmat(pointList,minCounter,4)); - wmat(Q,numberOfPoints,3, numberOfPoints) - wmat(Q,numberOfPoints,4, minCounted) - - # Now check to see if any of the other points can be redirected - nbCount = 0; - i = 0 - while(i goalBoundary.xmin and y > goalBoundary.ymin and y < goalBoundary.ymax) - goalReached = 1; - } - } else { - # Associate with the closest point - pointNum = findClosestPoint(x,y,arrayOfPoints); - - # Follow the line to see if it intersects anything - intersects = doesItIntersect(x,y,math.vec2.new(rmat(arrayOfPoints,1,pointNum),rmat(arrayOfPoints,2,pointNum)),map); - - # If there is no intersection we need to add to the tree - if(intersects != 1) { - arrayOfPoints.nb_row=arrayOfPoints.nb_row+1 - arrayOfPoints.mat[arrayOfPoints.nb_row]=x - arrayOfPoints.mat[arrayOfPoints.nb_row+1]=y - numberOfPoints = numberOfPoints + 1; - - wmat(Q,numberOfPoints,0, x) - wmat(Q,numberOfPoints,1, y) - wmat(Q,numberOfPoints,2, pointNum); - wmat(Q,numberOfPoints,3, numberOfPoints) - wmat(Q,numberOfPoints,4, rmat(Q,pointNum,5)+math.vec2.length(rmat(Q,pointNum,1)-x,rmat(Q,pointNum,2)-y)) - - # Check to see if this new point is within the goal - if(x < goalBoundary.xmax and x > goalBoundary.xmin and y > goalBoundary.ymin and y < goalBoundary.ymax) - goalReached = 1; - } - } - if(numberOfPoints % 100 == 0) { - log(numberOfPoints, " points processed. Still looking for goal."); - } - } - log("Goal found!"); -} - -function findClosestPoint(x,y,arrayOfPoints) { - # Go through each points and find the distances between them and the - # target point - distance = inf; - i=1 - while(i 0 and yi > 0) { - if(rmap(xi,yi) == 0) - return 1; - } else - return 1; - } - i = i + 1 - } - return 0 -} - -# Write to matrix -function wmat(mat, row, col, val) { - var index = (row-1)*mat.nb_col + col - mat.mat[index] = val -} - -# Read from matrix -function rmat(mat, row, col) { - var index = (row-1)*mat.nb_col + col - if (mat.mat[index] == nil) { - return -1 - } else { - return mat.mat[index] - } -} - -# copy a full matrix row -function mat_copyrow(out,ro,in,ri){ - var indexI = (ri-1)*in.nb_col - var indexO = (ro-1)*out.nb_col - i=0 - while(iGOTO_MAXDIST) + if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) log("Sorry this is too far.") - else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity - m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) - } else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination - transf() - else - uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + else { + if(Path==nil){ + Path = pathPlanner(rc_goal) + cur_goal_l = 1 + } else if(cur_goal_l<=Path.nb_row) { + cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude + cur_pt=vec_from_gps(cur_gps.x,cur_gps.y) + print(" first to ", cur_pt.x,cur_pt.y) + if(math.vec2.length(cur_pt)>GOTODIST_TOL) { + cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt)) + uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude) + } + else + cur_goal_l = cur_goal_l + 1 + } else { + Path = nil + transf() + } + } + +# if(math.vec2.length(m_navigation)>GOTO_MAXDIST) +# log("Sorry this is too far.") +# else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity +# m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) +# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) +# } else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination +# transf() +# else +# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) } function follow() { @@ -181,18 +202,18 @@ function LimitAngle(angle){ return angle } -function rb_from_gps(lat, lon) { -# print("gps2rb d: ",lat,lon) -# print("gps2rb h: ",position.latitude,position.longitude) +function vec_from_gps(lat, lon) { d_lon = lon - position.longitude d_lat = lat - position.latitude ned_x = d_lat/180*math.pi * 6371000.0; ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); - goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); - goal.bearing = LimitAngle(math.atan(ned_y,ned_x)); + #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); + return math.vec2.new(ned_x,ned_y) } function gps_from_vec(vec) { + Lgoal = {.latitude=0, .longitude=0} Vrange = math.vec2.length(vec) Vbearing = LimitAngle(math.atan(vec.y, vec.x)) # print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude) @@ -200,10 +221,12 @@ function gps_from_vec(vec) { lonR = position.longitude*math.pi/180.0; target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing)); target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat)); - goal.latitude = target_lat*180.0/math.pi; - goal.longitude = target_lon*180.0/math.pi; + Lgoal.latitude = target_lat*180.0/math.pi; + Lgoal.longitude = target_lon*180.0/math.pi; #d_lat = (vec.x / 6371000.0)*180.0/math.pi; #goal.latitude = d_lat + position.latitude; #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi; #goal.longitude = d_lon + position.longitude; + + return Lgoal } diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testalone.bzz index 215497e..bba5d7a 100644 --- a/buzz_scripts/testalone.bzz +++ b/buzz_scripts/testalone.bzz @@ -3,12 +3,12 @@ include "update.bzz" include "barrier.bzz" # don't use a stigmergy id=11 with this header. include "uavstates.bzz" # require an 'action' function to be defined here. include "vstigenv.bzz" -include "rttstar.bzz" +include "rrtstar.bzz" function action() { statef=action -# test moveto cmd dx, dy -# uav_moveto(0.5, 0.5, 0.0) + uav_storegoal(45.5085,-73.1531935979886,5.0) + set_goto(picture) } # Executed once at init time. @@ -16,8 +16,6 @@ function init() { statef=turnedoff UAVSTATE = "TURNEDOFF" uav_initstig() - make_test_map() - RRTSTAR(math.vec2.new(0,0),math.vec2.new(5,5),math.vec2.new(1,1)) } # Executed at each time step. diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index eeb9184..6a5d18a 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -38,6 +38,7 @@ void setWPlist(std::string path); * buzzuav_goto(latitude,longitude,altitude) function in Buzz * commands the UAV to go to a position supplied */ +int buzz_floor(buzzvm_t vm); int buzzuav_moveto(buzzvm_t vm); int buzzuav_storegoal(buzzvm_t vm); int buzzuav_setgimbal(buzzvm_t vm); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 1a02c36..947136e 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -297,6 +297,9 @@ void in_message_process(){ buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "floor", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_floor)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 5083dd1..68b0065 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -170,6 +170,15 @@ namespace buzzuav_closures{ fin.close(); } + + int buzz_floor(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 1); + buzzvm_lload(vm, 1); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + float fval = buzzvm_stack_at(vm, 1)->f.value; + buzzvm_pushi(vm, floor(fval)); + return buzzvm_ret1(vm); + } /*----------------------------------------/ / Buzz closure to move following a 2D vector /----------------------------------------*/ From 98c0dbc62f6c004124d6b5828aac2597d9df2730 Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 22 Nov 2017 19:06:22 -0500 Subject: [PATCH 37/39] removed floor, add PY rosprint, hard-coded swarm size, add states bzzcmd --- buzz_scripts/graphformGPS.bzz | 24 +- buzz_scripts/include/graphs/shapes_L.bzz | 2 +- buzz_scripts/include/graphs/shapes_O.bzz | 2 +- buzz_scripts/include/graphs/shapes_P.bzz | 2 +- buzz_scripts/include/graphs/shapes_Y.bzz | 2 +- buzz_scripts/include/mapmatrix.bzz | 110 ++++++--- buzz_scripts/include/rrtstar.bzz | 279 +++++++++++++++++++---- buzz_scripts/include/uavstates.bzz | 60 +++-- src/buzz_utility.cpp | 9 +- src/buzzuav_closures.cpp | 96 ++++---- 10 files changed, 439 insertions(+), 147 deletions(-) diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index 01fc3f9..998000e 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -8,9 +8,11 @@ include "update.bzz" #include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. include "uavstates.bzz" # require an 'action' function to be defined here. -include "rrtstar.bzz" +include "graphs/shapes_P.bzz" +include "graphs/shapes_O.bzz" include "graphs/shapes_L.bzz" +include "graphs/shapes_Y.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS @@ -680,7 +682,8 @@ function DoJoined(){ #if(v_tag.size()==ROBOTS){ # TransitionToLock() #} - barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1) + #barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1) + barrier_wait(6, TransitionToLock, DoJoined, -1) } # @@ -701,6 +704,9 @@ function DoLock(){ } #move uav_moveto(m_navigation.x, m_navigation.y, 0.0) + + # prepare to restart a new shape + statef=action } function action(){ @@ -745,7 +751,7 @@ function step() { # get the swarm commands uav_neicmd() # update the vstig (status/net/batt) - #uav_updatestig() + #uav_updatestig() #update the graph UpdateNodeInfo() @@ -797,8 +803,16 @@ function step() { # Executed when reset # function Reset(){ - Read_Graph() - m_nLabel=-1 + if(rc_State==0){ + Read_GraphP() + } else if(rc_State==1) { + Read_GraphO() + } else if(rc_State==2) { + Read_GraphL() + } else if(rc_State==3) { + Read_GraphY() + } + m_nLabel=-1 #start listening start_listen() diff --git a/buzz_scripts/include/graphs/shapes_L.bzz b/buzz_scripts/include/graphs/shapes_L.bzz index 16f2272..2988d81 100644 --- a/buzz_scripts/include/graphs/shapes_L.bzz +++ b/buzz_scripts/include/graphs/shapes_L.bzz @@ -1,7 +1,7 @@ #Table of the nodes in the graph m_vecNodes={} -function Read_Graph(){ +function Read_GraphL(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point .Pred = -1, # Label of its predecessor diff --git a/buzz_scripts/include/graphs/shapes_O.bzz b/buzz_scripts/include/graphs/shapes_O.bzz index 679a044..830bc60 100644 --- a/buzz_scripts/include/graphs/shapes_O.bzz +++ b/buzz_scripts/include/graphs/shapes_O.bzz @@ -1,7 +1,7 @@ #Table of the nodes in the graph m_vecNodes={} m_vecNodes_fixed={} -function Read_Graph(){ +function Read_GraphO(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point .Pred = -1, # Label of its predecessor diff --git a/buzz_scripts/include/graphs/shapes_P.bzz b/buzz_scripts/include/graphs/shapes_P.bzz index 34029be..2074531 100644 --- a/buzz_scripts/include/graphs/shapes_P.bzz +++ b/buzz_scripts/include/graphs/shapes_P.bzz @@ -1,7 +1,7 @@ #Table of the nodes in the graph m_vecNodes={} m_vecNodes_fixed={} -function Read_Graph(){ +function Read_GraphP(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point .Pred = -1, # Label of its predecessor diff --git a/buzz_scripts/include/graphs/shapes_Y.bzz b/buzz_scripts/include/graphs/shapes_Y.bzz index 29c78ec..03599cf 100644 --- a/buzz_scripts/include/graphs/shapes_Y.bzz +++ b/buzz_scripts/include/graphs/shapes_Y.bzz @@ -1,7 +1,7 @@ #Table of the nodes in the graph m_vecNodes={} m_vecNodes_fixed={} -function Read_Graph(){ +function Read_GraphY(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point .Pred = -1, # Label of its predecessor diff --git a/buzz_scripts/include/mapmatrix.bzz b/buzz_scripts/include/mapmatrix.bzz index 23052e2..a3ab889 100644 --- a/buzz_scripts/include/mapmatrix.bzz +++ b/buzz_scripts/include/mapmatrix.bzz @@ -1,5 +1,7 @@ # Write to matrix +robot_marker = "X" + function wmat(mat, row, col, val) { var index = (row-1)*mat.nb_col + (col - 1) if( row <= mat.nb_row) { # update val @@ -13,8 +15,9 @@ function wmat(mat, row, col, val) { # Read from matrix function rmat(mat, row, col) { #log("rmat ", mat, row, col) - var index = (row-1)*mat.nb_col + (col - 1) + index = (row-1)*mat.nb_col + (col - 1) if (mat.mat[index] == nil) { + log("Wrong matrix read index: ", row, " ", col) return -1 } else { return mat.mat[index] @@ -36,46 +39,56 @@ function getvec(t,row){ return math.vec2.new(rmat(t,row,1),rmat(t,row,2)) } -function init_test_map(){ - # creates a 10x10 map - map = {.nb_col=10, .nb_row=10, .mat={}} - cur_cell = math.vec2.new(1,1) +function init_test_map(len){ + map = {.nb_col=len, .nb_row=len, .mat={}} index = 0 - while(index<10*10){ - map.mat[index]=1 + while(index 0 and yi > 0) { + #log("Add obstacle in cell: ", xi, " ", yi) + old=rmat(map,xi,yi) + if(old-inc_trust > 0.0) + wmat(map,xi,yi,old-inc_trust) + else + wmat(map,xi,yi,0.0) + } +} + +function remove_obstacle(pos, off, dec_trust) { + xi = math.round(pos.x) + yi = math.round(pos.y) + if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0){ - wmat(map,xi,yi,0) - wmat(map,xi+1,yi,0) - wmat(map,xi-1,yi,0) - wmat(map,xi,yi+1,0) - wmat(map,xi,yi-1,0) + #log("Remove obstacle in cell: ", xi, " ", yi) + old=rmat(map, xi, yi) + if(old + dec_trust < 1.0) #x,y + wmat(map, xi, yi, old+dec_trust) + else + wmat(map, xi, yi, 1.0) } } @@ -84,10 +97,55 @@ function table_print(t) { log(key, " -> ", value) }) } + +function table_copy(t) { + var t2 = {} + foreach(t, function(key, value) { + t2[key] = value + }) + return t2 +} + function print_pos(t) { ir=1 while(ir<=t.nb_row){ - log(ir, ": ", rmat(t,ir,1), rmat(t,ir,2)) + log(ir, ": ", rmat(t,ir,1), " ", rmat(t,ir,2)) ir = ir + 1 } -} \ No newline at end of file +} +function print_map(t) { + ir=t.nb_row + log("Printing a ", t.nb_row, " by ", t.nb_col, " map") + while(ir>0){ + logst=string.concat("\t", string.tostring(ir), "\t:") + ic=t.nb_col + while(ic>0){ + if(ir==cur_cell.x and ic==cur_cell.y) + logst = string.concat(logst, " XXXXXXXX") + else + logst = string.concat(logst, " ", string.tostring(rmat(t,ir,ic))) + ic = ic - 1 + } + log(logst) + ir = ir - 1 + } +} + +function print_map_argos(t){ + ir=t.nb_row + msg = string.tostring(ir) + while(ir>0){ + ic=t.nb_col + while(ic>0){ + if(ir==cur_cell.x and ic==cur_cell.y){ + msg = string.concat(msg, ":", robot_marker) + } + else { + msg = string.concat(msg, ":", string.tostring(rmat(t,ir,ic))) + } + ic = ic - 1 + } + ir = ir - 1 + } + set_argos_map(msg) +} diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index f37fe10..f6c39ef 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -5,45 +5,187 @@ ##### include "mapmatrix.bzz" -map = {} +RRT_TIMEOUT = 200 +map = nil cur_cell = {} +nei_cell = {} -function pathPlanner(m_navigation) { +function UAVpathPlanner(m_navigation) { # create a map big enough for the goal - init_map(2*floor(math.vec2.length(m_navigation))+2) + init_map(2*math.round(math.vec2.length(m_navigation))+2) + # center the robot on the grid + cur_cell = math.vec2.new(math.round(len/2.0),math.round(len/2.0)) # create the goal in the map grid mapgoal = math.vec2.add(m_navigation,cur_cell) # add all neighbors as obstacles in the grid neighbors.foreach(function(rid, data) { - add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell)) + add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) }) # TODO: add proximity sensor obstacles to the grid # search for a path - return RRTSTAR(mapgoal,math.vec2.new(map.nb_col/20.0,map.nb_row/20.0)) + return RRTSTAR(mapgoal,math.vec2.new(5,5)) #map.nb_col/20.0,map.nb_row/20.0)) +} + +function Kh4pathPlanner(m_goal, m_pos) { + # move 0,0 to a corner instead of the center + m_goal = math.vec2.sub(m_goal,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y)) + + # place the robot on the grid + update_curcell(m_pos) + log("Starting from cell: ", cur_cell.x, " ", cur_cell.y) + log("Going to cell: ", m_goal.x, " ", m_goal.y) + + # search for a path + print_map(map) + # print_map_argos(map) + return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y)) +} + +function update_curcell(m_curpos) { + cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y)) + if(cur_cell.x>map.nb_row) + cur_cell.x=map.nb_row + if(cur_cell.y>map.nb_col) + cur_cell.y=map.nb_col + if(cur_cell.x<1) + cur_cell.x=1 + if(cur_cell.y<1) + cur_cell.y=1 +} + +function getneiobs (m_curpos) { + foundobstacle = 0 + update_curcell(m_curpos) + old_nei = table_copy(nei_cell) + #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) + + neighbors.foreach(function(rid, data) { + #log("nei rel pos: ", data.distance, "m ", data.azimuth*180.0/math.pi, "deg") + Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos) + #log("nei abs pos: ", Ncell.x, "cm ", Ncell.y, "cm") + Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y)) + nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y} + #log("Neighbor in : ", Ncell.x, " ", Ncell.y) + if(old_nei[rid]!=nil) { + if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) { + #log("Neighbor moved ! ", nei_cell[rid].x, " ", nei_cell[rid].y) + remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1) + remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1) + remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1) + remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1) + remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1) + add_obstacle(Ncell, 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) + foundobstacle = 1 + } + } else { + add_obstacle(Ncell, 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) + add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) + foundobstacle = 1 + } + }) + + #if(foundobstacle) { + #print_map(map) + #} + + return foundobstacle +} + +function getproxobs (m_curpos) { + foundobstacle = 0 + update_curcell(m_curpos) + + reduce(proximity, + function(key, value, acc) { + obs = math.vec2.add(math.vec2.newp(1.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell) + obs2 = math.vec2.add(math.vec2.newp(2.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell) + per = math.vec2.sub(obs,cur_cell) + obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs) + obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) + obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) + obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) + if(value.value > IR_SENSOR_THRESHOLD) { + if(rmat(map,math.round(obs.x),math.round(obs.y))!=0) { + add_obstacle(obs, 0, 0.25) + add_obstacle(obs2, 0, 0.25) + add_obstacle(obsr, 0, 0.25) + add_obstacle(obsr2, 0, 0.25) + add_obstacle(obsl, 0, 0.25) + add_obstacle(obsl2, 0, 0.25) + foundobstacle = 1 + } + } else if(rmat(map,math.round(obs.x),math.round(obs.y))!=1) { + remove_obstacle(obs, 0, 0.05) + foundobstacle = 1 + } + return acc + }, math.vec2.new(0, 0)) + + #if(foundobstacle) { + # reduce(proximity, + # function(key, value, acc){ + # log(value.value, ", ", value.angle) + # return acc + # }, math.vec2.new(0, 0)) + # print_map(map) + #} + + return foundobstacle +} + +function check_path(m_path, goal_l, m_curpos) { + pathisblocked = 0 + nb=goal_l + update_curcell(m_curpos) + #m_curpos = math.vec2.sub(m_curpos,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + #Cvec = math.vec2.new(math.round(m_curpos.x*CM2KH4.x), math.round(m_curpos.y*CM2KH4.y)) + Cvec = cur_cell + while(nb < m_path.nb_row and nb <= goal_l+1){ + Nvec = getvec(m_path, nb) + if(doesItIntersect(Cvec, Nvec)){ + log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") + pathisblocked = 1 + } + Cvec=Nvec + nb = nb + 1 + } + + return pathisblocked } function RRTSTAR(GOAL,TOL) { - HEIGHT = map.nb_col - WIDTH = map.nb_row - RADIUS = map.nb_col/10.0 # to consider 2 points consecutive - - rng.setseed(11) + HEIGHT = map.nb_col-1 + WIDTH = map.nb_row-1 + RADIUS = 1.25 #TOL.x #map.nb_col/10.0 # to consider 2 points consecutive + goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} - table_print(goalBoundary) + #table_print(goalBoundary) arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}} Path = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}} numberOfPoints = 1 Q = {.nb_col=5,.nb_row=1,.mat={.0=cur_cell.x,.1=cur_cell.y,.2=0,.3=1,.4=0}} - + goalReached = 0; timeout = 0 ## # main search loop ## - while(goalReached == 0 and timeout < 200) { - # Point generation - pt = math.vec2.new(HEIGHT*rng.uniform(1.0)+1,WIDTH*rng.uniform(1.0)+1) + while(goalReached == 0 and timeout < RRT_TIMEOUT) { + + # Point generation only as grid cell centers + pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1)) + #log("Point generated: ", pt.x, " ", pt.y) pointList = findPointsInRadius(pt,Q,RADIUS); @@ -51,14 +193,13 @@ function RRTSTAR(GOAL,TOL) { nbCount = 0; minCounted = 999; minCounter = 0; - + if(pointList.nb_row!=0) { - #log("Found ", pointList.nb_row, " close point:", pointList.mat) + #log("Found ", pointList.nb_row, " close point:", pointList.mat) ipt=1 while(ipt<=pointList.nb_row){ pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}} mat_copyrow(pointNumber,1,pointList,ipt) - #log("pointnumber: ", pointNumber.mat) # Follow the line to see if it intersects anything intersects = doesItIntersect(pt,getvec(pointNumber,1)); @@ -88,7 +229,7 @@ function RRTSTAR(GOAL,TOL) { wmat(Q,numberOfPoints,4, numberOfPoints) wmat(Q,numberOfPoints,5, minCounted) - log("added point to Q(", Q.nb_row, "): ", pt.x, pt.y) + #log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y) # Now check to see if any of the other points can be redirected nbCount = 0; @@ -138,7 +279,7 @@ function RRTSTAR(GOAL,TOL) { wmat(Q,numberOfPoints,4, numberOfPoints) wmat(Q,numberOfPoints,5, rmat(Q,pointNum,5)+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt))) - log("added point to Q(", Q.nb_row, "): ", pt.x, pt.y) + #log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y) # Check to see if this new point is within the goal if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax) @@ -154,6 +295,9 @@ function RRTSTAR(GOAL,TOL) { log("Goal found(",numberOfPoints,")!") Path = getPath(Q,numberOfPoints) print_pos(Path) + } else { + log("FAILED TO FIND A PATH!!!") + Path = nil } return Path } @@ -166,7 +310,7 @@ function findClosestPoint(point,aPt) { ifcp=1 while(ifcp<=aPt.nb_row) { range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) - + if(range < distance) { distance = range; pointNumber = ifcp; @@ -176,13 +320,16 @@ function findClosestPoint(point,aPt) { return pointNumber } +# Find the closest point in the tree function findPointsInRadius(point,q,r) { counted = 0; pointList = {.nb_col=q.nb_col, .nb_row=counted, .mat={}} iir=1 while(iir <= q.nb_row) { + tmpvv = getvec(q,iir) + #log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y) distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) - + if(distance < r) { counted = counted+1; pointList.nb_row=counted @@ -194,37 +341,54 @@ function findPointsInRadius(point,q,r) { return pointList } +# check if the line between 2 point intersect an obstacle function doesItIntersect(point,vector) { + #log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y) dif = math.vec2.sub(point,vector) distance = math.vec2.length(dif) - vec = math.vec2.scale(dif,1/distance) - - idii = 1 - while(idii<=100) { - range = distance/100.0*idii - pos_chk = math.vec2.sub(pt,math.vec2.scale(vec,range)); - + if(distance==0.0){ # Find what block we're in right now - xi = floor(pos_chk.x)+1 - yi = floor(pos_chk.y)+1 - #log("Grid :", pos_chk.x, pos_chk.y, xi, yi) - - if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) { - if(rmat(map,xi,yi) == 0) { - #log("Obstacle (", rmat(map,xi,yi), ")!") - return 1; - } - } else { - #log("Outside the map(", x, y, ")!") - return 1; + xi = math.round(point.x) #+1 + yi = math.round(point.y) #+1 + if(xi!=cur_cell.x and yi!=cur_cell.y){ + if(rmat(map,xi,yi) > 0.5) + return 1 + else + return 0 + } else + return 0 + } + vec = math.vec2.scale(dif,1.0/distance) + pathstep = map.nb_col - 2 + + idii = 1.0 + while(idii <= pathstep) { + range = distance*idii/pathstep + pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); + + # Find what block we're in right now + xi = math.round(pos_chk.x) #+1 + yi = math.round(pos_chk.y) #+1 + #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range) + + if(xi!=cur_cell.x and yi!=cur_cell.y){ + if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) { + if(rmat(map,xi,yi) < 0.5) { # because obstacle use trust values + #log("Obstacle in the way(", xi, " ", yi, ": ", rmat(map,xi,yi), ")!") + return 1; + } + } else { + #log("Outside the map(", xi, " ", yi, ")! ", range, "/", distance, " : ", pos_chk.x, " ", pos_chk.y, " : ", vec.x, " ", vec.y) + return 1; + } } - idii = idii + 1 + idii = idii + 1.0 } #log("No intersect!") return 0 } -function getPath(Q,nb){ +function getPathGPS(Q,nb){ path={.nb_col=2, .nb_row=0, .mat={}} npt=0 # get the path from the point list @@ -245,4 +409,31 @@ function getPath(Q,nb){ npt = npt - 1 } return pathR -} \ No newline at end of file +} + +# create a table with only the path's points in order +function getPath(Q,nb){ + path={.nb_col=2, .nb_row=0, .mat={}} + npt=0 + + # log("get the path from the point list") + while(nb!=1){ + npt=npt+1 + path.nb_row=npt + path.mat[(npt-1)*2]=rmat(Q,nb,1) + path.mat[(npt-1)*2+1]=rmat(Q,nb,2) + nb = rmat(Q,nb,3); + } + + # log("re-order the list") + # table_print(path.mat) + pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}} + while(npt>0){ + tmpgoal = getvec(path,npt) + pathR.mat[(path.nb_row-npt)*2]=tmpgoal.x + pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.y + npt = npt - 1 + } + log("Double-check path: ", check_path(pathR, 1, cur_cell)) + return pathR +} diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index efe4025..d5bc263 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -3,6 +3,8 @@ # FLIGHT-RELATED FUNCTIONS # ######################################## +include "rrtstar.bzz" + TARGET_ALTITUDE = 5.0 # m. UAVSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps @@ -11,6 +13,7 @@ GOTO_MAXDIST = 150 # m. GOTODIST_TOL = 0.5 # m. GOTOANG_TOL = 0.1 # rad. cur_goal_l = 0 +rc_State = 0 function uav_initswarm() { s = swarm.create(1) @@ -32,7 +35,8 @@ function takeoff() { statef=takeoff if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS, action, land, -1) + #barrier_set(ROBOTS, action, land, -1) + barrier_set(6, action, land, -1) barrier_ready() } else { log("Altitude: ", position.altitude) @@ -49,7 +53,8 @@ function land() { uav_land() if(flight.status != 2 and flight.status != 3) { - barrier_set(ROBOTS,turnedoff,land, 21) + #barrier_set(ROBOTS,turnedoff,land, 21) + barrier_set(6,turnedoff,land, 21) barrier_ready() } } @@ -85,7 +90,7 @@ function picture() { ptime=ptime+1 } -function gotoWP(transf) { +function gotoWPRRT(transf) { rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude) print(" has to move ", math.vec2.length(rc_goal)) @@ -93,7 +98,7 @@ function gotoWP(transf) { log("Sorry this is too far.") else { if(Path==nil){ - Path = pathPlanner(rc_goal) + Path = UAVpathPlanner(rc_goal) cur_goal_l = 1 } else if(cur_goal_l<=Path.nb_row) { cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude @@ -110,16 +115,21 @@ function gotoWP(transf) { transf() } } +} -# if(math.vec2.length(m_navigation)>GOTO_MAXDIST) -# log("Sorry this is too far.") -# else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity -# m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) -# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) -# } else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination -# transf() -# else -# uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) +function gotoWP(transf) { + m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) + + if(math.vec2.length(m_navigation)>GOTO_MAXDIST) + log("Sorry this is too far.") + else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity + m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) + } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination + transf() + else + uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) } function follow() { @@ -167,6 +177,18 @@ function uav_rccmd() { } else if (flight.rc_cmd==666){ flight.rc_cmd=0 stattab_send() + } else if (flight.rc_cmd==900){ + flight.rc_cmd=0 + rc_State = 0 + } else if (flight.rc_cmd==901){ + flight.rc_cmd=0 + rc_State = 1 + } else if (flight.rc_cmd==902){ + flight.rc_cmd=0 + rc_State = 2 + } else if (flight.rc_cmd==903){ + flight.rc_cmd=0 + rc_State = 3 } } @@ -176,14 +198,22 @@ function uav_neicmd() { print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")") if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") { statef=takeoff - UAVSTATE = "TAKEOFF" + UAVSTATE = "TAKEOFF" } else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") { statef=land - UAVSTATE = "LAND" + UAVSTATE = "LAND" } else if(value==400 and UAVSTATE=="TURNEDOFF") { uav_arm() } else if(value==401 and UAVSTATE=="TURNEDOFF"){ uav_disarm() + } else if(value==900){ + rc_State = 0 + } else if(value==901){ + rc_State = 1 + } else if(value==902){ + rc_State = 2 + } else if(value==903){ + rc_State = 3 } else if(value==16 and UAVSTATE=="IDLE"){ # neighbors.listen("gt",function(vid, value, rid) { # print("Got (", vid, ",", value, ") from robot #", rid) diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 947136e..06cd665 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -297,9 +297,6 @@ void in_message_process(){ buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "floor", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_floor)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); @@ -470,9 +467,11 @@ int create_stig_tables() { if(VM) buzzvm_destroy(&VM); Robot_id = robot_id; VM = buzzvm_new((int)robot_id); + ROS_INFO(" Robot ID -1: %i" , robot_id); /* Get rid of debug info */ if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); DBG_INFO = buzzdebug_new(); + ROS_INFO(" Robot ID -2: %i" , robot_id); /* Read bytecode and fill in data structure */ FILE* fd = fopen(bo_filename, "rb"); if(!fd) { @@ -491,6 +490,7 @@ int create_stig_tables() { return 0; } fclose(fd); + ROS_INFO(" Robot ID -3: %i" , robot_id); /* Read debug information */ if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { buzzvm_destroy(&VM); @@ -498,6 +498,7 @@ int create_stig_tables() { perror(bdbg_filename); return 0; } + ROS_INFO(" Robot ID -4: %i" , robot_id); /* Set byte code */ if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); @@ -505,6 +506,7 @@ int create_stig_tables() { ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename); return 0; } + ROS_INFO(" Robot ID -5: %i" , robot_id); /* Register hook functions */ if(buzz_register_hooks() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); @@ -512,6 +514,7 @@ int create_stig_tables() { ROS_ERROR("[%i] Error registering hooks", Robot_id); return 0; } + ROS_INFO(" Robot ID -6: %i" , robot_id); /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 68b0065..b1aa6c0 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -41,48 +41,52 @@ namespace buzzuav_closures{ /****************************************/ /****************************************/ - int buzzros_print(buzzvm_t vm) { - int i; - char buffer [100] = ""; - sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid()); - for(uint32_t i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { - buzzvm_lload(vm, i); - buzzobj_t o = buzzvm_stack_at(vm, 1); - buzzvm_pop(vm); - switch(o->o.type) { - case BUZZTYPE_NIL: - sprintf(buffer,"%s BUZZ - [nil]", buffer); - break; - case BUZZTYPE_INT: - sprintf(buffer,"%s %d", buffer, o->i.value); - //fprintf(stdout, "%d", o->i.value); - break; - case BUZZTYPE_FLOAT: - sprintf(buffer,"%s %f", buffer, o->f.value); - break; - case BUZZTYPE_TABLE: - sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value))); - break; - case BUZZTYPE_CLOSURE: - if(o->c.value.isnative) - sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref); - else - sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref); - break; - case BUZZTYPE_STRING: - sprintf(buffer,"%s %s", buffer, o->s.value.str); - break; - case BUZZTYPE_USERDATA: - sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value); - break; - default: - break; - } - } - ROS_INFO(buffer); - //fprintf(stdout, "\n"); - return buzzvm_ret0(vm); - } +int buzzros_print(buzzvm_t vm) +{ + std::ostringstream buffer (std::ostringstream::ate); + buffer << buzz_utility::get_robotid(); + for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index) + { + buzzvm_lload(vm, index); + buzzobj_t o = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + switch (o->o.type) + { + case BUZZTYPE_NIL: + buffer << " BUZZ - [nil]"; + break; + case BUZZTYPE_INT: + buffer << " " << o->i.value; + break; + case BUZZTYPE_FLOAT: + buffer << " " << o->f.value; + break; + case BUZZTYPE_TABLE: + buffer << " [table with " << buzzdict_size(o->t.value) << " elems]"; + break; + case BUZZTYPE_CLOSURE: + if (o->c.value.isnative) + { + buffer << " [n-closure @" << o->c.value.ref << "]"; + } + else + { + buffer << " [c-closure @" << o->c.value.ref << "]"; + } + break; + case BUZZTYPE_STRING: + buffer << " " << o->s.value.str; + break; + case BUZZTYPE_USERDATA: + buffer << " [userdata @" << o->u.value << "]"; + break; + default: + break; + } + } + ROS_INFO("%s",buffer.str().c_str()); + return buzzvm_ret0(vm); +} void setWPlist(string path){ WPlistname = path + "include/graphs/waypointlist.csv"; @@ -171,14 +175,6 @@ namespace buzzuav_closures{ } - int buzz_floor(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 1); - buzzvm_lload(vm, 1); - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - float fval = buzzvm_stack_at(vm, 1)->f.value; - buzzvm_pushi(vm, floor(fval)); - return buzzvm_ret1(vm); - } /*----------------------------------------/ / Buzz closure to move following a 2D vector /----------------------------------------*/ From 89b263e4dffc54639d858876c1645fe2d4f9512a Mon Sep 17 00:00:00 2001 From: dave Date: Wed, 22 Nov 2017 21:59:57 -0500 Subject: [PATCH 38/39] minor fixes for 24/11 demo --- buzz_scripts/graphformGPS.bzz | 110 ++++++++++++----------- buzz_scripts/include/graphs/shapes_O.bzz | 2 +- buzz_scripts/include/graphs/shapes_P.bzz | 12 +-- buzz_scripts/include/graphs/shapes_Y.bzz | 2 +- buzz_scripts/include/uavstates.bzz | 4 + buzz_scripts/include/vec2.bzz | 2 +- buzz_scripts/startswarmsimER.launch | 35 ++++++++ misc/cmdlinectr.sh | 4 + src/roscontroller.cpp | 4 +- 9 files changed, 114 insertions(+), 61 deletions(-) create mode 100644 buzz_scripts/startswarmsimER.launch diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index 998000e..138b320 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -18,6 +18,7 @@ ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER ROOT_ID = 2 +old_state = -1 # max velocity in cm/step ROBOT_MAXVEL = 150.0 @@ -46,21 +47,16 @@ m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Respo #The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} -#Current robot state -# m_eState="STATE_FREE" # replace with default UAVSTATE - #navigation vector m_navigation={.x=0,.y=0} -#Debug message to be displayed in qt-opengl -#m_ossDebugMsg - -#Debug vector to draw -#CVector2 m_cDebugVector - #Current label being requested or chosen (-1 when none) m_nLabel=-1 m_messageID={} +repeat_assign=0 +assign_label=-1 +assign_id=-1 +m_gotjoinedparent = 0 #neighbor distance to lock the current pattern lock_neighbor_id={} lock_neighbor_dis={} @@ -89,9 +85,6 @@ m_unJoiningLostPeriod=0 #Tolerance distance to a target location m_fTargetDistanceTolerance=0 -#step cunt -step_cunt=0 - # virtual stigmergy for the LOCK barrier. m_lockstig = 1 @@ -119,8 +112,9 @@ function count(table,value){ } return number } - +# #map from int to state +# function i2s(value){ if(value==1){ return "STATE_FREE" @@ -138,8 +132,9 @@ function i2s(value){ return "STATE_LOCK" } } - +# #map from state to int +# function s2i(value){ if(value=="STATE_FREE"){ return 1 @@ -157,7 +152,9 @@ function s2i(value){ return 5 } } +# #map form int to response +# function i2r(value){ if(value==1){ return "REQ_NONE" @@ -166,7 +163,9 @@ function i2r(value){ return "REQ_GRANTED" } } +# #map from response to int +# function r2i(value){ if(value=="REQ_NONE"){ return 1 @@ -175,8 +174,6 @@ function r2i(value){ return 2 } } - - # #return the index of value # @@ -190,7 +187,6 @@ function find(table,value){ } return ind } - # #pack message into 1 number # @@ -255,8 +251,9 @@ log(id,"I pass value=",recv_value) return_table.Bearing=b return return_table } - +# #get the target distance to neighbr nei_id +# function target4label(nei_id){ var return_val="miss" var i=0 @@ -268,7 +265,9 @@ function target4label(nei_id){ } return return_val } +# #calculate LJ vector for neibhor stored in i +# function LJ_vec(i){ var dis=m_MessageRange[i] var ljbearing=m_MessageBearing[i] @@ -282,7 +281,9 @@ function LJ_vec(i){ #log("x=",cDir.x,"y=",cDir.y) return cDir } +# #calculate the motion vector +# function motion_vector(){ var i=0 var m_vector={.x=0.0,.y=0.0} @@ -296,7 +297,9 @@ function motion_vector(){ #log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y) return m_vector } - +# +# starts the neighbors listener +# function start_listen(){ neighbors.listen("m", function(vid,value,rid){ @@ -314,13 +317,15 @@ neighbors.listen("m", m_MessageRange[m_neighbourCount]=m_receivedMessage.Range m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing m_messageID[m_neighbourCount]=rid -log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount]) + +#log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount]) m_neighbourCount=m_neighbourCount+1 }) } # #Function used to get the station info of the sender of the message +# function Get_DisAndAzi(id){ neighbors.foreach( function(rid, data) { @@ -330,7 +335,6 @@ function Get_DisAndAzi(id){ } }) } - # #Update node info according to neighbour robots # @@ -361,7 +365,6 @@ function UpdateNodeInfo(){ i=i+1 } } - # #Transistion to state free # @@ -370,7 +373,6 @@ function TransitionToFree(){ m_unWaitCount=m_unLabelSearchWaitTime m_selfMessage.State=s2i(UAVSTATE) } - # #Transistion to state asking # @@ -384,7 +386,6 @@ function TransitionToAsking(un_label){ m_unWaitCount=m_unResponseTimeThreshold } - # #Transistion to state joining # @@ -394,7 +395,6 @@ function TransitionToJoining(){ m_selfMessage.Label=m_nLabel m_unWaitCount=m_unJoiningLostPeriod } - # #Transistion to state joined # @@ -413,7 +413,6 @@ function TransitionToJoined(){ m_navigation.y=0.0 uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } - # #Transistion to state Lock, lock the current formation # @@ -434,8 +433,12 @@ function TransitionToLock(){ m_navigation.x=0.0 m_navigation.y=0.0 uav_moveto(m_navigation.x, m_navigation.y, 0.0) -} + # prepare to restart a new shape + old_state = rc_State + #stop listening + neighbors.ignore("m") +} # # Do free # @@ -482,7 +485,6 @@ function DoFree() { m_selfMessage.State=s2i(UAVSTATE) } - # #Do asking # @@ -544,11 +546,9 @@ function DoAsking(){ m_navigation.y=0.0 uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) } - # #Do joining # -m_gotjoinedparent = 0 function DoJoining(){ if(m_gotjoinedparent!=1) @@ -587,13 +587,9 @@ function set_rc_goto() { m_gotjoinedparent = 1 } } - # #Do joined # -repeat_assign=0 -assign_label=-1 -assign_id=-1 function DoJoined(){ m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.Label=m_nLabel @@ -685,7 +681,6 @@ function DoJoined(){ #barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1) barrier_wait(6, TransitionToLock, DoJoined, -1) } - # #Do Lock # @@ -705,18 +700,18 @@ function DoLock(){ #move uav_moveto(m_navigation.x, m_navigation.y, 0.0) - # prepare to restart a new shape - statef=action } - +# +# Executed after takeoff +# function action(){ + statef=action UAVSTATE="STATE_FREE" # reset the graph Reset() } - # # Executed at init # @@ -737,13 +732,12 @@ function init() { #v_tag = stigmergy.create(m_lockstig) #uav_initstig() # go to diff. height since no collision avoidance implemented yet - #TARGET_ALTITUDE = 3.0 + id * 1.25 + TARGET_ALTITUDE = 20.0 + id * 2.5 statef=turnedoff UAVSTATE = "TURNEDOFF" } - # -# Executed every step +# Executed every step (main loop) # function step() { # listen to potential RC @@ -762,15 +756,15 @@ function step() { # if(UAVSTATE=="STATE_FREE") statef=DoFree - else if(UAVSTATE=="STATE_ESCAPE") - statef=DoEscape else if(UAVSTATE=="STATE_ASKING") statef=DoAsking else if(UAVSTATE=="STATE_JOINING") statef=DoJoining else if(UAVSTATE=="STATE_JOINED") statef=DoJoined - else if(UAVSTATE=="STATE_LOCK") + else if(UAVSTATE=="STATE_LOCK" and old_state!=rc_State) + statef=action + else if(UAVSTATE=="STATE_LOCK" and old_state==rc_State) statef=DoLock # high level UAV state machine @@ -794,25 +788,40 @@ function step() { m_MessageBearing={}#store received neighbour message m_neighbourCount=0 - - #step cunt+1 - step_cunt=step_cunt+1 } - # # Executed when reset # function Reset(){ + m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} + m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} + m_navigation={.x=0,.y=0} + m_nLabel=-1 + m_messageID={} + lock_neighbor_id={} + lock_neighbor_dis={} + m_unRequestId=0 + m_bias=0 + m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + m_unWaitCount=0 + repeat_assign=0 + assign_label=-1 + assign_id=-1 + m_gotjoinedparent = 0 + if(rc_State==0){ + log("Loading P graph") Read_GraphP() } else if(rc_State==1) { + log("Loading O graph") Read_GraphO() } else if(rc_State==2) { + log("Loading L graph") Read_GraphL() } else if(rc_State==3) { + log("Loading Y graph") Read_GraphY() } - m_nLabel=-1 #start listening start_listen() @@ -829,7 +838,6 @@ function Reset(){ TransitionToFree() } } - # # Executed upon destroy # diff --git a/buzz_scripts/include/graphs/shapes_O.bzz b/buzz_scripts/include/graphs/shapes_O.bzz index 830bc60..068e5a2 100644 --- a/buzz_scripts/include/graphs/shapes_O.bzz +++ b/buzz_scripts/include/graphs/shapes_O.bzz @@ -1,6 +1,6 @@ #Table of the nodes in the graph m_vecNodes={} -m_vecNodes_fixed={} + function Read_GraphO(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point diff --git a/buzz_scripts/include/graphs/shapes_P.bzz b/buzz_scripts/include/graphs/shapes_P.bzz index 2074531..14e183b 100644 --- a/buzz_scripts/include/graphs/shapes_P.bzz +++ b/buzz_scripts/include/graphs/shapes_P.bzz @@ -1,6 +1,6 @@ #Table of the nodes in the graph m_vecNodes={} -m_vecNodes_fixed={} + function Read_GraphP(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point @@ -14,7 +14,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of Lab m_vecNodes[1] = { .Label = 1, .Pred = 0, - .distance = 2000, + .distance = 1000, .bearing = 0.0, .height = 5000, .State="UNASSIGNED", @@ -23,7 +23,7 @@ m_vecNodes[1] = { m_vecNodes[2] = { .Label = 2, .Pred = 0, - .distance = 2000, + .distance = 1000, .bearing = 1.57, .height = 7000, .State="UNASSIGNED", @@ -32,7 +32,7 @@ m_vecNodes[2] = { m_vecNodes[3] = { .Label = 3, .Pred = 0, - .distance = 2000, + .distance = 1000, .bearing = 4.71, .height = 9000, .State="UNASSIGNED", @@ -41,7 +41,7 @@ m_vecNodes[3] = { m_vecNodes[4] = { .Label = 4, .Pred = 1, - .distance = 1414, + .distance = 707, .bearing = 0.79, .height = 11000, .State="UNASSIGNED", @@ -50,7 +50,7 @@ m_vecNodes[4] = { m_vecNodes[5] = { .Label = 5, .Pred = 2, - .distance = 2000, + .distance = 1000, .bearing = 0.0, .height = 14000, .State="UNASSIGNED", diff --git a/buzz_scripts/include/graphs/shapes_Y.bzz b/buzz_scripts/include/graphs/shapes_Y.bzz index 03599cf..171c693 100644 --- a/buzz_scripts/include/graphs/shapes_Y.bzz +++ b/buzz_scripts/include/graphs/shapes_Y.bzz @@ -1,6 +1,6 @@ #Table of the nodes in the graph m_vecNodes={} -m_vecNodes_fixed={} + function Read_GraphY(){ m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing .Label = 0, # Label of the point diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index d5bc263..bb2d617 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -180,15 +180,19 @@ function uav_rccmd() { } else if (flight.rc_cmd==900){ flight.rc_cmd=0 rc_State = 0 + neighbors.broadcast("cmd", 900) } else if (flight.rc_cmd==901){ flight.rc_cmd=0 rc_State = 1 + neighbors.broadcast("cmd", 901) } else if (flight.rc_cmd==902){ flight.rc_cmd=0 rc_State = 2 + neighbors.broadcast("cmd", 902) } else if (flight.rc_cmd==903){ flight.rc_cmd=0 rc_State = 3 + neighbors.broadcast("cmd", 903) } } diff --git a/buzz_scripts/include/vec2.bzz b/buzz_scripts/include/vec2.bzz index e2fb9b0..ebfce96 100644 --- a/buzz_scripts/include/vec2.bzz +++ b/buzz_scripts/include/vec2.bzz @@ -41,7 +41,7 @@ math.vec2.length = function(v) { # RETURN: The angle of the vector. # math.vec2.angle = function(v) { - return math.atan2(v.y, v.x) + return math.atan(v.y, v.x) } # diff --git a/buzz_scripts/startswarmsimER.launch b/buzz_scripts/startswarmsimER.launch new file mode 100644 index 0000000..355d88b --- /dev/null +++ b/buzz_scripts/startswarmsimER.launch @@ -0,0 +1,35 @@ +include "vec2.bzz" +include "update.bzz" +include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header. +include "uavstates.bzz" # require an 'action' function to be defined here. +include "vstigenv.bzz" + +function action() { + uav_storegoal(-1.0,-1.0,-1.0) + set_goto(picture) +} + +# Executed once at init time. +function init() { + statef=turnedoff + UAVSTATE = "TURNEDOFF" + uav_initstig() + TARGET_ALTITUDE = 15.0 +} + +# Executed at each time step. +function step() { + uav_rccmd() + + statef() + + log("Current state: ", UAVSTATE) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} \ No newline at end of file diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 2de3326..0c95a5f 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -34,3 +34,7 @@ function updaterobot { rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch # rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch } +function uavstate { + let "a = $1 + 900" + rosservice call robot0/buzzcmd 0 $a 0 0 0 0 0 0 0 0 +} diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 022e624..b03ac07 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1166,7 +1166,9 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, res.success = true; break; default: - res.success = false; + buzzuav_closures::rc_call(req.command); + ROS_INFO("----> Received unregistered command: ", req.command); + res.success = true; break; } return true; From 6018b681d71a4e9d5c8bd6d58c1f19ee2d92a6bf Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 27 Nov 2017 22:55:32 -0500 Subject: [PATCH 39/39] more fixes to rrt, added Vivek fix to MSG size and fix crashes due to UAVSTATE variable --- buzz_scripts/empty.bzz | 17 ++++++++ buzz_scripts/graphformGPS.bzz | 15 +++---- buzz_scripts/include/barrier.bzz | 4 +- buzz_scripts/include/rrtstar.bzz | 63 ++++++++++++++++++------------ buzz_scripts/include/uavstates.bzz | 44 ++++++++++++++++----- src/buzz_utility.cpp | 26 ++++++++---- 6 files changed, 117 insertions(+), 52 deletions(-) create mode 100644 buzz_scripts/empty.bzz diff --git a/buzz_scripts/empty.bzz b/buzz_scripts/empty.bzz new file mode 100644 index 0000000..3bb5b12 --- /dev/null +++ b/buzz_scripts/empty.bzz @@ -0,0 +1,17 @@ +include "update.bzz" + +# Executed once at init time. +function init() { +} + +# Executed at each time step. +function step() { +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index 138b320..d4e5aae 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/buzz_scripts/graphformGPS.bzz @@ -237,7 +237,7 @@ function unpackmessage(recv_value){ #unpack guide message # function unpack_guide_msg(recv_value){ -log(id,"I pass value=",recv_value) +#log(id,"I pass value=",recv_value) var qian=(recv_value-recv_value%1000)/1000 recv_value=recv_value-qian*1000 var bai=(recv_value-recv_value%100)/100 @@ -497,10 +497,9 @@ function DoAsking(){ #the request Label be the same as requesed #get a respond if(m_MessageState[i]=="STATE_JOINED"){ - log("received label = ",m_MessageReqLabel[i]) + #log("received label = ",m_MessageReqLabel[i]) if(m_MessageReqLabel[i]==m_nLabel) if(m_MessageResponse[i]!="REQ_NONE"){ - log("get response") psResponse=i }} if(m_MessageState[i]=="STATE_JOINING" and m_MessageLabel[i]==m_nLabel){ @@ -604,13 +603,12 @@ function DoJoined(){ while(iBS: ", barrier.size(), " (", BARRIER_VSTIG, ")") + #log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")") if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) { barrier.put("d", 1) timeW = 0 diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index f6c39ef..b9e84a4 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -5,23 +5,25 @@ ##### include "mapmatrix.bzz" -RRT_TIMEOUT = 200 +RRT_TIMEOUT = 500 map = nil cur_cell = {} nei_cell = {} -function UAVpathPlanner(m_navigation) { +function UAVinit_map(m_navigation) { # create a map big enough for the goal - init_map(2*math.round(math.vec2.length(m_navigation))+2) + init_map(2*math.round(math.vec2.length(m_navigation))+10) # center the robot on the grid - cur_cell = math.vec2.new(math.round(len/2.0),math.round(len/2.0)) + cur_cell = math.vec2.new(math.round(map.nb_col/2.0),math.round(map.nb_row/2.0)) +} + +function UAVpathPlanner(m_navigation, m_pos) { + # place the robot on the grid + update_curcell(m_pos,0) # create the goal in the map grid mapgoal = math.vec2.add(m_navigation,cur_cell) - # add all neighbors as obstacles in the grid - neighbors.foreach(function(rid, data) { - add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) - }) - # TODO: add proximity sensor obstacles to the grid + mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y)) + # search for a path return RRTSTAR(mapgoal,math.vec2.new(5,5)) #map.nb_col/20.0,map.nb_row/20.0)) } @@ -32,7 +34,7 @@ function Kh4pathPlanner(m_goal, m_pos) { m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y)) # place the robot on the grid - update_curcell(m_pos) + update_curcell(m_pos,1) log("Starting from cell: ", cur_cell.x, " ", cur_cell.y) log("Going to cell: ", m_goal.x, " ", m_goal.y) @@ -42,9 +44,14 @@ function Kh4pathPlanner(m_goal, m_pos) { return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y)) } -function update_curcell(m_curpos) { - cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y)) +function update_curcell(m_curpos, kh4) { + if(kh4) { # for khepera playground + cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) + cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y)) + } else { # for uav map relative to home + cur_cell = math.vec2.add(cur_cell, m_curpos) + cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y)) + } if(cur_cell.x>map.nb_row) cur_cell.x=map.nb_row if(cur_cell.y>map.nb_col) @@ -55,9 +62,17 @@ function update_curcell(m_curpos) { cur_cell.y=1 } +function UAVgetneiobs (m_curpos) { + update_curcell(m_curpos,0) + # add all neighbors as obstacles in the grid + neighbors.foreach(function(rid, data) { + add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) + }) +} + function getneiobs (m_curpos) { foundobstacle = 0 - update_curcell(m_curpos) + update_curcell(m_curpos,1) old_nei = table_copy(nei_cell) #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) @@ -103,7 +118,7 @@ function getneiobs (m_curpos) { function getproxobs (m_curpos) { foundobstacle = 0 - update_curcell(m_curpos) + update_curcell(m_curpos,1) reduce(proximity, function(key, value, acc) { @@ -141,17 +156,17 @@ function getproxobs (m_curpos) { #} return foundobstacle -} +} -function check_path(m_path, goal_l, m_curpos) { +function check_path(m_path, goal_l, m_curpos, kh4) { pathisblocked = 0 nb=goal_l - update_curcell(m_curpos) - #m_curpos = math.vec2.sub(m_curpos,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - #Cvec = math.vec2.new(math.round(m_curpos.x*CM2KH4.x), math.round(m_curpos.y*CM2KH4.y)) + update_curcell(m_curpos,kh4) Cvec = cur_cell while(nb < m_path.nb_row and nb <= goal_l+1){ Nvec = getvec(m_path, nb) + if(kh4==0) + Nvec=vec_from_gps(Nvec.x,Nvec.y) if(doesItIntersect(Cvec, Nvec)){ log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") pathisblocked = 1 @@ -293,7 +308,7 @@ function RRTSTAR(GOAL,TOL) { } if(goalReached){ log("Goal found(",numberOfPoints,")!") - Path = getPath(Q,numberOfPoints) + Path = getPathGPS(Q,numberOfPoints) print_pos(Path) } else { log("FAILED TO FIND A PATH!!!") @@ -395,8 +410,8 @@ function getPathGPS(Q,nb){ while(nb!=1){ npt=npt+1 path.nb_row=npt - path.mat[npt*2]=rmat(Q,nb,1) - path.mat[npt*2+1]=rmat(Q,nb,2) + path.mat[(npt-1)*2]=rmat(Q,nb,1) + path.mat[(npt-1)*2+1]=rmat(Q,nb,2) nb = rmat(Q,nb,3); } @@ -434,6 +449,6 @@ function getPath(Q,nb){ pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.y npt = npt - 1 } - log("Double-check path: ", check_path(pathR, 1, cur_cell)) + #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1)) return pathR } diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index bb2d617..a789600 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -5,7 +5,7 @@ ######################################## include "rrtstar.bzz" -TARGET_ALTITUDE = 5.0 # m. +TARGET_ALTITUDE = 15.0 # m. UAVSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps GOTO_MAXVEL = 2 # m/steps @@ -14,6 +14,7 @@ GOTODIST_TOL = 0.5 # m. GOTOANG_TOL = 0.1 # rad. cur_goal_l = 0 rc_State = 0 +homegps = {.lat=0, .long=0} function uav_initswarm() { s = swarm.create(1) @@ -35,8 +36,7 @@ function takeoff() { statef=takeoff if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - #barrier_set(ROBOTS, action, land, -1) - barrier_set(6, action, land, -1) + barrier_set(ROBOTS, action, land, -1) barrier_ready() } else { log("Altitude: ", position.altitude) @@ -53,8 +53,7 @@ function land() { uav_land() if(flight.status != 2 and flight.status != 3) { - #barrier_set(ROBOTS,turnedoff,land, 21) - barrier_set(6,turnedoff,land, 21) + barrier_set(ROBOTS,turnedoff,land, 21) barrier_ready() } } @@ -62,7 +61,7 @@ function land() { function set_goto(transf) { UAVSTATE = "GOTOGPS" statef=function() { - gotoWP(transf); + gotoWP(transf) } if(rc_goto.id==id){ @@ -90,21 +89,48 @@ function picture() { ptime=ptime+1 } +# +# still requires to be tuned, replaning takes too much time.. +# DS 23/11/2017 function gotoWPRRT(transf) { rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude) - print(" has to move ", math.vec2.length(rc_goal)) + homegps.lat = position.latitude + homegps.long = position.longitude + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1) + print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y) if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) log("Sorry this is too far.") else { if(Path==nil){ - Path = UAVpathPlanner(rc_goal) + # create the map + if(map==nil) + UAVinit_map(rc_goal) + # add proximity sensor and neighbor obstacles to the map + while(Path==nil) { + #getproxobs(m_pos) + UAVgetneiobs (m_pos) + Path = UAVpathPlanner(rc_goal, m_pos) + } cur_goal_l = 1 } else if(cur_goal_l<=Path.nb_row) { cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude cur_pt=vec_from_gps(cur_gps.x,cur_gps.y) - print(" first to ", cur_pt.x,cur_pt.y) + print(" heading to ", cur_pt.x,cur_pt.y) if(math.vec2.length(cur_pt)>GOTODIST_TOL) { + m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1) + UAVgetneiobs(m_pos) + if(check_path(Path,cur_goal_l,m_pos,0)) { + uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) + Path = nil + rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + while(Path == nil) { + #getproxobs(m_pos) + UAVgetneiobs (m_pos) + Path = UAVpathPlanner(rc_goal, m_pos) + } + cur_goal_l = 1 + } cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt)) uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude) } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 06cd665..90d5dfa 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -17,7 +17,6 @@ namespace buzz_utility{ static char* BO_FNAME = 0; static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; - static uint32_t MSG_SIZE = 250; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size) static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets static uint8_t Robot_id = 0; static std::vector IN_MSG; @@ -229,7 +228,7 @@ void in_message_process(){ buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); /* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */ //ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t))); - if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MSG_SIZE) { + if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) { buzzmsg_payload_destroy(&m); break; } @@ -467,11 +466,9 @@ int create_stig_tables() { if(VM) buzzvm_destroy(&VM); Robot_id = robot_id; VM = buzzvm_new((int)robot_id); - ROS_INFO(" Robot ID -1: %i" , robot_id); /* Get rid of debug info */ if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); DBG_INFO = buzzdebug_new(); - ROS_INFO(" Robot ID -2: %i" , robot_id); /* Read bytecode and fill in data structure */ FILE* fd = fopen(bo_filename, "rb"); if(!fd) { @@ -490,7 +487,6 @@ int create_stig_tables() { return 0; } fclose(fd); - ROS_INFO(" Robot ID -3: %i" , robot_id); /* Read debug information */ if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { buzzvm_destroy(&VM); @@ -498,7 +494,6 @@ int create_stig_tables() { perror(bdbg_filename); return 0; } - ROS_INFO(" Robot ID -4: %i" , robot_id); /* Set byte code */ if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); @@ -506,7 +501,6 @@ int create_stig_tables() { ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename); return 0; } - ROS_INFO(" Robot ID -5: %i" , robot_id); /* Register hook functions */ if(buzz_register_hooks() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); @@ -514,7 +508,11 @@ int create_stig_tables() { ROS_ERROR("[%i] Error registering hooks", Robot_id); return 0; } - ROS_INFO(" Robot ID -6: %i" , robot_id); + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { @@ -578,6 +576,12 @@ int create_stig_tables() { ROS_ERROR("[%i] Error registering hooks (update)", Robot_id); return 0; } + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); @@ -634,6 +638,12 @@ int create_stig_tables() { ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id); return 0; } + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM);