diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index a58c367..38229c4 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -6,7 +6,8 @@ #include "uav_utility.h" #include "mavros_msgs/CommandCode.h" #include "ros/ros.h" - +#include "buzz_utility.h" +//#include "roscontroller.h" namespace buzzuav_closures{ /* @@ -46,8 +47,13 @@ void set_obstacle_dist(float dist[]); * Commands the UAV to takeoff */ int buzzuav_takeoff(buzzvm_t vm); +/* + * Arm command from Buzz + */ int buzzuav_arm(buzzvm_t vm); - +/* + * Disarm from buzz + */ int buzzuav_disarm(buzzvm_t vm) ; /* Commands the UAV to land */ @@ -66,8 +72,6 @@ int buzzuav_update_battery(buzzvm_t vm); */ int buzzuav_update_currentpos(buzzvm_t vm); -/*update obstacles in Buzz*/ -int buzzuav_update_obstacle(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 7e52e29..ee6ffe7 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -83,7 +83,8 @@ private: mavros_msgs::SetMode m_cmdSetMode; ros::ServiceClient mode_client; - + + /*Initialize publisher and subscriber, done in the constructor*/ void Initialize_pub_sub(ros::NodeHandle n_c); /*Obtain data from ros parameter server*/ @@ -92,6 +93,12 @@ private: /*compiles buzz script from the specified .bzz file*/ void Compile_bzz(); + /*Flight controller service call*/ + void flight_controler_service_call(); + + /*Neighbours pos publisher*/ + void neighbours_pos_publisher(); + /*Prepare messages and publish*/ void prepare_msg_and_publish(); @@ -133,14 +140,19 @@ private: /*robot id sub callback*/ void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); + /*Obstacle distance table callback*/ void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg); + /*Get publisher and subscriber from YML file*/ void GetSubscriptionParameters(ros::NodeHandle node_handle); + /*Arm/disarm method that can be called from buzz*/ void Arm(); + /*set mode like guided for solo*/ void SetMode(); + /*Robot independent subscribers*/ void Subscribe(ros::NodeHandle n_c); }; diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch index 7180e5b..5bc52f4 100644 --- a/launch/rosbuzz_2_parallel.launch +++ b/launch/rosbuzz_2_parallel.launch @@ -2,7 +2,7 @@ - + diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 9c88de0..995e33a 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -76,31 +76,8 @@ namespace buzz_utility{ memcpy(pl, payload ,size); /*size and robot id read*/ size_t tot = sizeof(uint32_t); - /*for(int i=0;i0){ - // code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize); - // tot+=unMsgSize; - //fprintf(stdout,"before in queue process : utils\n"); - // code_message_inqueue_process(); - //fprintf(stdout,"after in queue process : utils\n"); - // } - //} - //unMsgSize=0; /*Obtain Buzz messages only when they are present*/ do { @@ -131,30 +108,6 @@ namespace buzz_utility{ /* Send robot id */ *(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot; tot += sizeof(uint16_t); - //uint8_t updater_msg_pre = 0; - //uint16_t updater_msgSize= 0; - //if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){ - // fprintf(stdout,"transfer code \n"); - // updater_msg_pre =1; - //transfer_code=0; - // *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre; - // tot += sizeof(uint8_t); - /*Append updater msg size*/ - //*(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size()); - // updater_msgSize=*(uint16_t*) (getupdate_out_msg_size()); - // *(uint16_t*)(buff_send + tot)=updater_msgSize; - //fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize); - // tot += sizeof(uint16_t); - /*Append updater msgs*/ - // memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); - // tot += updater_msgSize; - /*destroy the updater out msg queue*/ - // destroy_out_msg_queue(); - //} - //else{ - // *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre; - // tot += sizeof(uint8_t); - //} /* Send messages from FIFO */ do { /* Are there more messages? */ @@ -507,7 +460,6 @@ namespace buzz_utility{ buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_flight_status(VM); - buzzuav_closures::buzzuav_update_obstacle(VM); /* * Call Buzz step() function */ @@ -518,7 +470,7 @@ namespace buzz_utility{ buzzvm_dump(VM); } /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ - /* look into this currently we don't have swarm feature at all */ + /* look into this currently we don't have swarm feature tested */ /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ /*Print swarm*/ //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); @@ -571,7 +523,6 @@ int update_step_test(){ buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_flight_status(VM); - buzzuav_closures::buzzuav_update_obstacle(VM); int a = buzzvm_function_call(VM, "step", 0); if(a != BUZZVM_STATE_READY){ diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index a2bcc38..f8fb491 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -7,365 +7,346 @@ */ //#define _GNU_SOURCE #include "buzzuav_closures.h" -#include "buzz_utility.h" +//#include "roscontroller.h" namespace buzzuav_closures{ -// TODO: Minimize the required global variables and put them in the header + // TODO: Minimize the required global variables and put them in the header + //static const rosbzz_node::roscontroller* roscontroller_ptr; + /*forward declaration of ros controller ptr storing function*/ + //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin); + static double goto_pos[3]; + static double rc_goto_pos[3]; + static float batt[3]; + static float obst[5]={0,0,0,0,0}; + static double cur_pos[3]; + static uint8_t status; + static int cur_cmd = 0; + static int rc_cmd=0; + static int buzz_cmd=0; + static float height=0; + /****************************************/ + /****************************************/ -static double goto_pos[3]; -static double rc_goto_pos[3]; -static float batt[3]; -static float obst[5]={0,0,0,0,0}; -static double cur_pos[3]; -static uint8_t status; -static int cur_cmd = 0; -static int rc_cmd=0; -static int buzz_cmd=0; -static float height=0; -/****************************************/ -/****************************************/ + int buzzros_print(buzzvm_t vm) { + int i; + for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { + buzzvm_lload(vm, i); + buzzobj_t o = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + switch(o->o.type) { + case BUZZTYPE_NIL: + ROS_INFO("BUZZ - [nil]"); + break; + case BUZZTYPE_INT: + ROS_INFO("%d", o->i.value); + //fprintf(stdout, "%d", o->i.value); + break; + case BUZZTYPE_FLOAT: + ROS_INFO("%f", o->f.value); + break; + case BUZZTYPE_TABLE: + ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value))); + break; + case BUZZTYPE_CLOSURE: + if(o->c.value.isnative) + ROS_INFO("[n-closure @%d]", o->c.value.ref); + else + ROS_INFO("[c-closure @%d]", o->c.value.ref); + break; + case BUZZTYPE_STRING: + ROS_INFO("%s", o->s.value.str); + break; + case BUZZTYPE_USERDATA: + ROS_INFO("[userdata @%p]", o->u.value); + break; + default: + break; + } + } + //fprintf(stdout, "\n"); + return buzzvm_ret0(vm); + } -int buzzros_print(buzzvm_t vm) { - int i; - for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { - buzzvm_lload(vm, i); - buzzobj_t o = buzzvm_stack_at(vm, 1); - buzzvm_pop(vm); - switch(o->o.type) { - case BUZZTYPE_NIL: - ROS_INFO("BUZZ - [nil]"); - break; - case BUZZTYPE_INT: - ROS_INFO("%d", o->i.value); - //fprintf(stdout, "%d", o->i.value); - break; - case BUZZTYPE_FLOAT: - ROS_INFO("%f", o->f.value); - break; - case BUZZTYPE_TABLE: - ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value))); - break; - case BUZZTYPE_CLOSURE: - if(o->c.value.isnative) - ROS_INFO("[n-closure @%d]", o->c.value.ref); - else - ROS_INFO("[c-closure @%d]", o->c.value.ref); - break; - case BUZZTYPE_STRING: - ROS_INFO("%s", o->s.value.str); - break; - case BUZZTYPE_USERDATA: - ROS_INFO("[userdata @%p]", o->u.value); - break; - default: - break; - } - } - //fprintf(stdout, "\n"); - return buzzvm_ret0(vm); -} + /*----------------------------------------/ + / 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; + out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing)); + out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0])); + out[0] = out[0]*180.0/M_PI; + out[1] = out[1]*180.0/M_PI; + out[2] = height; //constant height. + } -/*----------------------------------------/ -/ 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; - out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing)); - out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0])); - out[0] = out[0]*180.0/M_PI; - out[1] = out[1]*180.0/M_PI; - out[2] = height; //constant height. -} + // 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}; + double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677}; -// 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}; -double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677}; + /*----------------------------------------/ + / Buzz closure to move following a 2D vector + /----------------------------------------*/ + int buzzuav_moveto(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 2); + buzzvm_lload(vm, 1); /* dx */ + buzzvm_lload(vm, 2); /* dy */ + //buzzvm_lload(vm, 3); /* Latitude */ + //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + float dy = buzzvm_stack_at(vm, 1)->f.value; + float dx = buzzvm_stack_at(vm, 2)->f.value; + double d = sqrt(dx*dx+dy*dy); //range + goto_pos[0]=dx; + goto_pos[1]=dy; + goto_pos[2]=0; + /*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(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); + buzz_cmd=5; + return buzzvm_ret0(vm); + } -/*----------------------------------------/ -/ Buzz closure to move following a 2D vector -/----------------------------------------*/ -int buzzuav_moveto(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 2); - buzzvm_lload(vm, 1); /* dx */ - buzzvm_lload(vm, 2); /* dy */ - //buzzvm_lload(vm, 3); /* Latitude */ - //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - float dy = buzzvm_stack_at(vm, 1)->f.value; - float dx = buzzvm_stack_at(vm, 2)->f.value; - goto_pos[0]=dx; - goto_pos[1]=dy; - goto_pos[2]=0; - /*double d = sqrt(dx*dx+dy*dy); //range - 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(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); - buzz_cmd=5; - return buzzvm_ret0(vm); -} + /*----------------------------------------/ + / Buzz closure to go directly to a GPS destination from the Mission Planner + /----------------------------------------*/ + int buzzuav_goto(buzzvm_t vm) { + rc_goto_pos[2]=height; + set_goto(rc_goto_pos); + cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; + printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); + buzz_cmd=2; + return buzzvm_ret0(vm); + } -/*----------------------------------------/ -/ Buzz closure to go directly to a GPS destination from the Mission Planner -/----------------------------------------*/ -int buzzuav_goto(buzzvm_t vm) { - rc_goto_pos[2]=height; - set_goto(rc_goto_pos); - cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; - printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); - buzz_cmd=2; - return buzzvm_ret0(vm); -} + /*----------------------------------------/ + / Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running + /----------------------------------------*/ + int buzzuav_arm(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; + printf(" Buzz requested Arm \n"); + buzz_cmd=3; + return buzzvm_ret0(vm); + } + int buzzuav_disarm(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; + printf(" Buzz requested Disarm \n"); + buzz_cmd=4; + return buzzvm_ret0(vm); + } -/*----------------------------------------/ -/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and runing -/----------------------------------------*/ -int buzzuav_arm(buzzvm_t vm) { - cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; - printf(" Buzz requested Arm \n"); - buzz_cmd=3; - return buzzvm_ret0(vm); -} -int buzzuav_disarm(buzzvm_t vm) { - cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; - printf(" Buzz requested Disarm \n"); - buzz_cmd=4; - return buzzvm_ret0(vm); -} + /*---------------------------------------/ + / Buss closure for basic UAV commands + /---------------------------------------*/ + int buzzuav_takeoff(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 1); + buzzvm_lload(vm, 1); /* Altitude */ + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value; + height = goto_pos[2]; + cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; + printf(" Buzz requested Take off !!! \n"); + buzz_cmd = 1; + return buzzvm_ret0(vm); + } -/*---------------------------------------/ -/ Buss closure for basic UAV commands -/---------------------------------------*/ -int buzzuav_takeoff(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 1); - buzzvm_lload(vm, 1); /* Altitude */ - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value; - height = goto_pos[2]; - cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; - printf(" Buzz requested Take off !!! \n"); - buzz_cmd = 1; - return buzzvm_ret0(vm); -} + int buzzuav_land(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::NAV_LAND; + printf(" Buzz requested Land !!! \n"); + buzz_cmd = 1; + return buzzvm_ret0(vm); + } -int buzzuav_land(buzzvm_t vm) { - cur_cmd=mavros_msgs::CommandCode::NAV_LAND; - printf(" Buzz requested Land !!! \n"); - buzz_cmd = 1; - return buzzvm_ret0(vm); -} - -int buzzuav_gohome(buzzvm_t vm) { - cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; - printf(" Buzz requested gohome !!! \n"); - buzz_cmd = 1; - return buzzvm_ret0(vm); -} + int buzzuav_gohome(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + printf(" Buzz requested gohome !!! \n"); + buzz_cmd = 1; + return buzzvm_ret0(vm); + } -/*------------------------------- -/ Get/Set to transfer variable from Roscontroller to buzzuav -/------------------------------------*/ -double* getgoto() { - return goto_pos; -} + /*------------------------------- + / Get/Set to transfer variable from Roscontroller to buzzuav + /------------------------------------*/ + double* getgoto() { + return goto_pos; + } -int getcmd() { - return cur_cmd; -} + int getcmd() { + return cur_cmd; + } -void set_goto(double pos[]) { - goto_pos[0] = pos[0]; - goto_pos[1] = pos[1]; - goto_pos[2] = pos[2]; - -} - -int bzz_cmd() { - int cmd = buzz_cmd; - buzz_cmd = 0; - return cmd; -} - -void rc_set_goto(double pos[]) { - rc_goto_pos[0] = pos[0]; - rc_goto_pos[1] = pos[1]; - rc_goto_pos[2] = pos[2]; - -} -void rc_call(int rc_cmd_in) { - rc_cmd = rc_cmd_in; -} - -void set_obstacle_dist(float dist[]) { - for (int i = 0; i < 5; i++) - obst[i] = dist[i]; -} - -void set_battery(float voltage,float current,float remaining){ - batt[0]=voltage; - batt[1]=current; - batt[2]=remaining; -} - -int buzzuav_update_battery(buzzvm_t vm) { - //static char BATTERY_BUF[256]; - buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1)); - buzzvm_pushf(vm, batt[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); - buzzvm_pushf(vm, batt[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); - buzzvm_pushf(vm, batt[2]); - buzzvm_tput(vm); - buzzvm_gstore(vm); - return vm->state; -} -/****************************************/ -/*current pos update*/ -/***************************************/ -void set_currentpos(double latitude, double longitude, double altitude){ - cur_pos[0]=latitude; - cur_pos[1]=longitude; - cur_pos[2]=altitude; -} -/****************************************/ -int buzzuav_update_currentpos(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); - buzzvm_pushf(vm, cur_pos[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); - buzzvm_pushf(vm, cur_pos[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); - buzzvm_pushf(vm, cur_pos[2]); - buzzvm_tput(vm); - buzzvm_gstore(vm); - return vm->state; -} -/*----------------------------------------- -/ Create an obstacle Buzz table from proximity sensors -/ TODO: swap to proximity function below ---------------------------------------------*/ - -int buzzuav_update_obstacle(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "obstacle", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1)); - buzzvm_pushf(vm, obst[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1)); - buzzvm_pushf(vm, obst[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1)); - buzzvm_pushf(vm, obst[2]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1)); - buzzvm_pushf(vm, obst[3]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1)); - buzzvm_pushf(vm, obst[4]); - buzzvm_tput(vm); - buzzvm_gstore(vm); - return vm->state; -} - -void flight_status_update(uint8_t state){ - status=state; -} - -/*---------------------------------------------------- -/ Create the generic robot table with status, remote controller current comand and destination -/ and current position of the robot -/ TODO: change global name for robot -/------------------------------------------------------*/ -int buzzuav_update_flight_status(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1)); - buzzvm_pushi(vm, rc_cmd); - buzzvm_tput(vm); - buzzvm_dup(vm); - rc_cmd=0; - buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); - buzzvm_pushi(vm, status); - buzzvm_tput(vm); - buzzvm_gstore(vm); - //also set rc_controllers goto - buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); - buzzvm_pushf(vm, rc_goto_pos[2]); - buzzvm_tput(vm); - buzzvm_gstore(vm); - return vm->state; -} - - - -/****************************************/ -/*To do !!!!!*/ -/****************************************/ - -int buzzuav_update_prox(buzzvm_t vm) { - /* static char PROXIMITY_BUF[256]; - int i; - //kh4_proximity_ir(PROXIMITY_BUF, DSPIC); - buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity_ir", 1)); - buzzvm_pusht(vm); - for(i = 0; i < 8; i++) { - buzzvm_dup(vm); - buzzvm_pushi(vm, i+1); - buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8)); - buzzvm_tput(vm); - } - buzzvm_gstore(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "ground_ir", 1)); - buzzvm_pusht(vm); - for(i = 8; i < 12; i++) { - buzzvm_dup(vm); - buzzvm_pushi(vm, i-7); - buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8)); - buzzvm_tput(vm); - } - buzzvm_gstore(vm);*/ - return vm->state; -} - -/****************************************/ -/****************************************/ - -int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} + void set_goto(double pos[]) { + goto_pos[0] = pos[0]; + goto_pos[1] = pos[1]; + goto_pos[2] = pos[2]; + + } + + int bzz_cmd() { + int cmd = buzz_cmd; + buzz_cmd = 0; + return cmd; + } + + void rc_set_goto(double pos[]) { + rc_goto_pos[0] = pos[0]; + rc_goto_pos[1] = pos[1]; + rc_goto_pos[2] = pos[2]; + + } + void rc_call(int rc_cmd_in) { + rc_cmd = rc_cmd_in; + } + + void set_obstacle_dist(float dist[]) { + for (int i = 0; i < 5; i++) + obst[i] = dist[i]; + } + + void set_battery(float voltage,float current,float remaining){ + batt[0]=voltage; + batt[1]=current; + batt[2]=remaining; + } + + int buzzuav_update_battery(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1)); + buzzvm_pushf(vm, batt[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); + buzzvm_pushf(vm, batt[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); + buzzvm_pushf(vm, batt[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; + } + /****************************************/ + /*current pos update*/ + /***************************************/ + void set_currentpos(double latitude, double longitude, double altitude){ + cur_pos[0]=latitude; + cur_pos[1]=longitude; + cur_pos[2]=altitude; + } + /****************************************/ + int buzzuav_update_currentpos(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzvm_pushf(vm, cur_pos[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_pushf(vm, cur_pos[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_pushf(vm, cur_pos[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; + } + + void flight_status_update(uint8_t state){ + status=state; + } + + /*---------------------------------------------------- + / Create the generic robot table with status, remote controller current comand and destination + / and current position of the robot + / TODO: change global name for robot + /------------------------------------------------------*/ + int buzzuav_update_flight_status(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1)); + buzzvm_pushi(vm, rc_cmd); + buzzvm_tput(vm); + buzzvm_dup(vm); + rc_cmd=0; + buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); + buzzvm_pushi(vm, status); + buzzvm_tput(vm); + buzzvm_gstore(vm); + //also set rc_controllers goto + buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_pushf(vm, rc_goto_pos[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; + } + + + + /******************************************************/ + /*Create an obstacle Buzz table from proximity sensors*/ + /******************************************************/ + + int buzzuav_update_prox(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1)); + buzzvm_pushf(vm, obst[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1)); + buzzvm_pushf(vm, obst[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1)); + buzzvm_pushf(vm, obst[2]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1)); + buzzvm_pushf(vm, obst[3]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1)); + buzzvm_pushf(vm, obst[4]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; + } + + /**********************************************/ + /*Dummy closure for use during update testing */ + /**********************************************/ + + int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} + + /***********************************************/ + /* Store Ros controller object pointer */ + /***********************************************/ + + //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){ + //roscontroller_ptr = roscontroller_ptrin; + //} } diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index cfef7b2..5190944 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -18,6 +18,8 @@ int main(int argc, char **argv) ros::init(argc, argv, "rosBuzz"); ros::NodeHandle n_c("~"); rosbzz_node::roscontroller RosController(n_c); + /*Register ros controller object inside buzz*/ + //buzzuav_closures::set_ros_controller_ptr(&RosController); RosController.RosControllerRun(); return 0; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 2075f4f..520e8f0 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -2,7 +2,9 @@ namespace rosbzz_node{ - /***Constructor***/ + /*--------------- + /Constructor + ---------------*/ roscontroller::roscontroller(ros::NodeHandle n_c) { ROS_INFO("Buzz_node"); @@ -18,7 +20,9 @@ namespace rosbzz_node{ multi_msg = true; } - /***Destructor***/ + /*--------------------- + /Destructor + ---------------------*/ roscontroller::~roscontroller() { /* All done */ @@ -28,7 +32,9 @@ namespace rosbzz_node{ uav_done(); } - /*rosbuzz_node run*/ + /*------------------------------------------------- + /rosbuzz_node loop method executed once every step + /--------------------------------------------------*/ void roscontroller::RosControllerRun(){ /* Set the Buzz bytecode */ if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { @@ -38,33 +44,16 @@ namespace rosbzz_node{ { /*Update neighbors position inside Buzz*/ buzz_utility::neighbour_pos_callback(neighbours_pos_map); - auto current_time = ros::Time::now(); - map< int, buzz_utility::Pos_struct >::iterator it; - rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear(); - neigh_pos_array.header.frame_id = "/world"; - neigh_pos_array.header.stamp = current_time; - for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){ - sensor_msgs::NavSatFix neigh_tmp; - //cout<<"iterator it val: "<< it-> first << " After convertion: " <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<first; //custom robot id storage - neigh_tmp.latitude=(it->second).x; - neigh_tmp.longitude=(it->second).y; - neigh_tmp.altitude=(it->second).z; - neigh_pos_array.pos_neigh.push_back(neigh_tmp); - //std::cout<<"long obt"<(out_payload, 1000); - neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); - localsetpoint_pub = n_c.advertise("/setpoint_raw/local",1000); + neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); + localsetpoint_pub = n_c.advertise("/setpoint_raw/local",1000); /* Service Clients*/ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); @@ -176,7 +167,9 @@ namespace rosbzz_node{ multi_msg=true; } - + /*--------------------------------------- + /Robot independent subscribers + /--------------------------------------*/ void roscontroller::Subscribe(ros::NodeHandle n_c){ for(std::map::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){ @@ -201,6 +194,7 @@ namespace rosbzz_node{ / Create Buzz bytecode from the bzz script inputed /-------------------------------------------------------*/ void roscontroller::Compile_bzz(){ + /*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("\\/")); @@ -210,7 +204,6 @@ namespace rosbzz_node{ std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0,name.find_last_of(".")); bzzfile_in_compile << "bzzparse "<::iterator it; + rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear(); + neigh_pos_array.header.frame_id = "/world"; + neigh_pos_array.header.stamp = current_time; + for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){ + sensor_msgs::NavSatFix neigh_tmp; + //cout<<"iterator it val: "<< it-> first << " After convertion: " <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<first; //custom robot id storage + neigh_tmp.latitude=(it->second).x; + neigh_tmp.longitude=(it->second).y; + neigh_tmp.altitude=(it->second).z; + neigh_pos_array.pos_neigh.push_back(neigh_tmp); + //std::cout<<"long obt"<::const_iterator it = payload_out.payload64.begin(); - it != payload_out.payload64.end(); ++it){ - message_obt[i] =(uint64_t) *it; - i++; - }*/ - /*for(int i=0;i=10){ neighbours_pos_map.clear(); - //raw_neighbours_pos_map.clear(); + //raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but have to clear ! timer_step=0; } } - /*Puts neighbours position*/ + /*--------------------------------------------------------------------------------- + /Puts neighbours position into local struct storage that is cleared every 10 step + /---------------------------------------------------------------------------------*/ void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){ map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id); if(it!=neighbours_pos_map.end()) neighbours_pos_map.erase(it); neighbours_pos_map.insert(make_pair(id, pos_arr)); } - /*Puts raw neighbours position*/ + /*----------------------------------------------------------------------------------- + /Puts raw neighbours position into local storage for neighbours pos publisher + /-----------------------------------------------------------------------------------*/ 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()) @@ -420,14 +418,15 @@ namespace rosbzz_node{ raw_neighbours_pos_map.insert(make_pair(id, pos_arr)); } - /*Set the current position of the robot callback*/ + /*-------------------------------------------------------- + /Set the current position of the robot callback + /--------------------------------------------------------*/ void roscontroller::set_cur_pos(double latitude, double longitude, double altitude){ cur_pos [0] =latitude; cur_pos [1] =longitude; cur_pos [2] =altitude; - } @@ -448,13 +447,17 @@ namespace rosbzz_node{ out[2] = 0.0; } - /*battery status callback*/ + /*------------------------------------------------------ + / Update battery status into BVM from subscriber + /------------------------------------------------------*/ void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){ buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining); //ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, msg->current, msg ->remaining); } - /*flight extended status update*/ + /*---------------------------------------------------------------------- + /Update flight extended status into BVM from subscriber for solos + /---------------------------------------------------------------------*/ void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg){ // http://wiki.ros.org/mavros/CustomModes // TODO: Handle landing @@ -467,47 +470,47 @@ namespace rosbzz_node{ buzzuav_closures::flight_status_update(1);//? } - /*flight extended status update*/ + /*------------------------------------------------------------ + /Update flight extended status into BVM from subscriber + ------------------------------------------------------------*/ void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){ buzzuav_closures::flight_status_update(msg->landed_state); } - /*current position callback*/ + /*------------------------------------------------------------- + / Update current position into BVM from subscriber + /-------------------------------------------------------------*/ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){ set_cur_pos(msg->latitude,msg->longitude,msg->altitude); buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude); } - /*Obstacle distance call back*/ + /*------------------------------------------------------------- + /Set obstacle Obstacle distance table into BVM from subscriber + /-------------------------------------------------------------*/ void roscontroller::obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg){ float obst[5]; for(int i=0;i<5;i++) obst[i]=msg->ranges[i]; buzzuav_closures::set_obstacle_dist(obst); } - /*payload callback callback*/ + /*------------------------------------------------------------- + /Push payload into BVM FIFO + /-------------------------------------------------------------*/ /*******************************************************************************************************/ /* Message format of payload (Each slot is uint64_t) */ /* _________________________________________________________________________________________________ */ /*| | | | | | */ /*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */ /*|_____|_____|_____|________________________________________________|______________________________| */ - /*******************************************************************************************************/ - + /*******************************************************************************************************/ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){ - - /*Ack from xbee about its transfer complete of multi packet*/ - /*if((uint64_t)msg->payload64[0]==(uint64_t)XBEE_MESSAGE_CONSTANT && msg->payload64.size() == 1){ - multi_msg=true; - std::cout << "ACK From xbee after transimssion of code " << std::endl; - }*/ - if((uint64_t)msg->payload64[0]==(uint64_t)UPDATER_MESSAGE_CONSTANT && msg->payload64.size() > 10){ + /*Check for Updater message, if updater message push it into updater FIFO*/ + if((uint64_t)msg->payload64[0]==(uint64_t)UPDATER_MESSAGE_CONSTANT && msg->payload64.size() > 5){ uint16_t obt_msg_size=sizeof(uint64_t)*(msg->payload64.size()); uint64_t message_obt[obt_msg_size]; /* Go throught the obtained payload*/ for(int i=0;i < (int)msg->payload64.size();i++){ message_obt[i] =(uint64_t)msg->payload64[i]; - //cout<<"[Debug:] obtaind message "<0){ code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)),unMsgSize); //fprintf(stdout,"before in queue process : utils\n"); @@ -525,10 +528,8 @@ namespace rosbzz_node{ //fprintf(stdout,"after in queue process : utils\n"); } delete[] pl; - - } - + /*BVM FIFO message*/ else if(msg->payload64.size() > 3){ uint64_t message_obt[msg->payload64.size()]; /* Go throught the obtained payload*/ @@ -616,6 +617,10 @@ namespace rosbzz_node{ } return true; } + /*----------------------------------------------------- + /Obtain robot id by subscribing to xbee robot id topic + / TODO: check for integrity of this subscriber call back + /----------------------------------------------------*/ void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){ robot_id=(int)msg->data; diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index f74efde..d8843ec 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -86,12 +86,12 @@ function barrier_wait(threshold, transf) { if(barrier.size() >= threshold) { barrier = nil transf() - } else if(timeW>=WAIT_TIMEOUT) { - barrier = nil - statef=hexagon #idle - timeW=0 - } - timeW = timeW+1 + } else if(timeW>=WAIT_TIMEOUT) { + barrier = nil + statef=hexagon #idle + timeW=0 + } + timeW = timeW+1 } # flight status @@ -150,7 +150,6 @@ function init() { # Executed at each time step. function step() { - if(flight.rc_cmd==22) { log("cmd 22") flight.rc_cmd=0 @@ -172,6 +171,7 @@ function step() { uav_arm() neighbors.broadcast("cmd", 400) } else if (flight.rc_cmd==401){ + flight.rc_cmd=0 uav_disarm() neighbors.broadcast("cmd", 401) }