From 61d31e672da52e84cf7969f10eac07e12aa0efb7 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 11 May 2017 21:17:50 -0400 Subject: [PATCH 1/3] change gps mgt --- include/buzz_utility.h | 6 -- include/roscontroller.h | 34 +++++----- src/roscontroller.cpp | 147 ++++++++++++---------------------------- 3 files changed, 63 insertions(+), 124 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index aa5cb2a..381b417 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -21,12 +21,6 @@ struct pos_struct pos_struct(){} }; -#define function_register(TABLE, FNAME, FPOINTER) \ - buzzvm_push(VM, (TABLE)); \ - buzzvm_pushs(VM, buzzvm_string_register(VM, (FNAME), 1)); \ - buzzvm_pushcc(VM, buzzvm_function_register(VM, (FPOINTER))); \ - buzzvm_tput(VM); - typedef struct pos_struct Pos_struct ; uint16_t* u64_cvt_u16(uint64_t u64); diff --git a/include/roscontroller.h b/include/roscontroller.h index 5cc6e63..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,18 +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 target[3]; - 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; @@ -159,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); @@ -178,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/src/roscontroller.cpp b/src/roscontroller.cpp index b5832c2..300fa2f 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -7,10 +7,6 @@ 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; - cur_pos[0]=0.0;cur_pos[1]=0.0;cur_pos[2]=0.0; - target[0]=0.0;target[1]=0.0;target[2]=0.0; - ROS_INFO("Buzz_node"); /*Obtain parameters from ros parameter server*/ Rosparameters_get(n_c_priv); @@ -348,7 +344,8 @@ namespace rosbzz_node{ uint64_t* payload_out_ptr= buzz_utility::obt_out_msg(); uint64_t position[3]; /*Appened current position to message*/ - memcpy(position, cur_pos, 3*sizeof(uint64_t)); + double tmp[3];tmp[0]=cur_pos.latitude;tmp[1]=cur_pos.longitude;tmp[2]=cur_pos.altitude; + memcpy(position, tmp, 3*sizeof(uint64_t)); mavros_msgs::Mavlink payload_out; payload_out.payload64.push_back(position[0]); payload_out.payload64.push_back(position[1]); @@ -440,8 +437,8 @@ namespace rosbzz_node{ armstate = 1; Arm(); ros::Duration(0.5).sleep(); - // Registering HOME POINT. - home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2]; + // Registering HOME POINT. + home = cur_pos; } if(current_mode != "GUIDED") SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm) @@ -521,9 +518,9 @@ 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.latitude =latitude; + cur_pos.longitude =longitude; + cur_pos.altitude =altitude; } /*----------------------------------------------------------- @@ -564,48 +561,10 @@ 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));*/ - - /*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; - 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])); + void roscontroller::gps_rb(GPS nei_pos, double out[]) + { + float ned_x=0.0, ned_y=0.0; + gps_ned_cur(ned_x, ned_y, nei_pos); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); //out[0] = std::floor(out[0] * 1000000) / 1000000; out[1] = atan2(ned_y,ned_x); @@ -613,48 +572,30 @@ namespace rosbzz_node{ 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; - 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; - out[2] = 0.0; + void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t) + { + gps_convert_ned(ned_x, ned_y, + t.longitude, t.latitude, + cur_pos.longitude, cur_pos.latitude); } + void roscontroller::gps_ned_home(float &ned_x, float &ned_y) + { + gps_convert_ned(ned_x, ned_y, + cur_pos.longitude, cur_pos.latitude, + home.longitude, home.latitude); + } + + void roscontroller::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) + { + double d_lon = gps_t_lon - gps_r_lon; + double d_lat = gps_t_lat - gps_r_lat; + ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); + }; + /*------------------------------------------------------ / Update battery status into BVM from subscriber /------------------------------------------------------*/ @@ -713,9 +654,7 @@ namespace rosbzz_node{ us[0] = data.pos_neigh[it].latitude; us[1] = data.pos_neigh[it].longitude; 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]); + 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); } @@ -748,15 +687,15 @@ namespace rosbzz_node{ moveMsg.header.stamp = ros::Time::now(); moveMsg.header.seq = setpoint_counter++; moveMsg.header.frame_id = 1; - double local_pos[3]; - cvt_ned_coordinates(cur_pos,local_pos,home); + float ned_x, ned_y; + gps_ned_home(ned_x, ned_y); // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]); // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ - target[0]+=y; target[1]+=x; - moveMsg.pose.position.x = target[0];//local_pos[1]+y; - moveMsg.pose.position.y = target[1];//local_pos[0]+x; + //target[0]+=y; target[1]+=x; + moveMsg.pose.position.x = ned_y+y; + moveMsg.pose.position.y = ned_x+x; moveMsg.pose.position.z = z; moveMsg.pose.orientation.x = 0; @@ -850,9 +789,13 @@ namespace rosbzz_node{ double neighbours_pos_payload[3]; memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t)); buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]); - // cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; + GPS nei_pos; + nei_pos.latitude=neighbours_pos_payload[0]; + nei_pos.longitude=neighbours_pos_payload[1]; + nei_pos.altitude=neighbours_pos_payload[2]; double cvt_neighbours_pos_payload[3]; - cvt_rangebearing_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload, cur_pos); + // cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; + gps_rb(nei_pos, cvt_neighbours_pos_payload); /*Extract robot id of the neighbour*/ uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3)); cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl; From 664e7e4dc00fb8a5222ffc9ffa031ecf16f544ed Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 11 May 2017 22:12:59 -0400 Subject: [PATCH 2/3] changes to the updater --- include/buzz_update.h | 2 + script/stand_by.bzz | 2 + script/test1.bzz | 1 - script/update_sim/include/string.bzz | 92 ++++++++ script/update_sim/include/vec2.bzz | 107 +++++++++ ...estflockfev4.bzz => old_testflockfev1.bzz} | 1 + ...estflockfev3.bzz => old_testflockfev2.bzz} | 0 script/update_sim/testflockfev1.bzz | 44 +++- script/update_sim/testflockfev2.bzz | 42 +++- script/update_sim/testflockfev5.bzz | 208 ------------------ script/update_sim/testflockfev6.bzz | 208 ------------------ script/update_sim/testflockfev7.bzz | 208 ------------------ src/buzz_update.cpp | 88 ++++---- src/buzz_utility.cpp | 52 +++-- src/roscontroller.cpp | 7 +- 15 files changed, 362 insertions(+), 700 deletions(-) create mode 100644 script/update_sim/include/string.bzz create mode 100644 script/update_sim/include/vec2.bzz rename script/update_sim/{testflockfev4.bzz => old_testflockfev1.bzz} (99%) rename script/update_sim/{testflockfev3.bzz => old_testflockfev2.bzz} (100%) delete mode 100644 script/update_sim/testflockfev5.bzz delete mode 100644 script/update_sim/testflockfev6.bzz delete mode 100644 script/update_sim/testflockfev7.bzz 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/script/stand_by.bzz b/script/stand_by.bzz index bf2ac78..d51e99e 100644 --- a/script/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/script/test1.bzz b/script/test1.bzz index 3e9d1fb..7debdf4 100644 --- a/script/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/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/testflockfev4.bzz b/script/update_sim/old_testflockfev1.bzz similarity index 99% rename from script/update_sim/testflockfev4.bzz rename to script/update_sim/old_testflockfev1.bzz index 1845eb9..02abd4c 100644 --- a/script/update_sim/testflockfev4.bzz +++ b/script/update_sim/old_testflockfev1.bzz @@ -1,4 +1,5 @@ # 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/update_sim/testflockfev3.bzz b/script/update_sim/old_testflockfev2.bzz similarity index 100% rename from script/update_sim/testflockfev3.bzz rename to script/update_sim/old_testflockfev2.bzz diff --git a/script/update_sim/testflockfev1.bzz b/script/update_sim/testflockfev1.bzz index 1845eb9..e628847 100644 --- a/script/update_sim/testflockfev1.bzz +++ b/script/update_sim/testflockfev1.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 @@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.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) { @@ -26,7 +26,7 @@ function lj_magnitude(dist, target, epsilon) { # Neighbor data to LJ interaction vector function lj_vector(rid, data) { return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) -} +}cd sr # Accumulator of neighbor LJ interactions function lj_sum(rid, data, accum) { @@ -48,9 +48,12 @@ function hexagon() { # 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.0) +# uav_moveto(0.0,0.06) # } } @@ -142,11 +145,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 +221,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/update_sim/testflockfev2.bzz b/script/update_sim/testflockfev2.bzz index 1845eb9..06b3e32 100644 --- a/script/update_sim/testflockfev2.bzz +++ b/script/update_sim/testflockfev2.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 @@ -15,8 +15,8 @@ TARGET_ALTITUDE = 3.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) { @@ -48,9 +48,12 @@ function hexagon() { # 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.0) +# uav_moveto(0.0,0.06) # } } @@ -142,11 +145,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 +221,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/update_sim/testflockfev5.bzz b/script/update_sim/testflockfev5.bzz deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev5.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# 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 deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev6.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# 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 deleted file mode 100644 index 1845eb9..0000000 --- a/script/update_sim/testflockfev7.bzz +++ /dev/null @@ -1,208 +0,0 @@ -# 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 3a77dba..69d9ec8 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -29,7 +29,7 @@ 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"); + ROS_INFO("intiialized file monitor.\n"); fd=inotify_init1(IN_NONBLOCK); if ( fd < 0 ) { perror( "inotify_init error" ); @@ -48,7 +48,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ BO_BUF = (uint8_t*)malloc(bcode_size); if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { perror(bo_filename); - fclose(fp); + //fclose(fp); //return 0; } fclose(fp); @@ -65,7 +65,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){ 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); + //fclose(fp); //return 0; } fclose(fp); @@ -147,7 +147,7 @@ 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); + //ROS_INFO("[DEBUG] Updater append code of 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); @@ -156,9 +156,9 @@ void code_message_inqueue_append(uint8_t* msg,uint16_t 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)) ) ); + ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) ); + ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) ); + ROS_INFO("[Debug] Updater received code of 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"); @@ -191,7 +191,6 @@ void update_routine(const char* bcfname, const char* dbgfname){ 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); @@ -199,37 +198,13 @@ void update_routine(const char* bcfname, if(*(int*)updater->mode==CODE_RUNNING){ buzzvm_function_call(VM, "updated_neigh", 0); if(check_update()){ - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")); - bzzfile_in_compile<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)); + ROS_INFO("Current 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"); + ROS_INFO("Sending code... \n"); code_message_outqueue_append(); } delete_p(BO_BUF); @@ -268,7 +243,7 @@ void update_routine(const char* bcfname, else{ //gettimeofday(&t1, NULL); if(neigh==0 && (!is_msg_present())){ - fprintf(stdout,"Sending code... \n"); + ROS_INFO("Sending code... \n"); code_message_outqueue_append(); } @@ -277,7 +252,7 @@ void update_routine(const char* bcfname, buzzvm_gload(VM); buzzobj_t tObj = buzzvm_stack_at(VM, 1); buzzvm_pop(VM); - fprintf(stdout,"Barrier ..................... %i \n",tObj->i.value); + ROS_INFO("Barrier ..................... %i \n",tObj->i.value); if(tObj->i.value==no_of_robot) { *(int*)(updater->mode) = CODE_RUNNING; gettimeofday(&t2, NULL); @@ -307,12 +282,12 @@ return (uint8_t*)updater->outmsg_queue->size; int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){ - fprintf(stdout,"Initializtion of script test passed\n"); + ROS_WARN("Initializtion of script test passed\n"); if(buzz_utility::update_step_test()){ /*data logging*/ //start =1; /*data logging*/ - fprintf(stdout,"Step test passed\n"); + ROS_WARN("Step test passed\n"); *(int*) (updater->mode) = CODE_STANDBY; //fprintf(stdout,"updater value = %i\n",updater->mode); delete_p(updater->bcode); @@ -330,12 +305,12 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ /*Unable to step something wrong*/ else{ if(*(int*) (updater->mode) == CODE_RUNNING){ - fprintf(stdout,"step test failed, stick to old script\n"); + ROS_ERROR("step test failed, stick to old script\n"); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t)*(size_t*)(updater->bcode_size)); } else{ /*You will never reach here*/ - fprintf(stdout,"step test failed, Return to stand by\n"); + ROS_ERROR("step test failed, Return to stand by\n"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -350,12 +325,12 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ } else { if(*(int*) (updater->mode) == CODE_RUNNING){ - fprintf(stdout,"Initialization test failed, stick to old script\n"); + ROS_ERROR("Initialization test failed, stick to old script\n"); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,(int)*(size_t*) (updater->bcode_size)); } else{ /*You will never reach here*/ - fprintf(stdout,"Initialization test failed, Return to stand by\n"); + ROS_ERROR("Initialization test failed, Return to stand by\n"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -420,6 +395,23 @@ void updates_set_robots(int robots){ no_of_robot=robots; } +/*-------------------------------------------------------- +/ Create Buzz bytecode from the bzz script inputed +/-------------------------------------------------------*/ +int compile_bzz(){ + /*Compile the buzz code .bzz to .bo*/ + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0,name.find_last_of(".")); + bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<state); + return 0; + } + // Call the Init() function + if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } /* All OK */ - ROS_INFO("[%i] INIT DONE!!!", Robot_id); + ROS_INFO("[%i] INIT DONE!!!", Robot_id); return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size); } @@ -554,10 +561,17 @@ static int create_stig_tables() { //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; return 0; } + // Execute the global part of the script - buzzvm_execute_script(VM); + if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ + ROS_ERROR("Error executing global part, VM state : %i",VM->state); + return 0; + } // Call the Init() function - buzzvm_function_call(VM, "init", 0); + if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } // All OK return 1; } @@ -604,9 +618,15 @@ static int create_stig_tables() { return 0; } // Execute the global part of the script - buzzvm_execute_script(VM); + if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ + ROS_ERROR("Error executing global part, VM state : %i",VM->state); + return 0; + } // Call the Init() function - buzzvm_function_call(VM, "init", 0); + if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } // All OK return 1; } @@ -725,19 +745,25 @@ static int create_stig_tables() { } int update_step_test() { - + /*Process available messages*/ + in_message_process(); 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); + //set_robot_var(buzzdict_size(VM->swarmmembers)+1); int a = buzzvm_function_call(VM, "step", 0); - if(a != BUZZVM_STATE_READY) { + + if(a!= BUZZVM_STATE_READY) { + ROS_ERROR("%s: execution terminated abnormally: %s\n\n", + BO_FNAME, + buzz_error_info()); fprintf(stdout, "step test VM state %i\n",a); - fprintf(stdout, " execution terminated abnormally\n\n"); - } + } + return a == BUZZVM_STATE_READY; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index b5832c2..2160bb1 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -101,10 +101,11 @@ namespace rosbzz_node{ 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); - /*Set no of robots for updates*/ + /*Set no of robots for updates TODO only when not updating*/ + //if(multi_msg) 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(); From a99b889b163e4bd462c80d29feeaf2895e98f0a0 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 11 May 2017 22:18:50 -0400 Subject: [PATCH 3/3] correction to sim flockfev --- script/update_sim/testflockfev1.bzz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/script/update_sim/testflockfev1.bzz b/script/update_sim/testflockfev1.bzz index e628847..06b3e32 100644 --- a/script/update_sim/testflockfev1.bzz +++ b/script/update_sim/testflockfev1.bzz @@ -26,7 +26,7 @@ function lj_magnitude(dist, target, epsilon) { # Neighbor data to LJ interaction vector function lj_vector(rid, data) { return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth) -}cd sr +} # Accumulator of neighbor LJ interactions function lj_sum(rid, data, accum) {