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