From 61d1cab41b63079dd0712d9d0bdec07cfc88128f Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 24 Apr 2017 14:20:59 -0400 Subject: [PATCH] implement vstig users --- include/buzz_utility.h | 2 ++ include/buzzuav_closures.h | 3 +- include/roscontroller.h | 15 ++------ src/buzz_utility.cpp | 49 ++++++++++++++++++++++++- src/buzzuav_closures.cpp | 26 ++++++++++++++ src/roscontroller.cpp | 74 ++++++++++++++++++++++++++++++-------- src/testflocksim.bzz | 10 ++++-- 7 files changed, 148 insertions(+), 31 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index e276e76..3667363 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -30,6 +30,8 @@ int buzz_listen(const char* type, void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map); +int buzz_update_users_stigmergy(buzzobj_t tbl); + void in_msg_process(uint64_t* payload); uint64_t* out_msg_process(); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index dcfa26a..eff6835 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -46,6 +46,7 @@ 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*/ @@ -82,7 +83,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); /* * Updates flight status and rc command in Buzz, put it in a tabel to acess it diff --git a/include/roscontroller.h b/include/roscontroller.h index 05b28b5..802306f 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -39,19 +39,6 @@ #define XBEE_STOP_TRANSMISSION 4355356352 #define TIMEOUT 60 -/** Semi-major axis of the Earth, \f$ a \f$, in meters. - * This is a defining parameter of the WGS84 ellipsoid. */ -#define WGS84_A 6378137.0 -/** Inverse flattening of the Earth, \f$ 1/f \f$. - * This is a defining parameter of the WGS84 ellipsoid. */ -#define WGS84_IF 298.257223563 -/** The flattening of the Earth, \f$ f \f$. */ -#define WGS84_F (1/WGS84_IF) -/** Semi-minor axis of the Earth in meters, \f$ b = a(1-f) \f$. */ -#define WGS84_B (WGS84_A*(1-WGS84_F)) -/** Eccentricity of the Earth, \f$ e \f$ where \f$ e^2 = 2f - f^2 \f$ */ -#define WGS84_E (sqrt(2*WGS84_F - WGS84_F*WGS84_F)) - using namespace std; namespace rosbzz_node{ @@ -117,6 +104,7 @@ private: ros::Publisher localsetpoint_nonraw_pub; ros::ServiceServer service; ros::Subscriber current_position_sub; + ros::Subscriber users_sub; ros::Subscriber battery_sub; ros::Subscriber payload_sub; ros::Subscriber flight_status_sub; @@ -189,6 +177,7 @@ private: /*current position callback*/ void current_pos(const sensor_msgs::NavSatFix::ConstPtr& 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/buzz_utility.cpp b/src/buzz_utility.cpp index 01769fd..c9cbd69 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -20,6 +20,8 @@ namespace buzz_utility{ static uint8_t MSG_SIZE = 50; // 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; /****************************************/ /*adds neighbours position*/ @@ -302,6 +304,30 @@ namespace buzz_utility{ fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename); 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); /* Execute the global part of the script */ @@ -309,9 +335,25 @@ namespace buzz_utility{ /* Call the Init() function */ buzzvm_function_call(VM, "init", 0); /* All OK */ + 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 */ /****************************************/ @@ -447,7 +489,12 @@ namespace buzz_utility{ 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 */ diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index ae7557e..5d1433f 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -19,6 +19,7 @@ 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; @@ -244,6 +245,11 @@ 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; + } /****************************************/ int buzzuav_update_currentpos(buzzvm_t vm) { buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); @@ -263,6 +269,26 @@ 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 70efbe0..401f3d8 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -223,7 +223,9 @@ namespace rosbzz_node{ mav_client = n_c.serviceClient("/"+robot_name+fcclient_name); xbeestatus_srv = n_c.serviceClient("/"+robot_name+xbeesrv_name); stream_client = n_c.serviceClient("/"+robot_name+stream_client_name); - + + users_sub = n_c.subscribe("/"+robot_name+"/users_pos", 1000, &roscontroller::users_pos,this); + multi_msg=true; } /*--------------------------------------- @@ -424,6 +426,8 @@ namespace rosbzz_node{ Arm(); ros::Duration(0.5).sleep(); } + // Registering HOME POINT. + home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2]; if(current_mode != "GUIDED") SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm) if (mav_client.call(cmd_srv)) @@ -578,20 +582,32 @@ namespace rosbzz_node{ double M[3][3]; ecef2ned_matrix(ref_ecef, M); double ned[3]; - matrix_multiply(3, 3, 1, (double *)M, ecef, ned);*/ + matrix_multiply(3, 3, 1, (double *)M, ecef, ned); + + 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[1] = atan2(ned_y,ned_x); + out[1] = std::floor(out[1] * 1000000) / 1000000; + out[2] = 0.0;*/ + 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; + } 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)); + // 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]; @@ -617,7 +633,15 @@ namespace rosbzz_node{ 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);*/ + 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;*/ } /*------------------------------------------------------ @@ -657,14 +681,35 @@ namespace rosbzz_node{ fcu_timeout = TIMEOUT; double lat = std::floor(msg->latitude * 1000000) / 1000000; double lon = std::floor(msg->longitude * 1000000) / 1000000; - if(cur_rel_altitude<0.2){ + /*if(cur_rel_altitude<1.2){ home[0]=lat; home[1]=lon; home[2]=cur_rel_altitude; - } + }*/ set_cur_pos(lat,lon, cur_rel_altitude);//msg->altitude); buzzuav_closures::set_currentpos(lat,lon, cur_rel_altitude);//msg->altitude); } + void roscontroller::users_pos(const rosbuzz::neigh_pos data){ + //ROS_INFO("Altitude out: %f", cur_rel_altitude); + + double us[3]; + int n = data.pos_neigh.size(); + // ROS_INFO("Neighbors array size: %i\n", n); + if(n>0) + { + for(int it=0; it0.01 || fabs(y)>0.01) { + // To prevent drifting from stable position. + if(fabs(x)>0.01 || fabs(y)>0.01) { localsetpoint_nonraw_pub.publish(moveMsg); ROS_INFO("Sent local NON RAW position message!"); - //} + } } void roscontroller::SetMode(std::string mode, int delay_miliseconds){ diff --git a/src/testflocksim.bzz b/src/testflocksim.bzz index 21ca80b..695c6cb 100644 --- a/src/testflocksim.bzz +++ b/src/testflocksim.bzz @@ -11,11 +11,11 @@ function updated_neigh(){ neighbors.broadcast(updated, update_no) } -TARGET_ALTITUDE = 5.0 +TARGET_ALTITUDE = 10.0 CURSTATE = "TURNEDOFF" # Lennard-Jones parameters -TARGET = 10.0 +TARGET = 12.0 EPSILON = 8.0 # Lennard-Jones interaction magnitude @@ -196,6 +196,12 @@ 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) + # Get the number of keys in the structure + #log("The vstig has ", usersvstig.size(), " elements") } # Executed once when the robot (or the simulator) is reset.