diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 1ff186c..6b33843 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -33,46 +33,95 @@ typedef enum { */ int buzzros_print(buzzvm_t vm); void setWPlist(std::string path); + /* - * buzzuav_goto(latitude,longitude,altitude) function in Buzz - * commands the UAV to go to a position supplied + * closure to move following a vector */ -int buzz_floor(buzzvm_t vm); int buzzuav_moveto(buzzvm_t vm); +/* + * closure to store a new GPS goal + */ int buzzuav_storegoal(buzzvm_t vm); +/* + * closure to control the gimbal + */ int buzzuav_setgimbal(buzzvm_t vm); +/* + * parse a csv list of waypoints + */ void parse_gpslist(); +/* + * closure to take a picture + */ int buzzuav_takepicture(buzzvm_t vm); -/* Returns the current command from local variable*/ +/* + * Returns the current command from local variable + */ int getcmd(); -/*Sets goto position from rc client*/ +/* + * Sets goto position from rc client + */ void rc_set_goto(int id, double latitude, double longitude, double altitude); -/*Sets gimbal orientation from rc client*/ +/* + *Sets gimbal orientation from rc client + */ void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t); -/*sets rc requested command */ +/* + * sets rc requested command + */ void rc_call(int rc_cmd); -/* sets the battery state */ +/* + * sets the battery state + */ void set_battery(float voltage, float current, float remaining); +/* + * sets the xbee network status + */ void set_deque_full(bool state); void set_rssi(float value); void set_raw_packet_loss(float value); void set_filtered_packet_loss(float value); void set_api_rssi(float value); -/* sets current position */ +/* + * sets current position + */ void set_currentpos(double latitude, double longitude, double altitude); -/*retuns the current go to position */ +/* + * returns the current go to position + */ double* getgoto(); +/* + * returns the current Buzz state + */ std::string getuavstate(); +/* + * returns the gimbal commands + */ float* getgimbal(); -/* updates flight status*/ +/* + *updates flight status + */ void flight_status_update(uint8_t state); -/* Update neighbors table */ +/* + *Update neighbors table + */ void neighbour_pos_callback(int id, float range, float bearing, float elevation); +/* + * update neighbors from in msgs + */ void update_neighbors(buzzvm_t vm); +/* + * closure to add a neighbor status + */ int buzzuav_addNeiStatus(buzzvm_t vm); +/* + * returns the current array of neighbors status + */ mavros_msgs::Mavlink get_status(); -/*Flight status*/ +/* + *Flight status + */ void set_obstacle_dist(float dist[]); /* @@ -91,7 +140,8 @@ int buzzuav_disarm(buzzvm_t vm); */ int buzzuav_land(buzzvm_t vm); -/* Command the UAV to go to home location +/* + * Command the UAV to go to home location */ int buzzuav_gohome(buzzvm_t vm); @@ -107,7 +157,9 @@ int buzzuav_update_xbee_status(buzzvm_t vm); * Updates current position in Buzz */ int buzzuav_update_currentpos(buzzvm_t vm); -int buzzuav_update_targets(buzzvm_t vm); +/* + * add new target in the BVM + */ int buzzuav_addtargetRB(buzzvm_t vm); /* * Updates flight status and rc command in Buzz, put it in a tabel to acess it @@ -121,10 +173,10 @@ int buzzuav_update_flight_status(buzzvm_t vm); * Proximity and ground sensors to do !!!! */ int buzzuav_update_prox(buzzvm_t vm); - +/* + * returns the current FC command + */ int bzz_cmd(); int dummy_closure(buzzvm_t vm); - -//#endif } diff --git a/readme.md b/readme.md index 3c21113..4295150 100644 --- a/readme.md +++ b/readme.md @@ -99,4 +99,4 @@ References ------ * ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843 -* Over-The-Air Updates for Robotic Swarms. Accepted by IEEE Software (September 2017 - pending minor revision). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G. +* Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G. diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index db19fa4..38277d8 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -24,10 +24,10 @@ std::map users_map; /****************************************/ -/**************************************************************************/ -/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */ -/**************************************************************************/ uint16_t* u64_cvt_u16(uint64_t u64) +/* +/ Deserializes uint64_t into 4 uint16_t, freeing out is left to the user +------------------------------------------------------------------------*/ { uint16_t* out = new uint16_t[4]; uint32_t int32_1 = u64 & 0xFFFFFFFF; @@ -36,61 +36,66 @@ uint16_t* u64_cvt_u16(uint64_t u64) out[1] = (int32_1 & (0xFFFF0000)) >> 16; out[2] = int32_2 & 0xFFFF; out[3] = (int32_2 & (0xFFFF0000)) >> 16; + // DEBUG // cout << " values " < 0 && unMsgSize <= size - tot) { buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize)); @@ -100,33 +105,34 @@ void in_message_process() free(first_INmsg); IN_MSG.erase(IN_MSG.begin()); } - /* Process messages VM call*/ + // Process messages VM call* buzzvm_process_inmsgs(VM); } -/***************************************************/ -/*Obtains messages from buzz out message Queue*/ -/***************************************************/ uint64_t* obt_out_msg() +/* +/ Obtains messages from buzz out message Queue +-------------------------------------------------*/ { - /* Process out messages */ + // Process out messages buzzvm_process_outmsgs(VM); uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE); memset(buff_send, 0, MAX_MSG_SIZE); - /*Taking into consideration the sizes included at the end*/ + // Taking into consideration the sizes included at the end ssize_t tot = sizeof(uint16_t); - /* Send robot id */ + // Send robot id *(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot; tot += sizeof(uint16_t); - /* Send messages from FIFO */ + // Send messages from FIFO do { - /* Are there more messages? */ + // Are there more messages? if (buzzoutmsg_queue_isempty(VM)) break; - /* Get first message */ + // Get first message buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); - /* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */ + // Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes + // DEBUG // ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t))); if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) { @@ -134,15 +140,15 @@ uint64_t* obt_out_msg() break; } - /* Add message length to data buffer */ + // Add message length to data buffer *(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m); tot += sizeof(uint16_t); - /* Add payload to data buffer */ + // Add payload to data buffer memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m)); tot += buzzmsg_payload_size(m); - /* Get rid of message */ + // Get rid of message buzzoutmsg_queue_next(VM); buzzmsg_payload_destroy(&m); } while (1); @@ -154,18 +160,18 @@ uint64_t* obt_out_msg() memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); free(buff_send); - /*for(int i=0;ipc); char* msg; @@ -181,11 +187,10 @@ static const char* buzz_error_info() return msg; } -/****************************************/ -/*Buzz hooks that can be used inside .bzz file*/ -/****************************************/ - static int buzz_register_hooks() +/* +/ Buzz hooks that can be used inside .bzz file +------------------------------------------------*/ { buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); @@ -233,11 +238,10 @@ static int buzz_register_hooks() return VM->state; } -/**************************************************/ -/*Register dummy Buzz hooks for test during update*/ -/**************************************************/ - static int testing_buzz_register_hooks() +/* +/ Register dummy Buzz hooks for test during update +---------------------------------------------------*/ { buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); @@ -285,14 +289,14 @@ static int testing_buzz_register_hooks() return VM->state; } -/****************************************/ -/*Sets the .bzz and .bdbg file*/ -/****************************************/ int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) +/* +/ Sets the .bzz and .bdbg file +---------------------------------*/ { ROS_INFO(" Robot ID: %i", robot_id); Robot_id = robot_id; - /* Read bytecode from file and fill the bo buffer*/ + // Read bytecode from file and fill the bo buffer FILE* fd = fopen(bo_filename, "rb"); if (!fd) { @@ -313,16 +317,16 @@ int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robo } fclose(fd); - /* Save bytecode file name */ + // Save bytecode file name BO_FNAME = strdup(bo_filename); return buzz_update_set(BO_BUF, bdbg_filename, bcode_size); } -/****************************************/ -/*Sets a new update */ -/****************************************/ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size) +/* +/ Sets a new update +-----------------------*/ { // Reset the Buzz VM if (VM) @@ -379,10 +383,10 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_ return 1; } -/****************************************/ -/*Performs a initialization test */ -/****************************************/ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size) +/* +/ Performs a initialization test +-----------------------------------*/ { // Reset the Buzz VM if (VM) @@ -439,38 +443,40 @@ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t return 1; } -/****************************************/ -/*Swarm struct*/ -/****************************************/ - struct buzzswarm_elem_s +/* +/ Swarm struct +----------------*/ { buzzdarray_t swarms; uint16_t age; }; typedef struct buzzswarm_elem_s* buzzswarm_elem_t; -/*Step through the buzz script*/ void update_sensors() +/* +/ Update from all external inputs +-------------------------------*/ { - /* Update sensors*/ + // Update sensors buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_xbee_status(VM); buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::update_neighbors(VM); - buzzuav_closures::buzzuav_update_targets(VM); - // update_users(); buzzuav_closures::buzzuav_update_flight_status(VM); } void buzz_script_step() +/* +/ Step through the buzz script +-------------------------------*/ { - /*Process available messages*/ + // Process available messages in_message_process(); - /*Update sensors*/ + // Update sensors update_sensors(); - /* Call Buzz step() function */ + // Call Buzz step() function if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info()); @@ -478,11 +484,10 @@ void buzz_script_step() } } -/****************************************/ -/*Destroy the bvm and other resorces*/ -/****************************************/ - void buzz_script_destroy() +/* +/ Destroy the bvm and other related ressources +-------------------------------------*/ { if (VM) { @@ -499,21 +504,20 @@ void buzz_script_destroy() ROS_INFO("Script execution stopped."); } -/****************************************/ -/****************************************/ - -/****************************************/ -/*Execution completed*/ -/****************************************/ - int buzz_script_done() +/* +/ Check if the BVM execution terminated +---------------------------------------*/ { return VM->state != BUZZVM_STATE_READY; } int update_step_test() +/* +/ Step test for the update mechanism +------------------------------------*/ { - /*Process available messages*/ + // Process available messages in_message_process(); buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_prox(VM); @@ -533,11 +537,17 @@ int update_step_test() } buzzvm_t get_vm() +/* +/ return the BVM +----------------*/ { return VM; } void set_robot_var(int ROBOTS) +/* +/ set swarm size in the BVM +-----------------------------*/ { buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushi(VM, ROBOTS); @@ -545,6 +555,9 @@ void set_robot_var(int ROBOTS) } int get_inmsg_size() +/* +/ get the incoming msgs size +------------------------------*/ { return IN_MSG.size(); } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index f84cecc..ad27ed5 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -13,8 +13,6 @@ namespace buzzuav_closures { // 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 rc_gimbal[4]; @@ -43,6 +41,9 @@ std::map neighbors_status_map; /****************************************/ int buzzros_print(buzzvm_t vm) +/* +/ Buzz closure to print out +----------------------------------------------------------- */ { std::ostringstream buffer(std::ostringstream::ate); buffer << buzz_utility::get_robotid(); @@ -90,11 +91,17 @@ int buzzros_print(buzzvm_t vm) } void setWPlist(string path) +/* +/ set the absolute path for a csv list of waypoints +----------------------------------------------------------- */ { WPlistname = path + "include/graphs/waypointlist.csv"; } float constrainAngle(float x) +/* +/ Wrap the angle between -pi, pi +----------------------------------------------------------- */ { x = fmod(x, 2 * M_PI); if (x < 0.0) @@ -102,10 +109,10 @@ float constrainAngle(float x) return x; } -/*----------------------------------------/ +void rb_from_gps(double nei[], double out[], double cur[]) +/* / Compute Range and Bearing from 2 GPS set of coordinates /----------------------------------------*/ -void rb_from_gps(double nei[], double out[], double cur[]) { double d_lon = nei[1] - cur[1]; double d_lat = nei[0] - cur[0]; @@ -117,6 +124,9 @@ void rb_from_gps(double nei[], double out[], double cur[]) } void parse_gpslist() +/* +/ parse a csv of GPS targets +/----------------------------------------*/ { // Open file: ROS_INFO("WP list file: %s", WPlistname.c_str()); @@ -147,6 +157,7 @@ void parse_gpslist() lat = atof(strtok(NULL, DELIMS)); alt = atoi(strtok(NULL, DELIMS)); tilt = atoi(strtok(NULL, DELIMS)); + // DEBUG // ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid); RB_arr.latitude = lat; RB_arr.longitude = lon; @@ -164,15 +175,15 @@ void parse_gpslist() fin.close(); } -/*----------------------------------------/ +int buzzuav_moveto(buzzvm_t vm) +/* / Buzz closure to move following a 2D vector /----------------------------------------*/ -int buzzuav_moveto(buzzvm_t vm) { buzzvm_lnum_assert(vm, 3); - buzzvm_lload(vm, 1); /* dx */ - buzzvm_lload(vm, 2); /* dy */ - buzzvm_lload(vm, 3); /* dheight */ + buzzvm_lload(vm, 1); // dx + buzzvm_lload(vm, 2); // dy + buzzvm_lload(vm, 3); //* dheight buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); @@ -182,55 +193,17 @@ int buzzuav_moveto(buzzvm_t vm) goto_pos[0] = dx; goto_pos[1] = dy; goto_pos[2] = height + dh; + // DEBUG // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], // goto_pos[1], goto_pos[2]); - buzz_cmd = COMMAND_MOVETO; // TO DO what should we use + buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros? return buzzvm_ret0(vm); } -int buzzuav_update_targets(buzzvm_t vm) -{ - if (vm->state != BUZZVM_STATE_READY) - return vm->state; - buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1)); - buzzvm_pusht(vm); - buzzobj_t targettbl = buzzvm_stack_at(vm, 1); - double rb[3], tmp[3]; - map::iterator it; - for (it = targets_map.begin(); it != targets_map.end(); ++it) - { - tmp[0] = (it->second).latitude; - tmp[1] = (it->second).longitude; - tmp[2] = height; - rb_from_gps(tmp, rb, cur_pos); - // ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]); - buzzvm_push(vm, targettbl); - // When we get here, the "targets" table is on top of the stack - // ROS_INFO("Buzz_utility will save user %i.", it->first); - // Push user id - buzzvm_pushi(vm, it->first); - // Create entry table - buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); - // Insert range - buzzvm_push(vm, entry); - buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1)); - buzzvm_pushf(vm, rb[0]); - buzzvm_tput(vm); - // Insert longitude - buzzvm_push(vm, entry); - buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1)); - buzzvm_pushf(vm, rb[1]); - buzzvm_tput(vm); - // Save entry into data table - buzzvm_push(vm, entry); - buzzvm_tput(vm); - } - buzzvm_gstore(vm); - - return vm->state; -} - int buzzuav_addtargetRB(buzzvm_t vm) +/* +/ Buzz closure to add a target (goal) GPS +/----------------------------------------*/ { buzzvm_lnum_assert(vm, 3); buzzvm_lload(vm, 1); // longitude @@ -249,7 +222,6 @@ int buzzuav_addtargetRB(buzzvm_t vm) rb_from_gps(tmp, rb, cur_pos); if (fabs(rb[0]) < 100.0) { - // printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]); buzz_utility::RB_struct RB_arr; RB_arr.latitude = tmp[0]; RB_arr.longitude = tmp[1]; @@ -260,6 +232,7 @@ int buzzuav_addtargetRB(buzzvm_t vm) if (it != targets_map.end()) targets_map.erase(it); targets_map.insert(make_pair(uid, RB_arr)); + // DEBUG // ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); return vm->state; } @@ -270,6 +243,9 @@ int buzzuav_addtargetRB(buzzvm_t vm) } int buzzuav_addNeiStatus(buzzvm_t vm) +/* +/ closure to add neighbors status to the BVM +/----------------------------------------*/ { buzzvm_lnum_assert(vm, 5); buzzvm_lload(vm, 1); // fc @@ -296,6 +272,9 @@ int buzzuav_addNeiStatus(buzzvm_t vm) } mavros_msgs::Mavlink get_status() +/* +/ return neighbors status from BVM +/----------------------------------------*/ { mavros_msgs::Mavlink payload_out; map::iterator it; @@ -307,27 +286,25 @@ mavros_msgs::Mavlink get_status() payload_out.payload64.push_back(it->second.xbee); payload_out.payload64.push_back(it->second.flight_status); } - /*Add Robot id and message number to the published message*/ + // Add Robot id and message number to the published message payload_out.sysid = (uint8_t)neighbors_status_map.size(); - /*payload_out.msgid = (uint32_t)message_number; - message_number++;*/ return payload_out; } -/*----------------------------------------/ + +int buzzuav_takepicture(buzzvm_t vm) +/* / Buzz closure to take a picture here. /----------------------------------------*/ -int buzzuav_takepicture(buzzvm_t vm) { - // cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; buzz_cmd = COMMAND_PICTURE; return buzzvm_ret0(vm); } -/*----------------------------------------/ +int buzzuav_setgimbal(buzzvm_t vm) +/* / Buzz closure to change locally the gimbal orientation /----------------------------------------*/ -int buzzuav_setgimbal(buzzvm_t vm) { buzzvm_lnum_assert(vm, 4); buzzvm_lload(vm, 1); // time @@ -343,15 +320,15 @@ int buzzuav_setgimbal(buzzvm_t vm) rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value; rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value; - ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]); + ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]); buzz_cmd = COMMAND_GIMBAL; return buzzvm_ret0(vm); } -/*----------------------------------------/ +int buzzuav_storegoal(buzzvm_t vm) +/* / Buzz closure to store locally a GPS destination from the fleet /----------------------------------------*/ -int buzzuav_storegoal(buzzvm_t vm) { buzzvm_lnum_assert(vm, 3); buzzvm_lload(vm, 1); // altitude @@ -380,21 +357,25 @@ int buzzuav_storegoal(buzzvm_t vm) if (fabs(rb[0]) < 150.0) rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); - ROS_WARN("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); 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) +/* +/ Buzz closure to arm +/---------------------------------------*/ { cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; printf(" Buzz requested Arm \n"); buzz_cmd = COMMAND_ARM; return buzzvm_ret0(vm); } + int buzzuav_disarm(buzzvm_t vm) +/* +/ Buzz closure to disarm +/---------------------------------------*/ { cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; printf(" Buzz requested Disarm \n"); @@ -402,10 +383,10 @@ int buzzuav_disarm(buzzvm_t vm) return buzzvm_ret0(vm); } -/*---------------------------------------/ -/ Buzz closure for basic UAV commands -/---------------------------------------*/ int buzzuav_takeoff(buzzvm_t vm) +/* +/ Buzz closure to takeoff +/---------------------------------------*/ { buzzvm_lnum_assert(vm, 1); buzzvm_lload(vm, 1); /* Altitude */ @@ -419,6 +400,9 @@ int buzzuav_takeoff(buzzvm_t vm) } int buzzuav_land(buzzvm_t vm) +/* +/ Buzz closure to land +/-------------------------------------------------------------*/ { cur_cmd = mavros_msgs::CommandCode::NAV_LAND; printf(" Buzz requested Land !!! \n"); @@ -427,6 +411,9 @@ int buzzuav_land(buzzvm_t vm) } int buzzuav_gohome(buzzvm_t vm) +/* +/ Buzz closure to return Home +/-------------------------------------------------------------*/ { cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; printf(" Buzz requested gohome !!! \n"); @@ -434,20 +421,26 @@ int buzzuav_gohome(buzzvm_t vm) return buzzvm_ret0(vm); } -/*------------------------------- -/ Get/Set to transfer variable from Roscontroller to buzzuav -/------------------------------------*/ double* getgoto() +/* +/ return the GPS goal +/-------------------------------------------------------------*/ { return goto_pos; } float* getgimbal() +/* +/ return current gimbal commands +---------------------------------------*/ { return rc_gimbal; } string getuavstate() +/* +/ return current BVM state +---------------------------------------*/ { static buzzvm_t VM = buzz_utility::get_vm(); buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); @@ -458,11 +451,17 @@ string getuavstate() } int getcmd() +/* +/ return current mavros command to the FC +---------------------------------------*/ { return cur_cmd; } int bzz_cmd() +/* +/ return and clean the custom command from Buzz to the FC +----------------------------------------------------------*/ { int cmd = buzz_cmd; buzz_cmd = 0; @@ -470,6 +469,9 @@ int bzz_cmd() } void rc_set_goto(int id, double latitude, double longitude, double altitude) +/* +/ update interface RC GPS goal input +-----------------------------------*/ { rc_id = id; rc_goto_pos[0] = latitude; @@ -478,6 +480,9 @@ void rc_set_goto(int id, double latitude, double longitude, double altitude) } void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) +/* +/ update interface RC gimbal control input +-----------------------------------*/ { rc_id = id; rc_gimbal[0] = yaw; @@ -487,17 +492,26 @@ void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) } void rc_call(int rc_cmd_in) +/* +/ update interface RC command input +-----------------------------------*/ { rc_cmd = rc_cmd_in; } void set_obstacle_dist(float dist[]) +/* +/ update interface proximity value array +-----------------------------------*/ { for (int i = 0; i < 5; i++) obst[i] = dist[i]; } void set_battery(float voltage, float current, float remaining) +/* +/ update interface battery value array +-----------------------------------*/ { batt[0] = voltage; batt[1] = current; @@ -505,6 +519,9 @@ void set_battery(float voltage, float current, float remaining) } int buzzuav_update_battery(buzzvm_t vm) +/* +/ update BVM battery table +-----------------------------------*/ { buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); buzzvm_pusht(vm); @@ -524,6 +541,9 @@ int buzzuav_update_battery(buzzvm_t vm) return vm->state; } +/* +/ Set of function to update interface variable of xbee network status +----------------------------------------------------------------------*/ void set_deque_full(bool state) { deque_full = state; @@ -550,6 +570,9 @@ void set_api_rssi(float value) } int buzzuav_update_xbee_status(buzzvm_t vm) +/* +/ update BVM xbee_status table +-----------------------------------*/ { buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1)); buzzvm_pusht(vm); @@ -577,16 +600,16 @@ int buzzuav_update_xbee_status(buzzvm_t vm) return vm->state; } -/***************************************/ -/*current pos update*/ -/***************************************/ void set_currentpos(double latitude, double longitude, double altitude) +/* +/ update interface position array +-----------------------------------*/ { cur_pos[0] = latitude; cur_pos[1] = longitude; cur_pos[2] = altitude; } -/*adds neighbours position*/ +// adds neighbours position void neighbour_pos_callback(int id, float range, float bearing, float elevation) { buzz_utility::Pos_struct pos_arr; @@ -599,12 +622,12 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation) neighbors_map.insert(make_pair(id, pos_arr)); } -/* update at each step the VM table */ +// update at each step the VM table void update_neighbors(buzzvm_t vm) { - /* Reset neighbor information */ + // Reset neighbor information buzzneighbors_reset(vm); - /* Get robot id and update neighbor information */ + // Get robot id and update neighbor information map::iterator it; for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it) { @@ -612,8 +635,10 @@ void update_neighbors(buzzvm_t vm) } } -/****************************************/ int buzzuav_update_currentpos(buzzvm_t vm) +/* +/ Update the BVM position table +/------------------------------------------------------*/ { buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); buzzvm_pusht(vm); @@ -634,16 +659,18 @@ int buzzuav_update_currentpos(buzzvm_t vm) } void flight_status_update(uint8_t state) +/* +/ Update the interface status variable +/------------------------------------------------------*/ { status = state; } -/*---------------------------------------------------- +int buzzuav_update_flight_status(buzzvm_t vm) +/* / 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); @@ -657,7 +684,6 @@ int buzzuav_update_flight_status(buzzvm_t vm) 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); @@ -680,32 +706,31 @@ int buzzuav_update_flight_status(buzzvm_t vm) return vm->state; } -/******************************************************/ -/*Create an obstacle Buzz table from proximity sensors*/ -/* Acessing proximity in buzz script - proximity[0].angle and proximity[0].value - front - "" "" "" - right and back - proximity[3].angle and proximity[3].value - left - proximity[4].angle = -1 and proximity[4].value -bottom */ -/******************************************************/ - int buzzuav_update_prox(buzzvm_t vm) +/* +/ Create an obstacle Buzz table from proximity sensors +/ Acessing proximity in buzz script +/ proximity[0].angle and proximity[0].value - front +/ "" "" "" - right and back +/ proximity[3].angle and proximity[3].value - left +/ proximity[4].angle = -1 and proximity[4].value -bottom +-------------------------------------------------------------*/ { buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); buzzvm_pusht(vm); buzzobj_t tProxTable = buzzvm_stack_at(vm, 1); buzzvm_gstore(vm); - /* Fill into the proximity table */ + // Fill into the proximity table buzzobj_t tProxRead; float angle = 0; for (size_t i = 0; i < 4; ++i) { - /* Create table for i-th read */ + // Create table for i-th read buzzvm_pusht(vm); tProxRead = buzzvm_stack_at(vm, 1); buzzvm_pop(vm); - /* Fill in the read */ + // Fill in the read buzzvm_push(vm, tProxRead); buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); buzzvm_pushf(vm, obst[i + 1]); @@ -714,19 +739,19 @@ int buzzuav_update_prox(buzzvm_t vm) buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); buzzvm_pushf(vm, angle); buzzvm_tput(vm); - /* Store read table in the proximity table */ + // Store read table in the proximity table buzzvm_push(vm, tProxTable); buzzvm_pushi(vm, i); buzzvm_push(vm, tProxRead); buzzvm_tput(vm); angle += 1.5708; } - /* Create table for bottom read */ + // Create table for bottom read angle = -1; buzzvm_pusht(vm); tProxRead = buzzvm_stack_at(vm, 1); buzzvm_pop(vm); - /* Fill in the read */ + // Fill in the read buzzvm_push(vm, tProxRead); buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); buzzvm_pushf(vm, obst[0]); @@ -735,7 +760,7 @@ int buzzuav_update_prox(buzzvm_t vm) buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); buzzvm_pushf(vm, angle); buzzvm_tput(vm); - /*Store read table in the proximity table*/ + // Store read table in the proximity table buzzvm_push(vm, tProxTable); buzzvm_pushi(vm, 4); buzzvm_push(vm, tProxRead); @@ -743,11 +768,10 @@ int buzzuav_update_prox(buzzvm_t vm) return vm->state; } -/**********************************************/ -/*Dummy closure for use during update testing */ -/**********************************************/ - int dummy_closure(buzzvm_t vm) +/* +/ Dummy closure for use during update testing +----------------------------------------------------*/ { return buzzvm_ret0(vm); } diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index 195d672..b849cbd 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -8,19 +8,17 @@ #include "roscontroller.h" -/** - * This program implements Buzz node in ros using mavros_msgs. - */ - int main(int argc, char** argv) +/* + / This program implements Buzz in a ROS node using mavros_msgs. + -----------------------------------------------------------------*/ { - /*Initialize rosBuzz node*/ + // Initialize rosBuzz node ros::init(argc, argv, "rosBuzz"); ros::NodeHandle nh_priv("~"); ros::NodeHandle nh; rosbzz_node::roscontroller RosController(nh, nh_priv); - /*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 468c908..5bbdf0c 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -13,38 +13,36 @@ namespace rosbzz_node const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; const bool debug = false; -/*--------------- -/Constructor ----------------*/ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) +/* +/ roscontroller class Constructor +---------------*/ { ROS_INFO("Buzz_node"); - /*Obtain parameters from ros parameter server*/ + // Obtain parameters from ros parameter server Rosparameters_get(n_c_priv); - /*Initialize publishers, subscribers and client*/ + // Initialize publishers, subscribers and client Initialize_pub_sub(n_c); - /*Compile the .bzz file to .basm, .bo and .bdbg*/ + // Compile the .bzz file to .basm, .bo and .bdbg std::string fname = Compile_bzz(bzzfile_name); bcfname = fname + ".bo"; dbgfname = fname + ".bdb"; set_bzz_file(bzzfile_name.c_str()); buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); - /*Initialize variables*/ - // Solo things + // Initialize variables SetMode("LOITER", 0); armstate = 0; multi_msg = true; - // set stream rate - wait for the FC to be started - SetStreamRate(0, 10, 1); - // Get Robot Id - wait for Xbee to be started - setpoint_counter = 0; fcu_timeout = TIMEOUT; - cur_pos.longitude = 0; cur_pos.latitude = 0; cur_pos.altitude = 0; + // set stream rate - wait for the FC to be started + SetStreamRate(0, 10, 1); + + // Get Home position - wait for none-zero value while (cur_pos.latitude == 0.0f) { ROS_INFO("Waiting for GPS. "); @@ -52,6 +50,8 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) ros::spinOnce(); } + // Get Robot Id - wait for Xbee to be started + // or else parse it from the launch file parameter if (xbeeplugged) { GetRobotId(); @@ -62,19 +62,19 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) } } -/*--------------------- -/Destructor ----------------------*/ roscontroller::~roscontroller() +/* +/ roscontroller class Destructor +---------------------*/ { - /* All done */ - /* Cleanup */ + // Destroy the BVM buzz_utility::buzz_script_destroy(); - /* Stop the robot */ - log.close(); } void roscontroller::GetRobotId() +/* +/ Get robot ID from the Xbee service +---------------------*/ { mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id = "id"; @@ -88,6 +88,1040 @@ void roscontroller::GetRobotId() robot_id = robot_id_srv_response.value.integer; } +void roscontroller::send_MPpayload() +{ + MPpayload_pub.publish(buzzuav_closures::get_status()); +} + +void roscontroller::RosControllerRun() +/* +/rosbuzz_node main loop method +/--------------------------------------------------*/ +{ + // Set the Buzz bytecode + if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id)) + { + ROS_INFO("[%i] INIT DONE!!!", robot_id); + int packet_loss_tmp, time_step = 0; + double cur_packet_loss = 0; + ROS_INFO("[%i] Bytecode file found and set", robot_id); + std::string standby_bo = Compile_bzz(stand_by) + ".bo"; + // Intialize the update monitor + init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); + // set ROS loop rate + ros::Rate loop_rate(BUZZRATE); + // DEBUG + // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); + while (ros::ok() && !buzz_utility::buzz_script_done()) + { + // Neighbours of the robot published with id in respective topic + neighbours_pos_publisher(); + send_MPpayload(); + // Check updater state and step code + update_routine(); + if (time_step == BUZZRATE) + { + time_step = 0; + cur_packet_loss = 1 - ((double)packet_loss_tmp / (BUZZRATE * ((int)no_of_robots - 1))); + packet_loss_tmp = 0; + if (cur_packet_loss < 0) + cur_packet_loss = 0; + else if (cur_packet_loss > 1) + cur_packet_loss = 1; + } + else + { + packet_loss_tmp += (int)buzz_utility::get_inmsg_size(); + time_step++; + } + if (debug) + ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); + // Call Step from buzz script + buzz_utility::buzz_script_step(); + // Prepare messages and publish them + prepare_msg_and_publish(); + // Call the flight controler service + flight_controller_service_call(); + // Set multi message available after update + if (get_update_status()) + { + set_read_update_status(); + } + // Set ROBOTS variable (swarm size) + get_number_of_robots(); + buzz_utility::set_robot_var(no_of_robots); + updates_set_robots(no_of_robots); + // get_xbee_status(); // commented out because it may slow down the node too much, to be tested + + ros::spinOnce(); + loop_rate.sleep(); + // make sure to sleep for the rest of the loop time + if (loop_rate.cycleTime() > ros::Duration(1.0 / (float)BUZZRATE)) + ROS_WARN("ROSBuzz loop missed its desired rate of %iHz... the loop actually took %.4f seconds", BUZZRATE, + loop_rate.cycleTime().toSec()); + // Safety land if the data coming from the flight controller are too old + if (fcu_timeout <= 0) + buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); + else + fcu_timeout -= 1 / BUZZRATE; + timer_step += 1; + // Force a refresh on neighbors array once in a while + maintain_pos(timer_step); + // DEBUG + // std::cout<< "HOME: " << home.latitude << ", " << home.longitude; + } + } +} + +void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) +/* +/ Get all required parameters from the ROS launch file +/-------------------------------------------------------*/ +{ + // Obtain .bzz file name from launch file parameter + if (n_c.getParam("bzzfile_name", bzzfile_name)) + ; + else + { + ROS_ERROR("Provide a .bzz file to run in Launch file"); + system("rosnode kill rosbuzz_node"); + } + // Obtain rc service option from parameter server + if (n_c.getParam("rcclient", rcclient)) + { + if (rcclient == true) + { + if (!n_c.getParam("rcservice_name", rcservice_name)) + { + ROS_ERROR("Provide a name topic name for rc service in Launch file"); + system("rosnode kill rosbuzz_node"); + } + } + else if (rcclient == false) + { + ROS_INFO("RC service is disabled"); + } + } + else + { + ROS_ERROR("Provide a rc client option: yes or no in Launch file"); + system("rosnode kill rosbuzz_node"); + } + // Obtain out payload name + n_c.getParam("out_payload", out_payload); + // Obtain in payload name + n_c.getParam("in_payload", in_payload); + // Obtain standby script to run during update + n_c.getParam("stand_by", stand_by); + + if (n_c.getParam("xbee_plugged", xbeeplugged)) + ; + else + { + ROS_ERROR("Provide the xbee plugged boolean in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (!xbeeplugged) + { + if (n_c.getParam("name", robot_name)) + ; + else + { + ROS_ERROR("Provide the xbee plugged boolean in Launch file"); + system("rosnode kill rosbuzz_node"); + } + } + else + n_c.getParam("xbee_status_srv", xbeesrv_name); + + if (!n_c.getParam("capture_image_srv", capture_srv_name)) + { + capture_srv_name = CAPTURE_SRV_DEFAULT_NAME; + } + + GetSubscriptionParameters(n_c); +} + +void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) +/* +/Obtains publisher, subscriber and services from yml config file +/-----------------------------------------------------------------------------------*/ +{ + m_sMySubscriptions.clear(); + std::string gps_topic; + if (node_handle.getParam("topics/gps", gps_topic)) + ; + else + { + ROS_ERROR("Provide a gps topic in Launch file"); + system("rosnode kill rosbuzz_node"); + } + m_smTopic_infos.insert(pair(gps_topic, "sensor_msgs/NavSatFix")); + + std::string battery_topic; + node_handle.getParam("topics/battery", battery_topic); + m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryStatus")); + + std::string status_topic; + node_handle.getParam("topics/status", status_topic); + m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/State")); + node_handle.getParam("topics/estatus", status_topic); + m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/ExtendedState")); + + std::string altitude_topic; + node_handle.getParam("topics/altitude", altitude_topic); + m_smTopic_infos.insert(pair(altitude_topic, "std_msgs/Float64")); + + // Obtain required topic and service names from the parameter server + if (node_handle.getParam("topics/fcclient", fcclient_name)) + ; + else + { + ROS_ERROR("Provide a fc client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/setpoint", setpoint_name)) + ; + else + { + ROS_ERROR("Provide a Set Point name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/armclient", armclient)) + ; + else + { + ROS_ERROR("Provide an arm client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/modeclient", modeclient)) + ; + else + { + ROS_ERROR("Provide a mode client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/stream", stream_client_name)) + ; + else + { + ROS_ERROR("Provide a mode client name in Launch file"); + system("rosnode kill rosbuzz_node"); + } + if (node_handle.getParam("topics/localpos", local_pos_sub_name)) + ; + else + { + ROS_ERROR("Provide a localpos name in YAML file"); + system("rosnode kill rosbuzz_node"); + } + + node_handle.getParam("obstacles", obstacles_topic); +} + +void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) +/* +/ Create all required subscribers, publishers and ROS service clients +/-------------------------------------------------------*/ +{ + // subscribers + + Subscribe(n_c); + + payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); + + obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this); + + // publishers + payload_pub = n_c.advertise(out_payload, 5); + MPpayload_pub = n_c.advertise("fleet_status", 5); + neigh_pos_pub = n_c.advertise("neighbours_pos", 5); + uavstate_pub = n_c.advertise("uavstate", 5); + localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); + + // Service Clients + arm_client = n_c.serviceClient(armclient); + mode_client = n_c.serviceClient(modeclient); + mav_client = n_c.serviceClient(fcclient_name); + if (rcclient == true) + service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); + ROS_INFO("Ready to receive Mav Commands from RC client"); + xbeestatus_srv = n_c.serviceClient(xbeesrv_name); + capture_srv = n_c.serviceClient(capture_srv_name); + stream_client = n_c.serviceClient(stream_client_name); + + local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this); + + multi_msg = true; +} + +void roscontroller::Subscribe(ros::NodeHandle& n_c) +/* +/ Robot independent subscribers +/--------------------------------------*/ +{ + for (std::map::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it) + { + if (it->second == "mavros_msgs/ExtendedState") + { + flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this); + } + else if (it->second == "mavros_msgs/State") + { + flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); + } + else if (it->second == "mavros_msgs/BatteryStatus") + { + battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); + } + else if (it->second == "sensor_msgs/NavSatFix") + { + current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); + } + else if (it->second == "std_msgs/Float64") + { + relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); + } + + std::cout << "Subscribed to: " << it->first << endl; + } +} + +std::string roscontroller::Compile_bzz(std::string bzzfile_name) +/* +/ Create Buzz bytecode from the bzz script inputed +/-------------------------------------------------------*/ +{ + // Compile the buzz code .bzz to .bo + 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/"; + bzzfile_in_compile << " -b " << path << name << ".bo"; + bzzfile_in_compile << " -d " << path << name << ".bdb "; + bzzfile_in_compile << bzzfile_name; + + ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str()); + + system(bzzfile_in_compile.str().c_str()); + + return path + name; +} + +void roscontroller::neighbours_pos_publisher() +/* +/ Publish neighbours pos and id in neighbours pos topic +/----------------------------------------------------*/ +{ + auto current_time = ros::Time::now(); + map::iterator it; + rosbuzz::neigh_pos neigh_pos_array; + 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; + neigh_tmp.header.stamp = current_time; + neigh_tmp.header.frame_id = "/world"; + neigh_tmp.position_covariance_type = it->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); + // DEBUG + // cout<<"iterator it val: "<< it-> first << " After convertion: " + // <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<= BUZZRATE) + { + neighbours_pos_map.clear(); + // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but + // have to clear ! + timer_step = 0; + } +} + +void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr) +/* +/Puts neighbours position into local struct storage that is cleared every 10 +step +/---------------------------------------------------------------------------------*/ +{ + map::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)); +} + +void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr) +/* +/Puts raw neighbours position into local storage for neighbours pos publisher +/-----------------------------------------------------------------------------------*/ +{ + map::iterator it = raw_neighbours_pos_map.find(id); + if (it != raw_neighbours_pos_map.end()) + raw_neighbours_pos_map.erase(it); + raw_neighbours_pos_map.insert(make_pair(id, pos_arr)); +} + +void roscontroller::set_cur_pos(double latitude, double longitude, double altitude) +/* +/Set the current position of the robot callback +/--------------------------------------------------------*/ +{ + cur_pos.latitude = latitude; + cur_pos.longitude = longitude; + cur_pos.altitude = altitude; +} + +float roscontroller::constrainAngle(float x) +/* +/ Wrap the angle between -pi, pi +----------------------------------------------------------- */ +{ + x = fmod(x, 2 * M_PI); + if (x < 0.0) + x += 2 * M_PI; + return x; +} + +void roscontroller::gps_rb(GPS nei_pos, double out[]) +/* +/ Compute Range and Bearing of a neighbor in a local reference frame +/ from GPS coordinates +----------------------------------------------------------- */ +{ + 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); + out[1] = constrainAngle(atan2(ned_y, ned_x)); + // 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) +/* +/ Get GPS from NED and a reference GPS point (struct input) +----------------------------------------------------------- */ +{ + gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, cur_pos.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) +/* +/ Get GPS from NED and a reference GPS point +----------------------------------------------------------- */ +{ + 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)); +}; + +void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg) +/* +/ Update battery status into BVM from subscriber +/------------------------------------------------------*/ +{ + buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining); + // DEBUG + // ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, + // msg->current, msg ->remaining); +} + +void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg) +/* +/Update flight extended status into BVM from subscriber for solos +/---------------------------------------------------------------------*/ +{ + // http://wiki.ros.org/mavros/CustomModes + std::cout << "Message: " << msg->mode << std::endl; + if (msg->mode == "GUIDED") + buzzuav_closures::flight_status_update(2); + else if (msg->mode == "LAND") + buzzuav_closures::flight_status_update(1); + else + buzzuav_closures::flight_status_update(7); // default to fit mavros::extended_state +} + +void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg) +/* +/Update flight extended status into BVM from subscriber +------------------------------------------------------------*/ +{ + buzzuav_closures::flight_status_update(msg->landed_state); +} + +void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg) +/* +/ Update current GPS position into BVM from subscriber +/-------------------------------------------------------------*/ +{ + // DEBUG + // ROS_INFO("Altitude out: %f", cur_rel_altitude); + fcu_timeout = TIMEOUT; + set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); + buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); +} + +void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose) +/* +/ Update current position for flight controller NED offset +/-------------------------------------------------------------*/ +{ + local_pos_new[0] = pose->pose.position.x; + local_pos_new[1] = pose->pose.position.y; + local_pos_new[2] = pose->pose.position.z; +} + +void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg) +/* +/ Update altitude into BVM from subscriber +/-------------------------------------------------------------*/ +{ + // DEBUG + // ROS_INFO("Altitude in: %f", msg->data); + cur_rel_altitude = (double)msg->data; +} + +void roscontroller::obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg) +/* +/Set obstacle Obstacle distance table into BVM from subscriber +/-------------------------------------------------------------*/ +{ + float obst[5]; + for (int i = 0; i < 5; i++) + obst[i] = msg->ranges[i]; + buzzuav_closures::set_obstacle_dist(obst); +} + +void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) +/* +/ Publisher to send the flight controller navigation commands in local coordinates +/-------------------------------------------------------------*/ +{ + // http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html + // http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned + + geometry_msgs::PoseStamped moveMsg; + moveMsg.header.stamp = ros::Time::now(); + moveMsg.header.seq = setpoint_counter++; + moveMsg.header.frame_id = 1; + + // DEBUG + // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y); + moveMsg.pose.position.x = local_pos_new[0] + y; + moveMsg.pose.position.y = local_pos_new[1] + x; + moveMsg.pose.position.z = z; + + moveMsg.pose.orientation.x = 0; + moveMsg.pose.orientation.y = 0; + moveMsg.pose.orientation.z = 0; + moveMsg.pose.orientation.w = 1; + + // To prevent drifting from stable position, uncomment + // if(fabs(x)>0.005 || fabs(y)>0.005) { + localsetpoint_nonraw_pub.publish(moveMsg); + // } +} + +void roscontroller::SetMode(std::string mode, int delay_miliseconds) +/* +/ Use setmode service of the flight controller +/-------------------------------------------------------------*/ +{ + // wait if necessary + if (delay_miliseconds != 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(delay_miliseconds)); + } + // set mode + mavros_msgs::SetMode set_mode_message; + set_mode_message.request.base_mode = 0; + set_mode_message.request.custom_mode = mode; + current_mode = mode; + if (mode_client.call(set_mode_message)) + { + ; // DEBUG + // ROS_INFO("Set Mode Service call successful!"); + } + else + { + ROS_INFO("Set Mode Service call failed!"); + } +} + +void roscontroller::SetStreamRate(int id, int rate, int on_off) +/* +/ Set the streamrate for mavros publications +/-------------------------------------------------------------*/ +{ + mavros_msgs::StreamRate message; + message.request.stream_id = id; + message.request.message_rate = rate; + message.request.on_off = on_off; + + while (!stream_client.call(message)) + { + ROS_INFO("Set stream rate call failed!, trying again..."); + ros::Duration(0.1).sleep(); + } + // DEBUG + // ROS_INFO("Set stream rate call successful"); +} + +void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) +/* +/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......... | +/|_____|_____|_____|________________________________________________|______________________________| + +-----------------------------------------------------------------------------------------------------*/ +{ + // Check for Updater message, if updater message push it into updater FIFO + if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT) + { + 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]; + } + + uint8_t* pl = (uint8_t*)malloc(obt_msg_size); + memset(pl, 0, obt_msg_size); + // Copy packet into temporary buffer neglecting update constant + memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); + uint16_t unMsgSize = *(uint16_t*)(pl); + fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); + if (unMsgSize > 0) + { + code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); + code_message_inqueue_process(); + } + free(pl); + } + // BVM FIFO message + else if (msg->payload64[0] == (uint64_t)XBEE_MESSAGE_CONSTANT) + { + uint64_t message_obt[msg->payload64.size() - 1]; + // Go throught the obtained payload + for (int i = 1; i < (int)msg->payload64.size(); i++) + { + message_obt[i - 1] = (uint64_t)msg->payload64[i]; + } + // Extract neighbours position from payload + 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]); + 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]; + 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)); + if (debug) + ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); + // Pass neighbour position to local maintaner + buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1], + cvt_neighbours_pos_payload[2]); + // Put RID and pos + raw_neighbours_pos_put((int)out[1], raw_neigh_pos); + // TODO: remove roscontroller local map array for neighbors + neighbours_pos_put((int)out[1], n_pos); + buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); + delete[] out; + buzz_utility::in_msg_append((message_obt + 3)); + } +} + +bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res) +/* +/ Catch the ROS service call from a custom remote controller (Mission Planner) +/ and send the requested commands to Buzz +----------------------------------------------------------- */ +{ + int rc_cmd; + switch (req.command) + { + case mavros_msgs::CommandCode::NAV_TAKEOFF: + ROS_INFO("RC_call: TAKE OFF!!!!"); + rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case mavros_msgs::CommandCode::NAV_LAND: + ROS_INFO("RC_Call: LAND!!!!"); + rc_cmd = mavros_msgs::CommandCode::NAV_LAND; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: + rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; + armstate = req.param1; + if (armstate) + { + ROS_INFO("RC_Call: ARM!!!!"); + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + } + else + { + ROS_INFO("RC_Call: DISARM!!!!"); + buzzuav_closures::rc_call(rc_cmd + 1); + res.success = true; + } + break; + case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: + ROS_INFO("RC_Call: GO HOME!!!!"); + rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case mavros_msgs::CommandCode::NAV_WAYPOINT: + ROS_INFO("RC_Call: GO TO!!!! "); + buzzuav_closures::rc_set_goto(req.param1, req.param5, req.param6, req.param7); + rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: + ROS_INFO("RC_Call: Gimbal!!!! "); + buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); + rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + case CMD_REQUEST_UPDATE: + rc_cmd = CMD_REQUEST_UPDATE; + buzzuav_closures::rc_call(rc_cmd); + res.success = true; + break; + default: + buzzuav_closures::rc_call(req.command); + ROS_INFO("----> Received unregistered command: ", req.command); + res.success = true; + break; + } + return true; +} + +void roscontroller::get_number_of_robots() +/* +/ Garbage collector for the number of robots in the swarm +--------------------------------------------------------------------------*/ +{ + int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1; + if (no_of_robots == 0) + { + no_of_robots = cur_robots; + } + else + { + if (no_of_robots != cur_robots && no_cnt == 0) + { + no_cnt++; + old_val = cur_robots; + } + else if (no_cnt != 0 && old_val == cur_robots) + { + no_cnt++; + if (no_cnt >= 150 || cur_robots > no_of_robots) + { + no_of_robots = cur_robots; + no_cnt = 0; + } + } + else + { + no_cnt = 0; + } + } +} + +/* +/ Set of functions to grab network quality data from Zbee service +--------------------------------------------------------------------------*/ bool roscontroller::GetDequeFull(bool& result) { mavros_msgs::ParamGet::Request srv_request; @@ -201,1017 +1235,8 @@ bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float& result) return srv_response.success; } -void roscontroller::send_MPpayload() -{ - MPpayload_pub.publish(buzzuav_closures::get_status()); -} - -/*------------------------------------------------- -/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)) - { - ROS_INFO("[%i] INIT DONE!!!", robot_id); - int packet_loss_tmp, time_step = 0; - double cur_packet_loss = 0; - ROS_INFO("[%i] Bytecode file found and set", robot_id); - std::string standby_bo = Compile_bzz(stand_by) + ".bo"; - /*Intialize the update monitor*/ - init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); - /*loop rate of ros*/ - ros::Rate loop_rate(BUZZRATE); - /////////////////////////////////////////////////////// - // MAIN LOOP - ////////////////////////////////////////////////////// - // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); - while (ros::ok() && !buzz_utility::buzz_script_done()) - { - /*Neighbours of the robot published with id in respective topic*/ - neighbours_pos_publisher(); - send_MPpayload(); - /*Check updater state and step code*/ - update_routine(); - if (time_step == BUZZRATE) - { - time_step = 0; - cur_packet_loss = 1 - ((double)packet_loss_tmp / (BUZZRATE * ((int)no_of_robots - 1))); - packet_loss_tmp = 0; - if (cur_packet_loss < 0) - cur_packet_loss = 0; - else if (cur_packet_loss > 1) - cur_packet_loss = 1; - } - else - { - packet_loss_tmp += (int)buzz_utility::get_inmsg_size(); - time_step++; - } - if (debug) - ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); - /*Step buzz script */ - buzz_utility::buzz_script_step(); - /*Prepare messages and publish them in respective topic*/ - prepare_msg_and_publish(); - /*call flight controler service to set command long*/ - flight_controller_service_call(); - /*Set multi message available after update*/ - if (get_update_status()) - { - set_read_update_status(); - } - /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ - get_number_of_robots(); - buzz_utility::set_robot_var(no_of_robots); - updates_set_robots(no_of_robots); - // get_xbee_status(); // commented out because it may slow down the node too much, to be tested - /*run once*/ - ros::spinOnce(); - loop_rate.sleep(); - // make sure to sleep for the remainder of our cycle time - if (loop_rate.cycleTime() > ros::Duration(1.0 / (float)BUZZRATE)) - ROS_WARN("ROSBuzz loop missed its desired rate of %iHz... the loop actually took %.4f seconds", BUZZRATE, - loop_rate.cycleTime().toSec()); - if (fcu_timeout <= 0) - buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); - else - fcu_timeout -= 1 / BUZZRATE; - /*sleep for the mentioned loop rate*/ - timer_step += 1; - maintain_pos(timer_step); - // std::cout<< "HOME: " << home.latitude << ", " << home.longitude; - } - } -} - -/*-------------------------------------------------------- -/ Get all required parameters from the ROS launch file -/-------------------------------------------------------*/ -void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) -{ - /*Obtain .bzz file name from parameter server*/ - if (n_c.getParam("bzzfile_name", bzzfile_name)) - ; - else - { - ROS_ERROR("Provide a .bzz file to run in Launch file"); - system("rosnode kill rosbuzz_node"); - } - /*Obtain rc service option from parameter server*/ - if (n_c.getParam("rcclient", rcclient)) - { - if (rcclient == true) - { - /*Service*/ - if (!n_c.getParam("rcservice_name", rcservice_name)) - { - ROS_ERROR("Provide a name topic name for rc service in Launch file"); - system("rosnode kill rosbuzz_node"); - } - } - else if (rcclient == false) - { - ROS_INFO("RC service is disabled"); - } - } - else - { - ROS_ERROR("Provide a rc client option: yes or no in Launch file"); - system("rosnode kill rosbuzz_node"); - } - /*Obtain out payload name*/ - n_c.getParam("out_payload", out_payload); - /*Obtain in payload name*/ - n_c.getParam("in_payload", in_payload); - /*Obtain standby script to run during update*/ - n_c.getParam("stand_by", stand_by); - - if (n_c.getParam("xbee_plugged", xbeeplugged)) - ; - else - { - ROS_ERROR("Provide the xbee plugged boolean in Launch file"); - system("rosnode kill rosbuzz_node"); - } - if (!xbeeplugged) - { - if (n_c.getParam("name", robot_name)) - ; - else - { - ROS_ERROR("Provide the xbee plugged boolean in Launch file"); - system("rosnode kill rosbuzz_node"); - } - } - else - n_c.getParam("xbee_status_srv", xbeesrv_name); - - if (!n_c.getParam("capture_image_srv", capture_srv_name)) - { - capture_srv_name = CAPTURE_SRV_DEFAULT_NAME; - } - - GetSubscriptionParameters(n_c); - // initialize topics to null? -} -/*----------------------------------------------------------------------------------- -/Obtains publisher, subscriber and services from yml file based on the robot -used -/-----------------------------------------------------------------------------------*/ -void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) -{ - m_sMySubscriptions.clear(); - std::string gps_topic; - if (node_handle.getParam("topics/gps", gps_topic)) - ; - else - { - ROS_ERROR("Provide a gps topic in Launch file"); - system("rosnode kill rosbuzz_node"); - } - m_smTopic_infos.insert(pair(gps_topic, "sensor_msgs/NavSatFix")); - - std::string battery_topic; - node_handle.getParam("topics/battery", battery_topic); - m_smTopic_infos.insert(pair(battery_topic, "mavros_msgs/BatteryStatus")); - - std::string status_topic; - node_handle.getParam("topics/status", status_topic); - m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/State")); - node_handle.getParam("topics/estatus", status_topic); - m_smTopic_infos.insert(pair(status_topic, "mavros_msgs/ExtendedState")); - - std::string altitude_topic; - node_handle.getParam("topics/altitude", altitude_topic); - m_smTopic_infos.insert(pair(altitude_topic, "std_msgs/Float64")); - - // Obtain required topic and service names from the parameter server - if (node_handle.getParam("topics/fcclient", fcclient_name)) - ; - else - { - ROS_ERROR("Provide a fc client name in Launch file"); - system("rosnode kill rosbuzz_node"); - } - if (node_handle.getParam("topics/setpoint", setpoint_name)) - ; - else - { - ROS_ERROR("Provide a Set Point name in Launch file"); - system("rosnode kill rosbuzz_node"); - } - if (node_handle.getParam("topics/armclient", armclient)) - ; - else - { - ROS_ERROR("Provide an arm client name in Launch file"); - system("rosnode kill rosbuzz_node"); - } - if (node_handle.getParam("topics/modeclient", modeclient)) - ; - else - { - ROS_ERROR("Provide a mode client name in Launch file"); - system("rosnode kill rosbuzz_node"); - } - if (node_handle.getParam("topics/stream", stream_client_name)) - ; - else - { - ROS_ERROR("Provide a mode client name in Launch file"); - system("rosnode kill rosbuzz_node"); - } - if (node_handle.getParam("topics/localpos", local_pos_sub_name)) - ; - else - { - ROS_ERROR("Provide a localpos name in YAML file"); - system("rosnode kill rosbuzz_node"); - } - - node_handle.getParam("obstacles", obstacles_topic); -} - -/*-------------------------------------------------------- -/ Create all required subscribers, publishers and ROS service clients -/-------------------------------------------------------*/ -void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) -{ - /*subscribers*/ - - Subscribe(n_c); - - payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this); - - obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this); - - /*publishers*/ - payload_pub = n_c.advertise(out_payload, 5); - MPpayload_pub = n_c.advertise("fleet_status", 5); - neigh_pos_pub = n_c.advertise("neighbours_pos", 5); - uavstate_pub = n_c.advertise("uavstate", 5); - localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); - - /* Service Clients*/ - arm_client = n_c.serviceClient(armclient); - mode_client = n_c.serviceClient(modeclient); - mav_client = n_c.serviceClient(fcclient_name); - if (rcclient == true) - service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); - ROS_INFO("Ready to receive Mav Commands from RC client"); - xbeestatus_srv = n_c.serviceClient(xbeesrv_name); - capture_srv = n_c.serviceClient(capture_srv_name); - stream_client = n_c.serviceClient(stream_client_name); - - local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this); - - 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) - { - if (it->second == "mavros_msgs/ExtendedState") - { - flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this); - } - else if (it->second == "mavros_msgs/State") - { - flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); - } - else if (it->second == "mavros_msgs/BatteryStatus") - { - battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); - } - else if (it->second == "sensor_msgs/NavSatFix") - { - current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); - } - else if (it->second == "std_msgs/Float64") - { - relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); - } - - std::cout << "Subscribed to: " << it->first << endl; - } -} - -/*-------------------------------------------------------- -/ Create Buzz bytecode from the bzz script inputed -/-------------------------------------------------------*/ -std::string roscontroller::Compile_bzz(std::string bzzfile_name) -{ - /*Compile the buzz code .bzz to .bo*/ - 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/"; - bzzfile_in_compile << " -b " << path << name << ".bo"; - bzzfile_in_compile << " -d " << path << name << ".bdb "; - bzzfile_in_compile << bzzfile_name; - - ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str()); - - system(bzzfile_in_compile.str().c_str()); - - return path + name; -} -/*---------------------------------------------------- -/ Publish neighbours pos and id in neighbours pos topic -/----------------------------------------------------*/ -void roscontroller::neighbours_pos_publisher() -{ - auto current_time = ros::Time::now(); - map::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"<= BUZZRATE) - { - neighbours_pos_map.clear(); - // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but - // have to clear ! - timer_step = 0; - } -} - -/*--------------------------------------------------------------------------------- -/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::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 into local storage for neighbours pos publisher -/-----------------------------------------------------------------------------------*/ -void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr) -{ - map::iterator it = raw_neighbours_pos_map.find(id); - if (it != raw_neighbours_pos_map.end()) - raw_neighbours_pos_map.erase(it); - raw_neighbours_pos_map.insert(make_pair(id, pos_arr)); -} - -/*-------------------------------------------------------- -/Set the current position of the robot callback -/--------------------------------------------------------*/ -void roscontroller::set_cur_pos(double latitude, double longitude, double altitude) -{ - cur_pos.latitude = latitude; - cur_pos.longitude = longitude; - cur_pos.altitude = altitude; -} - -/*----------------------------------------------------------- -/ Compute Range and Bearing of a neighbor in a local reference frame -/ from GPS coordinates ------------------------------------------------------------ */ -float roscontroller::constrainAngle(float x) -{ - x = fmod(x, 2 * M_PI); - if (x < 0.0) - x += 2 * M_PI; - return x; -} - -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); - out[1] = constrainAngle(atan2(ned_y, ned_x)); - // 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_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 -/------------------------------------------------------*/ -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); -} - -/*---------------------------------------------------------------------- -/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 - std::cout << "Message: " << msg->mode << std::endl; - if (msg->mode == "GUIDED") - buzzuav_closures::flight_status_update(2); - else if (msg->mode == "LAND") - buzzuav_closures::flight_status_update(1); - else // ground standby = LOITER? - buzzuav_closures::flight_status_update(7); //? -} - -/*------------------------------------------------------------ -/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); -} -/*------------------------------------------------------------- -/ Update current position into BVM from subscriber -/-------------------------------------------------------------*/ -void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg) -{ - // ROS_INFO("Altitude out: %f", cur_rel_altitude); - fcu_timeout = TIMEOUT; - set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); - buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); -} - -void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose) -{ - local_pos_new[0] = pose->pose.position.x; - local_pos_new[1] = pose->pose.position.y; - local_pos_new[2] = pose->pose.position.z; -} - -/*------------------------------------------------------------- -/ Update altitude into BVM from subscriber -/-------------------------------------------------------------*/ -void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg) -{ - // ROS_INFO("Altitude in: %f", msg->data); - cur_rel_altitude = (double)msg->data; -} -/*------------------------------------------------------------- -/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); -} - -void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) -{ - // http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html - // http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned - - geometry_msgs::PoseStamped moveMsg; - moveMsg.header.stamp = ros::Time::now(); - moveMsg.header.seq = setpoint_counter++; - moveMsg.header.frame_id = 1; - - // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y); - moveMsg.pose.position.x = local_pos_new[0] + y; - moveMsg.pose.position.y = local_pos_new[1] + x; - moveMsg.pose.position.z = z; - - moveMsg.pose.orientation.x = 0; - moveMsg.pose.orientation.y = 0; - moveMsg.pose.orientation.z = 0; - moveMsg.pose.orientation.w = 1; - - // To prevent drifting from stable position. - // if(fabs(x)>0.005 || fabs(y)>0.005) { - localsetpoint_nonraw_pub.publish(moveMsg); - // ROS_INFO("Sent local NON RAW position message!"); - // } -} - -void roscontroller::SetMode(std::string mode, int delay_miliseconds) -{ - // wait if necessary - if (delay_miliseconds != 0) - { - std::this_thread::sleep_for(std::chrono::milliseconds(delay_miliseconds)); - } - // set mode - mavros_msgs::SetMode set_mode_message; - set_mode_message.request.base_mode = 0; - set_mode_message.request.custom_mode = mode; - current_mode = mode; - if (mode_client.call(set_mode_message)) - { - ; // ROS_INFO("Set Mode Service call successful!"); - } - else - { - ROS_INFO("Set Mode Service call failed!"); - } -} - -void roscontroller::SetStreamRate(int id, int rate, int on_off) -{ - mavros_msgs::StreamRate message; - message.request.stream_id = id; - message.request.message_rate = rate; - message.request.on_off = on_off; - - while (!stream_client.call(message)) - { - ROS_INFO("Set stream rate call failed!, trying again..."); - ros::Duration(0.1).sleep(); - } - // ROS_INFO("Set stream rate call successful"); -} - -/*------------------------------------------------------------- -/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) -{ - /*Check for Updater message, if updater message push it into updater FIFO*/ - if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT) - { - 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]; - } - - uint8_t* pl = (uint8_t*)malloc(obt_msg_size); - memset(pl, 0, obt_msg_size); - /* Copy packet into temporary buffer neglecting update constant */ - memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); - uint16_t unMsgSize = *(uint16_t*)(pl); - fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); - if (unMsgSize > 0) - { - code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); - code_message_inqueue_process(); - } - free(pl); - } - /*BVM FIFO message*/ - else if (msg->payload64[0] == (uint64_t)XBEE_MESSAGE_CONSTANT) - { - uint64_t message_obt[msg->payload64.size() - 1]; - /* Go throught the obtained payload*/ - for (int i = 1; i < (int)msg->payload64.size(); i++) - { - message_obt[i - 1] = (uint64_t)msg->payload64[i]; - } - /* Extract neighbours position from payload*/ - 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]); - 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]; - 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)); - if (debug) - ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); - /*pass neighbour position to local maintaner*/ - buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1], - cvt_neighbours_pos_payload[2]); - /*Put RID and pos*/ - raw_neighbours_pos_put((int)out[1], raw_neigh_pos); - /* TODO: remove roscontroller local map array for neighbors */ - neighbours_pos_put((int)out[1], n_pos); - buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); - delete[] out; - buzz_utility::in_msg_append((message_obt + 3)); - } -} - -/*----------------------------------------------------------- -/ Catch the ROS service call from a custom remote controller (Mission Planner) -/ and send the requested commands to Buzz ------------------------------------------------------------ */ -bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res) -{ - int rc_cmd; - switch (req.command) - { - case mavros_msgs::CommandCode::NAV_TAKEOFF: - ROS_INFO("RC_call: TAKE OFF!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::NAV_LAND: - ROS_INFO("RC_Call: LAND!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_LAND; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: - rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; - armstate = req.param1; - if (armstate) - { - ROS_INFO("RC_Call: ARM!!!!"); - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - } - else - { - ROS_INFO("RC_Call: DISARM!!!!"); - buzzuav_closures::rc_call(rc_cmd + 1); - res.success = true; - } - break; - case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: - ROS_INFO("RC_Call: GO HOME!!!!"); - rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::NAV_WAYPOINT: - ROS_INFO("RC_Call: GO TO!!!! "); - buzzuav_closures::rc_set_goto(req.param1, req.param5, req.param6, req.param7); - rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: - ROS_INFO("RC_Call: Gimbal!!!! "); - buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); - rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - case CMD_REQUEST_UPDATE: - // ROS_INFO("RC_Call: Update Fleet Status!!!!"); - rc_cmd = CMD_REQUEST_UPDATE; - buzzuav_closures::rc_call(rc_cmd); - res.success = true; - break; - default: - buzzuav_closures::rc_call(req.command); - ROS_INFO("----> Received unregistered command: ", req.command); - res.success = true; - break; - } - return true; -} - -void roscontroller::get_number_of_robots() -{ - int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1; - if (no_of_robots == 0) - { - no_of_robots = cur_robots; - } - else - { - if (no_of_robots != cur_robots && no_cnt == 0) - { - no_cnt++; - old_val = cur_robots; - } - else if (no_cnt != 0 && old_val == cur_robots) - { - no_cnt++; - if (no_cnt >= 150 || cur_robots > no_of_robots) - { - no_of_robots = cur_robots; - no_cnt = 0; - } - } - else - { - no_cnt = 0; - } - } -} - void roscontroller::get_xbee_status() -/* Description: +/* * Call all the xbee node services and update the xbee status ------------------------------------------------------------------ */ {