From 3d20b3b9e920709f3b76e2fa9432ed6e5c49bb06 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 14 Jun 2017 16:07:44 -0400 Subject: [PATCH 01/14] minor fix to cmdlinectr --- misc/cmdlinectr.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 755d436..22ab030 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -20,10 +20,10 @@ function clean { sudo rm /var/log/upstart/x3s* } function startrobot { - sudo service robot start + sudo service dji start } function stoprobot { - sudo service robot stop + sudo service dji stop } function updaterobot { # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch From 7d3e844f6c6ae547299678716f86ee2e57b70fdc Mon Sep 17 00:00:00 2001 From: py-humanitas Date: Mon, 19 Jun 2017 16:16:57 -0400 Subject: [PATCH 02/14] Fixed README and solo launch file --- launch/rosbuzz-solo.launch | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch index 054d18b..dade33d 100644 --- a/launch/rosbuzz-solo.launch +++ b/launch/rosbuzz-solo.launch @@ -1,14 +1,14 @@ - + - - - + + + @@ -36,13 +36,14 @@ - + + - - + + @@ -50,7 +51,7 @@ - + From 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7 Mon Sep 17 00:00:00 2001 From: pyhs Date: Fri, 23 Jun 2017 10:39:14 -0400 Subject: [PATCH 03/14] 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; + } + } + } + */ +} } From d7ca910e21bc29ea5dd4497f1c722ee6e956c546 Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 26 Jun 2017 14:09:33 -0400 Subject: [PATCH 04/14] fixed graph formation --- include/buzz_utility.h | 9 +- include/buzzuav_closures.h | 3 +- include/roscontroller.h | 1 + misc/cmdlinectr.sh | 3 + script/Graph_drone.graph | 3 +- script/flock.bzz | 2 +- script/graphform.bzz | 248 ++++++++++++++++------------------- script/include/uavstates.bzz | 24 +++- src/buzz_utility.cpp | 54 ++++---- src/buzzuav_closures.cpp | 111 +++++++++------- src/roscontroller.cpp | 18 ++- 11 files changed, 254 insertions(+), 222 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index cc0ef6a..40be6f0 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -20,8 +20,15 @@ struct pos_struct pos_struct(double x,double y,double z):x(x),y(y),z(z){}; pos_struct(){} }; +typedef struct pos_struct Pos_struct; +struct rb_struct +{ + double r,b,la,lo; + rb_struct(double la, double lo, double r,double b):la(la),lo(lo),r(r),b(b){}; + rb_struct(){} +}; +typedef struct rb_struct RB_struct; -typedef struct pos_struct Pos_struct ; uint16_t* u64_cvt_u16(uint64_t u64); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index e501285..6113d43 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -87,7 +87,8 @@ int buzzuav_update_battery(buzzvm_t vm); * Updates current position in Buzz */ int buzzuav_update_currentpos(buzzvm_t vm); -int buzzuav_adduserRB(buzzvm_t vm); +int buzzuav_update_targets(buzzvm_t vm); +int buzzuav_addtargetRB(buzzvm_t vm); /* * Updates flight status and rc command in Buzz, put it in a tabel to acess it * use flight.status for flight status diff --git a/include/roscontroller.h b/include/roscontroller.h index 006d78b..d743430 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -171,6 +171,7 @@ private: double longitude, double altitude); /*convert from spherical to cartesian coordinate system callback */ + float constrainAngle(float x); 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); diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index a33b820..2aa2b84 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -11,6 +11,9 @@ function arm { function disarm { rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0 } +function testWP { + rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10 +} function record { rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info } diff --git a/script/Graph_drone.graph b/script/Graph_drone.graph index 58ecadb..e7be4ad 100644 --- a/script/Graph_drone.graph +++ b/script/Graph_drone.graph @@ -1,4 +1,5 @@ 0 -1 -1 -1 1 0 1000.0 0.0 2 0 1000.0 1.57 -3 0 1000.0 3.14 \ No newline at end of file +3 0 1000.0 3.14 +4 0 1000.0 4.71 \ No newline at end of file diff --git a/script/flock.bzz b/script/flock.bzz index d95f779..94d3ebc 100644 --- a/script/flock.bzz +++ b/script/flock.bzz @@ -6,7 +6,7 @@ include "vstigenv.bzz" # Lennard-Jones parameters TARGET = 12.0 -EPSILON = 14.0 +EPSILON = 18.0 # Lennard-Jones interaction magnitude function lj_magnitude(dist, target, epsilon) { diff --git a/script/graphform.bzz b/script/graphform.bzz index 1070726..4a612a7 100644 --- a/script/graphform.bzz +++ b/script/graphform.bzz @@ -7,12 +7,7 @@ include "update.bzz" include "barrier.bzz" # don't use a stigmergy id=11 with this header. include "uavstates.bzz" # require an 'action' function to be defined here. -# -#Constant parameters, need to be adjust -#parameters for set_wheels -m_sWheelTurningParams={.MaxSpeed=1.0} - -ROBOT_RADIUS=0.5 +ROBOT_RADIUS=50 ROBOT_DIAMETER=2.0*ROBOT_RADIUS ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER @@ -98,34 +93,17 @@ EPSILON = 3.5 #3.5 # Lennard-Jones interaction magnitude function FlockInteraction(dist,target,epsilon){ -var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) -return mag -} - -function Norm(vector) { - var l = math.vec2.length(vector) - return { - .x = vector.x / l, - .y = vector.y / l - } + var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + return mag } function LimitAngle(angle){ -if(angle>2*math.pi) -return angle-2*math.pi -else if (angle<0) -return angle+2*math.pi -else -return angle -} - -# -# Calculates the length of the given vector2. -# PARAM v: The vector2. -# RETURN: The length of the vector. -# -Length = function(v) { - return math.sqrt(v.x * v.x + v.y * v.y) + if(angle>2*math.pi) + return angle-2*math.pi + else if (angle<0) + return angle+2*math.pi + else + return angle } # @@ -141,43 +119,43 @@ Angle = function(v) { #return the number of value in table # function count(table,value){ -var number=0 -var i=0 -while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ - m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 - if(m_vecNodes[i].StateAge==0) - m_vecNodes[i].State="UNASSIGNED" + while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ + m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 + if(m_vecNodes[i].StateAge==0) + m_vecNodes[i].State="UNASSIGNED" + } + i=i+1 } -i=i+1 -} } # #Transistion to state free # function TransitionToFree(){ -m_eState="STATE_FREE" -m_unWaitCount=m_unLabelSearchWaitTime -m_selfMessage.State=m_eState + m_eState="STATE_FREE" + m_unWaitCount=m_unLabelSearchWaitTime + m_selfMessage.State=m_eState } # #Transistion to state asking # function TransitionToAsking(un_label){ -m_eState="STATE_ASKING" -m_nLabel=un_label -m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different -m_selfMessage.State=m_eState -m_selfMessage.ReqLable=m_nLabel -m_selfMessage.ReqID=m_unRequestId + m_eState="STATE_ASKING" + m_nLabel=un_label + m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different + m_selfMessage.State=m_eState + m_selfMessage.ReqLable=m_nLabel + m_selfMessage.ReqID=m_unRequestId -m_unWaitCount=m_unResponseTimeThreshold + m_unWaitCount=m_unResponseTimeThreshold } # #Transistion to state joining # function TransitionToJoining(){ -m_eState="STATE_JOINING" -m_selfMessage.State=m_eState -m_selfMessage.Lable=m_nLabel -m_unWaitCount=m_unJoiningLostPeriod + m_eState="STATE_JOINING" + m_selfMessage.State=m_eState + m_selfMessage.Lable=m_nLabel + m_unWaitCount=m_unJoiningLostPeriod -neighbors.listen("reply", - function(vid,value,rid){ - #store the received message - if(value.Lable==m_nLabel){ - m_cMeToPred.GlobalBearing=value.GlobalBearing + neighbors.listen("reply", + function(vid,value,rid){ + #store the received message + if(value.Lable==m_nLabel){ + m_cMeToPred.GlobalBearing=value.GlobalBearing - } -}) + } + }) } @@ -359,32 +337,32 @@ neighbors.listen("reply", #Transistion to state joined # function TransitionToJoined(){ -m_eState="STATE_JOINED" -m_selfMessage.State=m_eState -m_selfMessage.Lable=m_nLabel -m_vecNodes[m_nLabel].State="ASSIGNED" -neighbors.ignore("reply") + m_eState="STATE_JOINED" + m_selfMessage.State=m_eState + m_selfMessage.Lable=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + neighbors.ignore("reply") -#write statues -v_tag.put(m_nLabel, 1) + #write statues + v_tag.put(m_nLabel, 1) -m_navigation.x=0.0 -m_navigation.y=0.0 -uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) } # #Transistion to state Lock, lock the current formation # function TransitionToLock(){ -m_eState="STATE_LOCK" -m_selfMessage.State=m_eState -m_selfMessage.Lable=m_nLabel -m_vecNodes[m_nLabel].State="ASSIGNED" + m_eState="STATE_LOCK" + m_selfMessage.State=m_eState + m_selfMessage.Lable=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" -m_navigation.x=0.0 -m_navigation.y=0.0 -uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) } # @@ -554,19 +532,18 @@ function DoJoining(){ #the vector from self to target in global coordinate var S2Target=math.vec2.add(S2Pred,P2Target) #change the vector to local coordinate of self - var S2Target_dis=Length(S2Target) var S2Target_bearing=Angle(S2Target) m_bias=m_cMeToPred.Bearing-S2PGlobalBearing - S2Target_bearing=S2Target_bearing+m_bias - m_navigation=math.vec2.newp(S2Target_dis,S2Target_bearing) + #S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17 + m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) #test if is already in desired position if(math.abs(S2Target.x)0) { + UAVSTATE = "FOLLOW" + statef=follow + attractor=math.vec2.newp(0,0) + foreach(targets, function(id, tab) { + force=(0.05)*(tab.range)^4 + attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) + }) + uav_moveto(attractor.x, attractor.y) + } else { + log("No target in local table!") + #statef=idle + } +} + function uav_rccmd() { if(flight.rc_cmd==22) { log("cmd 22") @@ -74,9 +90,11 @@ function uav_rccmd() { neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 - statef = idle + UAVSTATE = "FOLLOW" + log(rc_goto.latitude, " ", rc_goto.longitude) + add_targetrb(0,rc_goto.latitude,rc_goto.longitude) + statef = follow #uav_goto() - add_user_rb(10,rc_goto.latitude,rc_goto.longitude) } else if(flight.rc_cmd==400) { flight.rc_cmd=0 uav_arm() diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 1792330..355fd0e 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -40,10 +40,10 @@ namespace buzz_utility{ void update_users(){ if(users_map.size()>0) { - /* Reset users information */ - buzzusers_reset(); + // Reset users information +// buzzusers_reset(); // create_stig_tables(); - /* Get user id and update user information */ + // Get user id and update user information map< int, Pos_struct >::iterator it; for (it=users_map.begin(); it!=users_map.end(); ++it){ //ROS_INFO("Buzz_utility will save user %i.", it->first); @@ -52,21 +52,20 @@ namespace buzz_utility{ (it->second).y, (it->second).z); } - }/*else - ROS_INFO("[%i] No new users",Robot_id);*/ + } } - int buzzusers_reset() { + /*int buzzusers_reset() { if(VM->state != BUZZVM_STATE_READY) return VM->state; - /* Make new table */ + //Make new table buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); //make_table(&t); if(VM->state != BUZZVM_STATE_READY) return VM->state; - /* Register table as global symbol */ - /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); + //Register table as global symbol + //buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); buzzvm_gload(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); - buzzvm_tget(VM);*/ + buzzvm_tget(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); buzzvm_gload(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1)); @@ -75,55 +74,47 @@ namespace buzz_utility{ //buzzvm_pushi(VM, 2); //buzzvm_callc(VM); return VM->state; - } + }*/ int buzzusers_add(int id, double latitude, double longitude, double altitude) { if(VM->state != BUZZVM_STATE_READY) return VM->state; - /* Get users "p" table */ + // Get users "p" table /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); buzzvm_gload(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1)); buzzvm_tget(VM);*/ buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1)); - buzzvm_gload(VM); - //buzzvm_pushi(VM, 1); - //buzzvm_callc(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, "dataG", 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, "dataG", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 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 */ + // When we get here, the "data" table is on top of the stack + // Push user id buzzvm_pushi(VM, id); - /* Create entry table */ + // Create entry table buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE); - /* Insert latitude */ + // Insert latitude buzzvm_push(VM, entry); buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1)); buzzvm_pushf(VM, latitude); buzzvm_tput(VM); - /* Insert longitude */ + // Insert longitude buzzvm_push(VM, entry); buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1)); buzzvm_pushf(VM, longitude); buzzvm_tput(VM); - /* Insert altitude */ + // Insert altitude buzzvm_push(VM, entry); buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1)); buzzvm_pushf(VM, altitude); buzzvm_tput(VM); - /* Save entry into data table */ + // Save entry into data table buzzvm_push(VM, entry); buzzvm_tput(VM); //ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); @@ -329,8 +320,8 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_adduserRB)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB)); buzzvm_gstore(VM); return VM->state; @@ -371,7 +362,7 @@ namespace buzz_utility{ buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "add_user_rb", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_gstore(VM); @@ -692,6 +683,7 @@ int create_stig_tables() { buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::update_neighbors(VM); + buzzuav_closures::buzzuav_update_targets(VM); //update_users(); buzzuav_closures::buzzuav_update_flight_status(VM); } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 658d206..2261053 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -25,8 +25,8 @@ namespace buzzuav_closures{ static int rc_cmd=0; static int buzz_cmd=0; static float height=0; - - + + std::map< int, buzz_utility::RB_struct> targets_map; std::map< int, buzz_utility::Pos_struct> neighbors_map; /****************************************/ @@ -89,13 +89,20 @@ namespace buzzuav_closures{ out[2] = height; //constant height. } + float constrainAngle(float x){ + x = fmod(x,2*M_PI); + if (x < 0.0) + x += 2*M_PI; + return x; + } + 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[1] = constrainAngle(atan2(ned_y,ned_x)); out[2] = 0.0; } @@ -126,57 +133,57 @@ namespace buzzuav_closures{ 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]); + //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]); buzz_cmd= COMMAND_MOVETO; // TO DO what should we use return buzzvm_ret0(vm); } - int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) { + int buzzuav_update_targets(buzzvm_t vm) { 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_pushs(vm, buzzvm_string_register(vm, "targets", 1)); + //buzzobj_t t = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); + //buzzvm_push(vm, t); + buzzvm_pusht(vm); + buzzobj_t targettbl = buzzvm_stack_at(vm, 1); + //buzzvm_tput(vm); + //buzzvm_dup(vm); + double rb[3], tmp[3]; + map< int, buzz_utility::RB_struct >::iterator it; + for (it=targets_map.begin(); it!=targets_map.end(); ++it){ + tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height; + rb_from_gps(tmp, rb, cur_pos); + ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]); + buzzvm_push(vm, targettbl); + /* When we get here, the "targets" table is on top of the stack */ + //ROS_INFO("Buzz_utility will save user %i.", it->first); + /* Push user id */ + buzzvm_pushi(vm, it->first); + /* Create entry table */ + buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); + /* Insert range */ + buzzvm_push(vm, entry); + buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1)); + buzzvm_pushf(vm, rb[0]); + buzzvm_tput(vm); + /* Insert longitude */ + buzzvm_push(vm, entry); + buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1)); + buzzvm_pushf(vm, rb[1]); + buzzvm_tput(vm); + /* Save entry into data table */ + buzzvm_push(vm, entry); buzzvm_tput(vm); - buzzvm_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); + buzzvm_gstore(vm); + return vm->state; } - int buzzuav_adduserRB(buzzvm_t vm) { + int buzzuav_addtargetRB(buzzvm_t vm) { buzzvm_lnum_assert(vm, 3); - buzzvm_lload(vm, 1); /* longitude */ - buzzvm_lload(vm, 2); /* latitude */ - buzzvm_lload(vm, 3); /* id */ + 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); @@ -189,10 +196,20 @@ namespace buzzuav_closures{ 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]); + //printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]); + buzz_utility::RB_struct RB_arr; + RB_arr.la=tmp[0]; + RB_arr.lo=tmp[1]; + RB_arr.r=rb[0]; + RB_arr.b=rb[1]; + map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(uid); + if(it!=targets_map.end()) + targets_map.erase(it); + targets_map.insert(make_pair(uid, RB_arr)); + //ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); + return vm->state; } else - printf(" ---------- User too far %f\n",rb[0]); + printf(" ---------- Target too far %f\n",rb[0]); return 0; } @@ -427,7 +444,7 @@ namespace buzzuav_closures{ 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); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ec32acf..bae94ea 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -92,22 +92,28 @@ namespace rosbzz_node{ /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// - ROS_INFO("[%i] STARTING MAIN LOOP!", robot_id); + //ROS_WARN("[%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*/ + //ROS_WARN("[%i]-----------NEIG", robot_id); neighbours_pos_publisher(); /*Check updater state and step code*/ + //ROS_WARN("[%i]-----------UPD", robot_id); update_routine(bcfname.c_str(), dbgfname.c_str()); /*Step buzz script */ + //ROS_WARN("[%i]-----------STEP", robot_id); buzz_utility::buzz_script_step(); /*Prepare messages and publish them in respective topic*/ + //ROS_WARN("[%i]-----------MSG", robot_id); prepare_msg_and_publish(); /*call flight controler service to set command long*/ + //ROS_WARN("[%i]-----------FC", robot_id); flight_controller_service_call(); /*Set multi message available after update*/ + //ROS_WARN("[%i]-----------UPD", robot_id); if(get_update_status()){ set_read_update_status(); multi_msg=true; @@ -122,6 +128,7 @@ namespace rosbzz_node{ } /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ //no_of_robots=get_number_of_robots(); + //ROS_WARN("[%i]-----------ROBOTS", robot_id); 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; @@ -603,13 +610,20 @@ namespace rosbzz_node{ } } + float roscontroller::constrainAngle(float x){ + x = fmod(x,2*M_PI); + if (x < 0.0) + x += 2*M_PI; + return x; + } + void roscontroller::gps_rb(GPS nei_pos, double out[]) { float ned_x=0.0, ned_y=0.0; gps_ned_cur(ned_x, ned_y, nei_pos); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); //out[0] = std::floor(out[0] * 1000000) / 1000000; - out[1] = atan2(ned_y,ned_x); + out[1] = constrainAngle(atan2(ned_y,ned_x)); //out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = 0.0; } From 82adb66f7b2e6a2d0ad97b70e6af58c1f5a6c72f Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 26 Jun 2017 15:03:58 -0400 Subject: [PATCH 05/14] minor fixes to graph --- script/Graph_drone.graph | 4 ---- script/graphform.bzz | 4 ++-- script/include/uavstates.bzz | 13 ------------- 3 files changed, 2 insertions(+), 19 deletions(-) diff --git a/script/Graph_drone.graph b/script/Graph_drone.graph index 28837e9..f809476 100644 --- a/script/Graph_drone.graph +++ b/script/Graph_drone.graph @@ -1,9 +1,5 @@ 0 -1 -1 -1 1 0 1000.0 0.0 2 0 1000.0 1.57 -<<<<<<< HEAD 3 0 1000.0 3.14 4 0 1000.0 4.71 -======= -3 0 1000.0 3.14 ->>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7 diff --git a/script/graphform.bzz b/script/graphform.bzz index 4a612a7..c359b6f 100644 --- a/script/graphform.bzz +++ b/script/graphform.bzz @@ -88,7 +88,7 @@ step_cunt=0 v_tag = stigmergy.create(1) # Lennard-Jones parameters -EPSILON = 3.5 #3.5 +EPSILON = 13.5 #3.5 # Lennard-Jones interaction magnitude @@ -705,7 +705,7 @@ if(m_nLabel>1){ log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1) } #move -uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) +uav_moveto(m_navigation.x,m_navigation.y) } function action(){ diff --git a/script/include/uavstates.bzz b/script/include/uavstates.bzz index 08bc647..fef18ac 100644 --- a/script/include/uavstates.bzz +++ b/script/include/uavstates.bzz @@ -50,11 +50,7 @@ function land() { uav_land() } else { -<<<<<<< HEAD barrier_set(ROBOTS,turnedoff,land) -======= - barrier_set(ROBOTS,idle,land) ->>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7 barrier_ready() timeW=0 #barrier = nil @@ -62,7 +58,6 @@ function land() { } } -<<<<<<< HEAD function follow() { if(size(targets)>0) { UAVSTATE = "FOLLOW" @@ -79,8 +74,6 @@ function follow() { } } -======= ->>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7 function uav_rccmd() { if(flight.rc_cmd==22) { log("cmd 22") @@ -97,17 +90,11 @@ function uav_rccmd() { neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 -<<<<<<< HEAD UAVSTATE = "FOLLOW" log(rc_goto.latitude, " ", rc_goto.longitude) add_targetrb(0,rc_goto.latitude,rc_goto.longitude) statef = follow #uav_goto() -======= - statef = idle - #uav_goto() - add_user_rb(10,rc_goto.latitude,rc_goto.longitude) ->>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7 } else if(flight.rc_cmd==400) { flight.rc_cmd=0 uav_arm() From 8552cf79c887703bdf7c7998a5f4755770f9274b Mon Sep 17 00:00:00 2001 From: pyhs Date: Wed, 5 Jul 2017 09:37:35 -0400 Subject: [PATCH 06/14] Added xbee status update functions --- include/buzz_utility.h | 2 + include/buzzuav_closures.h | 10 +++++ include/roscontroller.h | 2 + src/buzz_utility.cpp | 25 ++++++++---- src/buzzuav_closures.cpp | 78 +++++++++++++++++++++++++++++++++++--- src/roscontroller.cpp | 36 ++++++++++++++++++ 6 files changed, 140 insertions(+), 13 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 40be6f0..b61faf3 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -49,6 +49,8 @@ uint64_t* obt_out_msg(); void update_sensors(); +void update_xbee_status(); + int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 1aa8022..0e9ff4a 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -47,6 +47,11 @@ void rc_set_goto(double pos[]); void rc_call(int rc_cmd); /* sets the battery state */ void set_battery(float voltage,float current,float remaining); +void set_deque_full(bool state); +void set_rssi(float value); +void set_raw_packet_loss(float value); +void set_filtered_packet_loss(float value); +void set_api_rssi(float value); /* sets current position */ void set_currentpos(double latitude, double longitude, double altitude); /*retuns the current go to position */ @@ -83,6 +88,11 @@ int buzzuav_gohome(buzzvm_t vm); * Updates battery information in Buzz */ int buzzuav_update_battery(buzzvm_t vm); +int buzzuav_update_deque_full(buzzvm_t vm); +int buzzuav_update_rssi(buzzvm_t vm); +int buzzuav_update_raw_packet_loss(buzzvm_t vm); +int buzzuav_update_filtered_packet_loss(buzzvm_t vm); +int buzzuav_update_api_rssi(buzzvm_t vm); /* * Updates current position in Buzz */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 3d2ecec..fe54707 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -244,6 +244,8 @@ private: bool GetRawPacketLoss(const uint8_t short_id, float &result); bool GetFilteredPacketLoss(const uint8_t short_id, float &result); + void get_xbee_status(); + }; } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 2d9a117..b644439 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -18,7 +18,7 @@ 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; @@ -78,7 +78,7 @@ namespace buzz_utility{ int buzzusers_add(int id, double latitude, double longitude, double altitude) { if(VM->state != BUZZVM_STATE_READY) return VM->state; - // Get users "p" table + // Get users "p" table /*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1)); buzzvm_gload(VM); buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1)); @@ -94,8 +94,8 @@ namespace buzz_utility{ buzzvm_tput(VM); buzzvm_push(VM, data); } - // When we get here, the "data" table is on top of the stack - // Push user id + // 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); @@ -489,7 +489,7 @@ int create_stig_tables() { ROS_ERROR("[%i] Error registering hooks", Robot_id); return 0; } - + /* Create vstig tables if(create_stig_tables() != BUZZVM_STATE_READY) { buzzvm_destroy(&VM); @@ -499,7 +499,7 @@ int create_stig_tables() { //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; return 0; }*/ - + /* Save bytecode file name */ BO_FNAME = strdup(bo_filename); @@ -561,7 +561,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); @@ -689,11 +689,22 @@ int create_stig_tables() { buzzuav_closures::buzzuav_update_flight_status(VM); } + void update_xbee_status(){ + /* Update sensors*/ + buzzuav_closures::buzzuav_update_deque_full(VM); + buzzuav_closures::buzzuav_update_rssi(VM); + buzzuav_closures::buzzuav_update_raw_packet_loss(VM); + buzzuav_closures::buzzuav_update_filtered_packet_loss(VM); + buzzuav_closures::buzzuav_update_api_rssi(VM); + } + void buzz_script_step() { /*Process available messages*/ in_message_process(); /*Update sensors*/ update_sensors(); + + update_xbee_status(); /* Call Buzz step() function */ if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { ROS_ERROR("%s: execution terminated abnormally: %s", diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 964170a..74e5ca8 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -24,7 +24,12 @@ namespace buzzuav_closures{ static int rc_cmd=0; static int buzz_cmd=0; static float height=0; - + static bool deque_full = false; + static float rssi = 0.0; + static float raw_packet_loss = 0.0; + static float filtered_packet_loss = 0.0; + static float api_rssi = 0.0; + std::map< int, buzz_utility::RB_struct> targets_map; std::map< int, buzz_utility::Pos_struct> neighbors_map; @@ -95,7 +100,7 @@ namespace buzzuav_closures{ return x; } - void rb_from_gps(double nei[], double out[], double cur[]){ + 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; @@ -148,7 +153,7 @@ namespace buzzuav_closures{ //buzzvm_dup(vm); double rb[3], tmp[3]; map< int, buzz_utility::RB_struct >::iterator it; - for (it=targets_map.begin(); it!=targets_map.end(); ++it){ + for (it=targets_map.begin(); it!=targets_map.end(); ++it){ tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height; rb_from_gps(tmp, rb, cur_pos); ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]); @@ -334,7 +339,68 @@ namespace buzzuav_closures{ buzzvm_gstore(vm); return vm->state; } - /****************************************/ + + void set_deque_full(bool state) + { + deque_full = state; + } + + int buzzuav_update_deque_full(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "deque_full", 1)); + buzzvm_pushi(vm, static_cast(deque_full)); + buzzvm_gstore(vm); + return vm->state; + } + + void set_rssi(float value) + { + rssi = value; + } + + int buzzuav_update_rssi(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1)); + buzzvm_pushf(vm, rssi); + buzzvm_gstore(vm); + return vm->state; + } + + void set_raw_packet_loss(float value) + { + raw_packet_loss = value; + } + + int buzzuav_update_raw_packet_loss(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1)); + buzzvm_pushf(vm, raw_packet_loss); + buzzvm_gstore(vm); + return vm->state; + } + + void set_filtered_packet_loss(float value) + { + filtered_packet_loss = value; + } + + int buzzuav_update_filtered_packet_loss(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1)); + buzzvm_pushf(vm, filtered_packet_loss); + buzzvm_gstore(vm); + return vm->state; + } + + void set_api_rssi(float value) + { + api_rssi = value; + } + + int buzzuav_update_api_rssi(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1)); + buzzvm_pushf(vm, api_rssi); + buzzvm_gstore(vm); + return vm->state; + } + + /***************************************/ /*current pos update*/ /***************************************/ void set_currentpos(double latitude, double longitude, double altitude){ @@ -409,7 +475,7 @@ namespace buzzuav_closures{ rc_cmd=0; buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); buzzvm_pushi(vm, status); - buzzvm_tput(vm); + buzzvm_tput(vm); buzzvm_gstore(vm); //also set rc_controllers goto buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); @@ -434,7 +500,7 @@ namespace buzzuav_closures{ /******************************************************/ /*Create an obstacle Buzz table from proximity sensors*/ - /* Acessing proximity in buzz script + /* Acessing proximity in buzz script proximity[0].angle and proximity[0].value - front "" "" "" - right and back proximity[3].angle and proximity[3].value - left diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c7b2303..27cdd4e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -231,6 +231,7 @@ void roscontroller::RosControllerRun() 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); + get_xbee_status(); /*run once*/ ros::spinOnce(); /*loop rate of ros*/ @@ -1202,4 +1203,39 @@ void roscontroller::get_number_of_robots() { } */ } + +void roscontroller::get_xbee_status() +/* Description: + * Call all the xbee node services and update the xbee status + ------------------------------------------------------------------ */ +{ + bool result_bool; + float result_float; + const uint8_t all_ids = 0xFF; + if(GetDequeFull(result_bool)) + { + buzzuav_closures::set_deque_full(result_bool); + } + if(GetRssi(result_float)) + { + buzzuav_closures::set_rssi(result_float); + } + if(GetRawPacketLoss(all_ids, result_float)) + { + buzzuav_closures::set_raw_packet_loss(result_float); + } + if(GetFilteredPacketLoss(all_ids, result_float)) + { + buzzuav_closures::set_filtered_packet_loss(result_float); + } + // This part needs testing since it can overload the xbee module + /* + * if(GetAPIRssi(all_ids, result_float)) + * { + * buzzuav_closures::set_api_rssi(result_float); + * } + * TriggerAPIRssi(all_ids); + */ +} + } From eadf2f6f59e4df721cb518d2b6888713976ff1a1 Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Wed, 5 Jul 2017 22:57:21 -0400 Subject: [PATCH 07/14] new dmp socket --- {script => buzz_scripts}/Update.log | 0 {script => buzz_scripts}/flock.bzz | 0 {script => buzz_scripts}/graphform.bzz | 4 ++-- {script => buzz_scripts/include}/Graph_drone.graph | 0 {script => buzz_scripts/include}/Graph_fixed.graph | 0 {script => buzz_scripts}/include/barrier.bzz | 0 {script => buzz_scripts}/include/string.bzz | 0 {script => buzz_scripts}/include/uavstates.bzz | 0 {script => buzz_scripts}/include/update.bzz | 0 {script => buzz_scripts}/include/vec2.bzz | 0 {script => buzz_scripts}/include/vstigenv.bzz | 0 {script => buzz_scripts}/stand_by.bzz | 0 {script => buzz_scripts}/tentacle.bzz | 0 {script => buzz_scripts}/testalone.bzz | 0 {script => buzz_scripts}/testflocksim.bzz | 0 15 files changed, 2 insertions(+), 2 deletions(-) rename {script => buzz_scripts}/Update.log (100%) rename {script => buzz_scripts}/flock.bzz (100%) rename {script => buzz_scripts}/graphform.bzz (98%) rename {script => buzz_scripts/include}/Graph_drone.graph (100%) rename {script => buzz_scripts/include}/Graph_fixed.graph (100%) rename {script => buzz_scripts}/include/barrier.bzz (100%) rename {script => buzz_scripts}/include/string.bzz (100%) rename {script => buzz_scripts}/include/uavstates.bzz (100%) rename {script => buzz_scripts}/include/update.bzz (100%) rename {script => buzz_scripts}/include/vec2.bzz (100%) rename {script => buzz_scripts}/include/vstigenv.bzz (100%) rename {script => buzz_scripts}/stand_by.bzz (100%) rename {script => buzz_scripts}/tentacle.bzz (100%) rename {script => buzz_scripts}/testalone.bzz (100%) rename {script => buzz_scripts}/testflocksim.bzz (100%) diff --git a/script/Update.log b/buzz_scripts/Update.log similarity index 100% rename from script/Update.log rename to buzz_scripts/Update.log diff --git a/script/flock.bzz b/buzz_scripts/flock.bzz similarity index 100% rename from script/flock.bzz rename to buzz_scripts/flock.bzz diff --git a/script/graphform.bzz b/buzz_scripts/graphform.bzz similarity index 98% rename from script/graphform.bzz rename to buzz_scripts/graphform.bzz index c359b6f..103c31b 100644 --- a/script/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -793,9 +793,9 @@ function step(){ # function Reset(){ m_vecNodes={} - m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/script/Graph_drone.graph")#change the .graph file when necessary + m_vecNodes = parse_graph("/home/ubuntu/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary m_vecNodes_fixed={} - m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/script/Graph_fixed.graph") + m_vecNodes_fixed=parse_graph_fixed("/home/ubuntu/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") m_nLabel=-1 #start listening diff --git a/script/Graph_drone.graph b/buzz_scripts/include/Graph_drone.graph similarity index 100% rename from script/Graph_drone.graph rename to buzz_scripts/include/Graph_drone.graph diff --git a/script/Graph_fixed.graph b/buzz_scripts/include/Graph_fixed.graph similarity index 100% rename from script/Graph_fixed.graph rename to buzz_scripts/include/Graph_fixed.graph diff --git a/script/include/barrier.bzz b/buzz_scripts/include/barrier.bzz similarity index 100% rename from script/include/barrier.bzz rename to buzz_scripts/include/barrier.bzz diff --git a/script/include/string.bzz b/buzz_scripts/include/string.bzz similarity index 100% rename from script/include/string.bzz rename to buzz_scripts/include/string.bzz diff --git a/script/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz similarity index 100% rename from script/include/uavstates.bzz rename to buzz_scripts/include/uavstates.bzz diff --git a/script/include/update.bzz b/buzz_scripts/include/update.bzz similarity index 100% rename from script/include/update.bzz rename to buzz_scripts/include/update.bzz diff --git a/script/include/vec2.bzz b/buzz_scripts/include/vec2.bzz similarity index 100% rename from script/include/vec2.bzz rename to buzz_scripts/include/vec2.bzz diff --git a/script/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz similarity index 100% rename from script/include/vstigenv.bzz rename to buzz_scripts/include/vstigenv.bzz diff --git a/script/stand_by.bzz b/buzz_scripts/stand_by.bzz similarity index 100% rename from script/stand_by.bzz rename to buzz_scripts/stand_by.bzz diff --git a/script/tentacle.bzz b/buzz_scripts/tentacle.bzz similarity index 100% rename from script/tentacle.bzz rename to buzz_scripts/tentacle.bzz diff --git a/script/testalone.bzz b/buzz_scripts/testalone.bzz similarity index 100% rename from script/testalone.bzz rename to buzz_scripts/testalone.bzz diff --git a/script/testflocksim.bzz b/buzz_scripts/testflocksim.bzz similarity index 100% rename from script/testflocksim.bzz rename to buzz_scripts/testflocksim.bzz From 69729d926b9ea6db9fc1e8ebacf7fdd5220950d2 Mon Sep 17 00:00:00 2001 From: pyhs Date: Thu, 6 Jul 2017 13:54:51 -0400 Subject: [PATCH 08/14] Removed Absolute Path in graphform.bzz --- CMakeLists.txt | 2 ++ buzz_scripts/{graphform.bzz => graphform.bzz.in} | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) rename buzz_scripts/{graphform.bzz => graphform.bzz.in} (98%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 28df175..4899923 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,8 @@ if(UNIX) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11") endif() +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/graphform.bzz.in ${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/graphform.bzz) + ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS roscpp diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz.in similarity index 98% rename from buzz_scripts/graphform.bzz rename to buzz_scripts/graphform.bzz.in index 103c31b..ea2d45e 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz.in @@ -793,9 +793,9 @@ function step(){ # function Reset(){ m_vecNodes={} - m_vecNodes = parse_graph("/home/ubuntu/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary + m_vecNodes = parse_graph("${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary m_vecNodes_fixed={} - m_vecNodes_fixed=parse_graph_fixed("/home/ubuntu/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") + m_vecNodes_fixed=parse_graph_fixed("${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/include/Graph_fixed.graph") m_nLabel=-1 #start listening From 0f37bc22e0c3c985f7942f59a2fa989c1f51953e Mon Sep 17 00:00:00 2001 From: pyhs Date: Thu, 6 Jul 2017 14:50:29 -0400 Subject: [PATCH 09/14] Combined Xbee status info into a structure for the buzz VM --- include/buzz_utility.h | 2 -- include/buzzuav_closures.h | 9 +++---- src/buzz_utility.cpp | 12 +-------- src/buzzuav_closures.cpp | 50 ++++++++++++++++---------------------- 4 files changed, 26 insertions(+), 47 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index b61faf3..40be6f0 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -49,8 +49,6 @@ uint64_t* obt_out_msg(); void update_sensors(); -void update_xbee_status(); - int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 0e9ff4a..fac506c 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -88,11 +88,10 @@ int buzzuav_gohome(buzzvm_t vm); * Updates battery information in Buzz */ int buzzuav_update_battery(buzzvm_t vm); -int buzzuav_update_deque_full(buzzvm_t vm); -int buzzuav_update_rssi(buzzvm_t vm); -int buzzuav_update_raw_packet_loss(buzzvm_t vm); -int buzzuav_update_filtered_packet_loss(buzzvm_t vm); -int buzzuav_update_api_rssi(buzzvm_t vm); +/* + * Updates xbee_status information in Buzz + */ +int buzzuav_update_xbee_status(buzzvm_t vm); /* * Updates current position in Buzz */ diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index b644439..3453a65 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -681,6 +681,7 @@ int create_stig_tables() { void update_sensors(){ /* Update sensors*/ buzzuav_closures::buzzuav_update_battery(VM); + buzzuav_closures::buzzuav_update_xbee_status(VM); buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::update_neighbors(VM); @@ -689,22 +690,11 @@ int create_stig_tables() { buzzuav_closures::buzzuav_update_flight_status(VM); } - void update_xbee_status(){ - /* Update sensors*/ - buzzuav_closures::buzzuav_update_deque_full(VM); - buzzuav_closures::buzzuav_update_rssi(VM); - buzzuav_closures::buzzuav_update_raw_packet_loss(VM); - buzzuav_closures::buzzuav_update_filtered_packet_loss(VM); - buzzuav_closures::buzzuav_update_api_rssi(VM); - } - void buzz_script_step() { /*Process available messages*/ in_message_process(); /*Update sensors*/ update_sensors(); - - update_xbee_status(); /* Call Buzz step() function */ if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { ROS_ERROR("%s: execution terminated abnormally: %s", diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 74e5ca8..fcfb752 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -345,57 +345,49 @@ namespace buzzuav_closures{ deque_full = state; } - int buzzuav_update_deque_full(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "deque_full", 1)); - buzzvm_pushi(vm, static_cast(deque_full)); - buzzvm_gstore(vm); - return vm->state; - } - void set_rssi(float value) { rssi = value; } - int buzzuav_update_rssi(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1)); - buzzvm_pushf(vm, rssi); - buzzvm_gstore(vm); - return vm->state; - } - void set_raw_packet_loss(float value) { raw_packet_loss = value; } - int buzzuav_update_raw_packet_loss(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1)); - buzzvm_pushf(vm, raw_packet_loss); - buzzvm_gstore(vm); - return vm->state; - } - void set_filtered_packet_loss(float value) { filtered_packet_loss = value; } - int buzzuav_update_filtered_packet_loss(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1)); - buzzvm_pushf(vm, filtered_packet_loss); - buzzvm_gstore(vm); - return vm->state; - } - void set_api_rssi(float value) { api_rssi = value; } - int buzzuav_update_api_rssi(buzzvm_t vm) { + int buzzuav_update_xbee_status(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "deque_full", 1)); + buzzvm_pushi(vm, static_cast(deque_full)); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1)); + buzzvm_pushf(vm, rssi); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1)); + buzzvm_pushf(vm, raw_packet_loss); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1)); + buzzvm_pushf(vm, filtered_packet_loss); + buzzvm_tput(vm); + buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1)); buzzvm_pushf(vm, api_rssi); + buzzvm_tput(vm); buzzvm_gstore(vm); return vm->state; } From af301a1c62bae52e6a401ed46f3c471f4bc3353a Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 7 Jul 2017 00:04:40 -0400 Subject: [PATCH 10/14] enhance graph script --- buzz_scripts/graphform.bzz | 228 ++++--- buzz_scripts/graphform.bzz_old | 826 +++++++++++++++++++++++++ buzz_scripts/include/Graph_drone.graph | 10 +- buzz_scripts/include/Graph_fixed.graph | 8 +- buzz_scripts/include/shapes.bzz | 111 ++++ 5 files changed, 1055 insertions(+), 128 deletions(-) create mode 100644 buzz_scripts/graphform.bzz_old create mode 100644 buzz_scripts/include/shapes.bzz diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz index 103c31b..dff7dd0 100644 --- a/buzz_scripts/graphform.bzz +++ b/buzz_scripts/graphform.bzz @@ -7,6 +7,8 @@ include "update.bzz" include "barrier.bzz" # don't use a stigmergy id=11 with this header. include "uavstates.bzz" # require an 'action' function to be defined here. +include "shapes.bzz" + ROBOT_RADIUS=50 ROBOT_DIAMETER=2.0*ROBOT_RADIUS ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER @@ -22,18 +24,18 @@ m_MessageState={}#store received neighbour message m_MessageLable={}#store received neighbour message m_MessageReqLable={}#store received neighbour message m_MessageReqID={}#store received neighbour message -m_MessageSide={}#store received neighbour message m_MessageResponse={}#store received neighbour message m_MessageRange={}#store received neighbour message m_MessageBearing={}#store received neighbour message m_neighbourCunt=0#used to cunt neighbours #Save message from one neighbour -#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Side,Response,Range,Bearing -m_receivedMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE",.Range=0,.Bearing=0} +#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Response,Range,Bearing +m_receivedMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} + # #Save the message to send -#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Side(side),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) -m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} +#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) +m_selfMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE")} #Current robot state m_eState="STATE_FREE" @@ -47,19 +49,12 @@ m_navigation={.x=0,.y=0} #Debug vector to draw #CVector2 m_cDebugVector -#Table of the nodes in the graph -m_vecNodes={} -m_vecNodes_fixed={} - #Current label being requested or chosen (-1 when none) m_nLabel=-1 #Label request id m_unRequestId=0 -#Side -m_unbSide=0 - #Global bias, used to map local coordinate to global coordinate m_bias=0 @@ -87,8 +82,9 @@ step_cunt=0 #virtual stigmergy v_tag = stigmergy.create(1) -# Lennard-Jones parameters -EPSILON = 13.5 #3.5 +# Lennard-Jones parameters, may need change +EPSILON = 13.5 #the LJ parameter for other robots +EPSILON_FOR1 = 10.0 #the LJ parameter for the robot labeled 1 # Lennard-Jones interaction magnitude @@ -130,6 +126,64 @@ function count(table,value){ return number } +#map from int to state +function i2s(value){ + if(value==1){ + return "STATE_FREE" + } + else if(value==2){ + return "STATE_ASKING" + } + else if(value==3){ + return "STATE_JOINING" + } + else if(value==4){ + return "STATE_JOINED" + } + else if(value==5){ + return "STATE_LOCK" + } +} + +#map from state to int +function s2i(value){ + if(value=="STATE_FREE"){ + return 1 + } + else if(value=="STATE_ASKING"){ + return 2 + } + else if(value=="STATE_JOINING"){ + return 3 + } + else if(value=="STATE_JOINED"){ + return 4 + } + else if(value=="STATE_LOCK"){ + return 5 + } +} +#map form int to response +function i2r(value){ + if(value==1){ + return "REQ_NONE" + } + else if(value==2){ + return "REQ_GRANTED" + } + +} +#map from response to int +function r2i(value){ + if(value=="REQ_NONE"){ + return 1 + } + else if(value=="REQ_GRANTED"){ + return 2 + } +} + + # #return the index of value # @@ -158,90 +212,25 @@ function pow(base,exponent){ } } -# -# Graph parsing -# -function parse_graph(fname) { - # Graph data - var gd = {} - # Open the file - var fd = io.fopen(fname, "r") - if(not fd) { - log("Can't open '", fname, "'") - return nil - } - # Parse the file line by line - var rrec # Record read from line - var grec # Record parsed into graph - io.fforeach(fd, function(line) { - # Parse file line - rrec = string.split(line, "\t ") - # Make record - gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing - .Lable = string.toint(rrec[0]), # Lable of the point - .Pred = string.toint(rrec[1]), # Lable of its predecessor - .distance = string.tofloat(rrec[2]), # distance to the predecessor - .bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot - .State="UNASSIGNED", - .StateAge=0 - }}) - # All done - io.fclose(fd) - return gd -} - -function parse_graph_fixed(fname) { - # Graph data - var gd = {} - # Open the file - var fd = io.fopen(fname, "r") - if(not fd) { - log("Can't open '", fname, "'") - return nil - } - # Parse the file line by line - var rrec # Record read from line - var grec # Record parsed into graph - io.fforeach(fd, function(line) { - # Parse file line - rrec = string.split(line, "\t ") - # Make record - gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2, side - .Pred1 = string.toint(rrec[1]), # Pred 1 lable - .Pred2 = string.toint(rrec[3]), # Pred 2 lable - .d1 = string.tofloat(rrec[2]), # Pred 1 distance - .d2 = string.tofloat(rrec[4]), # Pred 2 distance - #.S = string.toint(rrec[5]), # Side - .Lable=string.toint(rrec[0]), - .State="UNASSIGNED", - .StateAge=0 - }}) - # All done - io.fclose(fd) - return gd -} - function start_listen(){ -neighbors.listen("msg", +neighbors.listen("m", function(vid,value,rid){ #store the received message var temp_id=rid - m_receivedMessage.State=value.State + m_receivedMessage.State=i2s(value.State) m_receivedMessage.Lable=value.Lable m_receivedMessage.ReqLable=value.ReqLable m_receivedMessage.ReqID=value.ReqID - m_receivedMessage.Side=value.Side - m_receivedMessage.Response=value.Response + m_receivedMessage.Response=i2r(value.Response) Get_DisAndAzi(temp_id) #add the received message # - m_MessageState[m_neighbourCunt]=value.State + m_MessageState[m_neighbourCunt]=i2s(value.State) m_MessageLable[m_neighbourCunt]=value.Lable m_MessageReqLable[m_neighbourCunt]=value.ReqLable m_MessageReqID[m_neighbourCunt]=value.ReqID - m_MessageSide[m_neighbourCunt]=value.Side - m_MessageResponse[m_neighbourCunt]=value.Response + m_MessageResponse[m_neighbourCunt]=i2r(value.Response) m_MessageRange[m_neighbourCunt]=m_receivedMessage.Range m_MessageBearing[m_neighbourCunt]=m_receivedMessage.Bearing m_neighbourCunt=m_neighbourCunt+1 @@ -296,7 +285,7 @@ function UpdateNodeInfo(){ function TransitionToFree(){ m_eState="STATE_FREE" m_unWaitCount=m_unLabelSearchWaitTime - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) } # @@ -306,7 +295,7 @@ function TransitionToAsking(un_label){ m_eState="STATE_ASKING" m_nLabel=un_label m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.ReqLable=m_nLabel m_selfMessage.ReqID=m_unRequestId @@ -318,15 +307,15 @@ function TransitionToAsking(un_label){ # function TransitionToJoining(){ m_eState="STATE_JOINING" - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel m_unWaitCount=m_unJoiningLostPeriod - neighbors.listen("reply", + neighbors.listen("r", function(vid,value,rid){ #store the received message - if(value.Lable==m_nLabel){ - m_cMeToPred.GlobalBearing=value.GlobalBearing + if(value.Label==m_nLabel){ + m_cMeToPred.GlobalBearing=value.Bearing } }) @@ -338,10 +327,10 @@ function TransitionToJoining(){ # function TransitionToJoined(){ m_eState="STATE_JOINED" - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel m_vecNodes[m_nLabel].State="ASSIGNED" - neighbors.ignore("reply") + neighbors.ignore("r") #write statues v_tag.put(m_nLabel, 1) @@ -356,7 +345,7 @@ function TransitionToJoined(){ # function TransitionToLock(){ m_eState="STATE_LOCK" - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel m_vecNodes[m_nLabel].State="ASSIGNED" @@ -369,7 +358,8 @@ function TransitionToLock(){ # Do free # function DoFree() { - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) + #wait for a while before looking for a lable if(m_unWaitCount>0) m_unWaitCount=m_unWaitCount-1 @@ -442,7 +432,8 @@ function DoFree() { uav_moveto(0.0,0.0) } #set message - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) + } @@ -470,7 +461,7 @@ function DoAsking(){ if(psResponse==-1){ #no response, wait m_unWaitCount=m_unWaitCount-1 - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.ReqLable=m_nLabel m_selfMessage.ReqID=m_unRequestId if(m_unWaitCount==0){ @@ -520,7 +511,7 @@ function DoJoining(){ #attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate var S2PGlobalBearing=0 - #m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing) + m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing) if(m_cMeToPred.GlobalBearing>math.pi) S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi @@ -534,7 +525,7 @@ function DoJoining(){ #change the vector to local coordinate of self var S2Target_bearing=Angle(S2Target) m_bias=m_cMeToPred.Bearing-S2PGlobalBearing - #S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17 + S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17 m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) @@ -557,7 +548,7 @@ function DoJoining(){ } #pack the communication package - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel } @@ -566,7 +557,7 @@ function DoJoining(){ #Do joined # function DoJoined(){ - m_selfMessage.State=m_eState + m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel #collect all requests @@ -590,8 +581,8 @@ function DoJoined(){ JoiningLable=m_MessageLable[i] if(m_nLabel==m_vecNodes[JoiningLable].Pred){ ##joining wrt this dot,send the global bearing - var m_messageForJoining={.Lable=JoiningLable,.GlobalBearing=m_MessageBearing[i]-m_bias} - neighbors.broadcast("reply",m_messageForJoining) + var m_messageForJoining={.Label=JoiningLable,.Bearing=m_MessageBearing[i]-m_bias} + neighbors.broadcast("r",m_messageForJoining) } } #if it is the pred @@ -617,7 +608,7 @@ function DoJoined(){ var ReqID=m_MessageReqID[mapRequests[ReqIndex]] m_selfMessage.ReqLable=ReqLable m_selfMessage.ReqID=ReqID - m_selfMessage.Response="REQ_GRANTED" + m_selfMessage.Response=r2i("REQ_GRANTED") } #lost pred, wait for some time and transit to free @@ -638,16 +629,16 @@ function DoJoined(){ #check if should to transists to lock - if(v_tag.size()==ROBOTS){ - TransitionToLock() - } +# if(v_tag.size()==ROBOTS){ +# TransitionToLock() +# } } # #Do Lock # function DoLock(){ -m_selfMessage.State=m_eState +m_selfMessage.State=s2i(m_eState) m_selfMessage.Lable=m_nLabel m_navigation.x=0.0 @@ -675,14 +666,14 @@ i=i+1 #calculate motion vection if(m_nLabel==0){ - m_navigation.x=0.0 + m_navigation.x=0.0#change value so that robot 0 will move m_navigation.y=0.0 log(";",m_nLabel,";",0) } if(m_nLabel==1){ var tempvec={.Range=0.0,.Bearing=0.0} - tempvec.Range=FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,10) + tempvec.Range=FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON_FOR1) #tempvec.Range=mypred1.range-m_vecNodes_fixed[m_nLabel].d1 tempvec.Bearing=mypred1.bearing m_navigation=math.vec2.newp(tempvec.Range,tempvec.Bearing) @@ -698,14 +689,14 @@ if(m_nLabel>1){ #cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing) cDir=math.vec2.add(cDir1,cDir2) - cDir=math.vec2.scale(cDir,100) + cDir=math.vec2.scale(cDir,5) m_navigation.x=cDir.x m_navigation.y=cDir.y #log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2) log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1) } #move -uav_moveto(m_navigation.x,m_navigation.y) +uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) } function action(){ @@ -741,7 +732,7 @@ function step(){ #update the graph UpdateNodeInfo() #reset message package to be sent - m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} + m_selfMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE")} # #act according to current state # @@ -769,7 +760,7 @@ function step(){ #navigation #broadcast messag - neighbors.broadcast("msg",m_selfMessage) + neighbors.broadcast("m",m_selfMessage) # #clean message storage @@ -777,7 +768,6 @@ function step(){ m_MessageLable={}#store received neighbour message m_MessageReqLable={}#store received neighbour message m_MessageReqID={}#store received neighbour message - m_MessageSide={}#store received neighbour message m_MessageResponse={}#store received neighbour message m_MessageRange={}#store received neighbour message m_MessageBearing={}#store received neighbour message @@ -792,10 +782,10 @@ function step(){ # Executed when reset # function Reset(){ - m_vecNodes={} - m_vecNodes = parse_graph("/home/ubuntu/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary + #m_vecNodes={} + #m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary m_vecNodes_fixed={} - m_vecNodes_fixed=parse_graph_fixed("/home/ubuntu/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") + m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") m_nLabel=-1 #start listening @@ -822,5 +812,5 @@ function destroy() { uav_moveto(0.0,0.0) m_vecNodes={} #stop listening - neighbors.ignore("msg") -} + neighbors.ignore("m") +} \ No newline at end of file diff --git a/buzz_scripts/graphform.bzz_old b/buzz_scripts/graphform.bzz_old new file mode 100644 index 0000000..937d69c --- /dev/null +++ b/buzz_scripts/graphform.bzz_old @@ -0,0 +1,826 @@ +# +# Include files +# +include "string.bzz" +include "vec2.bzz" +include "update.bzz" +include "barrier.bzz" # don't use a stigmergy id=11 with this header. +include "uavstates.bzz" # require an 'action' function to be defined here. + +ROBOT_RADIUS=50 +ROBOT_DIAMETER=2.0*ROBOT_RADIUS +ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER + +# +# Global variables +# + +# +#Save message from all neighours +#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot +m_MessageState={}#store received neighbour message +m_MessageLable={}#store received neighbour message +m_MessageReqLable={}#store received neighbour message +m_MessageReqID={}#store received neighbour message +m_MessageSide={}#store received neighbour message +m_MessageResponse={}#store received neighbour message +m_MessageRange={}#store received neighbour message +m_MessageBearing={}#store received neighbour message +m_neighbourCunt=0#used to cunt neighbours +#Save message from one neighbour +#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Side,Response,Range,Bearing +m_receivedMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE",.Range=0,.Bearing=0} +# +#Save the message to send +#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Side(side),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) +m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} + +#Current robot state +m_eState="STATE_FREE" + +#navigation vector +m_navigation={.x=0,.y=0} + +#Debug message to be displayed in qt-opengl +#m_ossDebugMsg + +#Debug vector to draw +#CVector2 m_cDebugVector + +#Table of the nodes in the graph +m_vecNodes={} +m_vecNodes_fixed={} + +#Current label being requested or chosen (-1 when none) +m_nLabel=-1 + +#Label request id +m_unRequestId=0 + +#Side +m_unbSide=0 + +#Global bias, used to map local coordinate to global coordinate +m_bias=0 + +#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate +m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + +#Counter to wait for something to happen +m_unWaitCount=0 + +#Number of steps to wait before looking for a free label +m_unLabelSearchWaitTime=0 + +#Number of steps to wait for an answer to be received +m_unResponseTimeThreshold=0 + +#Number of steps to wait until giving up joining +m_unJoiningLostPeriod=0 + +#Tolerance distance to a target location +m_fTargetDistanceTolerance=0 + +#step cunt +step_cunt=0 + +#virtual stigmergy +v_tag = stigmergy.create(1) + +# Lennard-Jones parameters +EPSILON = 13.5 #3.5 + +# Lennard-Jones interaction magnitude + +function FlockInteraction(dist,target,epsilon){ + var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + return mag +} + +function LimitAngle(angle){ + if(angle>2*math.pi) + return angle-2*math.pi + else if (angle<0) + return angle+2*math.pi + else + return angle +} + +# +# Calculates the angle of the given vector2. +# PARAM v: The vector2. +# RETURN: The angle of the vector. +# +Angle = function(v) { + return math.atan(v.y, v.x) +} + +# +#return the number of value in table +# +function count(table,value){ + var number=0 + var i=0 + while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ + m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 + if(m_vecNodes[i].StateAge==0) + m_vecNodes[i].State="UNASSIGNED" + } + i=i+1 + } +} + +# +#Transistion to state free +# +function TransitionToFree(){ + m_eState="STATE_FREE" + m_unWaitCount=m_unLabelSearchWaitTime + m_selfMessage.State=m_eState +} + +# +#Transistion to state asking +# +function TransitionToAsking(un_label){ + m_eState="STATE_ASKING" + m_nLabel=un_label + m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different + m_selfMessage.State=m_eState + m_selfMessage.ReqLable=m_nLabel + m_selfMessage.ReqID=m_unRequestId + + m_unWaitCount=m_unResponseTimeThreshold +} + +# +#Transistion to state joining +# +function TransitionToJoining(){ + m_eState="STATE_JOINING" + m_selfMessage.State=m_eState + m_selfMessage.Lable=m_nLabel + m_unWaitCount=m_unJoiningLostPeriod + + neighbors.listen("reply", + function(vid,value,rid){ + #store the received message + if(value.Lable==m_nLabel){ + m_cMeToPred.GlobalBearing=value.GlobalBearing + + } + }) + +} + +# +#Transistion to state joined +# +function TransitionToJoined(){ + m_eState="STATE_JOINED" + m_selfMessage.State=m_eState + m_selfMessage.Lable=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + neighbors.ignore("reply") + + #write statues + v_tag.put(m_nLabel, 1) + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) +} + +# +#Transistion to state Lock, lock the current formation +# +function TransitionToLock(){ + m_eState="STATE_LOCK" + m_selfMessage.State=m_eState + m_selfMessage.Lable=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) +} + +# +# Do free +# +function DoFree() { + m_selfMessage.State=m_eState + #wait for a while before looking for a lable + if(m_unWaitCount>0) + m_unWaitCount=m_unWaitCount-1 + + #find a set of joined robots + var setJoinedLables={} + var setJoinedIndexes={} + var i=0 + var j=0 + while(i0){ + TransitionToAsking(unFoundLable) + return + } + + #navigation + #if there is a joined robot within sight, move around joined robots + #else, gather with other free robots + if(size(setJoinedIndexes)>0){ + var tempvec_P={.x=0.0,.y=0.0} + var tempvec_N={.x=0.0,.y=0.0} + i=0 + while(imath.pi) + S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi + else + S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi + + var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing) + + #the vector from self to target in global coordinate + var S2Target=math.vec2.add(S2Pred,P2Target) + #change the vector to local coordinate of self + var S2Target_bearing=Angle(S2Target) + m_bias=m_cMeToPred.Bearing-S2PGlobalBearing + #S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17 + m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + + + + #test if is already in desired position + if(math.abs(S2Target.x)m_MessageRange[mapRequests[i]]) + ReqIndex=i + i=i+1 + } + #get the best index, whose Reqlable and Reqid are + ReqLable=m_MessageReqLable[mapRequests[ReqIndex]] + var ReqID=m_MessageReqID[mapRequests[ReqIndex]] + m_selfMessage.ReqLable=ReqLable + m_selfMessage.ReqID=ReqID + m_selfMessage.Response="REQ_GRANTED" + } + + #lost pred, wait for some time and transit to free + #if(seenPred==0){ + #m_unWaitCount=m_unWaitCount-1 + #if(m_unWaitCount==0){ + #TransitionToFree() + #return + #} + #} + + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + + + #check if should to transists to lock + + + if(v_tag.size()==ROBOTS){ + TransitionToLock() + } +} + +# +#Do Lock +# +function DoLock(){ +m_selfMessage.State=m_eState +m_selfMessage.Lable=m_nLabel + +m_navigation.x=0.0 +m_navigation.y=0.0 + +#collect preds information +var i=0 +var mypred1={.range=0,.bearing=0} +var mypred2={.range=0,.bearing=0} + +while(i1){ + var cDir={.x=0.0,.y=0.0} + var cDir1={.x=0.0,.y=0.0} + var cDir2={.x=0.0,.y=0.0} + cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing) + cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing) + #cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing) + #cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing) + cDir=math.vec2.add(cDir1,cDir2) + + cDir=math.vec2.scale(cDir,100) + m_navigation.x=cDir.x + m_navigation.y=cDir.y + #log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2) + log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1) +} +#move +uav_moveto(m_navigation.x,m_navigation.y) +} + +function action(){ + statef=action + UAVSTATE="GRAPH" +} + +# +# Executed at init +# +function init() { + # + #Adjust parameters here + # + m_unResponseTimeThreshold=10 + m_unLabelSearchWaitTime=10 + m_fTargetDistanceTolerance=10 + m_unJoiningLostPeriod=100 + + # + # Join Swarm + # + uav_initswarm() + Reset(); +} + +# +# Executed every step +# +function step(){ + uav_rccmd() + uav_neicmd() + #update the graph + UpdateNodeInfo() + #reset message package to be sent + m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"} + # + #act according to current state + # + if(UAVSTATE=="GRAPH"){ + if(m_eState=="STATE_FREE") + DoFree() + else if(m_eState=="STATE_ESCAPE") + DoEscape() + else if(m_eState=="STATE_ASKING") + DoAsking() + else if(m_eState=="STATE_JOINING") + DoJoining() + else if(m_eState=="STATE_JOINED") + DoJoined() + else if(m_eState=="STATE_LOCK") + DoLock() + } + + statef() + + + debug(m_eState,m_nLabel) + log("Current state: ", UAVSTATE) + log("Swarm size: ", ROBOTS) + #navigation + + #broadcast messag + neighbors.broadcast("msg",m_selfMessage) + + # + #clean message storage + m_MessageState={}#store received neighbour message + m_MessageLable={}#store received neighbour message + m_MessageReqLable={}#store received neighbour message + m_MessageReqID={}#store received neighbour message + m_MessageSide={}#store received neighbour message + m_MessageResponse={}#store received neighbour message + m_MessageRange={}#store received neighbour message + m_MessageBearing={}#store received neighbour message + m_neighbourCunt=0 + + + #step cunt+1 + step_cunt=step_cunt+1 +} + +# +# Executed when reset +# +function Reset(){ + m_vecNodes={} + m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary + m_vecNodes_fixed={} + m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") + m_nLabel=-1 + + #start listening + start_listen() + # + #set initial state, only one robot choose [A], while the rest choose [B] + # + #[A]The robot used to triger the formation process is defined as joined, + if(id==0){ + m_nLabel=0 + TransitionToJoined() + } + #[B]Other robots are defined as free. + else{ + TransitionToFree() + } +} + +# +# Executed upon destroy +# +function destroy() { + #clear neighbour message + uav_moveto(0.0,0.0) + m_vecNodes={} + #stop listening + neighbors.ignore("msg") +} diff --git a/buzz_scripts/include/Graph_drone.graph b/buzz_scripts/include/Graph_drone.graph index f809476..edd6764 100644 --- a/buzz_scripts/include/Graph_drone.graph +++ b/buzz_scripts/include/Graph_drone.graph @@ -1,5 +1,5 @@ -0 -1 -1 -1 -1 0 1000.0 0.0 -2 0 1000.0 1.57 -3 0 1000.0 3.14 -4 0 1000.0 4.71 +0 -1 -1 -1 3000.0 +1 0 1000.0 0.0 5000.0 +2 0 1000.0 1.57 7000.0 +3 0 1000.0 3.14 9000.0 +4 0 1000.0 4.71 11000.0 diff --git a/buzz_scripts/include/Graph_fixed.graph b/buzz_scripts/include/Graph_fixed.graph index 6935e27..c0d6de7 100644 --- a/buzz_scripts/include/Graph_fixed.graph +++ b/buzz_scripts/include/Graph_fixed.graph @@ -1,5 +1,5 @@ 0 -1 -1 -1 -1 -1 0 1000.0 -1 -1 -2 0 1000.0 1 1414.2 -3 0 1000.0 2 1414.2 -4 0 1000.0 1 1414.2 \ No newline at end of file +1 0 10.0 -1 -1 +2 0 10.0 1 1414.2 +3 0 10.0 2 1414.2 +4 0 10.0 1 1414.2 \ No newline at end of file diff --git a/buzz_scripts/include/shapes.bzz b/buzz_scripts/include/shapes.bzz new file mode 100644 index 0000000..53cdd3b --- /dev/null +++ b/buzz_scripts/include/shapes.bzz @@ -0,0 +1,111 @@ +#Table of the nodes in the graph +m_vecNodes={} +m_vecNodes_fixed={} +m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing + .Lable = 0, # Lable of the point + .Pred = -1, # Lable of its predecessor + .distance = -1, # distance to the predecessor + .bearing = -1, # bearing form the predecessor to this dot + .height = 3000, # height of this dot + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[1] = { + .Lable = 1, + .Pred = 0, + .distance = 1000, + .bearing = 0.0, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Lable = 2, + .Pred = 0, + .distance = 1000, + .bearing = 1.57, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Lable = 3, + .Pred = 0, + .distance = 1000, + .bearing = 3.14, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Lable = 4, + .Pred = 0, + .distance = 1000, + .bearing = 4.71, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} + +# +# Graph parsing +# +function parse_graph(fname) { + # Graph data + var gd = {} + # Open the file + var fd = io.fopen(fname, "r") + if(not fd) { + log("Can't open '", fname, "'") + return nil + } + # Parse the file line by line + var rrec # Record read from line + var grec # Record parsed into graph + io.fforeach(fd, function(line) { + # Parse file line + rrec = string.split(line, "\t ") + # Make record + gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing + .Lable = string.toint(rrec[0]), # Lable of the point + .Pred = string.toint(rrec[1]), # Lable of its predecessor + .distance = string.tofloat(rrec[2]), # distance to the predecessor + .bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot + .height = string.tofloat(rrec[4]), # height of this dot + .State="UNASSIGNED", + .StateAge=0 + }}) + # All done + io.fclose(fd) + return gd +} + +function parse_graph_fixed(fname) { + # Graph data + var gd = {} + # Open the file + var fd = io.fopen(fname, "r") + if(not fd) { + log("Can't open '", fname, "'") + return nil + } + # Parse the file line by line + var rrec # Record read from line + var grec # Record parsed into graph + io.fforeach(fd, function(line) { + # Parse file line + rrec = string.split(line, "\t ") + # Make record + gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2 + .Pred1 = string.toint(rrec[1]), # Pred 1 lable + .Pred2 = string.toint(rrec[3]), # Pred 2 lable + .d1 = string.tofloat(rrec[2]), # Pred 1 distance + .d2 = string.tofloat(rrec[4]), # Pred 2 distance + .Lable=string.toint(rrec[0]), + .State="UNASSIGNED", + .StateAge=0 + }}) + # All done + io.fclose(fd) + return gd +} \ No newline at end of file From edc1b31174897f32dbda2d6a3e4c3ad3d0583e8b Mon Sep 17 00:00:00 2001 From: pyhs Date: Fri, 7 Jul 2017 15:00:22 -0400 Subject: [PATCH 11/14] Fixed if condition in roscontroller.cpp --- src/roscontroller.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 27cdd4e..feb39b3 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -113,7 +113,7 @@ bool roscontroller::TriggerAPIRssi(const uint8_t short_id) 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;} + if(!xbeestatus_srv.call(srv_request, srv_response)){return false;} return srv_response.success; } @@ -130,7 +130,7 @@ bool roscontroller::GetAPIRssi(const uint8_t short_id, float &result) 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;} + if(!xbeestatus_srv.call(srv_request, srv_response)){return false;} result = srv_response.value.real; return srv_response.success; @@ -148,7 +148,7 @@ bool roscontroller::GetRawPacketLoss(const uint8_t short_id, float &result) 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;} + if(!xbeestatus_srv.call(srv_request, srv_response)){return false;} result = srv_response.value.real; return srv_response.success; @@ -166,7 +166,7 @@ bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float &result) 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;} + if(!xbeestatus_srv.call(srv_request, srv_response)){return false;} result = srv_response.value.real; return srv_response.success; From 30fe6e1599aaa7c5822561e9a17bff42d0b54f0a Mon Sep 17 00:00:00 2001 From: pyhs Date: Fri, 7 Jul 2017 15:57:15 -0400 Subject: [PATCH 12/14] Commit after merge --- CMakeLists.txt | 2 - .../{graphform.bzz.in => graphform.bzz_old} | 4 +- buzz_scripts/include/Graph_drone.graph | 10 +- buzz_scripts/include/Graph_fixed.graph | 8 +- buzz_scripts/include/shapes.bzz | 111 ++++++++++++++++++ launch/rosbuzz-solo.launch | 4 +- 6 files changed, 124 insertions(+), 15 deletions(-) rename buzz_scripts/{graphform.bzz.in => graphform.bzz_old} (98%) create mode 100644 buzz_scripts/include/shapes.bzz diff --git a/CMakeLists.txt b/CMakeLists.txt index 4899923..28df175 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,8 +5,6 @@ if(UNIX) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11") endif() -configure_file(${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/graphform.bzz.in ${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/graphform.bzz) - ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS roscpp diff --git a/buzz_scripts/graphform.bzz.in b/buzz_scripts/graphform.bzz_old similarity index 98% rename from buzz_scripts/graphform.bzz.in rename to buzz_scripts/graphform.bzz_old index ea2d45e..937d69c 100644 --- a/buzz_scripts/graphform.bzz.in +++ b/buzz_scripts/graphform.bzz_old @@ -793,9 +793,9 @@ function step(){ # function Reset(){ m_vecNodes={} - m_vecNodes = parse_graph("${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary + m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary m_vecNodes_fixed={} - m_vecNodes_fixed=parse_graph_fixed("${CMAKE_CURRENT_SOURCE_DIR}/buzz_scripts/include/Graph_fixed.graph") + m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") m_nLabel=-1 #start listening diff --git a/buzz_scripts/include/Graph_drone.graph b/buzz_scripts/include/Graph_drone.graph index f809476..edd6764 100644 --- a/buzz_scripts/include/Graph_drone.graph +++ b/buzz_scripts/include/Graph_drone.graph @@ -1,5 +1,5 @@ -0 -1 -1 -1 -1 0 1000.0 0.0 -2 0 1000.0 1.57 -3 0 1000.0 3.14 -4 0 1000.0 4.71 +0 -1 -1 -1 3000.0 +1 0 1000.0 0.0 5000.0 +2 0 1000.0 1.57 7000.0 +3 0 1000.0 3.14 9000.0 +4 0 1000.0 4.71 11000.0 diff --git a/buzz_scripts/include/Graph_fixed.graph b/buzz_scripts/include/Graph_fixed.graph index 6935e27..c0d6de7 100644 --- a/buzz_scripts/include/Graph_fixed.graph +++ b/buzz_scripts/include/Graph_fixed.graph @@ -1,5 +1,5 @@ 0 -1 -1 -1 -1 -1 0 1000.0 -1 -1 -2 0 1000.0 1 1414.2 -3 0 1000.0 2 1414.2 -4 0 1000.0 1 1414.2 \ No newline at end of file +1 0 10.0 -1 -1 +2 0 10.0 1 1414.2 +3 0 10.0 2 1414.2 +4 0 10.0 1 1414.2 \ No newline at end of file diff --git a/buzz_scripts/include/shapes.bzz b/buzz_scripts/include/shapes.bzz new file mode 100644 index 0000000..53cdd3b --- /dev/null +++ b/buzz_scripts/include/shapes.bzz @@ -0,0 +1,111 @@ +#Table of the nodes in the graph +m_vecNodes={} +m_vecNodes_fixed={} +m_vecNodes[0] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing + .Lable = 0, # Lable of the point + .Pred = -1, # Lable of its predecessor + .distance = -1, # distance to the predecessor + .bearing = -1, # bearing form the predecessor to this dot + .height = 3000, # height of this dot + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[1] = { + .Lable = 1, + .Pred = 0, + .distance = 1000, + .bearing = 0.0, + .height = 5000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[2] = { + .Lable = 2, + .Pred = 0, + .distance = 1000, + .bearing = 1.57, + .height = 7000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[3] = { + .Lable = 3, + .Pred = 0, + .distance = 1000, + .bearing = 3.14, + .height = 9000, + .State="UNASSIGNED", + .StateAge=0 +} +m_vecNodes[4] = { + .Lable = 4, + .Pred = 0, + .distance = 1000, + .bearing = 4.71, + .height = 11000, + .State="UNASSIGNED", + .StateAge=0 +} + +# +# Graph parsing +# +function parse_graph(fname) { + # Graph data + var gd = {} + # Open the file + var fd = io.fopen(fname, "r") + if(not fd) { + log("Can't open '", fname, "'") + return nil + } + # Parse the file line by line + var rrec # Record read from line + var grec # Record parsed into graph + io.fforeach(fd, function(line) { + # Parse file line + rrec = string.split(line, "\t ") + # Make record + gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing + .Lable = string.toint(rrec[0]), # Lable of the point + .Pred = string.toint(rrec[1]), # Lable of its predecessor + .distance = string.tofloat(rrec[2]), # distance to the predecessor + .bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot + .height = string.tofloat(rrec[4]), # height of this dot + .State="UNASSIGNED", + .StateAge=0 + }}) + # All done + io.fclose(fd) + return gd +} + +function parse_graph_fixed(fname) { + # Graph data + var gd = {} + # Open the file + var fd = io.fopen(fname, "r") + if(not fd) { + log("Can't open '", fname, "'") + return nil + } + # Parse the file line by line + var rrec # Record read from line + var grec # Record parsed into graph + io.fforeach(fd, function(line) { + # Parse file line + rrec = string.split(line, "\t ") + # Make record + gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2 + .Pred1 = string.toint(rrec[1]), # Pred 1 lable + .Pred2 = string.toint(rrec[3]), # Pred 2 lable + .d1 = string.tofloat(rrec[2]), # Pred 1 distance + .d2 = string.tofloat(rrec[4]), # Pred 2 distance + .Lable=string.toint(rrec[0]), + .State="UNASSIGNED", + .StateAge=0 + }}) + # All done + io.fclose(fd) + return gd +} \ No newline at end of file diff --git a/launch/rosbuzz-solo.launch b/launch/rosbuzz-solo.launch index dade33d..fce3b00 100644 --- a/launch/rosbuzz-solo.launch +++ b/launch/rosbuzz-solo.launch @@ -43,7 +43,7 @@ - + @@ -51,7 +51,7 @@ - + From f2374137a7775077efba5f1b97fc1e54c25bda15 Mon Sep 17 00:00:00 2001 From: pyhs Date: Tue, 11 Jul 2017 14:09:48 -0400 Subject: [PATCH 13/14] Add graphform.bzz which was removed during merge --- buzz_scripts/graphform.bzz | 816 +++++++++++++++++++++++++++++++++++++ 1 file changed, 816 insertions(+) create mode 100644 buzz_scripts/graphform.bzz diff --git a/buzz_scripts/graphform.bzz b/buzz_scripts/graphform.bzz new file mode 100644 index 0000000..cfb5b5d --- /dev/null +++ b/buzz_scripts/graphform.bzz @@ -0,0 +1,816 @@ +# +# Include files +# +include "string.bzz" +include "vec2.bzz" +include "update.bzz" +include "barrier.bzz" # don't use a stigmergy id=11 with this header. +include "uavstates.bzz" # require an 'action' function to be defined here. + +include "shapes.bzz" + +ROBOT_RADIUS=50 +ROBOT_DIAMETER=2.0*ROBOT_RADIUS +ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER + +# +# Global variables +# + +# +#Save message from all neighours +#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot +m_MessageState={}#store received neighbour message +m_MessageLable={}#store received neighbour message +m_MessageReqLable={}#store received neighbour message +m_MessageReqID={}#store received neighbour message +m_MessageResponse={}#store received neighbour message +m_MessageRange={}#store received neighbour message +m_MessageBearing={}#store received neighbour message +m_neighbourCunt=0#used to cunt neighbours +#Save message from one neighbour +#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Response,Range,Bearing +m_receivedMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} + +# +#Save the message to send +#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) +m_selfMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE")} + +#Current robot state +m_eState="STATE_FREE" + +#navigation vector +m_navigation={.x=0,.y=0} + +#Debug message to be displayed in qt-opengl +#m_ossDebugMsg + +#Debug vector to draw +#CVector2 m_cDebugVector + +#Current label being requested or chosen (-1 when none) +m_nLabel=-1 + +#Label request id +m_unRequestId=0 + +#Global bias, used to map local coordinate to global coordinate +m_bias=0 + +#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate +m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0} + +#Counter to wait for something to happen +m_unWaitCount=0 + +#Number of steps to wait before looking for a free label +m_unLabelSearchWaitTime=0 + +#Number of steps to wait for an answer to be received +m_unResponseTimeThreshold=0 + +#Number of steps to wait until giving up joining +m_unJoiningLostPeriod=0 + +#Tolerance distance to a target location +m_fTargetDistanceTolerance=0 + +#step cunt +step_cunt=0 + +#virtual stigmergy +v_tag = stigmergy.create(1) + +# Lennard-Jones parameters, may need change +EPSILON = 13.5 #the LJ parameter for other robots +EPSILON_FOR1 = 10.0 #the LJ parameter for the robot labeled 1 + +# Lennard-Jones interaction magnitude + +function FlockInteraction(dist,target,epsilon){ + var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2) + return mag +} + +function LimitAngle(angle){ + if(angle>2*math.pi) + return angle-2*math.pi + else if (angle<0) + return angle+2*math.pi + else + return angle +} + +# +# Calculates the angle of the given vector2. +# PARAM v: The vector2. +# RETURN: The angle of the vector. +# +Angle = function(v) { + return math.atan(v.y, v.x) +} + +# +#return the number of value in table +# +function count(table,value){ + var number=0 + var i=0 + while(i0) and (m_vecNodes[i].State=="ASSIGNING")){ + m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1 + if(m_vecNodes[i].StateAge==0) + m_vecNodes[i].State="UNASSIGNED" + } + i=i+1 + } +} + +# +#Transistion to state free +# +function TransitionToFree(){ + m_eState="STATE_FREE" + m_unWaitCount=m_unLabelSearchWaitTime + m_selfMessage.State=s2i(m_eState) +} + +# +#Transistion to state asking +# +function TransitionToAsking(un_label){ + m_eState="STATE_ASKING" + m_nLabel=un_label + m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different + m_selfMessage.State=s2i(m_eState) + m_selfMessage.ReqLable=m_nLabel + m_selfMessage.ReqID=m_unRequestId + + m_unWaitCount=m_unResponseTimeThreshold +} + +# +#Transistion to state joining +# +function TransitionToJoining(){ + m_eState="STATE_JOINING" + m_selfMessage.State=s2i(m_eState) + m_selfMessage.Lable=m_nLabel + m_unWaitCount=m_unJoiningLostPeriod + + neighbors.listen("r", + function(vid,value,rid){ + #store the received message + if(value.Label==m_nLabel){ + m_cMeToPred.GlobalBearing=value.Bearing + + } + }) + +} + +# +#Transistion to state joined +# +function TransitionToJoined(){ + m_eState="STATE_JOINED" + m_selfMessage.State=s2i(m_eState) + m_selfMessage.Lable=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + neighbors.ignore("r") + + #write statues + v_tag.put(m_nLabel, 1) + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) +} + +# +#Transistion to state Lock, lock the current formation +# +function TransitionToLock(){ + m_eState="STATE_LOCK" + m_selfMessage.State=s2i(m_eState) + m_selfMessage.Lable=m_nLabel + m_vecNodes[m_nLabel].State="ASSIGNED" + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) +} + +# +# Do free +# +function DoFree() { + m_selfMessage.State=s2i(m_eState) + + #wait for a while before looking for a lable + if(m_unWaitCount>0) + m_unWaitCount=m_unWaitCount-1 + + #find a set of joined robots + var setJoinedLables={} + var setJoinedIndexes={} + var i=0 + var j=0 + while(i0){ + TransitionToAsking(unFoundLable) + return + } + + #navigation + #if there is a joined robot within sight, move around joined robots + #else, gather with other free robots + if(size(setJoinedIndexes)>0){ + var tempvec_P={.x=0.0,.y=0.0} + var tempvec_N={.x=0.0,.y=0.0} + i=0 + while(imath.pi) + S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi + else + S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi + + var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing) + + #the vector from self to target in global coordinate + var S2Target=math.vec2.add(S2Pred,P2Target) + #change the vector to local coordinate of self + var S2Target_bearing=Angle(S2Target) + m_bias=m_cMeToPred.Bearing-S2PGlobalBearing + S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17 + m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing) + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + + + + #test if is already in desired position + if(math.abs(S2Target.x)m_MessageRange[mapRequests[i]]) + ReqIndex=i + i=i+1 + } + #get the best index, whose Reqlable and Reqid are + ReqLable=m_MessageReqLable[mapRequests[ReqIndex]] + var ReqID=m_MessageReqID[mapRequests[ReqIndex]] + m_selfMessage.ReqLable=ReqLable + m_selfMessage.ReqID=ReqID + m_selfMessage.Response=r2i("REQ_GRANTED") + } + + #lost pred, wait for some time and transit to free + #if(seenPred==0){ + #m_unWaitCount=m_unWaitCount-1 + #if(m_unWaitCount==0){ + #TransitionToFree() + #return + #} + #} + + + m_navigation.x=0.0 + m_navigation.y=0.0 + uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) + + + #check if should to transists to lock + + +# if(v_tag.size()==ROBOTS){ +# TransitionToLock() +# } +} + +# +#Do Lock +# +function DoLock(){ +m_selfMessage.State=s2i(m_eState) +m_selfMessage.Lable=m_nLabel + +m_navigation.x=0.0 +m_navigation.y=0.0 + +#collect preds information +var i=0 +var mypred1={.range=0,.bearing=0} +var mypred2={.range=0,.bearing=0} + +while(i1){ + var cDir={.x=0.0,.y=0.0} + var cDir1={.x=0.0,.y=0.0} + var cDir2={.x=0.0,.y=0.0} + cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing) + cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing) + #cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing) + #cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing) + cDir=math.vec2.add(cDir1,cDir2) + + cDir=math.vec2.scale(cDir,5) + m_navigation.x=cDir.x + m_navigation.y=cDir.y + #log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2) + log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1) +} +#move +uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0) +} + +function action(){ + statef=action + UAVSTATE="GRAPH" +} + +# +# Executed at init +# +function init() { + # + #Adjust parameters here + # + m_unResponseTimeThreshold=10 + m_unLabelSearchWaitTime=10 + m_fTargetDistanceTolerance=10 + m_unJoiningLostPeriod=100 + + # + # Join Swarm + # + uav_initswarm() + Reset(); +} + +# +# Executed every step +# +function step(){ + uav_rccmd() + uav_neicmd() + #update the graph + UpdateNodeInfo() + #reset message package to be sent + m_selfMessage={.State=s2i("STATE_FREE"),.Lable=0,.ReqLable=0,.ReqID=0,.Response=r2i("REQ_NONE")} + # + #act according to current state + # + if(UAVSTATE=="GRAPH"){ + if(m_eState=="STATE_FREE") + DoFree() + else if(m_eState=="STATE_ESCAPE") + DoEscape() + else if(m_eState=="STATE_ASKING") + DoAsking() + else if(m_eState=="STATE_JOINING") + DoJoining() + else if(m_eState=="STATE_JOINED") + DoJoined() + else if(m_eState=="STATE_LOCK") + DoLock() + } + + statef() + + + debug(m_eState,m_nLabel) + log("Current state: ", UAVSTATE) + log("Swarm size: ", ROBOTS) + #navigation + + #broadcast messag + neighbors.broadcast("m",m_selfMessage) + + # + #clean message storage + m_MessageState={}#store received neighbour message + m_MessageLable={}#store received neighbour message + m_MessageReqLable={}#store received neighbour message + m_MessageReqID={}#store received neighbour message + m_MessageResponse={}#store received neighbour message + m_MessageRange={}#store received neighbour message + m_MessageBearing={}#store received neighbour message + m_neighbourCunt=0 + + + #step cunt+1 + step_cunt=step_cunt+1 +} + +# +# Executed when reset +# +function Reset(){ + #m_vecNodes={} + #m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary + m_vecNodes_fixed={} + m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/Graph_fixed.graph") + m_nLabel=-1 + + #start listening + start_listen() + # + #set initial state, only one robot choose [A], while the rest choose [B] + # + #[A]The robot used to triger the formation process is defined as joined, + if(id==0){ + m_nLabel=0 + TransitionToJoined() + } + #[B]Other robots are defined as free. + else{ + TransitionToFree() + } +} + +# +# Executed upon destroy +# +function destroy() { + #clear neighbour message + uav_moveto(0.0,0.0) + m_vecNodes={} + #stop listening + neighbors.ignore("m") +} From f24fb637230800de47d5b978bbebca1d4338cec9 Mon Sep 17 00:00:00 2001 From: pyhs Date: Tue, 11 Jul 2017 15:28:24 -0400 Subject: [PATCH 14/14] Fixed code for new buzz (especially commit a854c5cdc6830e4c1ec246b46886d4298ac872f1) --- src/buzz_utility.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 3453a65..c9d9ea6 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -182,6 +182,7 @@ namespace buzz_utility{ uint16_t* data= u64_cvt_u16((uint64_t)first_INmsg[0]); /*Size is at first 2 bytes*/ uint16_t size=data[0]*sizeof(uint64_t); + uint16_t robot_id = data[1]; delete[] data; /*size and robot id read*/ size_t tot = sizeof(uint32_t); @@ -195,6 +196,7 @@ namespace buzz_utility{ /* Append message to the Buzz input message queue */ if(unMsgSize > 0 && unMsgSize <= size - tot ) { buzzinmsg_queue_append(VM, + robot_id, buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize)); tot += unMsgSize; }