From 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7 Mon Sep 17 00:00:00 2001 From: pyhs Date: Fri, 23 Jun 2017 10:39:14 -0400 Subject: [PATCH] Untabification and Added get functions for Xbee_status --- include/buzzuav_closures.h | 8 +- include/roscontroller.h | 309 ++--- src/buzz_utility.cpp | 44 +- src/buzzuav_closures.cpp | 895 +++++++-------- src/roscontroller.cpp | 2179 ++++++++++++++++++++---------------- 5 files changed, 1828 insertions(+), 1607 deletions(-) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index e501285..cb405a3 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -14,8 +14,8 @@ namespace buzzuav_closures{ typedef enum { - COMMAND_NIL = 0, // Dummy command - COMMAND_TAKEOFF, // Take off + COMMAND_NIL = 0, // Dummy command + COMMAND_TAKEOFF, // Take off COMMAND_LAND, COMMAND_GOHOME, COMMAND_ARM, @@ -64,7 +64,7 @@ void set_obstacle_dist(float dist[]); */ int buzzuav_takeoff(buzzvm_t vm); /* - * Arm command from Buzz + * Arm command from Buzz */ int buzzuav_arm(buzzvm_t vm); /* @@ -89,7 +89,7 @@ int buzzuav_update_battery(buzzvm_t vm); int buzzuav_update_currentpos(buzzvm_t vm); int buzzuav_adduserRB(buzzvm_t vm); /* - * Updates flight status and rc command in Buzz, put it in a tabel to acess it + * Updates flight status and rc command in Buzz, put it in a tabel to acess it * use flight.status for flight status * use flight.rc_cmd for current rc cmd */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 006d78b..8f74af8 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -42,197 +42,206 @@ using namespace std; -namespace rosbzz_node{ - -class roscontroller{ +namespace rosbzz_node +{ + +class roscontroller +{ public: - roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); - ~roscontroller(); - //void RosControllerInit(); - void RosControllerRun(); - + roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); + ~roscontroller(); + //void RosControllerInit(); + void RosControllerRun(); + private: - struct num_robot_count - { - uint8_t history[10]; - uint8_t index=0; - uint8_t current=0; - num_robot_count(){} - - }; typedef struct num_robot_count Num_robot_count ; + struct num_robot_count + { + uint8_t history[10]; + uint8_t index = 0; + uint8_t current = 0; + num_robot_count(){} + }; typedef struct num_robot_count Num_robot_count ; // not useful in cpp - struct gps - { - double longitude=0.0; - double latitude=0.0; - float altitude=0.0; - }; typedef struct gps GPS ; - - GPS target, home, cur_pos; - double cur_rel_altitude; + struct gps + { + double longitude=0.0; + double latitude=0.0; + float altitude=0.0; + }; typedef struct gps GPS ; // not useful in cpp - uint64_t payload; - std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; - std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map; - //std::map< int, buzz_utility::Pos_struct> pub_neigh_pos; - int timer_step=0; - int robot_id=0; - std::string robot_name = ""; + GPS target, home, cur_pos; + double cur_rel_altitude; + + uint64_t payload; + std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; + std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map; + //std::map< int, buzz_utility::Pos_struct> pub_neigh_pos; + int timer_step=0; + int robot_id=0; + std::string robot_name = ""; //int oldcmdID=0; - int rc_cmd; - float fcu_timeout; - int armstate; - int barrier; - int message_number=0; - uint8_t no_of_robots=0; - /*tmp to be corrected*/ - uint8_t no_cnt=0; - uint8_t old_val=0; - std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name; - std::string stream_client_name; - std::string relative_altitude_sub_name; - std::string setpoint_nonraw; - bool rcclient; - bool xbeeplugged = false; - bool multi_msg; - Num_robot_count count_robots; - ros::ServiceClient mav_client; - ros::ServiceClient xbeestatus_srv; - ros::Publisher payload_pub; - ros::Publisher neigh_pos_pub; - ros::Publisher localsetpoint_nonraw_pub; - ros::ServiceServer service; - ros::Subscriber current_position_sub; - ros::Subscriber users_sub; - ros::Subscriber battery_sub; - ros::Subscriber payload_sub; - ros::Subscriber flight_status_sub; - ros::Subscriber obstacle_sub; - ros::Subscriber Robot_id_sub; - ros::Subscriber relative_altitude_sub; + int rc_cmd; + float fcu_timeout; + int armstate; + int barrier; + int message_number=0; + uint8_t no_of_robots=0; + /*tmp to be corrected*/ + uint8_t no_cnt=0; + uint8_t old_val=0 ; + std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name; + std::string stream_client_name; + std::string relative_altitude_sub_name; + std::string setpoint_nonraw; + bool rcclient; + bool xbeeplugged = false; + bool multi_msg; + Num_robot_count count_robots; + ros::ServiceClient mav_client; + ros::ServiceClient xbeestatus_srv; + ros::Publisher payload_pub; + ros::Publisher neigh_pos_pub; + ros::Publisher localsetpoint_nonraw_pub; + ros::ServiceServer service; + ros::Subscriber current_position_sub; + ros::Subscriber users_sub; + ros::Subscriber battery_sub; + ros::Subscriber payload_sub; + ros::Subscriber flight_status_sub; + ros::Subscriber obstacle_sub; + ros::Subscriber Robot_id_sub; + ros::Subscriber relative_altitude_sub; - std::string local_pos_sub_name; - ros::Subscriber local_pos_sub; - double local_pos_new[3]; + std::string local_pos_sub_name; + ros::Subscriber local_pos_sub; + double local_pos_new[3]; - ros::ServiceClient stream_client; + ros::ServiceClient stream_client; - int setpoint_counter; - double my_x = 0, my_y = 0; - - std::ofstream log; + int setpoint_counter; + double my_x = 0, my_y = 0; - /*Commands for flight controller*/ - //mavros_msgs::CommandInt cmd_srv; - mavros_msgs::CommandLong cmd_srv; - std::vector m_sMySubscriptions; - std::map m_smTopic_infos; + std::ofstream log; - mavros_msgs::CommandBool m_cmdBool; - ros::ServiceClient arm_client; + /*Commands for flight controller*/ + //mavros_msgs::CommandInt cmd_srv; + mavros_msgs::CommandLong cmd_srv; + std::vector m_sMySubscriptions; + std::map m_smTopic_infos; - 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); + mavros_msgs::CommandBool m_cmdBool; + ros::ServiceClient arm_client; - std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode + mavros_msgs::SetMode m_cmdSetMode; + ros::ServiceClient mode_client; - /*Obtain data from ros parameter server*/ - void Rosparameters_get(ros::NodeHandle& n_c_priv); + /*Initialize publisher and subscriber, done in the constructor*/ + void Initialize_pub_sub(ros::NodeHandle& n_c); - /*compiles buzz script from the specified .bzz file*/ - std::string Compile_bzz(std::string bzzfile_name); + std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode - /*Flight controller service call*/ - void flight_controller_service_call(); - - /*Neighbours pos publisher*/ - void neighbours_pos_publisher(); + /*Obtain data from ros parameter server*/ + void Rosparameters_get(ros::NodeHandle& n_c_priv); - /*Prepare messages and publish*/ - void prepare_msg_and_publish(); + /*compiles buzz script from the specified .bzz file*/ + std::string Compile_bzz(std::string bzzfile_name); - - /*Refresh neighbours Position for every ten step*/ - void maintain_pos(int tim_step); + /*Flight controller service call*/ + void flight_controller_service_call(); - /*Puts neighbours position inside neigbours_pos_map*/ - void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); + /*Neighbours pos publisher*/ + void neighbours_pos_publisher(); - /*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/ - void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); + /*Prepare messages and publish*/ + void prepare_msg_and_publish(); - /*Set the current position of the robot callback*/ - void set_cur_pos(double latitude, - double longitude, - double altitude); - /*convert from spherical to cartesian coordinate system callback */ - void gps_rb(GPS nei_pos, double out[]); - void gps_ned_cur(float &ned_x, float &ned_y, GPS t); - void gps_ned_home(float &ned_x, float &ned_y); - void gps_convert_ned(float &ned_x, float &ned_y, - double gps_t_lon, double gps_t_lat, - double gps_r_lon, double gps_r_lat); - /*battery status callback*/ - void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); - - /*flight extended status callback*/ - void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg); + /*Refresh neighbours Position for every ten step*/ + void maintain_pos(int tim_step); - /*flight status callback*/ - void flight_status_update(const mavros_msgs::State::ConstPtr& msg); - - /*current position callback*/ - void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); + /*Puts neighbours position inside neigbours_pos_map*/ + void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); + + /*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/ + void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); + + /*Set the current position of the robot callback*/ + void set_cur_pos(double latitude, + double longitude, + double altitude); + /*convert from spherical to cartesian coordinate system callback */ + void gps_rb(GPS nei_pos, double out[]); + void gps_ned_cur(float &ned_x, float &ned_y, GPS t); + void gps_ned_home(float &ned_x, float &ned_y); + void gps_convert_ned(float &ned_x, float &ned_y, + double gps_t_lon, double gps_t_lat, + double gps_r_lon, double gps_r_lat); + + /*battery status callback */ + void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); + + /*flight extended status callback*/ + void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg); + + /*flight status callback*/ + void flight_status_update(const mavros_msgs::State::ConstPtr& msg); + + /*current position callback*/ + void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); void users_pos(const rosbuzz::neigh_pos msg); - /*current relative altitude callback*/ - void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); + /*current relative altitude callback*/ + void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); - /*payload callback callback*/ - void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); + /*payload callback callback*/ + void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); - /* RC commands service */ - bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res); + /* RC commands service */ + bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res); - /*robot id sub callback*/ - void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); + /*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); + /*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); + /*Get publisher and subscriber from YML file*/ + void GetSubscriptionParameters(ros::NodeHandle& node_handle); - /*Arm/disarm method that can be called from buzz*/ - void Arm(); + /*Arm/disarm method that can be called from buzz*/ + void Arm(); - /*set mode like guided for solo*/ - void SetMode(std::string mode, int delay_miliseconds); + /*set mode like guided for solo*/ + void SetMode(std::string mode, int delay_miliseconds); - /*Robot independent subscribers*/ - void Subscribe(ros::NodeHandle& n_c); + /*Robot independent subscribers*/ + void Subscribe(ros::NodeHandle& n_c); - void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose); + void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose); - //void WaypointMissionSetup(float lat, float lng, float alt); + //void WaypointMissionSetup(float lat, float lng, float alt); - void fc_command_setup(); + void fc_command_setup(); - void SetLocalPosition(float x, float y, float z, float yaw); + void SetLocalPosition(float x, float y, float z, float yaw); - void SetLocalPositionNonRaw(float x, float y, float z, float yaw); + void SetLocalPositionNonRaw(float x, float y, float z, float yaw); + + void SetStreamRate(int id, int rate, int on_off); + + void get_number_of_robots(); + + void GetRobotId(); + bool GetDequeFull(bool &result); + bool GetRssi(float &result); + bool TriggerAPIRssi(const uint8_t short_id); + bool GetAPIRssi(const uint8_t short_id, float &result); + bool GetRawPacketLoss(const uint8_t short_id, float &result); + bool GetFilteredPacketLoss(const uint8_t short_id, float &result); - void SetStreamRate(int id, int rate, int on_off); - - void get_number_of_robots(); - void GetRobotId(); }; } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 1792330..cbbd445 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -1,7 +1,7 @@ /** @file buzz_utility.cpp - * @version 1.0 + * @version 1.0 * @date 27.09.2016 - * @brief Buzz Implementation as a node in ROS for Dji M100 Drone. + * @brief Buzz Implementation as a node in ROS for Dji M100 Drone. * @author Vivek Shankar Varadharajan * @copyright 2016 MistLab. All rights reserved. */ @@ -18,11 +18,11 @@ namespace buzz_utility{ static uint8_t* BO_BUF = 0; static buzzdebug_t DBG_INFO = 0; static uint32_t MSG_SIZE = 600;//250; // Only 100 bytes of Buzz messages every step - static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets + static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets static uint8_t Robot_id = 0; static std::vector IN_MSG; std::map< int, Pos_struct> users_map; - + /****************************************/ void add_user(int id, double latitude, double longitude, float altitude) @@ -181,12 +181,12 @@ namespace buzz_utility{ /* Copy packet into temporary buffer */ memcpy(pl, payload ,size); IN_MSG.push_back(pl); - + } - + void in_message_process(){ while(!IN_MSG.empty()){ - uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); + uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); /* Go through messages and append them to the FIFO */ uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]); /*Size is at first 2 bytes*/ @@ -220,7 +220,7 @@ namespace buzz_utility{ /***************************************************/ uint64_t* obt_out_msg(){ /* Process out messages */ - buzzvm_process_outmsgs(VM); + 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*/ @@ -361,7 +361,7 @@ namespace buzz_utility{ buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); - buzzvm_gstore(VM); + buzzvm_gstore(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); @@ -414,7 +414,7 @@ int create_stig_tables() { //buzzvm_gstore(VM); //buzzvm_dump(VM); - /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); buzzvm_gload(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); buzzvm_tget(VM); @@ -435,7 +435,7 @@ int create_stig_tables() { buzzobj_t data = buzzvm_stack_at(VM, 1); buzzvm_tput(VM); buzzvm_push(VM, data); - + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); buzzvm_gload(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1)); @@ -498,7 +498,7 @@ int create_stig_tables() { ROS_ERROR("[%i] Error registering hooks", Robot_id); return 0; } - /* Create vstig tables + /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); @@ -507,10 +507,10 @@ int create_stig_tables() { //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; return 0; }*/ - + /* Save bytecode file name */ BO_FNAME = strdup(bo_filename); - + // Execute the global part of the script if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ ROS_ERROR("Error executing global part, VM state : %i",VM->state); @@ -527,7 +527,7 @@ int create_stig_tables() { return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size); } - + /****************************************/ /*Sets a new update */ /****************************************/ @@ -560,7 +560,7 @@ int create_stig_tables() { fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); return 0; } - /* Create vstig tables + /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); @@ -569,7 +569,7 @@ int create_stig_tables() { //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; return 0; }*/ - + // Execute the global part of the script if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ ROS_ERROR("Error executing global part, VM state : %i",VM->state); @@ -616,7 +616,7 @@ int create_stig_tables() { fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); return 0; } - /* Create vstig tables + /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); buzzdebug_destroy(&DBG_INFO); @@ -708,12 +708,12 @@ int create_stig_tables() { buzz_error_info()); buzzvm_dump(VM); } - + /*Print swarm*/ //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //int SwarmSize = buzzdict_size(VM->swarmmembers)+1; //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); - + /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */ //int status = 1; @@ -771,7 +771,7 @@ int create_stig_tables() { buzz_error_info()); fprintf(stdout, "step test VM state %i\n",a); } - + return a == BUZZVM_STATE_READY; } @@ -785,5 +785,3 @@ int create_stig_tables() { buzzvm_gstore(VM); } } - - diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 658d206..52f865d 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -1,7 +1,7 @@ /** @file buzzuav_closures.cpp - * @version 1.0 + * @version 1.0 * @date 27.09.2016 - * @brief Buzz Implementation as a node in ROS for Dji M100 Drone. + * @brief Buzz Implementation as a node in ROS for Dji M100 Drone. * @author Vivek Shankar Varadharajan * @copyright 2016 MistLab. All rights reserved. */ @@ -11,427 +11,432 @@ 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 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; + // 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; - std::map< int, buzz_utility::Pos_struct> neighbors_map; + std::map< int, buzz_utility::Pos_struct> neighbors_map; - /****************************************/ - /****************************************/ + /****************************************/ + /****************************************/ - int buzzros_print(buzzvm_t vm) { - int i; - char buffer [50] = ""; - sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid()); - 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: - sprintf(buffer,"%s BUZZ - [nil]", buffer); - break; - case BUZZTYPE_INT: - sprintf(buffer,"%s %d", buffer, o->i.value); - //fprintf(stdout, "%d", o->i.value); - break; - case BUZZTYPE_FLOAT: - sprintf(buffer,"%s %f", buffer, o->f.value); - break; - case BUZZTYPE_TABLE: - sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value))); - break; - case BUZZTYPE_CLOSURE: - if(o->c.value.isnative) - sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref); - else - sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref); - break; - case BUZZTYPE_STRING: - sprintf(buffer,"%s %s", buffer, o->s.value.str); - break; - case BUZZTYPE_USERDATA: - sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value); - break; - default: - break; - } - } - ROS_INFO(buffer); - //fprintf(stdout, "\n"); - return buzzvm_ret0(vm); - } + int buzzros_print(buzzvm_t vm) + { + int i; + char buffer [50] = ""; + sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid()); + 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: + sprintf(buffer,"%s BUZZ - [nil]", buffer); + break; + case BUZZTYPE_INT: + sprintf(buffer,"%s %d", buffer, o->i.value); + //fprintf(stdout, "%d", o->i.value); + break; + case BUZZTYPE_FLOAT: + sprintf(buffer,"%s %f", buffer, o->f.value); + break; + case BUZZTYPE_TABLE: + sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value))); + break; + case BUZZTYPE_CLOSURE: + if(o->c.value.isnative) + sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref); + else + sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref); + break; + case BUZZTYPE_STRING: + sprintf(buffer,"%s %s", buffer, o->s.value.str); + break; + case BUZZTYPE_USERDATA: + sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value); + break; + default: + break; + } + } + ROS_INFO("%s",buffer); + //fprintf(stdout, "\n"); + return buzzvm_ret0(vm); + } - /*----------------------------------------/ - / Compute GPS destination from current position and desired Range and Bearing - /----------------------------------------*/ + /*----------------------------------------/ + / Compute GPS destination from current position and desired Range and Bearing + /----------------------------------------*/ - 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. - } + 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. + } - void rb_from_gps(double nei[], double out[], double cur[]){ - double d_lon = nei[1] - cur[1]; - double d_lat = nei[0] - cur[0]; - double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; - double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); - out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); - out[1] = atan2(ned_y,ned_x); - out[2] = 0.0; - } + void rb_from_gps(double nei[], double out[], double cur[]){ + double d_lon = nei[1] - cur[1]; + double d_lat = nei[0] - cur[0]; + double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); + out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); + out[1] = atan2(ned_y,ned_x); + out[2] = 0.0; + } - // Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests - double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336}; - double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958}; - 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]=height; - /*double b = atan2(dy,dx); //bearing - printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); - gps_from_rb(d, b, goto_pos); - cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ + /*----------------------------------------/ + / 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; + // The compiler warnings are useful! + // double d = sqrt(dx*dx+dy*dy); //range + goto_pos[0]=dx; + goto_pos[1]=dy; + goto_pos[2]=height; + /*double b = atan2(dy,dx); //bearing + printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); + gps_from_rb(d, b, goto_pos); + cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); - //printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); - buzz_cmd= COMMAND_MOVETO; // TO DO what should we use - return buzzvm_ret0(vm); - } + //printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); + buzz_cmd= COMMAND_MOVETO; // TO DO what should we use + return buzzvm_ret0(vm); + } - int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) { - if(vm->state != BUZZVM_STATE_READY) return vm->state; - buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1)); - buzzvm_gload(vm); - buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); - buzzobj_t nbr = buzzvm_stack_at(vm, 1); - /* Get "data" field */ - buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); - buzzvm_tget(vm); - if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) { - //ROS_INFO("Empty data, create a new table"); - buzzvm_pop(vm); - buzzvm_push(vm, nbr); - buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); - buzzvm_pusht(vm); - buzzobj_t data = buzzvm_stack_at(vm, 1); - buzzvm_tput(vm); - buzzvm_push(vm, data); - } - /* When we get here, the "data" table is on top of the stack */ - /* Push user id */ - buzzvm_pushi(vm, id); - /* Create entry table */ - buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); - /* Insert range */ - buzzvm_push(vm, entry); - buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1)); - buzzvm_pushf(vm, range); - buzzvm_tput(vm); - /* Insert longitude */ - buzzvm_push(vm, entry); - buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1)); - buzzvm_pushf(vm, bearing); - buzzvm_tput(vm); - /* Save entry into data table */ - buzzvm_push(vm, entry); - buzzvm_tput(vm); - //printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing); - return vm->state; - } + int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) { + if(vm->state != BUZZVM_STATE_READY) return vm->state; + buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1)); + buzzvm_gload(vm); + buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); + buzzobj_t nbr = buzzvm_stack_at(vm, 1); + /* Get "data" field */ + buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); + buzzvm_tget(vm); + if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) { + //ROS_INFO("Empty data, create a new table"); + buzzvm_pop(vm); + buzzvm_push(vm, nbr); + buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1)); + buzzvm_pusht(vm); + buzzobj_t data = buzzvm_stack_at(vm, 1); + buzzvm_tput(vm); + buzzvm_push(vm, data); + } + /* When we get here, the "data" table is on top of the stack */ + /* Push user id */ + buzzvm_pushi(vm, id); + /* Create entry table */ + buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); + /* Insert range */ + buzzvm_push(vm, entry); + buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1)); + buzzvm_pushf(vm, range); + buzzvm_tput(vm); + /* Insert longitude */ + buzzvm_push(vm, entry); + buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1)); + buzzvm_pushf(vm, bearing); + buzzvm_tput(vm); + /* Save entry into data table */ + buzzvm_push(vm, entry); + buzzvm_tput(vm); + //printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing); + return vm->state; + } - int buzzuav_adduserRB(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 3); - buzzvm_lload(vm, 1); /* longitude */ - buzzvm_lload(vm, 2); /* latitude */ - buzzvm_lload(vm, 3); /* id */ - buzzvm_type_assert(vm, 3, BUZZTYPE_INT); - buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - double tmp[3]; - tmp[0] = buzzvm_stack_at(vm, 2)->f.value; - tmp[1] = buzzvm_stack_at(vm, 1)->f.value; - tmp[2] = 0.0; - int uid = buzzvm_stack_at(vm, 3)->i.value; - double rb[3]; + int buzzuav_adduserRB(buzzvm_t vm) { + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); /* longitude */ + buzzvm_lload(vm, 2); /* latitude */ + buzzvm_lload(vm, 3); /* id */ + buzzvm_type_assert(vm, 3, BUZZTYPE_INT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + double tmp[3]; + tmp[0] = buzzvm_stack_at(vm, 2)->f.value; + tmp[1] = buzzvm_stack_at(vm, 1)->f.value; + tmp[2] = 0.0; + int uid = buzzvm_stack_at(vm, 3)->i.value; + double rb[3]; - rb_from_gps(tmp, rb, cur_pos); - if(fabs(rb[0])<100.0) { - //printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]); - return users_add2localtable(vm, uid, rb[0], rb[1]); - } else - printf(" ---------- User too far %f\n",rb[0]); + 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]); + return users_add2localtable(vm, uid, rb[0], rb[1]); + } else + printf(" ---------- User too far %f\n",rb[0]); - return 0; - } + return 0; + } - /*----------------------------------------/ - / 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=COMMAND_GOTO; - 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=COMMAND_GOTO; + 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=COMMAND_ARM; - 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=COMMAND_DISARM; - 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=COMMAND_ARM; + 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=COMMAND_DISARM; + 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 = COMMAND_TAKEOFF; - 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 = COMMAND_TAKEOFF; + return buzzvm_ret0(vm); + } - int buzzuav_land(buzzvm_t vm) { - cur_cmd=mavros_msgs::CommandCode::NAV_LAND; - printf(" Buzz requested Land !!! \n"); - buzz_cmd = COMMAND_LAND; - return buzzvm_ret0(vm); - } + int buzzuav_land(buzzvm_t vm) { + cur_cmd=mavros_msgs::CommandCode::NAV_LAND; + printf(" Buzz requested Land !!! \n"); + buzz_cmd = COMMAND_LAND; + 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 = COMMAND_GOHOME; - 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 = COMMAND_GOHOME; + 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]; + 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; - } + 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_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 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_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; - } + 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; - } - /*adds neighbours position*/ - void neighbour_pos_callback(int id, float range, float bearing, float elevation){ - buzz_utility::Pos_struct pos_arr; - pos_arr.x=range; - pos_arr.y=bearing; - pos_arr.z=elevation; - map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id); - if(it!=neighbors_map.end()) - neighbors_map.erase(it); - neighbors_map.insert(make_pair(id, pos_arr)); - } + 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; + } + /*adds neighbours position*/ + void neighbour_pos_callback(int id, float range, float bearing, float elevation){ + buzz_utility::Pos_struct pos_arr; + pos_arr.x=range; + pos_arr.y=bearing; + pos_arr.z=elevation; + map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id); + if(it!=neighbors_map.end()) + neighbors_map.erase(it); + neighbors_map.insert(make_pair(id, pos_arr)); + } - /* update at each step the VM table */ - void update_neighbors(buzzvm_t vm){ - /* Reset neighbor information */ - buzzneighbors_reset(vm); - /* Get robot id and update neighbor information */ - map< int, buzz_utility::Pos_struct >::iterator it; - for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){ - buzzneighbors_add(vm, - it->first, - (it->second).x, - (it->second).y, - (it->second).z); - } - } + /* update at each step the VM table */ + void update_neighbors(buzzvm_t vm){ + /* Reset neighbor information */ + buzzneighbors_reset(vm); + /* Get robot id and update neighbor information */ + map< int, buzz_utility::Pos_struct >::iterator it; + for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){ + buzzneighbors_add(vm, + it->first, + (it->second).x, + (it->second).y, + (it->second).z); + } + } - /****************************************/ - int buzzuav_update_currentpos(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); - 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; - } + /****************************************/ + 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; - } + 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 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*/ - /* 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 */ - /******************************************************/ + /******************************************************/ + /*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) { + int buzzuav_update_prox(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); + 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 */ buzzobj_t tProxRead; float angle =0; @@ -441,80 +446,80 @@ namespace buzzuav_closures{ tProxRead = buzzvm_stack_at(vm, 1); buzzvm_pop(vm); /* Fill in the read */ - buzzvm_push(vm, tProxRead); - buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); - buzzvm_pushf(vm, obst[i+1]); - buzzvm_tput(vm); - buzzvm_push(vm, tProxRead); - buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); - buzzvm_pushf(vm, angle); - buzzvm_tput(vm); + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); + buzzvm_pushf(vm, obst[i+1]); + buzzvm_tput(vm); + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); + buzzvm_pushf(vm, angle); + buzzvm_tput(vm); /* 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; + buzzvm_pushi(vm, i); + buzzvm_push(vm, tProxRead); + buzzvm_tput(vm); + angle+=1.5708; } - /* Create table for bottom read */ - angle =-1; + /* Create table for bottom read */ + angle =-1; buzzvm_pusht(vm); tProxRead = buzzvm_stack_at(vm, 1); buzzvm_pop(vm); /* Fill in the read */ - buzzvm_push(vm, tProxRead); - buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); - buzzvm_pushf(vm, obst[0]); - buzzvm_tput(vm); - buzzvm_push(vm, tProxRead); - buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); - buzzvm_pushf(vm, angle); - buzzvm_tput(vm); - /*Store read table in the proximity table*/ - buzzvm_push(vm, tProxTable); - buzzvm_pushi(vm, 4); - buzzvm_push(vm, tProxRead); - buzzvm_tput(vm); + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); + buzzvm_pushf(vm, obst[0]); + buzzvm_tput(vm); + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); + buzzvm_pushf(vm, angle); + buzzvm_tput(vm); + /*Store read table in the proximity table*/ + buzzvm_push(vm, tProxTable); + buzzvm_pushi(vm, 4); + buzzvm_push(vm, tProxRead); + buzzvm_tput(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; - } + /* + 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 */ - /**********************************************/ + /**********************************************/ + /*Dummy closure for use during update testing */ + /**********************************************/ - int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} + 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; - //} + /***********************************************/ + /* Store Ros controller object pointer */ + /***********************************************/ + + //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){ + //roscontroller_ptr = roscontroller_ptrin; + //} } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ec32acf..c402605 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -1,988 +1,1197 @@ #include "roscontroller.h" #include -namespace rosbzz_node{ - - /*--------------- - /Constructor - ---------------*/ - roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) - { - ROS_INFO("Buzz_node"); - /*Obtain parameters from ros parameter server*/ - Rosparameters_get(n_c_priv); - /*Initialize publishers, subscribers and client*/ - Initialize_pub_sub(n_c); - /*Compile the .bzz file to .basm, .bo and .bdbg*/ - std::string fname = Compile_bzz(bzzfile_name); - bcfname = fname + ".bo"; - dbgfname = fname + ".bdb"; - set_bzz_file(bzzfile_name.c_str()); - /*Initialize variables*/ - // Solo things - 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; - - while(cur_pos.latitude == 0.0f){ - ROS_INFO("Waiting for GPS. "); - ros::Duration(0.5).sleep(); - ros::spinOnce(); - } - - - if(xbeeplugged){ - GetRobotId(); - } else { - robot_id= strtol(robot_name.c_str() + 5, NULL, 10); - } - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - path+="Update.log"; - log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); - } - - /*--------------------- - /Destructor - ---------------------*/ - roscontroller::~roscontroller() - { - /* All done */ - /* Cleanup */ - buzz_utility::buzz_script_destroy(); - /* Stop the robot */ - uav_done(); - log.close(); - } - - void roscontroller::GetRobotId() - { - - - mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; - mavros_msgs::ParamGet::Response robot_id_srv_response; - while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ - ros::Duration(0.1).sleep(); - ROS_ERROR("Waiting for Xbee to respond to get device ID"); - } - - robot_id=robot_id_srv_response.value.integer; - - //robot_id = 8; - } - - /*------------------------------------------------- - /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] Bytecode file found and set", robot_id); - std::string standby_bo = Compile_bzz(stand_by) + ".bo"; - init_update_monitor(bcfname.c_str(),standby_bo.c_str()); - /////////////////////////////////////////////////////// - // MAIN LOOP - ////////////////////////////////////////////////////// - ROS_INFO("[%i] STARTING MAIN LOOP!", robot_id); - while (ros::ok() && !buzz_utility::buzz_script_done()) - { - /*Update neighbors position inside Buzz*/ - //buzz_closure::neighbour_pos_callback(neighbours_pos_map); - /*Neighbours of the robot published with id in respective topic*/ - neighbours_pos_publisher(); - /*Check updater state and step code*/ - update_routine(bcfname.c_str(), dbgfname.c_str()); - /*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(); - multi_msg=true; - log<::iterator it = neighbours_pos_map.begin(); - log<<","<second.x<<","<<(double)it->second.y<<","<<(double)it->second.z; - } - log<0) no_of_robots =neighbours_pos_map.size()+1; - //buzz_utility::set_robot_var(no_of_robots); - /*Set no of robots for updates TODO only when not updating*/ - //if(multi_msg) - updates_set_robots(no_of_robots); - //ROS_INFO("ROBOTS: %i , acutal : %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1); - /*run once*/ - ros::spinOnce(); - /*loop rate of ros*/ - ros::Rate loop_rate(BUZZRATE); - loop_rate.sleep(); - 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; - } - /* Destroy updater and Cleanup */ - //update_routine(bcfname.c_str(), dbgfname.c_str(),1); - } - } - - /*-------------------------------------------------------- - / 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 robot_id from parameter server*/ - //n_c.getParam("robot_id", robot_id); - //robot_id=(int)buzz_utility::get_robotid(); - /*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); - - std::cout<< "////////////////// " << xbeesrv_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){ - //todo: make it as an array in yaml? - m_sMySubscriptions.clear(); - std::string gps_topic, gps_type; - if(node_handle.getParam("topics/gps", gps_topic)); - else {ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node");} - node_handle.getParam("type/gps", gps_type); - m_smTopic_infos.insert(pair (gps_topic, gps_type)); - - std::string battery_topic, battery_type; - node_handle.getParam("topics/battery", battery_topic); - node_handle.getParam("type/battery", battery_type); - m_smTopic_infos.insert(pair (battery_topic, battery_type)); - - std::string status_topic, status_type; - node_handle.getParam("topics/status", status_topic); - node_handle.getParam("type/status", status_type); - m_smTopic_infos.insert(pair (status_topic, status_type)); - - std::string altitude_topic, altitude_type; - node_handle.getParam("topics/altitude", altitude_topic); - node_handle.getParam("type/altitude", altitude_type); - m_smTopic_infos.insert(pair (altitude_topic, altitude_type)); - - /*Obtain fc client name from 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");} - - - - } - - /*-------------------------------------------------------- - / Create all required subscribers, publishers and ROS service clients - /-------------------------------------------------------*/ - void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c){ - /*subscribers*/ - - Subscribe(n_c); - - //current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this); - //battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this); - payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this); - //flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this); - //Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this); - obstacle_sub= n_c.subscribe("guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this); - /*publishers*/ - payload_pub = n_c.advertise(out_payload, 1000); - neigh_pos_pub = n_c.advertise("neighbours_pos",1000); - localsetpoint_nonraw_pub = n_c.advertise(setpoint_name,1000); - /* 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); - stream_client = n_c.serviceClient(stream_client_name); - - users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos,this); - local_pos_sub = n_c.subscribe(local_pos_sub_name, 1000, &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_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this); - } - else if(it->second == "mavros_msgs/State"){ - flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this); - } - else if(it->second == "mavros_msgs/BatteryStatus"){ - battery_sub = n_c.subscribe(it->first, 1000, &roscontroller::battery, this); - } - else if(it->second == "sensor_msgs/NavSatFix"){ - current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this); - } - else if(it->second == "std_msgs/Float64"){ - relative_altitude_sub = n_c.subscribe(it->first, 1000, &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){ - /*TODO: change to bzzc instead of bzzparse and also add -I for includes*/ - /*Compile the buzz code .bzz to .bo*/ - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - //bzzfile_in_compile << path << "/"; - //path = bzzfile_in_compile.str(); - //bzzfile_in_compile.str(""); - 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/"; //<<" "<::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< 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 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()) - 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 - ----------------------------------------------------------- */ - void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) { - double hyp_az, hyp_el; - double sin_el, cos_el, sin_az, cos_az; - - /* Convert reference point to spherical earth centered coordinates. */ - hyp_az = sqrt(ref_ecef[0]*ref_ecef[0] + ref_ecef[1]*ref_ecef[1]); - hyp_el = sqrt(hyp_az*hyp_az + ref_ecef[2]*ref_ecef[2]); - sin_el = ref_ecef[2] / hyp_el; - cos_el = hyp_az / hyp_el; - sin_az = ref_ecef[1] / hyp_az; - cos_az = ref_ecef[0] / hyp_az; - - M[0][0] = -sin_el * cos_az; - M[0][1] = -sin_el * sin_az; - M[0][2] = cos_el; - M[1][0] = -sin_az; - M[1][1] = cos_az; - M[1][2] = 0.0; - M[2][0] = -cos_el * cos_az; - M[2][1] = -cos_el * sin_az; - M[2][2] = -sin_el; - } - void matrix_multiply(uint32_t n, uint32_t m, uint32_t p, const double *a, - const double *b, double *c) - { - uint32_t i, j, k; - for (i = 0; i < n; i++) - for (j = 0; j < p; j++) { - c[p*i + j] = 0; - for (k = 0; k < m; k++) - c[p*i + j] += a[m*i+k] * b[p*k + j]; - } - } - - 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] = std::floor(out[1] * 1000000) / 1000000; - out[2] = 0.0; - } - - void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t) - { - gps_convert_ned(ned_x, ned_y, - t.longitude, t.latitude, - cur_pos.longitude, cur_pos.latitude); - } - - void roscontroller::gps_ned_home(float &ned_x, float &ned_y) - { - gps_convert_ned(ned_x, ned_y, - cur_pos.longitude, cur_pos.latitude, - home.longitude, home.latitude); - } - - void roscontroller::gps_convert_ned(float &ned_x, float &ned_y, - double gps_t_lon, double gps_t_lat, - double gps_r_lon, double gps_r_lat) - { - double d_lon = gps_t_lon - gps_r_lon; - double d_lat = gps_t_lat - gps_r_lat; - ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; - ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); - }; - - /*------------------------------------------------------ - / Update battery status into BVM from subscriber - /------------------------------------------------------*/ - 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; - //double lat = std::floor(msg->latitude * 1000000) / 1000000; - //double lon = std::floor(msg->longitude * 1000000) / 1000000; - /*if(cur_rel_altitude<1.2){ - home[0]=lat; - home[1]=lon; - home[2]=cur_rel_altitude; - }*/ - 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; - } - - void roscontroller::users_pos(const rosbuzz::neigh_pos data){ - - int n = data.pos_neigh.size(); - // ROS_INFO("Users array size: %i\n", n); - if(n>0) - { - for(int it=0; itdata); - 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; -// float ned_x, ned_y; -// gps_ned_home(ned_x, ned_y); - // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]); - // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); - - /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ - //target[0]+=y; target[1]+=x; - moveMsg.pose.position.x = local_pos_new[0]+y;//ned_y+y; - moveMsg.pose.position.y = local_pos_new[1]+x;//ned_x+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 && 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]; - } - - 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); - //uint16_t tot; - //tot+=sizeof(uint16_t); - fprintf(stdout,"Update packet, read msg size : %u \n",unMsgSize); - if(unMsgSize>0){ - code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)),unMsgSize); - //fprintf(stdout,"before in queue process : utils\n"); - code_message_inqueue_process(); - //fprintf(stdout,"after in queue process : utils\n"); - } - free(pl); - } - /*BVM FIFO message*/ - else if(msg->payload64.size() > 3){ - uint64_t message_obt[msg->payload64.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 "<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; - } - } - /* - if(count_robots.current !=0){ - std::map< int, int> count_count; - uint8_t index=0; - count_robots.history[count_robots.index]=neighbours_pos_map.size()+1; - //count_robots.current=neighbours_pos_map.size()+1; - count_robots.index++; - if(count_robots.index >9) count_robots.index =0; - for(int i=0;i<10;i++){ - if(count_robots.history[i]==count_robots.current){ - current_count++; - } - else{ - if(count_robots.history[i]!=0){ - odd_count++; - odd_val=count_robots.history[i]; - } - } - } - if(odd_count>current_count){ - count_robots.current=odd_val; - } - } - else{ - if(neighbours_pos_map.size()!=0){ - count_robots.history[count_robots.index]=neighbours_pos_map.size()+1; - //count_robots.current=neighbours_pos_map.size()+1; - count_robots.index++; - if(count_robots.index >9){ - count_robots.index =0; - count_robots.current=neighbours_pos_map.size()+1; - } - } - } - */ - } - +namespace rosbzz_node { + +/*--------------- +/Constructor +---------------*/ +roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv) +{ + ROS_INFO("Buzz_node"); + /*Obtain parameters from ros parameter server*/ + Rosparameters_get(n_c_priv); + /*Initialize publishers, subscribers and client*/ + Initialize_pub_sub(n_c); + /*Compile the .bzz file to .basm, .bo and .bdbg*/ + std::string fname = Compile_bzz(bzzfile_name); + bcfname = fname + ".bo"; + dbgfname = fname + ".bdb"; + set_bzz_file(bzzfile_name.c_str()); + /*Initialize variables*/ + // Solo things + 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; + + while (cur_pos.latitude == 0.0f) { + ROS_INFO("Waiting for GPS. "); + ros::Duration(0.5).sleep(); + ros::spinOnce(); + } + + if (xbeeplugged) { + GetRobotId(); + } else { + robot_id = strtol(robot_name.c_str() + 5, NULL, 10); + } + std::string path = + bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + path += "Update.log"; + log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); +} + +/*--------------------- +/Destructor +---------------------*/ +roscontroller::~roscontroller() +{ + /* All done */ + /* Cleanup */ + buzz_utility::buzz_script_destroy(); + /* Stop the robot */ + uav_done(); + log.close(); +} + +void roscontroller::GetRobotId() +{ + mavros_msgs::ParamGet::Request robot_id_srv_request; + robot_id_srv_request.param_id = "id"; + mavros_msgs::ParamGet::Response robot_id_srv_response; + while (!xbeestatus_srv.call(robot_id_srv_request, robot_id_srv_response)) + { + ros::Duration(0.1).sleep(); + ROS_ERROR("Waiting for Xbee to respond to get device ID"); + } + + robot_id = robot_id_srv_response.value.integer; +} + +bool roscontroller::GetDequeFull(bool &result) +{ + mavros_msgs::ParamGet::Request srv_request; + srv_request.param_id = "deque_full"; + mavros_msgs::ParamGet::Response srv_response; + + if(!xbeestatus_srv.call(srv_request, srv_response)){return false;} + + result = (srv_response.value.integer == 1) ? true : false; + return srv_response.success; +} + +bool roscontroller::GetRssi(float &result) +{ + mavros_msgs::ParamGet::Request srv_request; + srv_request.param_id = "rssi"; + mavros_msgs::ParamGet::Response srv_response; + + if(!xbeestatus_srv.call(srv_request, srv_response)){return false;} + + result = srv_response.value.real; + return srv_response.success; +} + +bool roscontroller::TriggerAPIRssi(const uint8_t short_id) +{ + mavros_msgs::ParamGet::Request srv_request; + if(short_id == 0xFF) + { + srv_request.param_id = "trig_rssi_api_avg"; + } + else + { + srv_request.param_id = "trig_rssi_api_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if(xbeestatus_srv.call(srv_request, srv_response)){return false;} + + return srv_response.success; +} + +bool roscontroller::GetAPIRssi(const uint8_t short_id, float &result) +{ + mavros_msgs::ParamGet::Request srv_request; + if(short_id == 0xFF) + { + srv_request.param_id = "get_rssi_api_avg"; + } + else + { + srv_request.param_id = "get_rssi_api_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if(xbeestatus_srv.call(srv_request, srv_response)){return false;} + + result = srv_response.value.real; + return srv_response.success; +} + +bool roscontroller::GetRawPacketLoss(const uint8_t short_id, float &result) +{ + mavros_msgs::ParamGet::Request srv_request; + if(short_id == 0xFF) + { + srv_request.param_id = "pl_raw_avg"; + } + else + { + srv_request.param_id = "pl_raw_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if(xbeestatus_srv.call(srv_request, srv_response)){return false;} + + result = srv_response.value.real; + return srv_response.success; +} + +bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float &result) +{ + mavros_msgs::ParamGet::Request srv_request; + if(short_id == 0xFF) + { + srv_request.param_id = "pl_filtered_avg"; + } + else + { + srv_request.param_id = "pl_filtered_" + std::to_string(short_id); + } + mavros_msgs::ParamGet::Response srv_response; + if(xbeestatus_srv.call(srv_request, srv_response)){return false;} + + result = srv_response.value.real; + return srv_response.success; +} + +/*------------------------------------------------- +/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] Bytecode file found and set", robot_id); + std::string standby_bo = Compile_bzz(stand_by) + ".bo"; + init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + /////////////////////////////////////////////////////// + // MAIN LOOP + ////////////////////////////////////////////////////// + ROS_INFO("[%i] STARTING MAIN LOOP!", robot_id); + while (ros::ok() && !buzz_utility::buzz_script_done()) { + /*Update neighbors position inside Buzz*/ + // buzz_closure::neighbour_pos_callback(neighbours_pos_map); + /*Neighbours of the robot published with id in respective topic*/ + neighbours_pos_publisher(); + /*Check updater state and step code*/ + update_routine(bcfname.c_str(), dbgfname.c_str()); + /*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(); + multi_msg = true; + log << cur_pos.latitude << "," << cur_pos.longitude << "," + << cur_pos.altitude << ","; + collect_data(log); + map::iterator it = + neighbours_pos_map.begin(); + log << "," << neighbours_pos_map.size(); + for (; it != neighbours_pos_map.end(); ++it) + { + log << "," << (double)it->second.x << "," << (double)it->second.y + << "," << (double)it->second.z; + } + log << std::endl; + } + /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ + // no_of_robots=get_number_of_robots(); + get_number_of_robots(); + buzz_utility::set_robot_var(no_of_robots); + // if(neighbours_pos_map.size() >0) no_of_robots + // =neighbours_pos_map.size()+1; + // buzz_utility::set_robot_var(no_of_robots); + /*Set no of robots for updates TODO only when not updating*/ + // if(multi_msg) + updates_set_robots(no_of_robots); + // ROS_INFO("ROBOTS: %i , acutal : + // %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1); + /*run once*/ + ros::spinOnce(); + /*loop rate of ros*/ + ros::Rate loop_rate(BUZZRATE); + loop_rate.sleep(); + 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; + } + /* Destroy updater and Cleanup */ + // update_routine(bcfname.c_str(), dbgfname.c_str(),1); + } +} + +/*-------------------------------------------------------- +/ 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 robot_id from parameter server*/ + // n_c.getParam("robot_id", robot_id); + // robot_id=(int)buzz_utility::get_robotid(); + /*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); + + std::cout << "////////////////// " << xbeesrv_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) +{ + // todo: make it as an array in yaml? + m_sMySubscriptions.clear(); + std::string gps_topic, gps_type; + if (node_handle.getParam("topics/gps", gps_topic)) + ; + else { + ROS_ERROR("Provide a gps topic in Launch file"); + system("rosnode kill rosbuzz_node"); + } + node_handle.getParam("type/gps", gps_type); + m_smTopic_infos.insert(pair(gps_topic, gps_type)); + + std::string battery_topic, battery_type; + node_handle.getParam("topics/battery", battery_topic); + node_handle.getParam("type/battery", battery_type); + m_smTopic_infos.insert( + pair(battery_topic, battery_type)); + + std::string status_topic, status_type; + node_handle.getParam("topics/status", status_topic); + node_handle.getParam("type/status", status_type); + m_smTopic_infos.insert( + pair(status_topic, status_type)); + + std::string altitude_topic, altitude_type; + node_handle.getParam("topics/altitude", altitude_topic); + node_handle.getParam("type/altitude", altitude_type); + m_smTopic_infos.insert( + pair(altitude_topic, altitude_type)); + + /*Obtain fc client name from 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"); + } +} + +/*-------------------------------------------------------- +/ Create all required subscribers, publishers and ROS service clients +/-------------------------------------------------------*/ +void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c) +{ + /*subscribers*/ + + Subscribe(n_c); + + // current_position_sub = n_c.subscribe("/global_position", 1000, + // &roscontroller::current_pos,this); + // battery_sub = n_c.subscribe("/power_status", 1000, + // &roscontroller::battery,this); + payload_sub = + n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt, this); + // flight_status_sub =n_c.subscribe("/flight_status",100, + // &roscontroller::flight_extended_status_update,this); + // Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, + // &roscontroller::set_robot_id,this); + obstacle_sub = n_c.subscribe("guidance/obstacle_distance", 100, + &roscontroller::obstacle_dist, this); + /*publishers*/ + payload_pub = n_c.advertise(out_payload, 1000); + neigh_pos_pub = n_c.advertise("neighbours_pos", 1000); + localsetpoint_nonraw_pub = + n_c.advertise(setpoint_name, 1000); + /* 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); + stream_client = + n_c.serviceClient(stream_client_name); + + users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos, this); + local_pos_sub = n_c.subscribe(local_pos_sub_name, 1000, + &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_status_sub = n_c.subscribe( + it->first, 100, &roscontroller::flight_extended_status_update, this); + } else if (it->second == "mavros_msgs/State") { + flight_status_sub = n_c.subscribe( + it->first, 100, &roscontroller::flight_status_update, this); + } else if (it->second == "mavros_msgs/BatteryStatus") { + battery_sub = + n_c.subscribe(it->first, 1000, &roscontroller::battery, this); + } else if (it->second == "sensor_msgs/NavSatFix") { + current_position_sub = + n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this); + } else if (it->second == "std_msgs/Float64") { + relative_altitude_sub = + n_c.subscribe(it->first, 1000, &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) +{ + /*TODO: change to bzzc instead of bzzparse and also add -I for includes*/ + /*Compile the buzz code .bzz to .bo*/ + stringstream bzzfile_in_compile; + std::string path = + bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + // bzzfile_in_compile << path << "/"; + // path = bzzfile_in_compile.str(); + // bzzfile_in_compile.str(""); + 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/"; //<<" "<::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 +----------------------------------------------------------- */ +void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) { + double hyp_az, hyp_el; + double sin_el, cos_el, sin_az, cos_az; + + /* Convert reference point to spherical earth centered coordinates. */ + hyp_az = sqrt(ref_ecef[0] * ref_ecef[0] + ref_ecef[1] * ref_ecef[1]); + hyp_el = sqrt(hyp_az * hyp_az + ref_ecef[2] * ref_ecef[2]); + sin_el = ref_ecef[2] / hyp_el; + cos_el = hyp_az / hyp_el; + sin_az = ref_ecef[1] / hyp_az; + cos_az = ref_ecef[0] / hyp_az; + + M[0][0] = -sin_el * cos_az; + M[0][1] = -sin_el * sin_az; + M[0][2] = cos_el; + M[1][0] = -sin_az; + M[1][1] = cos_az; + M[1][2] = 0.0; + M[2][0] = -cos_el * cos_az; + M[2][1] = -cos_el * sin_az; + M[2][2] = -sin_el; +} +void matrix_multiply(uint32_t n, uint32_t m, uint32_t p, const double *a, + const double *b, double *c) { + uint32_t i, j, k; + for (i = 0; i < n; i++) + for (j = 0; j < p; j++) { + c[p * i + j] = 0; + for (k = 0; k < m; k++) + c[p * i + j] += a[m * i + k] * b[p * k + j]; + } +} + +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] = std::floor(out[1] * 1000000) / 1000000; + out[2] = 0.0; +} + +void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t) { + gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, + cur_pos.latitude); +} + +void roscontroller::gps_ned_home(float &ned_x, float &ned_y) { + gps_convert_ned(ned_x, ned_y, cur_pos.longitude, cur_pos.latitude, + home.longitude, home.latitude); +} + +void roscontroller::gps_convert_ned(float &ned_x, float &ned_y, + double gps_t_lon, double gps_t_lat, + double gps_r_lon, double gps_r_lat) { + double d_lon = gps_t_lon - gps_r_lon; + double d_lat = gps_t_lat - gps_r_lat; + ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; + ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat)); +}; + +/*------------------------------------------------------ +/ Update battery status into BVM from subscriber +/------------------------------------------------------*/ +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; + // double lat = std::floor(msg->latitude * 1000000) / 1000000; + // double lon = std::floor(msg->longitude * 1000000) / 1000000; + /*if(cur_rel_altitude<1.2){ + home[0]=lat; + home[1]=lon; + home[2]=cur_rel_altitude; + }*/ + 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; +} + +void roscontroller::users_pos(const rosbuzz::neigh_pos data) { + + int n = data.pos_neigh.size(); + // ROS_INFO("Users array size: %i\n", n); + if (n > 0) { + for (int it = 0; it < n; ++it) { + buzz_utility::add_user(data.pos_neigh[it].position_covariance_type, + data.pos_neigh[it].latitude, + data.pos_neigh[it].longitude, + data.pos_neigh[it].altitude); + } + } +} +/*------------------------------------------------------------- +/ 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; + // float ned_x, ned_y; + // gps_ned_home(ned_x, ned_y); + // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], + // home[1]); + // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, + // local_pos[0], local_pos[1]); + + /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD + * (then converted to NED)*/ + // target[0]+=y; target[1]+=x; + moveMsg.pose.position.x = local_pos_new[0] + y; // ned_y+y; + moveMsg.pose.position.y = local_pos_new[1] + x; // ned_x+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 && + 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]; + } + + 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); + // uint16_t tot; + // tot+=sizeof(uint16_t); + fprintf(stdout, "Update packet, read msg size : %u \n", unMsgSize); + if (unMsgSize > 0) { + code_message_inqueue_append((uint8_t *)(pl + sizeof(uint16_t)), + unMsgSize); + // fprintf(stdout,"before in queue process : utils\n"); + code_message_inqueue_process(); + // fprintf(stdout,"after in queue process : utils\n"); + } + free(pl); + } + /*BVM FIFO message*/ + else if (msg->payload64.size() > 3) { + uint64_t message_obt[msg->payload64.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 "<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; + } + } + /* + if(count_robots.current !=0){ + std::map< int, int> count_count; + uint8_t index=0; + count_robots.history[count_robots.index]=neighbours_pos_map.size()+1; + //count_robots.current=neighbours_pos_map.size()+1; + count_robots.index++; + if(count_robots.index >9) count_robots.index =0; + for(int i=0;i<10;i++){ + if(count_robots.history[i]==count_robots.current){ + current_count++; + } + else{ + if(count_robots.history[i]!=0){ + odd_count++; + odd_val=count_robots.history[i]; + } + } + } + if(odd_count>current_count){ + count_robots.current=odd_val; + } + } + else{ + if(neighbours_pos_map.size()!=0){ + count_robots.history[count_robots.index]=neighbours_pos_map.size()+1; + //count_robots.current=neighbours_pos_map.size()+1; + count_robots.index++; + if(count_robots.index >9){ + count_robots.index =0; + count_robots.current=neighbours_pos_map.size()+1; + } + } + } + */ +} }