diff --git a/.gitignore b/.gitignore index 7d62213..64f22eb 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,7 @@ src/test* -.cproject -.project +*.cproject +*.project +*.basm +*.bo +*.bdb +*.bdbg diff --git a/include/buzz_update.h b/include/buzz_update.h index c8bbaf0..9c51601 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -129,6 +129,8 @@ int get_update_status(); void set_read_update_status(); +int compile_bzz(); + void updates_set_robots(int robots); #endif diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 3667363..381b417 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -28,13 +28,18 @@ uint16_t* u64_cvt_u16(uint64_t u64); int buzz_listen(const char* type, int msg_size); -void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map); +void add_user(int id, double latitude, double longitude, float altitude); +void update_users(); +int make_table(buzzobj_t* t); +int buzzusers_add(int id, double latitude, double longitude, double altitude); +int buzzusers_reset(); + int compute_users_rb(); -int buzz_update_users_stigmergy(buzzobj_t tbl); +void in_msg_append(uint64_t* payload); -void in_msg_process(uint64_t* payload); +uint64_t* obt_out_msg(); -uint64_t* out_msg_process(); +void update_sensors(); int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index eff6835..e501285 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -9,6 +9,9 @@ #include "buzz_utility.h" //#include "roscontroller.h" + #define EARTH_RADIUS (double) 6371000.0 + #define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0))) + namespace buzzuav_closures{ typedef enum { COMMAND_NIL = 0, // Dummy command @@ -46,12 +49,13 @@ void rc_call(int rc_cmd); void set_battery(float voltage,float current,float remaining); /* sets current position */ void set_currentpos(double latitude, double longitude, double altitude); -void set_userspos(double latitude, double longitude, double altitude); /*retuns the current go to position */ double* getgoto(); /* updates flight status*/ void flight_status_update(uint8_t state); - +/* Update neighbors table */ +void neighbour_pos_callback(int id, float range, float bearing, float elevation); +void update_neighbors(buzzvm_t vm); /*Flight status*/ void set_obstacle_dist(float dist[]); @@ -83,8 +87,7 @@ int buzzuav_update_battery(buzzvm_t vm); * Updates current position in Buzz */ int buzzuav_update_currentpos(buzzvm_t vm); -buzzobj_t buzzuav_update_userspos(buzzvm_t vm); - +int buzzuav_adduserRB(buzzvm_t vm); /* * Updates flight status and rc command in Buzz, put it in a tabel to acess it * use flight.status for flight status diff --git a/include/roscontroller.h b/include/roscontroller.h index e1e00c8..6dc1859 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -52,7 +52,7 @@ public: void RosControllerRun(); private: - struct num_robot_count + struct num_robot_count { uint8_t history[10]; uint8_t index=0; @@ -61,17 +61,16 @@ private: }; typedef struct num_robot_count Num_robot_count ; - // WGS84 constants - double equatorial_radius = 6378137.0; - double flattening = 1.0/298.257223563; - double excentrity2 = 2*flattening - flattening*flattening; - // default reference position - double DEFAULT_REFERENCE_LATITUDE = 45.457817; - double DEFAULT_REFERENCE_LONGITUDE = -73.636075; - - double cur_pos[3]; - double home[3]; + struct gps + { + double longitude=0.0; + double latitude=0.0; + float altitude=0.0; + }; typedef struct gps GPS ; + + GPS target, home, cur_pos; double cur_rel_altitude; + uint64_t payload; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map; @@ -137,7 +136,7 @@ private: void Rosparameters_get(ros::NodeHandle& n_c_priv); /*compiles buzz script from the specified .bzz file*/ - void Compile_bzz(); + std::string Compile_bzz(std::string bzzfile_name); /*Flight controller service call*/ void flight_controller_service_call(); @@ -158,13 +157,17 @@ private: /*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/ void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); - /*Set the current position of the robot callback*/ + /*Set the current position of the robot callback*/ void set_cur_pos(double latitude, double longitude, double altitude); /*convert from spherical to cartesian coordinate system callback */ - void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]); - void cvt_ned_coordinates(double neighbours_pos_payload [], double out[], double pos[]); + void gps_rb(GPS nei_pos, double out[]); + void gps_ned_cur(float &ned_x, float &ned_y, GPS t); + void gps_ned_home(float &ned_x, float &ned_y); + void gps_convert_ned(float &ned_x, float &ned_y, + double gps_t_lon, double gps_t_lat, + double gps_r_lon, double gps_r_lat); /*battery status callback*/ void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); @@ -177,7 +180,7 @@ private: /*current position callback*/ void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); - void users_pos(const rosbuzz::neigh_pos msg); + void users_pos(const rosbuzz::neigh_pos msg); /*current relative altitude callback*/ void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch index 26ba3e7..054d18b 100644 --- a/launch/rosbuzz-solo.launch +++ b/launch/rosbuzz-solo.launch @@ -20,23 +20,37 @@ - + + + + + + + + + + + + - + - - - - - + + + + + + + diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch index 141f0a0..c09d2c5 100644 --- a/launch/rosbuzz-testing-sitl.launch +++ b/launch/rosbuzz-testing-sitl.launch @@ -27,13 +27,15 @@ - + - - - - - + + + + + + + - - - - - - - - - - - - - - + + + + + + + + + + + + diff --git a/script/include/string.bzz b/script/include/string.bzz new file mode 100644 index 0000000..4140a1b --- /dev/null +++ b/script/include/string.bzz @@ -0,0 +1,92 @@ +# +# Returns the string character at the given position. +# PARAM s: The string +# PARAM n: The position of the wanted character +# RETURN The character at the wanted position, or nil +# +string.charat = function(s, n) { + return string.sub(s, n, n+1) +} + +# +# Returns the index of the first occurrence of the given string m +# within another string s. If none is found, this function returns +# nil. +# PARAM s: The string +# PARAM m: The string to match +# RETURN: The position of the first match, or nil +# +string.indexoffirst = function(s, m) { + var ls = string.length(s) + var lm = string.length(m) + var i = 0 + while(i < ls-lm+1) { + if(string.sub(s, i, i+lm) == m) return i + i = i + 1 + } + return nil +} + +# +# Returns the index of the last occurrence of the given string m +# within another string s. If none is found, this function returns +# nil. +# PARAM s: The string +# PARAM m: The string to match +# RETURN: The position of the last match, or nil +# +string.indexoflast = function(s, m) { + var ls = string.length(s) + var lm = string.length(m) + var i = ls - lm + 1 + while(i >= 0) { + if(string.sub(s, i, i+lm) == m) return i + i = i - 1 + } + return nil +} + +# Splits a string s using the delimiters in d. The string list is +# returned in a table indexed by value (starting at 0). +# PARAM s: The string +# PARAM d: A string containing the delimiters +# RETURN: A table containing the tokens +string.split = function(s, d) { + var i1 = 0 # index to move along s (token start) + var i2 = 0 # index to move along s (token end) + var c = 0 # token count + var t = {} # token list + var ls = string.length(s) + var ld = string.length(d) + # Go through string s + while(i2 < ls) { + # Try every delimiter + var j = 0 # index to move along d + var f = nil # whether the delimiter was found or not + while(j < ld and (not f)) { + if(string.charat(s, i2) == string.charat(d, j)) { + # Delimiter found + f = 1 + # Is it worth adding a new token? + if(i2 > i1) { + t[c] = string.sub(s, i1, i2) + c = c + 1 + } + # Start new token + i1 = i2 + 1 + } + else { + # Next delimiter + j = j + 1 + } + } + # Next string character + i2 = i2 + 1 + } + # Is it worth adding a new token? + if(i2 > i1) { + t[c] = string.sub(s, i1, i2) + } + # Return token list + return t; +} diff --git a/script/include/vec2.bzz b/script/include/vec2.bzz new file mode 100644 index 0000000..e2fb9b0 --- /dev/null +++ b/script/include/vec2.bzz @@ -0,0 +1,107 @@ +# +# Create a new namespace for vector2 functions +# +math.vec2 = {} + +# +# Creates a new vector2. +# PARAM x: The x coordinate. +# PARAM y: The y coordinate. +# RETURN: A new vector2. +# +math.vec2.new = function(x, y) { + return { .x = x, .y = y } +} + +# +# Creates a new vector2 from polar coordinates. +# PARAM l: The length of the vector2. +# PARAM a: The angle of the vector2. +# RETURN: A new vector2. +# +math.vec2.newp = function(l, a) { + return { + .x = l * math.cos(a), + .y = l * math.sin(a) + } +} + +# +# Calculates the length of the given vector2. +# PARAM v: The vector2. +# RETURN: The length of the vector. +# +math.vec2.length = function(v) { + return math.sqrt(v.x * v.x + v.y * v.y) +} + +# +# Calculates the angle of the given vector2. +# PARAM v: The vector2. +# RETURN: The angle of the vector. +# +math.vec2.angle = function(v) { + return math.atan2(v.y, v.x) +} + +# +# Returns the normalized form of a vector2. +# PARAM v: The vector2. +# RETURN: The normalized form. +# +math.vec2.norm = function(v) { + var l = math.length(v) + return { + .x = v.x / l, + .y = v.y / l + } +} + +# +# Calculates v1 + v2. +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 + v2 +# +math.vec2.add = function(v1, v2) { + return { + .x = v1.x + v2.x, + .y = v1.y + v2.y + } +} + +# +# Calculates v1 - v2. +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 + v2 +# +math.vec2.sub = function(v1, v2) { + return { + .x = v1.x - v2.x, + .y = v1.y - v2.y + } +} + +# +# Scales a vector by a numeric constant. +# PARAM v: A vector2. +# PARAM s: A number (float or int). +# RETURN: s * v +# +math.vec2.scale = function(v, s) { + return { + .x = v.x * s, + .y = v.y * s + } +} + +# +# Calculates v1 . v2 (the dot product) +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 . v2 +# +math.vec2.dot = function(v1, v2) { + return v1.x * v2.x + v1.y * v2.y +} diff --git a/src/stand_by.bzz b/script/stand_by.bzz similarity index 94% rename from src/stand_by.bzz rename to script/stand_by.bzz index bf2ac78..d51e99e 100644 --- a/src/stand_by.bzz +++ b/script/stand_by.bzz @@ -9,6 +9,8 @@ if(r. data < l. data or (r. data == l. data )) return l else return r }) +s = swarm.create(1) +s.join() } function stand_by(){ diff --git a/src/test.bzz b/script/test.bzz similarity index 100% rename from src/test.bzz rename to script/test.bzz diff --git a/src/test1.bzz b/script/test1.bzz similarity index 99% rename from src/test1.bzz rename to script/test1.bzz index 3e9d1fb..7debdf4 100644 --- a/src/test1.bzz +++ b/script/test1.bzz @@ -1,4 +1,3 @@ - # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." include "/home/ubuntu/buzz/src/include/vec2.bzz" diff --git a/script/testalone.bdb b/script/testalone.bdb new file mode 100644 index 0000000..9695b14 Binary files /dev/null and b/script/testalone.bdb differ diff --git a/script/testalone.bo b/script/testalone.bo new file mode 100644 index 0000000..63d34da Binary files /dev/null and b/script/testalone.bo differ diff --git a/src/testalone.bzz b/script/testalone.bzz similarity index 91% rename from src/testalone.bzz rename to script/testalone.bzz index 086de53..070baba 100644 --- a/src/testalone.bzz +++ b/script/testalone.bzz @@ -1,7 +1,8 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" +#include "/home/ubuntu/buzz/src/include/vec2.bzz" +#include "vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater @@ -20,7 +21,7 @@ function init() { # Executed at each time step. function step() { - + log("Altitude: ", position.altitude) if(flight.rc_cmd==22) { flight.rc_cmd=0 uav_takeoff(TARGET_ALTITUDE) @@ -31,7 +32,6 @@ function step() { flight.rc_cmd=0 uav_goto() } - # test moveto cmd #if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) # uav_moveto(0.5, 0.5) diff --git a/script/testflockfev.bdb b/script/testflockfev.bdb new file mode 100644 index 0000000..76c0075 Binary files /dev/null and b/script/testflockfev.bdb differ diff --git a/script/testflockfev.bo b/script/testflockfev.bo new file mode 100644 index 0000000..44e24c1 Binary files /dev/null and b/script/testflockfev.bo differ diff --git a/src/testflockfev.bzz b/script/testflockfev.bzz similarity index 82% rename from src/testflockfev.bzz rename to script/testflockfev.bzz index 30e5b80..06b2e0e 100644 --- a/src/testflockfev.bzz +++ b/script/testflockfev.bzz @@ -1,5 +1,6 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." +#include "vec2.bzz" include "/home/ubuntu/buzz/src/include/vec2.bzz" #################################################################################################### # Updater related @@ -11,12 +12,12 @@ function updated_neigh(){ neighbors.broadcast(updated, update_no) } -TARGET_ALTITUDE = 3.0 +TARGET_ALTITUDE = 5.0 CURSTATE = "TURNEDOFF" # Lennard-Jones parameters -TARGET = 10.0 #0.000001001 -EPSILON = 18.0 #0.001 +TARGET = 12.0 #0.000001001 +EPSILON = 3.0 #0.001 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) { @@ -43,15 +44,18 @@ function hexagon() { math.vec2.scale(accum, 1.0 / neighbors.count()) # Move according to vector #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) -# uav_moveto(accum.x,accum.y) + uav_moveto(accum.x,accum.y) - if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS - timeW =0 - statef=land - } else { - timeW = timeW+1 - uav_moveto(0.0,0.0) - } +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else if(timeW>=WAIT_TIMEOUT/2) { +# timeW = timeW+1 +# uav_moveto(0.06,0.0) +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.06) +# } } ######################################## @@ -142,11 +146,32 @@ function land() { } } +function users_save(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + #log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } +} + +function table_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + # Executed once at init time. function init() { s = swarm.create(1) -# s.select(1) s.join() + + vt = stigmergy.create(5) + t = {} + vt.put("p",t) + statef=idle CURSTATE = "IDLE" } @@ -197,6 +222,14 @@ neighbors.listen("cmd", statef() log("Current state: ", CURSTATE) log("Swarm size: ",ROBOTS) + + # Check local users and push to v.stig + if(size(users.dataG)>0) + vt.put("p", users.dataG) + + # Save locally the users and print RG + users_save(vt.get("p")) + table_print(users.dataL) } # Executed once when the robot (or the simulator) is reset. diff --git a/script/testflockfev.bzz.save b/script/testflockfev.bzz.save new file mode 100644 index 0000000..4de66e5 --- /dev/null +++ b/script/testflockfev.bzz.save @@ -0,0 +1,216 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 12.0 #0.000001001 +EPSILON = 6.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else if(timeW>=WAIT_TIMEOUT/2) { +# timeW = timeW+1 +# uav_moveto(0.06,0.0) +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.06) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "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,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) + s.join() + statef=idle + CURSTATE = "IDLE" + vt = stigmergy.cerate(5) + t = {} + vt.put("p",t) +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) + if(users.dataG) + vt.put("p",users.dataG) + table_print(users.dataL) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/src/testflockfev_barrier.bzz b/script/testflockfev_barrier.bzz similarity index 100% rename from src/testflockfev_barrier.bzz rename to script/testflockfev_barrier.bzz diff --git a/src/testflocksim.bzz b/script/testflocksim.bzz similarity index 84% rename from src/testflocksim.bzz rename to script/testflocksim.bzz index 107edbf..48f47dd 100644 --- a/src/testflocksim.bzz +++ b/script/testflocksim.bzz @@ -1,6 +1,6 @@ # We need this for 2D vectors # Make sure you pass the correct include path to "bzzc -I ..." -include "/home/ubuntu/buzz/src/include/vec2.bzz" +include "vec2.bzz" #################################################################################################### # Updater related # This should be here for the updater to work, changing position of code will crash the updater @@ -8,7 +8,7 @@ include "/home/ubuntu/buzz/src/include/vec2.bzz" updated="update_ack" update_no=0 function updated_neigh(){ -neighbors.broadcast(updated, update_no) + neighbors.broadcast(updated, update_no) } TARGET_ALTITUDE = 10.0 @@ -141,11 +141,37 @@ function land() { } } +function users_save(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + #log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } +} + +# printing the contents of a table: a custom function +function table_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + +######################################## +# +# MAIN FUNCTIONS +# +######################################## + # Executed once at init time. function init() { s = swarm.create(1) -# s.select(1) s.join() + vt = stigmergy.create(5) + t = {} + vt.put("p",t) statef=idle CURSTATE = "IDLE" } @@ -196,12 +222,17 @@ neighbors.listen("cmd", statef() log("Current state: ", CURSTATE) log("Swarm size: ",ROBOTS) - log("User position: ", users.range) # Read a value from the structure - #x = usersvstig.get(46) + log(users) + #users_print(users.dataG) + if(size(users.dataG)>0) + vt.put("p", users.dataG) + # Get the number of keys in the structure - #log("The vstig has ", usersvstig.size(), " elements") + log("The vstig has ", vt.size(), " elements") + users_save(vt.get("p")) + table_print(users.dataL) } # Executed once when the robot (or the simulator) is reset. diff --git a/script/testsolo.basm b/script/testsolo.basm new file mode 100644 index 0000000..869b93d --- /dev/null +++ b/script/testsolo.basm @@ -0,0 +1,1156 @@ +!95 +'math +'vec2 +'new +'x +'y +'newp +'cos +'sin +'length +'sqrt +'angle +'atan2 +'norm +'add +'sub +'scale +'dot +'updated +'update_ack +'update_no +'updated_neigh +'neighbors +'broadcast +'TARGET_ALTITUDE +'CURSTATE +'TURNEDOFF +'TARGET +'EPSILON +'lj_magnitude +'lj_vector +'distance +'azimuth +'lj_sum +'hexagon +'statef +'HEXAGON +'map +'reduce +'count +'log +'Time: +'timeW +'WAIT_TIMEOUT +'land +'uav_moveto +'BARRIER_VSTIG +'barrier_set +'barrier_wait +'barrier +'stigmergy +'create +'barrier_ready +'put +'id +'get +'BARRIERWAIT +'size +'idle +'IDLE +'takeoff +'TAKEOFF +'TakeOff: +'flight +'status +'Relative position: +'position +'altitude +'ROBOTS +'Altitude: +'cmd +'uav_takeoff +'LAND +'Land: +'uav_land +'init +'s +'swarm +'join +'step +'rc_cmd +'cmd 22 +'cmd 21 +'To land +'uav_goto +'uav_arm +'uav_disarm +'listen +'print +'Got ( +', +') from robot # +'Current state: +'Swarm size: +'reset +'destroy + + pushs 20 + pushcn @__label_10 + gstore + pushs 28 + pushcn @__label_11 + gstore + pushs 29 + pushcn @__label_12 + gstore + pushs 32 + pushcn @__label_13 + gstore + pushs 33 + pushcn @__label_14 + gstore + pushs 46 + pushcn @__label_21 + gstore + pushs 51 + pushcn @__label_23 + gstore + pushs 47 + pushcn @__label_24 + gstore + pushs 57 + pushcn @__label_29 + gstore + pushs 59 + pushcn @__label_30 + gstore + pushs 43 + pushcn @__label_33 + gstore + pushs 74 + pushcn @__label_36 + gstore + pushs 78 + pushcn @__label_37 + gstore + pushs 93 + pushcn @__label_57 + gstore + pushs 94 + pushcn @__label_58 + gstore + nop + +@__label_0 + pushs 0 |4,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |4,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |4,5,/home/ubuntu/buzz/src/include/vec2.bzz + pusht |4,13,/home/ubuntu/buzz/src/include/vec2.bzz + tput |4,14,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |12,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |12,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |12,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |12,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 2 |12,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_1 |12,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |14,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |22,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |22,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |22,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |22,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 5 |22,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_2 |22,17,/home/ubuntu/buzz/src/include/vec2.bzz + tput |27,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |34,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |34,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |34,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |34,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 8 |34,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_3 |34,19,/home/ubuntu/buzz/src/include/vec2.bzz + tput |36,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |43,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |43,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |43,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |43,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 10 |43,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_4 |43,18,/home/ubuntu/buzz/src/include/vec2.bzz + tput |45,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |52,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |52,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |52,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |52,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 12 |52,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_5 |52,17,/home/ubuntu/buzz/src/include/vec2.bzz + tput |58,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |66,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |66,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |66,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |66,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 13 |66,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_6 |66,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |71,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |79,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |79,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |79,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |79,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 14 |79,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_7 |79,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |84,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |92,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |92,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |92,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |92,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 15 |92,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_8 |92,18,/home/ubuntu/buzz/src/include/vec2.bzz + tput |97,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |105,4,/home/ubuntu/buzz/src/include/vec2.bzz + gload |105,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 1 |105,5,/home/ubuntu/buzz/src/include/vec2.bzz + tget |105,9,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 16 |105,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushl @__label_9 |105,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |107,1,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 17 |8,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 18 |8,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |8,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 19 |9,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |9,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |9,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |14,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 3.0 |14,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |14,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |15,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 25 |15,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |15,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 26 |18,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 10.0 |18,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |19,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 27 |19,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 18.0 |19,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |20,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 45 |71,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |71,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |71,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 42 |94,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 300 |94,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |94,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |95,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |95,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |95,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__exitpoint + done + +@__label_1 + pusht |13,11,/home/ubuntu/buzz/src/include/vec2.bzz + dup |13,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |13,12,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |13,17,/home/ubuntu/buzz/src/include/vec2.bzz + tput |13,17,/home/ubuntu/buzz/src/include/vec2.bzz + dup |13,17,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |13,20,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |13,26,/home/ubuntu/buzz/src/include/vec2.bzz + tput |13,26,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |13,27,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |14,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_2 + pusht |24,4,/home/ubuntu/buzz/src/include/vec2.bzz + dup |24,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |24,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |24,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |24,17,/home/ubuntu/buzz/src/include/vec2.bzz + gload |24,17,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 6 |24,18,/home/ubuntu/buzz/src/include/vec2.bzz + tget |24,21,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |24,23,/home/ubuntu/buzz/src/include/vec2.bzz + pushi 1 |24,24,/home/ubuntu/buzz/src/include/vec2.bzz + callc |24,24,/home/ubuntu/buzz/src/include/vec2.bzz + mul |24,24,/home/ubuntu/buzz/src/include/vec2.bzz + tput |24,24,/home/ubuntu/buzz/src/include/vec2.bzz + dup |24,24,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |25,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |25,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 0 |25,17,/home/ubuntu/buzz/src/include/vec2.bzz + gload |25,17,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 7 |25,18,/home/ubuntu/buzz/src/include/vec2.bzz + tget |25,21,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |25,23,/home/ubuntu/buzz/src/include/vec2.bzz + pushi 1 |25,24,/home/ubuntu/buzz/src/include/vec2.bzz + callc |25,24,/home/ubuntu/buzz/src/include/vec2.bzz + mul |25,24,/home/ubuntu/buzz/src/include/vec2.bzz + tput |25,24,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |26,3,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |27,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_3 + pushs 0 |35,13,/home/ubuntu/buzz/src/include/vec2.bzz + gload |35,13,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 9 |35,14,/home/ubuntu/buzz/src/include/vec2.bzz + tget |35,18,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |35,20,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |35,21,/home/ubuntu/buzz/src/include/vec2.bzz + tget |35,23,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |35,26,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |35,27,/home/ubuntu/buzz/src/include/vec2.bzz + tget |35,29,/home/ubuntu/buzz/src/include/vec2.bzz + mul |35,29,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |35,32,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |35,33,/home/ubuntu/buzz/src/include/vec2.bzz + tget |35,35,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |35,38,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |35,39,/home/ubuntu/buzz/src/include/vec2.bzz + tget |35,40,/home/ubuntu/buzz/src/include/vec2.bzz + mul |35,40,/home/ubuntu/buzz/src/include/vec2.bzz + add |35,40,/home/ubuntu/buzz/src/include/vec2.bzz + pushi 1 |35,41,/home/ubuntu/buzz/src/include/vec2.bzz + callc |35,41,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |35,41,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |36,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_4 + pushs 0 |44,13,/home/ubuntu/buzz/src/include/vec2.bzz + gload |44,13,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 11 |44,14,/home/ubuntu/buzz/src/include/vec2.bzz + tget |44,19,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |44,21,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |44,22,/home/ubuntu/buzz/src/include/vec2.bzz + tget |44,23,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |44,26,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |44,27,/home/ubuntu/buzz/src/include/vec2.bzz + tget |44,28,/home/ubuntu/buzz/src/include/vec2.bzz + pushi 2 |44,29,/home/ubuntu/buzz/src/include/vec2.bzz + callc |44,29,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |44,29,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |45,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_5 + pushs 0 |53,14,/home/ubuntu/buzz/src/include/vec2.bzz + gload |53,14,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 8 |53,15,/home/ubuntu/buzz/src/include/vec2.bzz + tget |53,21,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |53,23,/home/ubuntu/buzz/src/include/vec2.bzz + pushi 1 |53,24,/home/ubuntu/buzz/src/include/vec2.bzz + callc |53,24,/home/ubuntu/buzz/src/include/vec2.bzz + lstore 2 |53,24,/home/ubuntu/buzz/src/include/vec2.bzz + pusht |55,4,/home/ubuntu/buzz/src/include/vec2.bzz + dup |55,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |55,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |55,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |55,11,/home/ubuntu/buzz/src/include/vec2.bzz + tget |55,13,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |55,16,/home/ubuntu/buzz/src/include/vec2.bzz + div |55,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |55,16,/home/ubuntu/buzz/src/include/vec2.bzz + dup |55,16,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |56,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |56,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |56,11,/home/ubuntu/buzz/src/include/vec2.bzz + tget |56,13,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |56,16,/home/ubuntu/buzz/src/include/vec2.bzz + div |56,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |56,16,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |57,3,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |58,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_6 + pusht |68,4,/home/ubuntu/buzz/src/include/vec2.bzz + dup |68,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |68,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |68,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |68,12,/home/ubuntu/buzz/src/include/vec2.bzz + tget |68,14,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |68,18,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |68,19,/home/ubuntu/buzz/src/include/vec2.bzz + tget |68,20,/home/ubuntu/buzz/src/include/vec2.bzz + add |68,20,/home/ubuntu/buzz/src/include/vec2.bzz + tput |68,20,/home/ubuntu/buzz/src/include/vec2.bzz + dup |68,20,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |69,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |69,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |69,12,/home/ubuntu/buzz/src/include/vec2.bzz + tget |69,14,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |69,18,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |69,19,/home/ubuntu/buzz/src/include/vec2.bzz + tget |69,20,/home/ubuntu/buzz/src/include/vec2.bzz + add |69,20,/home/ubuntu/buzz/src/include/vec2.bzz + tput |69,20,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |70,3,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |71,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_7 + pusht |81,4,/home/ubuntu/buzz/src/include/vec2.bzz + dup |81,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |81,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |81,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |81,12,/home/ubuntu/buzz/src/include/vec2.bzz + tget |81,14,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |81,18,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |81,19,/home/ubuntu/buzz/src/include/vec2.bzz + tget |81,20,/home/ubuntu/buzz/src/include/vec2.bzz + sub |81,20,/home/ubuntu/buzz/src/include/vec2.bzz + tput |81,20,/home/ubuntu/buzz/src/include/vec2.bzz + dup |81,20,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |82,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |82,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |82,12,/home/ubuntu/buzz/src/include/vec2.bzz + tget |82,14,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |82,18,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |82,19,/home/ubuntu/buzz/src/include/vec2.bzz + tget |82,20,/home/ubuntu/buzz/src/include/vec2.bzz + sub |82,20,/home/ubuntu/buzz/src/include/vec2.bzz + tput |82,20,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |83,3,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |84,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_8 + pusht |94,4,/home/ubuntu/buzz/src/include/vec2.bzz + dup |94,4,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |94,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |94,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |94,11,/home/ubuntu/buzz/src/include/vec2.bzz + tget |94,13,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |94,16,/home/ubuntu/buzz/src/include/vec2.bzz + mul |94,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |94,16,/home/ubuntu/buzz/src/include/vec2.bzz + dup |94,16,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |95,5,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |95,10,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |95,11,/home/ubuntu/buzz/src/include/vec2.bzz + tget |95,13,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |95,16,/home/ubuntu/buzz/src/include/vec2.bzz + mul |95,16,/home/ubuntu/buzz/src/include/vec2.bzz + tput |95,16,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |96,3,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |97,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_9 + lload 1 |106,11,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |106,12,/home/ubuntu/buzz/src/include/vec2.bzz + tget |106,14,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |106,18,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 3 |106,19,/home/ubuntu/buzz/src/include/vec2.bzz + tget |106,21,/home/ubuntu/buzz/src/include/vec2.bzz + mul |106,21,/home/ubuntu/buzz/src/include/vec2.bzz + lload 1 |106,25,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |106,26,/home/ubuntu/buzz/src/include/vec2.bzz + tget |106,28,/home/ubuntu/buzz/src/include/vec2.bzz + lload 2 |106,32,/home/ubuntu/buzz/src/include/vec2.bzz + pushs 4 |106,33,/home/ubuntu/buzz/src/include/vec2.bzz + tget |106,34,/home/ubuntu/buzz/src/include/vec2.bzz + mul |106,34,/home/ubuntu/buzz/src/include/vec2.bzz + add |106,34,/home/ubuntu/buzz/src/include/vec2.bzz + ret1 |106,34,/home/ubuntu/buzz/src/include/vec2.bzz + ret0 |107,1,/home/ubuntu/buzz/src/include/vec2.bzz + +@__label_10 + pushs 21 |11,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |11,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |11,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |11,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 17 |11,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |11,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 19 |11,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |11,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |11,39,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |11,39,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |12,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_11 + lload 3 |23,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |23,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |23,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + unm |23,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |23,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |23,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |23,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 4 |23,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pow |23,48,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |23,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |23,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |23,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |23,66,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pow |23,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + sub |23,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + mul |23,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret1 |23,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |24,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_12 + pushs 0 |28,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |28,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 1 |28,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |28,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 5 |28,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |28,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 28 |28,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |28,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |28,41,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 30 |28,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |28,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 26 |28,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |28,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 27 |28,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |28,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 3 |28,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |28,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |28,74,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 31 |28,75,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |28,82,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |28,83,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |28,83,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret1 |28,83,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |29,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_13 + pushs 0 |33,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |33,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 1 |33,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |33,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 13 |33,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |33,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |33,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 3 |33,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |33,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |33,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret1 |33,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |34,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_14 + pushs 34 |38,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 33 |38,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |38,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |38,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |39,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 35 |39,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |39,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |41,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |41,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 36 |41,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |41,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 29 |41,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |41,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |41,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |41,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 37 |41,39,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |41,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 32 |41,52,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |41,52,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 0 |41,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |41,58,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 1 |41,59,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |41,63,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 2 |41,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |41,67,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.0 |41,68,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.0 |41,73,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |41,77,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |41,77,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |41,78,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |41,78,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lstore 1 |41,78,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |42,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |42,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 38 |42,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |42,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |42,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |42,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |42,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gt |42,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_15 |42,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 0 |43,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |43,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 1 |43,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |43,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 15 |43,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |43,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |43,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 1.0 |43,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |43,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |43,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 38 |43,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |43,48,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |43,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |43,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |43,50,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |43,51,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |43,51,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_15 |48,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |48,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |48,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 40 |48,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |48,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |48,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |48,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |48,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |49,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |49,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 42 |49,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |49,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |49,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_17 |49,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |50,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |50,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |50,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |51,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |51,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |51,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |51,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_18 |52,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_17 |52,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |53,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |53,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 42 |53,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |53,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |53,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |53,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |53,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_19 |53,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 44 |54,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |54,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.03 |54,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.0 |54,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |54,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |54,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_20 |55,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_19 |55,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 44 |56,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |56,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.0 |56,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 0.03 |56,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |56,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |56,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_20 |57,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |58,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |58,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |58,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |58,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + add |58,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |58,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_18 |59,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |60,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_21 + pushs 34 |78,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushl @__label_22 |78,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |80,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |81,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 49 |81,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |81,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |81,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |81,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 45 |81,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |81,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |81,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |81,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |81,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |82,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_22 + pushs 47 |79,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |79,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |79,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |79,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |79,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |79,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |80,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_23 + pushs 48 |88,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |88,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 52 |88,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |88,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 53 |88,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |88,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |88,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |88,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |88,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |89,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_24 + pushs 48 |97,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |97,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 54 |97,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |97,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 53 |97,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |97,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |97,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |97,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |98,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 55 |98,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |98,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |99,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |99,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 56 |99,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |99,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |99,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |99,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |99,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |99,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_25 |99,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |100,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushnil |100,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |100,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |101,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |101,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |101,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_26 |102,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_25 |102,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |102,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |102,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 42 |102,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |102,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |102,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_27 |102,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |103,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushnil |103,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |103,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |104,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |104,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |104,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |104,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |105,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |105,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |105,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_27 |107,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_26 |107,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |107,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |107,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |107,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |107,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + add |107,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |107,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |108,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_29 + pushs 34 |113,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |113,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |113,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |113,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |114,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |114,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |114,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |116,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_30 + pushs 24 |119,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 60 |119,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |119,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |120,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 59 |120,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |120,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |120,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |121,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |121,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 61 |121,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |121,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |121,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |121,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |121,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |121,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |121,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |122,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |122,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 64 |122,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 65 |122,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |122,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 66 |122,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |122,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |122,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |122,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |124,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |124,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |124,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |124,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |124,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |124,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 65 |124,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |124,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 66 |124,37,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |124,46,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |124,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |124,64,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |124,80,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |124,80,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushf 20.0 |124,81,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + div |124,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + sub |124,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gte |124,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |124,85,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_31 |124,87,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 46 |125,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |125,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |125,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |125,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 33 |125,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |125,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |125,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |125,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 51 |127,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |127,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |127,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |127,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_32 |130,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_31 |130,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |131,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |131,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 68 |131,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |131,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |131,35,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |131,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |131,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |132,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |132,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |132,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |132,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |132,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |132,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |132,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |132,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 70 |133,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |133,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 23 |133,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |133,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |133,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |133,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_32 |134,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |135,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_33 + pushs 24 |137,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 71 |137,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |137,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |138,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |138,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |138,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |138,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |139,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |139,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 72 |139,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |139,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |139,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |139,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |139,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |139,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |139,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |140,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |140,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |140,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |140,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |140,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |140,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |140,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |140,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 63 |140,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |140,40,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 3 |140,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |140,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + or |140,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_34 |140,45,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |141,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |141,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |141,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |141,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |141,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |141,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |141,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |141,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 73 |142,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |142,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |142,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |142,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_35 |144,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_34 |144,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 41 |145,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |145,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |145,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 48 |146,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushnil |146,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |146,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |147,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |147,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |147,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |147,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_35 |148,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |149,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_36 + pushs 75 |153,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 76 |153,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |153,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 50 |153,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |153,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |153,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |153,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |153,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |153,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 75 |155,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |155,2,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 77 |155,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |155,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |155,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |155,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |156,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |156,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |156,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |156,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |157,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |157,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |157,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |158,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_37 + pushs 62 |162,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |162,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |162,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |162,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |162,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |162,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_38 |162,23,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |163,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |163,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 80 |163,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |163,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |163,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |164,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |164,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |164,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |164,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |164,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |165,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 59 |165,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |165,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |165,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |166,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 60 |166,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |166,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |167,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |167,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |167,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |167,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |167,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |167,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |167,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |167,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_39 |168,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_38 |168,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |168,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |168,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |168,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |168,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |168,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |168,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_40 |168,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |169,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |169,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 81 |169,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |169,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |169,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |170,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |170,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 82 |170,6,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 1 |170,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |170,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |171,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |171,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |171,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |171,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |171,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |172,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |172,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |172,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |172,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |173,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 71 |173,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |173,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |174,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |174,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |174,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |174,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |174,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |174,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |174,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |174,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_41 |175,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_40 |175,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |175,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |175,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |175,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |175,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 16 |175,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |175,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_42 |175,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |176,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |176,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |176,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |176,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |176,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |177,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 57 |177,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |177,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |177,15,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 83 |178,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |178,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |178,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |178,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_43 |179,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_42 |179,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |179,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |179,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |179,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |179,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 400 |179,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |179,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_44 |179,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |180,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |180,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |180,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |180,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |180,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 84 |181,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |181,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |181,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |181,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |182,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |182,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |182,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |182,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |182,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 400 |182,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |182,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |182,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_45 |183,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_44 |183,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |183,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |183,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |183,19,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |183,25,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 401 |183,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |183,30,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_46 |183,31,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 62 |184,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |184,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 79 |184,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |184,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tput |184,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 85 |185,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |185,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |185,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |185,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |186,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |186,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 22 |186,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |186,21,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |186,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 401 |186,29,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |186,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |186,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_46 |188,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_45 |188,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_43 |188,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_41 |188,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_39 |188,0,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 21 |188,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |188,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 86 |188,10,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + tget |188,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 69 |188,17,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushl @__label_48 |189,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |202,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |202,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |203,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |203,7,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |203,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |203,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |204,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |204,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 91 |204,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |204,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |204,32,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |204,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |204,33,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 39 |205,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |205,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 92 |205,5,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 67 |205,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |205,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 2 |205,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |205,27,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |206,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_48 + pushs 87 |190,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |190,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 88 |190,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 1 |190,24,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 89 |190,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |190,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 90 |190,38,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 3 |190,59,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 6 |190,60,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |190,60,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |191,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 22 |191,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |191,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |191,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |191,26,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |191,28,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |191,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |191,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_49 |191,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |192,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 59 |192,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |192,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |192,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_50 |193,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_49 |193,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |193,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 21 |193,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |193,20,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_51 |193,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 34 |194,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 43 |194,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |194,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gstore |194,13,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_52 |195,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_51 |195,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |195,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 400 |195,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |195,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |195,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |195,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |195,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |195,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |195,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_53 |195,44,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 84 |196,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |196,9,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |196,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |196,11,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jump @__label_54 |197,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_53 |197,8,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + lload 2 |197,16,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 401 |197,18,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |197,22,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 24 |197,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |197,34,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 58 |197,36,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + eq |197,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + and |197,42,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + jumpz @__label_55 |197,43,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushs 85 |198,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + gload |198,12,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + pushi 0 |198,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + callc |198,14,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_55 |200,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_54 |200,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_52 |200,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz +@__label_50 |200,3,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + ret0 |200,4,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_57 + ret0 |210,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz + +@__label_58 + ret0 |214,1,/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz diff --git a/script/testsolo.bdb b/script/testsolo.bdb new file mode 100644 index 0000000..e3f057f Binary files /dev/null and b/script/testsolo.bdb differ diff --git a/script/testsolo.bdbg b/script/testsolo.bdbg new file mode 100644 index 0000000..d25ea48 Binary files /dev/null and b/script/testsolo.bdbg differ diff --git a/script/testsolo.bo b/script/testsolo.bo new file mode 100644 index 0000000..294076a Binary files /dev/null and b/script/testsolo.bo differ diff --git a/script/testsolo.bzz b/script/testsolo.bzz new file mode 100644 index 0000000..4118e14 --- /dev/null +++ b/script/testsolo.bzz @@ -0,0 +1,214 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) +# uav_moveto(accum.x,accum.y) + + log("Time: ", timeW) + if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS + timeW =0 + statef=land + } else { + if(timeW >= (WAIT_TIMEOUT / 2)){ + uav_moveto(0.03,0.0) + } else { + uav_moveto(0.0,0.03) + } + timeW = timeW+1 + } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 300 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "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, hexagon) + #barrier_set(ROBOTS, land); + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/teststig.bzz b/script/teststig.bzz new file mode 100644 index 0000000..dc6c89e --- /dev/null +++ b/script/teststig.bzz @@ -0,0 +1,39 @@ +include "vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +function init(){ + s = swarm.create(1) + s.join() + v = stigmergy.create(5) + t= {} + v.put("p",t) + v.put("u",1) +} + +function step() { + log("Swarm size: ",ROBOTS) + log("The vstig has ", v.size(), " elements") + log(v.get("u")) + if (id==1) { + tmp = { .x=3} + v.put("p",tmp) + v.put("u",2) + } + log(v.get("p")) +} + +# 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/script/update_sim/include/string.bzz b/script/update_sim/include/string.bzz new file mode 100644 index 0000000..4140a1b --- /dev/null +++ b/script/update_sim/include/string.bzz @@ -0,0 +1,92 @@ +# +# Returns the string character at the given position. +# PARAM s: The string +# PARAM n: The position of the wanted character +# RETURN The character at the wanted position, or nil +# +string.charat = function(s, n) { + return string.sub(s, n, n+1) +} + +# +# Returns the index of the first occurrence of the given string m +# within another string s. If none is found, this function returns +# nil. +# PARAM s: The string +# PARAM m: The string to match +# RETURN: The position of the first match, or nil +# +string.indexoffirst = function(s, m) { + var ls = string.length(s) + var lm = string.length(m) + var i = 0 + while(i < ls-lm+1) { + if(string.sub(s, i, i+lm) == m) return i + i = i + 1 + } + return nil +} + +# +# Returns the index of the last occurrence of the given string m +# within another string s. If none is found, this function returns +# nil. +# PARAM s: The string +# PARAM m: The string to match +# RETURN: The position of the last match, or nil +# +string.indexoflast = function(s, m) { + var ls = string.length(s) + var lm = string.length(m) + var i = ls - lm + 1 + while(i >= 0) { + if(string.sub(s, i, i+lm) == m) return i + i = i - 1 + } + return nil +} + +# Splits a string s using the delimiters in d. The string list is +# returned in a table indexed by value (starting at 0). +# PARAM s: The string +# PARAM d: A string containing the delimiters +# RETURN: A table containing the tokens +string.split = function(s, d) { + var i1 = 0 # index to move along s (token start) + var i2 = 0 # index to move along s (token end) + var c = 0 # token count + var t = {} # token list + var ls = string.length(s) + var ld = string.length(d) + # Go through string s + while(i2 < ls) { + # Try every delimiter + var j = 0 # index to move along d + var f = nil # whether the delimiter was found or not + while(j < ld and (not f)) { + if(string.charat(s, i2) == string.charat(d, j)) { + # Delimiter found + f = 1 + # Is it worth adding a new token? + if(i2 > i1) { + t[c] = string.sub(s, i1, i2) + c = c + 1 + } + # Start new token + i1 = i2 + 1 + } + else { + # Next delimiter + j = j + 1 + } + } + # Next string character + i2 = i2 + 1 + } + # Is it worth adding a new token? + if(i2 > i1) { + t[c] = string.sub(s, i1, i2) + } + # Return token list + return t; +} diff --git a/script/update_sim/include/vec2.bzz b/script/update_sim/include/vec2.bzz new file mode 100644 index 0000000..e2fb9b0 --- /dev/null +++ b/script/update_sim/include/vec2.bzz @@ -0,0 +1,107 @@ +# +# Create a new namespace for vector2 functions +# +math.vec2 = {} + +# +# Creates a new vector2. +# PARAM x: The x coordinate. +# PARAM y: The y coordinate. +# RETURN: A new vector2. +# +math.vec2.new = function(x, y) { + return { .x = x, .y = y } +} + +# +# Creates a new vector2 from polar coordinates. +# PARAM l: The length of the vector2. +# PARAM a: The angle of the vector2. +# RETURN: A new vector2. +# +math.vec2.newp = function(l, a) { + return { + .x = l * math.cos(a), + .y = l * math.sin(a) + } +} + +# +# Calculates the length of the given vector2. +# PARAM v: The vector2. +# RETURN: The length of the vector. +# +math.vec2.length = function(v) { + return math.sqrt(v.x * v.x + v.y * v.y) +} + +# +# Calculates the angle of the given vector2. +# PARAM v: The vector2. +# RETURN: The angle of the vector. +# +math.vec2.angle = function(v) { + return math.atan2(v.y, v.x) +} + +# +# Returns the normalized form of a vector2. +# PARAM v: The vector2. +# RETURN: The normalized form. +# +math.vec2.norm = function(v) { + var l = math.length(v) + return { + .x = v.x / l, + .y = v.y / l + } +} + +# +# Calculates v1 + v2. +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 + v2 +# +math.vec2.add = function(v1, v2) { + return { + .x = v1.x + v2.x, + .y = v1.y + v2.y + } +} + +# +# Calculates v1 - v2. +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 + v2 +# +math.vec2.sub = function(v1, v2) { + return { + .x = v1.x - v2.x, + .y = v1.y - v2.y + } +} + +# +# Scales a vector by a numeric constant. +# PARAM v: A vector2. +# PARAM s: A number (float or int). +# RETURN: s * v +# +math.vec2.scale = function(v, s) { + return { + .x = v.x * s, + .y = v.y * s + } +} + +# +# Calculates v1 . v2 (the dot product) +# PARAM v1: A vector2. +# PARAM v2: A vector2. +# RETURN: v1 . v2 +# +math.vec2.dot = function(v1, v2) { + return v1.x * v2.x + v1.y * v2.y +} diff --git a/script/update_sim/old_testflockfev1.bzz b/script/update_sim/old_testflockfev1.bzz new file mode 100644 index 0000000..02abd4c --- /dev/null +++ b/script/update_sim/old_testflockfev1.bzz @@ -0,0 +1,209 @@ +# We need this for 2D vectors + +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "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,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/old_testflockfev2.bzz b/script/update_sim/old_testflockfev2.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/old_testflockfev2.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "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,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev1.bzz b/script/update_sim/testflockfev1.bzz new file mode 100644 index 0000000..06b3e32 --- /dev/null +++ b/script/update_sim/testflockfev1.bzz @@ -0,0 +1,240 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 12.0 #0.000001001 +EPSILON = 3.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else if(timeW>=WAIT_TIMEOUT/2) { +# timeW = timeW+1 +# uav_moveto(0.06,0.0) +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.06) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "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,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +function users_save(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + #log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } +} + +function table_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) + s.join() + + vt = stigmergy.create(5) + t = {} + vt.put("p",t) + + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) + + # Check local users and push to v.stig + if(size(users.dataG)>0) + vt.put("p", users.dataG) + + # Save locally the users and print RG + users_save(vt.get("p")) + table_print(users.dataL) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev2.bzz b/script/update_sim/testflockfev2.bzz new file mode 100644 index 0000000..06b3e32 --- /dev/null +++ b/script/update_sim/testflockfev2.bzz @@ -0,0 +1,240 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 12.0 #0.000001001 +EPSILON = 3.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else if(timeW>=WAIT_TIMEOUT/2) { +# timeW = timeW+1 +# uav_moveto(0.06,0.0) +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.06) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "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,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +function users_save(t) { + if(size(t)>0) { + foreach(t, function(id, tab) { + #log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo) + add_user_rb(id,tab.la,tab.lo) + }) + } +} + +function table_print(t) { + if(size(t)>0) { + foreach(t, function(u, tab) { + log("id: ",u," Range ", tab.r, "Bearing ", tab.b) + }) + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) + s.join() + + vt = stigmergy.create(5) + t = {} + vt.put("p",t) + + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) + + # Check local users and push to v.stig + if(size(users.dataG)>0) + vt.put("p", users.dataG) + + # Save locally the users and print RG + users_save(vt.get("p")) + table_print(users.dataL) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev5.bzz b/script/update_sim/testflockfev5.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev5.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "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,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev6.bzz b/script/update_sim/testflockfev6.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev6.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "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,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/script/update_sim/testflockfev7.bzz b/script/update_sim/testflockfev7.bzz new file mode 100644 index 0000000..1845eb9 --- /dev/null +++ b/script/update_sim/testflockfev7.bzz @@ -0,0 +1,208 @@ +# We need this for 2D vectors +# Make sure you pass the correct include path to "bzzc -I ..." +include "/home/ubuntu/buzz/src/include/vec2.bzz" +#################################################################################################### +# Updater related +# This should be here for the updater to work, changing position of code will crash the updater +#################################################################################################### +updated="update_ack" +update_no=0 +function updated_neigh(){ +neighbors.broadcast(updated, update_no) +} + +TARGET_ALTITUDE = 3.0 +CURSTATE = "TURNEDOFF" + +# Lennard-Jones parameters +TARGET = 10.0 #0.000001001 +EPSILON = 18.0 #0.001 + +# Lennard-Jones interaction magnitude +function lj_magnitude(dist, target, epsilon) { + return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) +} + +# Neighbor data to LJ interaction vector +function lj_vector(rid, data) { + return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) +} + +# Accumulator of neighbor LJ interactions +function lj_sum(rid, data, accum) { + return math.vec2.add(data, accum) +} + +# Calculates and actuates the flocking interaction +function hexagon() { + statef=hexagon + CURSTATE = "HEXAGON" + # Calculate accumulator + var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) + if(neighbors.count() > 0) + math.vec2.scale(accum, 1.0 / neighbors.count()) + # Move according to vector + #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) + uav_moveto(accum.x,accum.y) + +# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS +# timeW =0 +# statef=land +# } else { +# timeW = timeW+1 +# uav_moveto(0.0,0.0) +# } +} + +######################################## +# +# BARRIER-RELATED FUNCTIONS +# +######################################## + +# +# Constants +# +BARRIER_VSTIG = 1 +# ROBOTS = 3 # number of robots in the swarm + +# +# Sets a barrier +# +function barrier_set(threshold, transf) { + statef = function() { + barrier_wait(threshold, transf); + } + barrier = stigmergy.create(BARRIER_VSTIG) +} + +# +# Make yourself ready +# +function barrier_ready() { + barrier.put(id, 1) +} + +# +# Executes the barrier +# +WAIT_TIMEOUT = 200 +timeW=0 +function barrier_wait(threshold, transf) { + barrier.get(id) + CURSTATE = "BARRIERWAIT" + if(barrier.size() >= threshold) { + barrier = nil + transf() + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=land + timeW=0 + } + timeW = timeW+1 +} + +# flight status + +function idle() { +statef=idle +CURSTATE = "IDLE" + +} + +function takeoff() { + CURSTATE = "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,hexagon) + barrier_ready() + #statef=hexagon + } + else { + log("Altitude: ", TARGET_ALTITUDE) + neighbors.broadcast("cmd", 22) + uav_takeoff(TARGET_ALTITUDE) + } +} +function land() { + CURSTATE = "LAND" + statef=land + log("Land: ", flight.status) + if(flight.status == 2 or flight.status == 3){ + neighbors.broadcast("cmd", 21) + uav_land() + } + else { + timeW=0 + barrier = nil + statef=idle + } +} + +# Executed once at init time. +function init() { + s = swarm.create(1) +# s.select(1) + s.join() + statef=idle + CURSTATE = "IDLE" +} + +# Executed at each time step. +function step() { + if(flight.rc_cmd==22) { + log("cmd 22") + flight.rc_cmd=0 + statef = takeoff + CURSTATE = "TAKEOFF" + neighbors.broadcast("cmd", 22) + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") + flight.rc_cmd=0 + statef = land + CURSTATE = "LAND" + neighbors.broadcast("cmd", 21) + } else if(flight.rc_cmd==16) { + flight.rc_cmd=0 + statef = idle + uav_goto() + } else if(flight.rc_cmd==400) { + flight.rc_cmd=0 + uav_arm() + neighbors.broadcast("cmd", 400) + } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 + uav_disarm() + neighbors.broadcast("cmd", 401) + } +neighbors.listen("cmd", + function(vid, value, rid) { + print("Got (", vid, ",", value, ") from robot #", rid) + if(value==22 and CURSTATE=="IDLE") { + statef=takeoff + } else if(value==21) { + statef=land + } else if(value==400 and CURSTATE=="IDLE") { + uav_arm() + } else if(value==401 and CURSTATE=="IDLE"){ + uav_disarm() + } + } + +) + statef() + log("Current state: ", CURSTATE) + log("Swarm size: ",ROBOTS) +} + +# Executed once when the robot (or the simulator) is reset. +function reset() { +} + +# Executed once at the end of experiment. +function destroy() { +} diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index d622228..3a77dba 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -27,80 +27,72 @@ static int neigh=-1; static int updater_msg_ready ; static int updated=0; +/*Initialize updater*/ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ fprintf(stdout,"intiialized file monitor.\n"); fd=inotify_init1(IN_NONBLOCK); if ( fd < 0 ) { - perror( "inotify_init error" ); - } - /* watch /.bzz file for any activity and report it back to me */ + perror( "inotify_init error" ); + } + /* watch /.bzz file for any activity and report it back to update */ wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS ); - + /*load the .bo under execution into the updater*/ uint8_t* BO_BUF = 0; FILE* fp = fopen(bo_filename, "rb"); - - if(!fp) { - perror(bo_filename); - - } - fseek(fp, 0, SEEK_END); - - size_t bcode_size = ftell(fp); - rewind(fp); - - BO_BUF = (uint8_t*)malloc(bcode_size); - if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { - perror(bo_filename); - - fclose(fp); - //return 0; - } - fclose(fp); + if(!fp) { + perror(bo_filename); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { + perror(bo_filename); + fclose(fp); + //return 0; + } + fclose(fp); + /*Load stand_by .bo file into the updater*/ uint8_t* STD_BO_BUF = 0; fp = fopen(stand_by_script, "rb"); + if(!fp) { + perror(stand_by_script); - if(!fp) { - perror(stand_by_script); - - } - fseek(fp, 0, SEEK_END); - - size_t stdby_bcode_size = ftell(fp); - rewind(fp); - - STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); - if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { - perror(stand_by_script); - - fclose(fp); - //return 0; - } - fclose(fp); - - updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); - /* Create a new table for updater*/ - updater->bcode = BO_BUF; - updater->outmsg_queue = NULL; - updater->inmsg_queue = NULL; - updater->bcode_size = (size_t*) malloc(sizeof(size_t)); - updater->update_no = (uint8_t*) malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->update_no) =0; - *(size_t*)(updater->bcode_size)=bcode_size; - updater->standby_bcode = STD_BO_BUF; - updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); - *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size; - updater->mode=(int*)malloc(sizeof(int)); - *(int*)(updater->mode)=CODE_RUNNING; - //no_of_robot=barrier; - updater_msg_ready=0; - //neigh = 0; - //updater->outmsg_queue= - // update_table->barrier=nvs; + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { + perror(stand_by_script); + fclose(fp); + //return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->bcode_size = (size_t*) malloc(sizeof(size_t)); + updater->update_no = (uint8_t*) malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) =0; + *(size_t*)(updater->bcode_size)=bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size; + updater->mode=(int*)malloc(sizeof(int)); + *(int*)(updater->mode)=CODE_RUNNING; + //no_of_robot=barrier; + updater_msg_ready=0; + //neigh = 0; + //updater->outmsg_queue= + // update_table->barrier=nvs; } - +/*Check for .bzz file chages*/ int check_update(){ - struct inotify_event *event; char buf[1024]; int check =0; @@ -108,7 +100,6 @@ int check_update(){ int len=read(fd,buf,1024); while(imask & (IN_MODIFY| IN_DELETE_SELF)){ @@ -123,20 +114,12 @@ int check_update(){ check=1; old_update =1; } - } - /* update index to start of next event */ i+=sizeof(struct inotify_event)+event->len; - } - if (!check) old_update=0; - /*if(update){ - buzz_script_set(update_bo, update_bdbg); - update = 0; - }*/ - return check; +return check; } @@ -163,57 +146,56 @@ void code_message_outqueue_append(){ } void code_message_inqueue_append(uint8_t* msg,uint16_t size){ -updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); -fprintf(stdout,"in ms append code size %d\n", (int) size); -updater->inmsg_queue->queue = (uint8_t*)malloc(size); -updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); -memcpy(updater->inmsg_queue->queue, msg, size); -*(uint16_t*)(updater->inmsg_queue->size) = size; - + updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + fprintf(stdout,"in ms append code size %d\n", (int) size); + updater->inmsg_queue->queue = (uint8_t*)malloc(size); + updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memcpy(updater->inmsg_queue->queue, msg, size); + *(uint16_t*)(updater->inmsg_queue->size) = size; } void code_message_inqueue_process(){ -int size=0; -fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) ); -fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); -fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); + int size=0; + fprintf(stdout,"[debug]Updater mode %d \n", *(int*)(updater->mode) ); + fprintf(stdout,"[debug] %u : current update number, %u : received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); + fprintf(stdout,"[debug]Updater code size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) ); -if( *(int*) (updater->mode) == CODE_RUNNING){ - //fprintf(stdout,"[debug]Inside inmsg code running"); - if( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){ - //fprintf(stdout,"[debug]Inside update number comparision"); - uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue); - size +=sizeof(uint16_t); - uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size); - size +=sizeof(uint16_t); - //fprintf(stdout,"in queue process Update no %d\n", (int) update_no); - //fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); - //FILE *fp; - //fp=fopen("update.bo", "wb"); - //fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); - //fclose(fp); - if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size), - (char*) dbgf_name,(size_t)update_bcode_size) ) { - *(uint16_t*)(updater->update_no)=update_no; - neigh=1; + if( *(int*) (updater->mode) == CODE_RUNNING){ + //fprintf(stdout,"[debug]Inside inmsg code running"); + if( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){ + //fprintf(stdout,"[debug]Inside update number comparision"); + uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue); + size +=sizeof(uint16_t); + uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size); + size +=sizeof(uint16_t); + //fprintf(stdout,"in queue process Update no %d\n", (int) update_no); + //fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); + //FILE *fp; + //fp=fopen("update.bo", "wb"); + //fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); + //fclose(fp); + if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size), + (char*) dbgf_name,(size_t)update_bcode_size) ) { + *(uint16_t*)(updater->update_no)=update_no; + neigh=1; + } } } -} -//fprintf(stdout,"in queue freed\n"); -delete_p(updater->inmsg_queue->queue); -delete_p(updater->inmsg_queue->size); -delete_p(updater->inmsg_queue); + //fprintf(stdout,"in queue freed\n"); + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); } void update_routine(const char* bcfname, const char* dbgfname){ -dbgf_name=(char*)dbgfname; -buzzvm_t VM = buzz_utility::get_vm(); + dbgf_name=(char*)dbgfname; + buzzvm_t VM = buzz_utility::get_vm(); -buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); -//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + //fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); if(*(int*)updater->mode==CODE_RUNNING){ buzzvm_function_call(VM, "updated_neigh", 0); if(check_update()){ @@ -226,65 +208,55 @@ buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0,name.find_last_of(".")); bzzfile_in_compile << "bzzparse "<update_no); - *(uint16_t*)(updater->update_no) =update_no +1; - code_message_outqueue_append(); - VM = buzz_utility::get_vm(); - fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); - neigh=-1; - fprintf(stdout,"Sending code... \n"); - code_message_outqueue_append(); + uint16_t update_no =*(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) =update_no +1; + code_message_outqueue_append(); + VM = buzz_utility::get_vm(); + fprintf(stdout,"Update no %d\n", *(uint16_t*)(updater->update_no)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + neigh=-1; + fprintf(stdout,"Sending code... \n"); + code_message_outqueue_append(); } delete_p(BO_BUF); } @@ -330,7 +302,6 @@ return (uint8_t*)updater->outmsg_queue->queue; uint8_t* getupdate_out_msg_size(){ //fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); - return (uint8_t*)updater->outmsg_queue->size; } @@ -397,17 +368,17 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ } void destroy_out_msg_queue(){ -//fprintf(stdout,"out queue freed\n"); -delete_p(updater->outmsg_queue->queue); -delete_p(updater->outmsg_queue->size); -delete_p(updater->outmsg_queue); -updater_msg_ready=0; + //fprintf(stdout,"out queue freed\n"); + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + updater_msg_ready=0; } int get_update_status(){ return updated; } void set_read_update_status(){ -updated=0; + updated=0; } int get_update_mode(){ return (int)*(int*)(updater->mode); @@ -420,44 +391,44 @@ buzz_updater_elem_t get_updater(){ return updater; } void destroy_updater(){ -delete_p(updater->bcode); -delete_p(updater->bcode_size); -delete_p(updater->standby_bcode); -delete_p(updater->standby_bcode_size); -delete_p(updater->mode); -delete_p(updater->update_no); -if(updater->outmsg_queue){ -delete_p(updater->outmsg_queue->queue); -delete_p(updater->outmsg_queue->size); -delete_p(updater->outmsg_queue); -} -if(updater->inmsg_queue){ -delete_p(updater->inmsg_queue->queue); -delete_p(updater->inmsg_queue->size); -delete_p(updater->inmsg_queue); -} -// -inotify_rm_watch(fd,wd); -close(fd); -} + delete_p(updater->bcode); + delete_p(updater->bcode_size); + delete_p(updater->standby_bcode); + delete_p(updater->standby_bcode_size); + delete_p(updater->mode); + delete_p(updater->update_no); + if(updater->outmsg_queue){ + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + } + if(updater->inmsg_queue){ + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + // + inotify_rm_watch(fd,wd); + close(fd); + } void set_bzz_file(const char* in_bzz_file){ -bzz_file=in_bzz_file; + bzz_file=in_bzz_file; } void updates_set_robots(int robots){ -no_of_robot=robots; + no_of_robot=robots; } void collect_data(){ -//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end); -double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; -time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; -//int bytecodesize=(int); -fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no); -//FILE *Fileptr; -//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a"); -//fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no); -//fclose(Fileptr); + //fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end); + double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; + time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; + //int bytecodesize=(int); + fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no); + //FILE *Fileptr; + //Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a"); + //fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no); + //fclose(Fileptr); } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index c9cbd69..f1679c4 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -6,7 +6,7 @@ * @copyright 2016 MistLab. All rights reserved. */ -#include +#include "buzz_utility.h" namespace buzz_utility{ @@ -17,40 +17,139 @@ namespace buzz_utility{ static char* BO_FNAME = 0; static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; - static uint8_t MSG_SIZE = 50; // Only 100 bytes of Buzz messages every step + static uint8_t MSG_SIZE = 250; // Only 100 bytes of Buzz messages every step static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets static int Robot_id = 0; - buzzobj_t usersvstig; - buzzobj_t key; + static std::vector IN_MSG; + std::map< int, Pos_struct> users_map; /****************************************/ - /*adds neighbours position*/ - void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){ - /* Reset neighbor information */ - buzzneighbors_reset(VM); - /* Get robot id and update neighbor information */ - map< int, Pos_struct >::iterator it; - for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){ - buzzneighbors_add(VM, - it->first, - (it->second).x, - (it->second).y, - (it->second).z); - } + + void add_user(int id, double latitude, double longitude, float altitude) + { + Pos_struct pos_arr; + pos_arr.x=latitude; + pos_arr.y=longitude; + pos_arr.z=altitude; + map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id); + if(it!=users_map.end()) + users_map.erase(it); + users_map.insert(make_pair(id, pos_arr)); + //ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); + } + + void update_users(){ + if(users_map.size()>0) { + /* Reset users information */ + buzzusers_reset(); + /* Get user id and update user information */ + map< int, Pos_struct >::iterator it; + for (it=users_map.begin(); it!=users_map.end(); ++it){ + //ROS_INFO("Buzz_utility will save user %i.", it->first); + buzzusers_add(it->first, + (it->second).x, + (it->second).y, + (it->second).z); + } + }/*else + ROS_INFO("[%i] No new users",Robot_id);*/ + } + + int buzzusers_reset() { + if(VM->state != BUZZVM_STATE_READY) return VM->state; + /* Make new table */ + buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); + //make_table(&t); + if(VM->state != BUZZVM_STATE_READY) return VM->state; + /* Register table as global symbol */ + /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); + buzzvm_tget(VM);*/ + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); + buzzvm_push(VM, t); + buzzvm_gstore(VM); + //buzzvm_pushi(VM, 2); + //buzzvm_callc(VM); + return VM->state; + } + + int buzzusers_add(int id, double latitude, double longitude, double altitude) { + if(VM->state != BUZZVM_STATE_READY) return VM->state; + /* Get users "p" table */ + /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1)); + buzzvm_tget(VM);*/ + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); + buzzvm_gload(VM); + //buzzvm_pushi(VM, 1); + //buzzvm_callc(VM); + buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE); + buzzobj_t nbr = buzzvm_stack_at(VM, 1); + /* Get "data" field */ + buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); + buzzvm_tget(VM); + if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) { + //ROS_INFO("Empty data, create a new table"); + buzzvm_pop(VM); + buzzvm_push(VM, nbr); + buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); + buzzvm_pusht(VM); + buzzobj_t data = buzzvm_stack_at(VM, 1); + buzzvm_tput(VM); + buzzvm_push(VM, data); + } + /* When we get here, the "data" table is on top of the stack */ + /* Push user id */ + buzzvm_pushi(VM, id); + /* Create entry table */ + buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); + /* Insert latitude */ + buzzvm_push(VM, entry); + buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1)); + buzzvm_pushf(VM, latitude); + buzzvm_tput(VM); + /* Insert longitude */ + buzzvm_push(VM, entry); + buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1)); + buzzvm_pushf(VM, longitude); + buzzvm_tput(VM); + /* Insert altitude */ + buzzvm_push(VM, entry); + buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1)); + buzzvm_pushf(VM, altitude); + buzzvm_tput(VM); + /* Save entry into data table */ + buzzvm_push(VM, entry); + buzzvm_tput(VM); + ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); + // forcing the new table into the stigmergy.... + /*buzzobj_t newt = buzzvm_stack_at(VM, 0); + buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); + buzzvm_tget(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); + buzzvm_push(VM, nbr); + buzzvm_pushi(VM, 2); + buzzvm_callc(VM);*/ + //buzzvm_gstore(VM); + return VM->state; } /**************************************************************************/ /*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */ /**************************************************************************/ uint16_t* u64_cvt_u16(uint64_t u64){ - uint16_t* out = new uint16_t[4]; - uint32_t int32_1 = u64 & 0xFFFFFFFF; - uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32; - out[0] = int32_1 & 0xFFFF; - out[1] = (int32_1 & (0xFFFF0000) ) >> 16; - out[2] = int32_2 & 0xFFFF; - out[3] = (int32_2 & (0xFFFF0000) ) >> 16; - //cout << " values " <> 32; + out[0] = int32_1 & 0xFFFF; + out[1] = (int32_1 & (0xFFFF0000) ) >> 16; + out[2] = int32_2 & 0xFFFF; + out[3] = (int32_2 & (0xFFFF0000) ) >> 16; + //cout << " values " < 0 && unMsgSize <= size - tot ) { - buzzinmsg_queue_append(VM, - buzzmsg_payload_frombuffer(pl +tot, unMsgSize)); - tot += unMsgSize; - } - }while(size - tot > sizeof(uint16_t) && unMsgSize > 0); - free(pl); - /* Process messages */ + /* Copy packet into temporary buffer */ + memcpy(pl, payload ,size); + IN_MSG.push_back(pl); + + } + + void in_message_process(){ + while(!IN_MSG.empty()){ + uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); + /* Go through messages and append them to the FIFO */ + uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]); + /*Size is at first 2 bytes*/ + uint16_t size=data[0]*sizeof(uint64_t); + delete[] data; + /*size and robot id read*/ + size_t tot = sizeof(uint32_t); + /* Go through the messages until there's nothing else to read */ + uint16_t unMsgSize=0; + /*Obtain Buzz messages push it into queue*/ + do { + /* Get payload size */ + unMsgSize = *(uint16_t*)(first_INmsg + tot); + tot += sizeof(uint16_t); + /* Append message to the Buzz input message queue */ + if(unMsgSize > 0 && unMsgSize <= size - tot ) { + buzzinmsg_queue_append(VM, + buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize)); + tot += unMsgSize; + } + }while(size - tot > sizeof(uint16_t) && unMsgSize > 0); + IN_MSG.erase(IN_MSG.begin()); + free(first_INmsg); + } + /* Process messages VM call*/ buzzvm_process_inmsgs(VM); } /***************************************************/ /*Obtains messages from buzz out message Queue*/ /***************************************************/ - uint64_t* out_msg_process(){ - buzzvm_process_outmsgs(VM); + uint64_t* obt_out_msg(){ + /* Process out messages */ + buzzvm_process_outmsgs(VM); uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE); memset(buff_send, 0, MAX_MSG_SIZE); /*Taking into consideration the sizes included at the end*/ @@ -151,8 +263,8 @@ namespace buzz_utility{ cout<<" payload from out msg "<<*(payload_64+i)<pc, VM->errormsg); } - return msg; + return msg; } /****************************************/ @@ -189,15 +301,15 @@ namespace buzz_utility{ buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); 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, "uav_moveto", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + 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_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); @@ -212,7 +324,11 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land)); buzzvm_gstore(VM); - return BUZZVM_STATE_READY; + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_adduserRB)); + buzzvm_gstore(VM); + + return VM->state; } /**************************************************/ @@ -224,15 +340,15 @@ namespace buzz_utility{ buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); 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, "uav_moveto", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + 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_goto", 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)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); @@ -247,86 +363,142 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - return BUZZVM_STATE_READY; + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + + return VM->state; } +static int create_stig_tables() { +/* + // usersvstig = stigmergy.create(123) + buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + // get the stigmergy table from the global scope + buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1)); + buzzvm_gload(VM); + // get the create method from the stigmergy table + buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1)); + buzzvm_tget(VM); + // value of the stigmergy id + buzzvm_pushi(VM, 5); + // call the stigmergy.create() method + buzzvm_dump(VM); +// buzzvm_closure_call(VM, 1); + buzzvm_pushi(VM, 1); + buzzvm_callc(VM); + buzzvm_gstore(VM); + buzzvm_dump(VM); + //buzzusers_reset(); + buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); + + buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); + buzzvm_tget(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1)); + buzzvm_push(VM, t); + buzzvm_dump(VM); +// buzzvm_closure_call(VM, 2); + buzzvm_pushi(VM, 2); + buzzvm_callc(VM); + //buzzvm_gstore(VM); + //buzzvm_dump(VM); + + /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); + buzzvm_tget(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "u", 1)); + buzzvm_pusht(VM); + buzzvm_pushi(VM, 2); + buzzvm_call(VM, 0); + buzzvm_gstore(VM);*/ + + buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); + buzzvm_push(VM,t); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); + buzzvm_pusht(VM); + buzzobj_t data = buzzvm_stack_at(VM, 1); + buzzvm_tput(VM); + buzzvm_push(VM, data); + + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); + buzzvm_gload(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1)); + buzzvm_pusht(VM); + data = buzzvm_stack_at(VM, 1); + buzzvm_tput(VM); + buzzvm_push(VM, data); + + return VM->state; +} /****************************************/ /*Sets the .bzz and .bdbg file*/ /****************************************/ - int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) { - cout << " Robot ID: " <errormsg << endl; + //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; return 0; } - - -buzzvm_dup(VM); - // usersvstig = stigmergy.create(123) - buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1)); - // value of the stigmergy id - buzzvm_pushi(VM, 5); - // get the stigmergy table from the global scope -// buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1)); -// buzzvm_gload(VM); - // get the create method from the stigmergy table -// buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1)); -// buzzvm_tget(VM); - // call the stigmergy.create() method -// buzzvm_closure_call(VM, 1); - // now the stigmergy is at the top of the stack - save it for future reference -// usersvstig = buzzvm_stack_at(VM, 0); -//buzzvm_pop(VM); - // assign the virtual stigmergy to the global symbol v - // create also a global variable for it, so the garbage collector does not remove it -//buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1)); -//buzzvm_push(VM, usersvstig); - buzzvm_gstore(VM); /* Save bytecode file name */ BO_FNAME = strdup(bo_filename); @@ -336,29 +508,16 @@ buzzvm_dup(VM); buzzvm_function_call(VM, "init", 0); /* All OK */ + ROS_INFO("[%i] INIT DONE!!!", Robot_id); + return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size); } - int buzz_update_users_stigmergy(buzzobj_t tbl){ - // push the key (here it's an int with value 46) - buzzvm_pushi(VM, 46); - // push the table - buzzvm_push(VM, tbl); - // the stigmergy is stored in the vstig variable - // let's push it on the stack - buzzvm_push(VM, usersvstig); - // get the put method from myvstig - buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); - buzzvm_tget(VM); - // call the vstig.put() method - buzzvm_closure_call(VM, 2); - return 1; - } /****************************************/ /*Sets a new update */ /****************************************/ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ -/* // Reset the Buzz VM + // Reset the Buzz VM if(VM) buzzvm_destroy(&VM); VM = buzzvm_new(Robot_id); // Get rid of debug info @@ -386,20 +545,28 @@ buzzvm_dup(VM); fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); return 0; } - + /* Create vstig tables */ + if(create_stig_tables() != BUZZVM_STATE_READY) { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id); + //cout << "ERROR!!!! ---------- " << VM->errormsg << endl; + //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; + return 0; + } // Execute the global part of the script buzzvm_execute_script(VM); // Call the Init() function buzzvm_function_call(VM, "init", 0); // All OK -*/ return 1; + return 1; } /****************************************/ /*Performs a initialization test */ /****************************************/ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ -/* // Reset the Buzz VM + // Reset the Buzz VM if(VM) buzzvm_destroy(&VM); VM = buzzvm_new(Robot_id); // Get rid of debug info @@ -427,13 +594,21 @@ buzzvm_dup(VM); fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); return 0; } - + /* Create vstig tables */ + if(create_stig_tables() != BUZZVM_STATE_READY) { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id); + //cout << "ERROR!!!! ---------- " << VM->errormsg << endl; + //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; + return 0; + } // Execute the global part of the script buzzvm_execute_script(VM); // Call the Init() function buzzvm_function_call(VM, "init", 0); // All OK -*/ return 1; + return 1; } /****************************************/ @@ -447,73 +622,74 @@ buzzvm_dup(VM); typedef struct buzzswarm_elem_s* buzzswarm_elem_t; void check_swarm_members(const void* key, void* data, void* params) { - buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; - int* status = (int*)params; - if(*status == 3) return; - fprintf(stderr, "CHECKING SWARM MEMBERS:%i\n",buzzdarray_get(e->swarms, 0, uint16_t)); - if(buzzdarray_size(e->swarms) != 1) { - fprintf(stderr, "Swarm list size is not 1\n"); - *status = 3; - } - else { - int sid = 1; - if(!buzzdict_isempty(VM->swarms)) { - if(*buzzdict_get(VM->swarms, &sid, uint8_t) && - buzzdarray_get(e->swarms, 0, uint16_t) != sid) { - fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", - sid, - buzzdarray_get(e->swarms, 0, uint16_t)); - *status = 3; - return; - } - } - if(buzzdict_size(VM->swarms)>1) { - sid = 2; - if(*buzzdict_get(VM->swarms, &sid, uint8_t) && - buzzdarray_get(e->swarms, 0, uint16_t) != sid) { - fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", - sid, - buzzdarray_get(e->swarms, 0, uint16_t)); - *status = 3; - return; - } - } - } + buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; + int* status = (int*)params; + if(*status == 3) return; + fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n", + buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age); + if(buzzdarray_size(e->swarms) != 1) { + fprintf(stderr, "Swarm list size is not 1\n"); + *status = 3; + } + else { + int sid = 1; + if(!buzzdict_isempty(VM->swarms)) { + if(*buzzdict_get(VM->swarms, &sid, uint8_t) && + buzzdarray_get(e->swarms, 0, uint16_t) != sid) { + fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", + sid, + buzzdarray_get(e->swarms, 0, uint16_t)); + *status = 3; + return; + } + } + if(buzzdict_size(VM->swarms)>1) { + sid = 2; + if(*buzzdict_get(VM->swarms, &sid, uint8_t) && + buzzdarray_get(e->swarms, 0, uint16_t) != sid) { + fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", + sid, + buzzdarray_get(e->swarms, 0, uint16_t)); + *status = 3; + return; + } + } + } } + /*Step through the buzz script*/ + void update_sensors(){ + /* Update sensors*/ + buzzuav_closures::buzzuav_update_battery(VM); + buzzuav_closures::buzzuav_update_prox(VM); + buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::update_neighbors(VM); + update_users(); + buzzuav_closures::buzzuav_update_flight_status(VM); + } void buzz_script_step() { - /* - * Update sensors - */ - buzzuav_closures::buzzuav_update_battery(VM); - buzzuav_closures::buzzuav_update_prox(VM); - buzzuav_closures::buzzuav_update_currentpos(VM); - buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM); - buzzuav_closures::buzzuav_update_flight_status(VM); - - //buzz_update_users_stigmergy(tbl); - - - /* - * Call Buzz step() function - */ - if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { - fprintf(stderr, "%s: execution terminated abnormally: %s\n\n", + /*Process available messages*/ + in_message_process(); + /*Update sensors*/ + update_sensors(); + /* Call Buzz step() function */ + if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { + fprintf(stderr, "%s: execution terminated abnormally: %s\n\n", BO_FNAME, buzz_error_info()); - buzzvm_dump(VM); - } - //buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step - //usleep(10000); - /*Print swarm*/ - //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); - //int SwarmSize = buzzdict_size(VM->swarmmembers)+1; - //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); + buzzvm_dump(VM); + } + + /*Print swarm*/ + //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); + //int SwarmSize = buzzdict_size(VM->swarmmembers)+1; + //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); + set_robot_var(buzzdict_size(VM->swarmmembers)+1); - /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ -// int status = 1; -// buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); + /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ + //int status = 1; + //buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); } /****************************************/ @@ -553,6 +729,8 @@ buzzvm_dup(VM); buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::update_neighbors(VM); + update_users(); buzzuav_closures::buzzuav_update_flight_status(VM); int a = buzzvm_function_call(VM, "step", 0); @@ -563,26 +741,15 @@ buzzvm_dup(VM); return a == BUZZVM_STATE_READY; } -/* uint16_t get_robotid() { - // Get hostname - char hstnm[30]; - gethostname(hstnm, 30); - // Make numeric id from hostname - // NOTE: here we assume that the hostname is in the format Knn - int id = strtol(hstnm + 4, NULL, 10); - //fprintf(stdout, "Robot id from get rid buzz util: %i\n",id); - return (uint16_t)id; - }*/ - buzzvm_t get_vm() { return VM; } -void set_robot_var(int ROBOTS){ - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, ROBOTS); - buzzvm_gstore(VM); -} + void set_robot_var(int ROBOTS){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, ROBOTS); + buzzvm_gstore(VM); + } } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 5d1433f..c75357d 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -7,7 +7,7 @@ */ //#define _GNU_SOURCE #include "buzzuav_closures.h" -//#include "roscontroller.h" + namespace buzzuav_closures{ // TODO: Minimize the required global variables and put them in the header @@ -19,12 +19,15 @@ namespace buzzuav_closures{ static float batt[3]; static float obst[5]={0,0,0,0,0}; static double cur_pos[3]; - static double users_pos[3]; static uint8_t status; static int cur_cmd = 0; static int rc_cmd=0; static int buzz_cmd=0; static float height=0; + + + std::map< int, buzz_utility::Pos_struct> neighbors_map; + /****************************************/ /****************************************/ @@ -74,7 +77,7 @@ namespace buzzuav_closures{ /*----------------------------------------/ / Compute GPS destination from current position and desired Range and Bearing /----------------------------------------*/ - #define EARTH_RADIUS (double) 6371000.0 + void gps_from_rb(double range, double bearing, double out[3]) { double lat = cur_pos[0]*M_PI/180.0; double lon = cur_pos[1]*M_PI/180.0; @@ -85,6 +88,16 @@ namespace buzzuav_closures{ out[2] = height; //constant height. } + 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])); + out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); + out[1] = atan2(ned_y,ned_x); + out[2] = 0.0; + } + // Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336}; double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958}; @@ -105,17 +118,81 @@ namespace buzzuav_closures{ 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; + 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); printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); buzz_cmd= COMMAND_MOVETO; // TO DO what should we use return buzzvm_ret0(vm); } + int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) { + if(vm->state != BUZZVM_STATE_READY) return vm->state; + buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1)); + buzzvm_gload(vm); + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); + buzzobj_t nbr = buzzvm_stack_at(vm, 1); + /* Get "data" field */ + buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); + buzzvm_tget(vm); + if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) { + //ROS_INFO("Empty data, create a new table"); + buzzvm_pop(vm); + buzzvm_push(vm, nbr); + buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); + buzzvm_pusht(vm); + buzzobj_t data = buzzvm_stack_at(vm, 1); + buzzvm_tput(vm); + buzzvm_push(vm, data); + } + /* When we get here, the "data" table is on top of the stack */ + /* Push user id */ + buzzvm_pushi(vm, id); + /* Create entry table */ + buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); + /* Insert range */ + buzzvm_push(vm, entry); + buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1)); + buzzvm_pushf(vm, range); + buzzvm_tput(vm); + /* Insert longitude */ + buzzvm_push(vm, entry); + buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1)); + buzzvm_pushf(vm, bearing); + buzzvm_tput(vm); + /* Save entry into data table */ + buzzvm_push(vm, entry); + buzzvm_tput(vm); + //printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing); + return vm->state; + } + + int buzzuav_adduserRB(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); /* longitude */ + buzzvm_lload(vm, 2); /* latitude */ + buzzvm_lload(vm, 3); /* id */ + buzzvm_type_assert(vm, 3, BUZZTYPE_INT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + double tmp[3]; + tmp[0] = buzzvm_stack_at(vm, 2)->f.value; + tmp[1] = buzzvm_stack_at(vm, 1)->f.value; + tmp[2] = 0.0; + int uid = buzzvm_stack_at(vm, 3)->i.value; + double rb[3]; + + rb_from_gps(tmp, rb, cur_pos); + + //printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]); + + return users_add2localtable(vm, uid, rb[0], rb[1]); + } + /*----------------------------------------/ / Buzz closure to go directly to a GPS destination from the Mission Planner /----------------------------------------*/ @@ -245,11 +322,33 @@ namespace buzzuav_closures{ cur_pos[1]=longitude; cur_pos[2]=altitude; } - void set_userspos(double latitude, double longitude, double altitude){ - users_pos[0]=latitude; - users_pos[1]=longitude; - users_pos[2]=altitude; + /*adds neighbours position*/ + void neighbour_pos_callback(int id, float range, float bearing, float elevation){ + buzz_utility::Pos_struct pos_arr; + pos_arr.x=range; + pos_arr.y=bearing; + pos_arr.z=elevation; + map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id); + if(it!=neighbors_map.end()) + neighbors_map.erase(it); + neighbors_map.insert(make_pair(id, pos_arr)); } + + /* update at each step the VM table */ + void update_neighbors(buzzvm_t vm){ + /* Reset neighbor information */ + buzzneighbors_reset(vm); + /* Get robot id and update neighbor information */ + map< int, buzz_utility::Pos_struct >::iterator it; + for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){ + buzzneighbors_add(vm, + it->first, + (it->second).x, + (it->second).y, + (it->second).z); + } + } + /****************************************/ int buzzuav_update_currentpos(buzzvm_t vm) { buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); @@ -269,26 +368,6 @@ namespace buzzuav_closures{ buzzvm_gstore(vm); return vm->state; } - buzzobj_t buzzuav_update_userspos(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1)); - buzzvm_pushf(vm, users_pos[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1)); - buzzvm_pushf(vm, users_pos[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1)); - buzzvm_pushf(vm, users_pos[2]); - buzzvm_tput(vm); - buzzvm_gstore(vm); - - return buzzvm_stack_at(vm, 0); - //return vm->state; - } void flight_status_update(uint8_t state){ status=state; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index d4f0830..273c912 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -7,13 +7,20 @@ namespace rosbzz_node{ ---------------*/ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { + + home[0]=0.0;home[1]=0.0;home[2]=0.0; + target[0]=0.0;target[1]=0.0;target[2]=0.0; + cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0; + ROS_INFO("Buzz_node"); /*Obtain parameters from ros parameter server*/ Rosparameters_get(n_c_priv); /*Initialize publishers, subscribers and client*/ Initialize_pub_sub(n_c); /*Compile the .bzz file to .basm, .bo and .bdbg*/ - Compile_bzz(); + std::string fname = Compile_bzz(bzzfile_name); + bcfname = fname + ".bo"; + dbgfname = fname + ".bdb"; set_bzz_file(bzzfile_name.c_str()); /*Initialize variables*/ // Solo things @@ -22,14 +29,23 @@ namespace rosbzz_node{ multi_msg = true; // set stream rate - wait for the FC to be started SetStreamRate(0, 10, 1); - /// Get Robot Id - wait for Xbee to be started - if(xbeeplugged) - GetRobotId(); - else - robot_id=strtol(robot_name.c_str() + 5, NULL, 10);; + // Get Robot Id - wait for Xbee to be started + setpoint_counter = 0; fcu_timeout = TIMEOUT; - home[0]=0.0;home[1]=0.0;home[2]=0.0; + + while(cur_pos[2] == 0.0f){ + ROS_INFO("Waiting for GPS. "); + ros::Duration(0.5).sleep(); + ros::spinOnce(); + } + + + if(xbeeplugged){ + GetRobotId(); + } else { + robot_id= strtol(robot_name.c_str() + 5, NULL, 10); + } } /*--------------------- @@ -47,8 +63,9 @@ namespace rosbzz_node{ void roscontroller::GetRobotId() { + mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; - mavros_msgs::ParamGet::Response robot_id_srv_response; + mavros_msgs::ParamGet::Response robot_id_srv_response; while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ ros::Duration(0.1).sleep(); ROS_ERROR("Waiting for Xbee to respond to get device ID"); @@ -67,38 +84,39 @@ namespace rosbzz_node{ /* Set the Buzz bytecode */ if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { fprintf(stdout, "Bytecode file found and set\n"); - //init_update_monitor(bcfname.c_str(),stand_by.c_str()); + std::string standby_bo = Compile_bzz(stand_by) + ".bo"; + init_update_monitor(bcfname.c_str(),standby_bo.c_str()); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// while (ros::ok() && !buzz_utility::buzz_script_done()) { - /*Update neighbors position inside Buzz*/ - buzz_utility::neighbour_pos_callback(neighbours_pos_map); + /*Update neighbors position inside Buzz*/ + //buzz_closure::neighbour_pos_callback(neighbours_pos_map); /*Neighbours of the robot published with id in respective topic*/ neighbours_pos_publisher(); /*Check updater state and step code*/ - //update_routine(bcfname.c_str(), dbgfname.c_str()); + update_routine(bcfname.c_str(), dbgfname.c_str()); /*Step buzz script */ - buzz_utility::buzz_script_step(); + buzz_utility::buzz_script_step(); /*Prepare messages and publish them in respective topic*/ prepare_msg_and_publish(); /*call flight controler service to set command long*/ flight_controller_service_call(); /*Set multi message available after update*/ - /*if(get_update_status()){ + if(get_update_status()){ set_read_update_status(); multi_msg=true; - }*/ + } /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ //no_of_robots=get_number_of_robots(); get_number_of_robots(); //if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1; - buzz_utility::set_robot_var(no_of_robots); + //buzz_utility::set_robot_var(no_of_robots); /*Set no of robots for updates*/ updates_set_robots(no_of_robots); - /*run once*/ - ros::spinOnce(); + /*run once*/ + ros::spinOnce(); /*loop rate of ros*/ ros::Rate loop_rate(10); loop_rate.sleep(); @@ -107,10 +125,10 @@ namespace rosbzz_node{ else fcu_timeout -= 1/10; /*sleep for the mentioned loop rate*/ - timer_step+=1; + timer_step+=1; maintain_pos(timer_step); - - + + std::cout<< "HOME: " << home[0] << ", " << home[1]; } /* Destroy updater and Cleanup */ //update_routine(bcfname.c_str(), dbgfname.c_str(),1); @@ -150,7 +168,10 @@ namespace rosbzz_node{ if(!xbeeplugged){ if(n_c.getParam("name", robot_name)); 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? @@ -218,8 +239,8 @@ namespace rosbzz_node{ 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); + if(rcclient==true) + 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); @@ -257,28 +278,35 @@ namespace rosbzz_node{ /*-------------------------------------------------------- / Create Buzz bytecode from the bzz script inputed /-------------------------------------------------------*/ - void roscontroller::Compile_bzz(){ + 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<::iterator it = neighbours_pos_map.find(id); if(it!=neighbours_pos_map.end()) - neighbours_pos_map.erase(it); + neighbours_pos_map.erase(it); neighbours_pos_map.insert(make_pair(id, pos_arr)); } /*----------------------------------------------------------------------------------- @@ -499,7 +533,7 @@ namespace rosbzz_node{ void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){ map< int, buzz_utility::Pos_struct >::iterator it = raw_neighbours_pos_map.find(id); if(it!=raw_neighbours_pos_map.end()) - raw_neighbours_pos_map.erase(it); + raw_neighbours_pos_map.erase(it); raw_neighbours_pos_map.insert(make_pair(id, pos_arr)); } @@ -509,18 +543,15 @@ namespace rosbzz_node{ void roscontroller::set_cur_pos(double latitude, double longitude, double altitude){ - cur_pos [0] =latitude; - cur_pos [1] =longitude; - cur_pos [2] =altitude; + cur_pos [0] = latitude; + cur_pos [1] = longitude; + cur_pos [2] = altitude; } /*----------------------------------------------------------- / Compute Range and Bearing of a neighbor in a local reference frame / from GPS coordinates ----------------------------------------------------------- */ - #define EARTH_RADIUS (double) 6371000.0 - #define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0))) - void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) { double hyp_az, hyp_el; double sin_el, cos_el, sin_az, cos_az; @@ -557,95 +588,92 @@ namespace rosbzz_node{ void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){ - // calculate earth radii - double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); - double prime_vertical_radius = equatorial_radius * sqrt(temp); - double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; - double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)); + // calculate earth radii + /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); + double prime_vertical_radius = equatorial_radius * sqrt(temp); + double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; + double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));*/ /*double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - double ned[3]; - ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; - ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS - double ecef[3]; - double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2]; - double d = WGS84_E * sin(llh[0]); - double N = WGS84_A / sqrt(1. - d*d); - ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double ref_ecef[3]; - llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2]; - d = WGS84_E * sin(llh[0]); - N = WGS84_A / sqrt(1. - d*d); - ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double M[3][3]; - ecef2ned_matrix(ref_ecef, M); - double ned[3]; - matrix_multiply(3, 3, 1, (double *)M, ecef, ned); - - out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]); - out[0] = std::floor(out[0] * 1000000) / 1000000; + double d_lat = nei[0] - cur[0]; + double ned[3]; + ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; + ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS + double ecef[3]; + double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2]; + double d = WGS84_E * sin(llh[0]); + double N = WGS84_A / sqrt(1. - d*d); + ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); + ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); + ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); + double ref_ecef[3]; + llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2]; + d = WGS84_E * sin(llh[0]); + N = WGS84_A / sqrt(1. - d*d); + ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); + ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); + ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); + double M[3][3]; + ecef2ned_matrix(ref_ecef, M); + double ned[3]; + matrix_multiply(3, 3, 1, (double *)M, ecef, ned); + + out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]); + out[0] = std::floor(out[0] * 1000000) / 1000000; out[1] = atan2(ned[1],ned[0]); out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = 0.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])); + 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[0] = std::floor(out[0] * 1000000) / 1000000; + //out[0] = std::floor(out[0] * 1000000) / 1000000; out[1] = atan2(ned_y,ned_x); - //out[1] = std::floor(out[1] * 1000000) / 1000000; + //out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = 0.0; - - - } void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){ - // calculate earth radii - /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); - double prime_vertical_radius = equatorial_radius * sqrt(temp); - double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; - double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)); - - double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; - out[0] = std::floor(out[0] * 1000000) / 1000000; - out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS - out[1] = std::floor(out[1] * 1000000) / 1000000; + // calculate earth radii + /*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE))); + double prime_vertical_radius = equatorial_radius * sqrt(temp); + double radius_north = prime_vertical_radius * (1 - excentrity2) * temp; + double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)); + + double d_lon = nei[1] - cur[1]; + double d_lat = nei[0] - cur[0]; + out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS; + out[0] = std::floor(out[0] * 1000000) / 1000000; + out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS + out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = cur[2]; - // Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav) - double ecef[3]; - double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2]; - double d = WGS84_E * sin(llh[0]); - double N = WGS84_A / sqrt(1. - d*d); - ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double ref_ecef[3]; - llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2]; - d = WGS84_E * sin(llh[0]); - N = WGS84_A / sqrt(1. - d*d); - ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); - ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); - ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); - double M[3][3]; - ecef2ned_matrix(ref_ecef, M); - matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/ - - double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - out[0] = DEG2RAD(d_lat) * EARTH_RADIUS; - //out[0] = std::floor(out[0] * 1000000) / 1000000; - out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); - //out[1] = std::floor(out[1] * 1000000) / 1000000; + // Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav) + double ecef[3]; + double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2]; + double d = WGS84_E * sin(llh[0]); + double N = WGS84_A / sqrt(1. - d*d); + ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); + ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); + ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); + double ref_ecef[3]; + llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2]; + d = WGS84_E * sin(llh[0]); + N = WGS84_A / sqrt(1. - d*d); + ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); + ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); + ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]); + double M[3][3]; + ecef2ned_matrix(ref_ecef, M); + matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/ + + double d_lon = nei[1] - cur[1]; + double d_lat = nei[0] - cur[0]; + out[0] = DEG2RAD(d_lat) * EARTH_RADIUS; + //out[0] = std::floor(out[0] * 1000000) / 1000000; + out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + //out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = 0.0; } @@ -709,7 +737,8 @@ namespace rosbzz_node{ us[2] = data.pos_neigh[it].altitude; double out[3]; cvt_rangebearing_coordinates(us, out, cur_pos); - buzzuav_closures::set_userspos(out[0], out[1], out[2]); + //buzzuav_closures::set_userspos(out[0], out[1], out[2]); + buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude); } } @@ -743,12 +772,14 @@ namespace rosbzz_node{ moveMsg.header.frame_id = 1; double local_pos[3]; cvt_ned_coordinates(cur_pos,local_pos,home); - 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 Home: %.7f, %.7f", robot_id, home[0], home[1]); + // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ - moveMsg.pose.position.x = local_pos[1]+y; - moveMsg.pose.position.y = local_pos[0]+x; + target[0]+=y; + target[1]+=x; + moveMsg.pose.position.x = target[0];//local_pos[1]+y; + moveMsg.pose.position.y = target[1]; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0; @@ -757,7 +788,7 @@ namespace rosbzz_node{ moveMsg.pose.orientation.w = 1; // To prevent drifting from stable position. - if(fabs(x)>0.1 || fabs(y)>0.1) { + if(fabs(x)>0.01 || fabs(y)>0.01) { localsetpoint_nonraw_pub.publish(moveMsg); ROS_INFO("Sent local NON RAW position message!"); } @@ -780,6 +811,7 @@ namespace rosbzz_node{ } } + void roscontroller::SetStreamRate(int id, int rate, int on_off){ mavros_msgs::StreamRate message; message.request.stream_id = id; @@ -851,10 +883,12 @@ namespace rosbzz_node{ /*pass neighbour position to local maintaner*/ buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]); /*Put RID and pos*/ - raw_neighbours_pos_put((int)out[1],raw_neigh_pos); + raw_neighbours_pos_put((int)out[1],raw_neigh_pos); + /* TODO: remove roscontroller local map array for neighbors */ neighbours_pos_put((int)out[1],n_pos); + buzzuav_closures::neighbour_pos_callback((int)out[1],n_pos.x,n_pos.y,n_pos.z); delete[] out; - buzz_utility::in_msg_process((message_obt+3)); + buzz_utility::in_msg_append((message_obt+3)); } } diff --git a/src/stand_by.basm b/src/stand_by.basm deleted file mode 100644 index 4033176..0000000 --- a/src/stand_by.basm +++ /dev/null @@ -1,144 +0,0 @@ -!19 -'updated -'update_ack -'init -'barrier -'stigmergy -'create -'put -'id -'barrier_val -'onconflict -'data -'stand_by -'get -'size -'neighbors -'listen -'update_no -'ROBOTS -'step - - pushs 2 - pushcn @__label_1 - gstore - pushs 11 - pushcn @__label_5 - gstore - pushs 18 - pushcn @__label_9 - gstore - nop - -@__label_0 - pushs 0 |1,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 1 |1,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gstore |1,20,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - -@__exitpoint - done - -@__label_1 - pushs 3 |4,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 4 |4,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |4,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 5 |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |4,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 101 |4,27,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 1 |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gstore |4,31,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 3 |5,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |5,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 6 |5,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |5,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 7 |5,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |5,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 1 |5,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 2 |5,17,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |5,17,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 8 |6,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 0 |6,12,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gstore |6,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 3 |7,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |7,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 9 |7,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |7,18,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushl @__label_2 |7,19,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 1 |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret0 |12,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - -@__label_2 - lload 3 |8,4,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 10 |8,6,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |8,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lload 2 |8,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 10 |8,16,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |8,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lt |8,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lload 3 |8,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 10 |8,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |8,33,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lload 2 |8,37,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 10 |8,39,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |8,44,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - eq |8,44,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - or |8,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - jumpz @__label_3 |8,47,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lload 2 |8,55,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret1 |8,55,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - jump @__label_4 |9,5,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz -@__label_3 |9,5,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - lload 3 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret1 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz -@__label_4 |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret0 |10,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - -@__label_5 - pushs 3 |16,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |16,7,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 12 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |16,11,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 7 |16,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |16,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 1 |16,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |16,15,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 8 |17,12,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 3 |17,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |17,21,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 13 |17,22,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |17,26,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 0 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gstore |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 14 |19,9,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |19,9,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 15 |19,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - tget |19,16,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 0 |19,24,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |19,24,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushl @__label_6 |20,3,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 2 |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |23,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret0 |25,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - -@__label_6 - lload 2 |21,14,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 16 |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - eq |21,25,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - jumpz @__label_7 |21,27,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 8 |21,38,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushs 17 |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gstore |21,45,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz -@__label_7 |22,3,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret0 |22,4,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - -@__label_9 - pushs 11 |30,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - gload |30,8,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - pushi 0 |30,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - callc |30,10,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz - ret0 |32,1,/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bzz diff --git a/src/stand_by.bdb b/src/stand_by.bdb deleted file mode 100644 index 72f975b..0000000 Binary files a/src/stand_by.bdb and /dev/null differ diff --git a/src/stand_by.bdbg b/src/stand_by.bdbg deleted file mode 100644 index cda97bc..0000000 Binary files a/src/stand_by.bdbg and /dev/null differ diff --git a/src/stand_by.bo b/src/stand_by.bo deleted file mode 100644 index 62e6638..0000000 Binary files a/src/stand_by.bo and /dev/null differ