From 74f7f740e03ff65f731a84bf78bdef4e9aa3c09d Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 8 Dec 2017 18:53:32 -0500 Subject: [PATCH] beautified --- include/buzz_update.h | 111 +-- include/buzz_utility.h | 37 +- include/buzzuav_closures.h | 38 +- include/roscontroller.h | 86 ++- include/uav_utility.h | 1 - src/buzz_update.cpp | 1230 ++++++++++++++++--------------- src/buzz_utility.cpp | 1121 +++++++++++++++-------------- src/buzzuav_closures.cpp | 1399 +++++++++++++++++++----------------- src/rosbuzz.cpp | 16 +- src/roscontroller.cpp | 517 +++++++------ src/uav_utility.cpp | 10 +- 11 files changed, 2384 insertions(+), 2182 deletions(-) diff --git a/include/buzz_update.h b/include/buzz_update.h index 7a2a8f5..f8dac52 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -10,76 +10,84 @@ #include #include #include -#define delete_p(p) do { free(p); p = NULL; } while(0) +#define delete_p(p) \ + do \ + { \ + free(p); \ + p = NULL; \ + } while (0) -static const uint16_t CODE_REQUEST_PADDING=250; -static const uint16_t MIN_UPDATE_PACKET=251; -static const uint16_t UPDATE_CODE_HEADER_SIZE=5; -static const uint16_t TIMEOUT_FOR_ROLLBACK=50; +static const uint16_t CODE_REQUEST_PADDING = 250; +static const uint16_t MIN_UPDATE_PACKET = 251; +static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; +static const uint16_t TIMEOUT_FOR_ROLLBACK = 50; /*********************/ /* Updater states */ /********************/ typedef enum { - CODE_RUNNING = 0, // Code executing - CODE_STANDBY, // Standing by for others to update - } code_states_e; + CODE_RUNNING = 0, // Code executing + CODE_STANDBY, // Standing by for others to update +} code_states_e; /*********************/ /*Message types */ /********************/ typedef enum { - SENT_CODE = 0, // Broadcast code - RESEND_CODE, // ReBroadcast request - } code_message_e; + SENT_CODE = 0, // Broadcast code + RESEND_CODE, // ReBroadcast request +} code_message_e; /*************************/ /*Updater message queue */ /*************************/ -struct updater_msgqueue_s { - uint8_t* queue; - uint8_t* size; - } ; - typedef struct updater_msgqueue_s* updater_msgqueue_t; +struct updater_msgqueue_s +{ + uint8_t* queue; + uint8_t* size; +}; +typedef struct updater_msgqueue_s* updater_msgqueue_t; -struct updater_code_s { - uint8_t* bcode; - uint8_t* bcode_size; -} ; +struct updater_code_s +{ + uint8_t* bcode; + uint8_t* bcode_size; +}; typedef struct updater_code_s* updater_code_t; /**************************/ /*Updater data*/ /**************************/ -struct buzz_updater_elem_s { - /* robot id */ - //uint16_t robotid; - /*current Bytecode content */ - uint8_t* bcode; - /*old Bytecode name */ - const char* old_bcode; - /*current bcode size*/ - size_t* bcode_size; - /*Update patch*/ - uint8_t* patch; - /* Update patch size*/ - size_t* patch_size; - /*current Bytecode content */ - uint8_t* standby_bcode; - /*current bcode size*/ - size_t* standby_bcode_size; - /*updater out msg queue */ - updater_msgqueue_t outmsg_queue; - /*updater in msg queue*/ - updater_msgqueue_t inmsg_queue; - /*Current state of the updater one in code_states_e ENUM*/ - int* mode; - uint8_t* update_no; - }; - typedef struct buzz_updater_elem_s* buzz_updater_elem_t; +struct buzz_updater_elem_s +{ + /* robot id */ + // uint16_t robotid; + /*current Bytecode content */ + uint8_t* bcode; + /*old Bytecode name */ + const char* old_bcode; + /*current bcode size*/ + size_t* bcode_size; + /*Update patch*/ + uint8_t* patch; + /* Update patch size*/ + size_t* patch_size; + /*current Bytecode content */ + uint8_t* standby_bcode; + /*current bcode size*/ + size_t* standby_bcode_size; + /*updater out msg queue */ + updater_msgqueue_t outmsg_queue; + /*updater in msg queue*/ + updater_msgqueue_t inmsg_queue; + /*Current state of the updater one in code_states_e ENUM*/ + int* mode; + uint8_t* update_no; +}; +typedef struct buzz_updater_elem_s* buzz_updater_elem_t; /**************************************************************************/ /*Updater routine from msg processing to file checks to be called from main*/ @@ -89,15 +97,13 @@ void update_routine(); /************************************************/ /*Initalizes the updater */ /************************************************/ -void init_update_monitor(const char* bo_filename,const char* stand_by_script, - const char* dbgfname, int robot_id); - +void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); /*********************************************************/ /*Appends buffer of given size to in msg queue of updater*/ /*********************************************************/ -void code_message_inqueue_append(uint8_t* msg,uint16_t size); +void code_message_inqueue_append(uint8_t* msg, uint16_t size); /*********************************************************/ /*Processes messages inside the queue of the updater*/ @@ -125,14 +131,13 @@ void destroy_out_msg_queue(); /***************************************************/ int get_update_mode(); - buzz_updater_elem_t get_updater(); /***************************************************/ /*sets bzz file name*/ /***************************************************/ void set_bzz_file(const char* in_bzz_file); -int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size); +int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); /****************************************************/ /*Destroys the updater*/ @@ -152,5 +157,5 @@ void updates_set_robots(int robots); void set_packet_id(int packet_id); -void collect_data(std::ofstream &logger); +void collect_data(std::ofstream& logger); #endif diff --git a/include/buzz_utility.h b/include/buzz_utility.h index d2a4e60..12cf011 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -12,20 +12,25 @@ #include using namespace std; -namespace buzz_utility{ - +namespace buzz_utility +{ struct pos_struct { - double x,y,z; - pos_struct(double x,double y,double z):x(x),y(y),z(z){}; - pos_struct(){} + double x, y, z; + 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,latitude,longitude,altitude; - rb_struct(double la, double lo, double al, double r,double b):latitude(la),longitude(lo),altitude(al),r(r),b(b){}; - rb_struct(){} + double r, b, latitude, longitude, altitude; + rb_struct(double la, double lo, double al, double r, double b) + : latitude(la), longitude(lo), altitude(al), r(r), b(b){}; + rb_struct() + { + } }; typedef struct rb_struct RB_struct; @@ -35,16 +40,15 @@ struct neiStatus uint batt_lvl = 0; uint xbee = 0; uint flight_status = 0; -}; typedef struct neiStatus neighbors_status ; - +}; +typedef struct neiStatus neighbors_status; uint16_t* u64_cvt_u16(uint64_t u64); -int buzz_listen(const char* type, - int msg_size); +int buzz_listen(const char* type, int msg_size); int make_table(buzzobj_t* t); int buzzusers_reset(); - int create_stig_tables(); +int create_stig_tables(); void in_msg_append(uint64_t* payload); @@ -52,12 +56,11 @@ uint64_t* obt_out_msg(); void update_sensors(); -int buzz_script_set(const char* bo_filename, - const char* bdbg_filename, int robot_id); +int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id); -int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size); +int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size); -int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size); +int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size); void buzz_script_step(); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 2c1d387..6beb81c 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -8,23 +8,24 @@ #include "ros/ros.h" #include "buzz_utility.h" -#define EARTH_RADIUS (double) 6371000.0 -#define DEG2RAD(DEG) (double) ((DEG)*((M_PI)/(180.0))) -#define RAD2DEG(RAD) (double) ((RAD)*((180.0)/(M_PI))) +#define EARTH_RADIUS (double)6371000.0 +#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0))) +#define RAD2DEG(RAD) (double)((RAD) * ((180.0) / (M_PI))) -namespace buzzuav_closures{ - typedef enum { - COMMAND_NIL = 0, // Dummy command - COMMAND_TAKEOFF, // Take off - COMMAND_LAND, - COMMAND_GOHOME, - COMMAND_ARM, - COMMAND_DISARM, - COMMAND_GOTO, - COMMAND_MOVETO, - COMMAND_PICTURE, - COMMAND_GIMBAL, - } Custom_CommandCode; +namespace buzzuav_closures +{ +typedef enum { + COMMAND_NIL = 0, // Dummy command + COMMAND_TAKEOFF, // Take off + COMMAND_LAND, + COMMAND_GOHOME, + COMMAND_ARM, + COMMAND_DISARM, + COMMAND_GOTO, + COMMAND_MOVETO, + COMMAND_PICTURE, + COMMAND_GIMBAL, +} Custom_CommandCode; /* * prextern int() function in Buzz @@ -54,7 +55,7 @@ void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t); /*sets rc requested command */ void rc_call(int rc_cmd); /* sets the battery state */ -void set_battery(float voltage,float current,float remaining); +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); @@ -88,7 +89,7 @@ int buzzuav_arm(buzzvm_t vm); /* * Disarm from buzz */ -int buzzuav_disarm(buzzvm_t vm) ; +int buzzuav_disarm(buzzvm_t vm); /* Commands the UAV to land */ int buzzuav_land(buzzvm_t vm); @@ -126,7 +127,6 @@ int buzzuav_update_prox(buzzvm_t vm); int bzz_cmd(); - int dummy_closure(buzzvm_t vm); //#endif diff --git a/include/roscontroller.h b/include/roscontroller.h index 0dbe19e..61f635c 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -37,7 +37,7 @@ #define UPDATER_MESSAGE_CONSTANT 987654321 #define XBEE_MESSAGE_CONSTANT 586782343 #define XBEE_STOP_TRANSMISSION 4355356352 -#define TIMEOUT 60 +#define TIMEOUT 60 #define BUZZRATE 10 #define CMD_REQUEST_UPDATE 666 @@ -45,14 +45,12 @@ using namespace std; namespace rosbzz_node { - class roscontroller { - public: roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); ~roscontroller(); - //void RosControllerInit(); + // void RosControllerInit(); void RosControllerRun(); static const string CAPTURE_SRV_DEFAULT_NAME; @@ -63,43 +61,47 @@ private: 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 + 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 ; + 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; 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::map neighbours_pos_map; + std::map 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 oldcmdID=0; int rc_cmd; float fcu_timeout; int armstate; int barrier; - int message_number=0; - uint8_t no_of_robots=0; + int message_number = 0; + uint8_t no_of_robots = 0; /*tmp to be corrected*/ - uint8_t no_cnt=0; - uint8_t old_val=0 ; + uint8_t no_cnt = 0; + uint8_t old_val = 0; std::string bzzfile_name; std::string fcclient_name; std::string armclient; std::string modeclient; std::string rcservice_name; - std::string bcfname,dbgfname; + std::string bcfname, dbgfname; std::string out_payload; std::string in_payload; std::string obstacles_topic; @@ -136,7 +138,6 @@ private: ros::Subscriber local_pos_sub; double local_pos_new[3]; - ros::ServiceClient stream_client; int setpoint_counter; @@ -145,7 +146,7 @@ private: std::ofstream log; /*Commands for flight controller*/ - //mavros_msgs::CommandInt cmd_srv; + // mavros_msgs::CommandInt cmd_srv; mavros_msgs::CommandLong cmd_srv; std::vector m_sMySubscriptions; std::map m_smTopic_infos; @@ -159,7 +160,7 @@ private: /*Initialize publisher and subscriber, done in the constructor*/ void Initialize_pub_sub(ros::NodeHandle& n_c); - std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode + std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode /*Obtain data from ros parameter server*/ void Rosparameters_get(ros::NodeHandle& n_c_priv); @@ -178,28 +179,24 @@ private: /*Prepare messages and publish*/ void prepare_msg_and_publish(); - /*Refresh neighbours Position for every ten step*/ void maintain_pos(int tim_step); /*Puts neighbours position inside neigbours_pos_map*/ - void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ); + 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 ); + 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); + void set_cur_pos(double latitude, double longitude, double altitude); /*convert from spherical to cartesian coordinate system callback */ - float constrainAngle(float x); + 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); - 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); + 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); @@ -213,7 +210,6 @@ private: /*current position callback*/ void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); - /*current relative altitude callback*/ void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); @@ -221,7 +217,7 @@ private: 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); + 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); @@ -243,7 +239,7 @@ private: 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(); @@ -256,15 +252,13 @@ private: void get_number_of_robots(); void GetRobotId(); - bool GetDequeFull(bool &result); - bool GetRssi(float &result); + 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); + 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 get_xbee_status(); - }; - } diff --git a/include/uav_utility.h b/include/uav_utility.h index 5d37e73..69c447d 100644 --- a/include/uav_utility.h +++ b/include/uav_utility.h @@ -5,5 +5,4 @@ extern void uav_setup(); extern void uav_done(); - #endif diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 4a1974a..8c37589 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -1,7 +1,7 @@ #include #include -#include -#include +#include +#include #include #include "buzz_update.h" #include @@ -9,676 +9,720 @@ #include #include - /*Temp for data collection*/ -//static int neigh=-1; +// static int neigh=-1; static struct timeval t1, t2; -static int timer_steps=0; -//static clock_t end; +static int timer_steps = 0; +// static clock_t end; /*Temp end */ -static int fd,wd =0; -static int old_update =0; +static int fd, wd = 0; +static int old_update = 0; static buzz_updater_elem_t updater; static int no_of_robot; static const char* dbgf_name; static const char* bcfname; -static const char* old_bcfname="old_bcode.bo"; +static const char* old_bcfname = "old_bcode.bo"; static const char* bzz_file; -static int Robot_id=0; -static int neigh=-1; -static int updater_msg_ready ; -static uint16_t update_try_counter=0; -static int updated=0; -static const uint16_t MAX_UPDATE_TRY=5; -static int packet_id_=0; -static size_t old_byte_code_size=0; - +static int Robot_id = 0; +static int neigh = -1; +static int updater_msg_ready; +static uint16_t update_try_counter = 0; +static int updated = 0; +static const uint16_t MAX_UPDATE_TRY = 5; +static int packet_id_ = 0; +static size_t old_byte_code_size = 0; /*Initialize updater*/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script, - const char* dbgfname, int robot_id){ - Robot_id=robot_id; - dbgf_name=dbgfname; - bcfname=bo_filename; - ROS_INFO("Initializing file monitor..."); - fd=inotify_init1(IN_NONBLOCK); - if ( fd < 0 ) { - perror( "inotify_init error" ); - } - /*If simulation set the file monitor only for robot 1*/ - if(SIMULATION==1 && robot_id==0){ - /* watch /.bzz file for any activity and report it back to update */ - wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS ); - } - else if (SIMULATION==0){ - /* watch /.bzz file for any activity and report it back to update */ - wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS ); - } - /*load the .bo under execution into the updater*/ - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bo_filename, "rb"); - if(!fp) { - perror(bo_filename); - } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { - perror(bo_filename); - //fclose(fp); - //return 0; - } - fclose(fp); - /*Load stand_by .bo file into the updater*/ - uint8_t* STD_BO_BUF = 0; - fp = fopen(stand_by_script, "rb"); - if(!fp) { - perror(stand_by_script); +void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) +{ + Robot_id = robot_id; + dbgf_name = dbgfname; + bcfname = bo_filename; + ROS_INFO("Initializing file monitor..."); + fd = inotify_init1(IN_NONBLOCK); + if (fd < 0) + { + perror("inotify_init error"); + } + /*If simulation set the file monitor only for robot 1*/ + if (SIMULATION == 1 && robot_id == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + else if (SIMULATION == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + /*load the .bo under execution into the updater*/ + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bo_filename, "rb"); + if (!fp) + { + perror(bo_filename); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bo_filename); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Load stand_by .bo file into the updater*/ + uint8_t* STD_BO_BUF = 0; + fp = fopen(stand_by_script, "rb"); + if (!fp) + { + perror(stand_by_script); + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) + { + perror(stand_by_script); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + /*Store a copy of the Bcode for rollback*/ + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->patch = NULL; + updater->patch_size = (size_t*)malloc(sizeof(size_t)); + updater->bcode_size = (size_t*)malloc(sizeof(size_t)); + updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) = 0; + *(size_t*)(updater->bcode_size) = bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; + updater->mode = (int*)malloc(sizeof(int)); + *(int*)(updater->mode) = CODE_RUNNING; + // no_of_robot=barrier; + updater_msg_ready = 0; - } - fseek(fp, 0, SEEK_END); - size_t stdby_bcode_size = ftell(fp); - rewind(fp); - STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); - if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { - perror(stand_by_script); - //fclose(fp); - //return 0; - } - fclose(fp); - /*Create the updater*/ - updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); - /*Intialize the updater with the required data*/ - updater->bcode = BO_BUF; - /*Store a copy of the Bcode for rollback*/ - updater->outmsg_queue = NULL; - updater->inmsg_queue = NULL; - updater->patch = NULL; - updater->patch_size = (size_t*) malloc(sizeof(size_t)); - updater->bcode_size = (size_t*) malloc(sizeof(size_t)); - updater->update_no = (uint8_t*) malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->update_no) =0; - *(size_t*)(updater->bcode_size)=bcode_size; - updater->standby_bcode = STD_BO_BUF; - updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); - *(size_t*)(updater->standby_bcode_size)=stdby_bcode_size; - updater->mode=(int*)malloc(sizeof(int)); - *(int*)(updater->mode)=CODE_RUNNING; - //no_of_robot=barrier; - updater_msg_ready=0; - - /*Write the bcode to a file for rollback*/ - fp=fopen("old_bcode.bo", "wb"); - fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); - fclose(fp); - + /*Write the bcode to a file for rollback*/ + fp = fopen("old_bcode.bo", "wb"); + fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); + fclose(fp); } /*Check for .bzz file chages*/ -int check_update(){ - struct inotify_event *event; - char buf[1024]; - int check =0; - int i=0; - int len=read(fd,buf,1024); - while(imask & (IN_MODIFY| IN_DELETE_SELF)){ - /*respawn watch if the file is self deleted */ - inotify_rm_watch(fd,wd); - close(fd); - fd=inotify_init1(IN_NONBLOCK); - wd=inotify_add_watch(fd,bzz_file,IN_ALL_EVENTS); - //fprintf(stdout,"event.\n"); - /* To mask multiple writes from editors*/ - if(!old_update){ - check=1; - old_update =1; - } - } - /* update index to start of next event */ - i+=sizeof(struct inotify_event)+event->len; - } - if (!check) old_update=0; -return check; +int check_update() +{ + struct inotify_event* event; + char buf[1024]; + int check = 0; + int i = 0; + int len = read(fd, buf, 1024); + while (i < len) + { + event = (struct inotify_event*)&buf[i]; + /* file was modified this flag is true in nano and self delet in gedit and other editors */ + // fprintf(stdout,"inside file monitor.\n"); + if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) + { + /*respawn watch if the file is self deleted */ + inotify_rm_watch(fd, wd); + close(fd); + fd = inotify_init1(IN_NONBLOCK); + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + // fprintf(stdout,"event.\n"); + /* To mask multiple writes from editors*/ + if (!old_update) + { + check = 1; + old_update = 1; + } + } + /* update index to start of next event */ + i += sizeof(struct inotify_event) + event->len; + } + if (!check) + old_update = 0; + return check; } -int test_patch(std::string path, std::string name1,size_t update_patch_size, uint8_t* patch){ - if(SIMULATION==1){ - return 1; - } - else { - /*Patch the old bo with new patch*/ - std::stringstream patch_writefile; - patch_writefile<< path<bcode = patched_BO_Buf; - update_code->bcode_size = (uint8_t*) malloc(sizeof(uint16_t)); - *(uint16_t*) (update_code->bcode_size) = patched_size; +updater_code_t obtain_patched_bo(std::string path, std::string name1) +{ + if (SIMULATION == 1) + { + /*Read the exsisting file to simulate the patching*/ + std::stringstream read_patched; + read_patched << path << name1 << ".bo"; + fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); + uint8_t* patched_BO_Buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_BO_Buf = (uint8_t*)malloc(patched_size); + if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + fclose(fp); + } + fclose(fp); + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_BO_Buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; - return update_code; - } + return update_code; + } - else{ + else + { + /*Read the new patched file*/ + std::stringstream read_patched; + read_patched << path << name1 << "-patched.bo"; + fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); + uint8_t* patched_BO_Buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_BO_Buf = (uint8_t*)malloc(patched_size); + if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + fclose(fp); + } + fclose(fp); - /*Read the new patched file*/ - std::stringstream read_patched; - read_patched<bcode = patched_BO_Buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; - update_code->bcode_size = (uint8_t*) malloc(sizeof(uint16_t)); - *(uint16_t*) (update_code->bcode_size) = patched_size; - - return update_code; - } + return update_code; + } } -void code_message_outqueue_append(){ - updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - /* if size less than 250 Pad with zeors to assure transmission*/ - uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); - uint16_t padding_size= ( size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET-size; - size +=padding_size; - updater->outmsg_queue->queue = (uint8_t*)malloc(size); - memset(updater->outmsg_queue->queue, 0, size); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->outmsg_queue->size)=size; - size=0; - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue+size) = SENT_CODE; - size+=sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue+size) = *(uint16_t*) (updater->update_no); - size+=sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue+size) =(uint16_t) *(size_t*) (updater->patch_size); - size+=sizeof(uint16_t); - memcpy(updater->outmsg_queue->queue+size, updater->patch, *(size_t*)(updater->patch_size)); - size+=(uint16_t)*(size_t*)(updater->patch_size); - updater_msg_ready=1; +void code_message_outqueue_append() +{ + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + /* if size less than 250 Pad with zeors to assure transmission*/ + uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); + uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; + size += padding_size; + updater->outmsg_queue->queue = (uint8_t*)malloc(size); + memset(updater->outmsg_queue->queue, 0, size); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->outmsg_queue->size) = size; + size = 0; + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); + size += sizeof(uint16_t); + memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); + size += (uint16_t) * (size_t*)(updater->patch_size); + updater_msg_ready = 1; } -void outqueue_append_code_request(uint16_t update_no){ - updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - uint16_t size =0; - updater->outmsg_queue->queue = (uint8_t*)malloc(2*sizeof(uint16_t) + sizeof(uint8_t) +CODE_REQUEST_PADDING); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) +CODE_REQUEST_PADDING); - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue+size) = RESEND_CODE; - size+=sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue+size) = update_no; - size+=sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue+size) = update_try_counter; - size+=sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->size)=size+CODE_REQUEST_PADDING; - updater_msg_ready=1; - ROS_INFO("[Debug] Requested update no. %u with try: %u \n",update_no,update_try_counter); +void outqueue_append_code_request(uint16_t update_no) +{ + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + uint16_t size = 0; + updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; + updater_msg_ready = 1; + ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter); } -void code_message_inqueue_append(uint8_t* msg,uint16_t size){ - updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - //ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); - updater->inmsg_queue->queue = (uint8_t*)malloc(size); - updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memcpy(updater->inmsg_queue->queue, msg, size); - *(uint16_t*)(updater->inmsg_queue->size) = size; +void code_message_inqueue_append(uint8_t* msg, uint16_t size) +{ + updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + // ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); + updater->inmsg_queue->queue = (uint8_t*)malloc(size); + updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memcpy(updater->inmsg_queue->queue, msg, size); + *(uint16_t*)(updater->inmsg_queue->size) = size; } -void set_packet_id(int packet_id){ -/*Used for data logging*/ -packet_id_=packet_id; +void set_packet_id(int packet_id) +{ + /*Used for data logging*/ + packet_id_ = packet_id; } -void code_message_inqueue_process(){ - int size=0; - updater_code_t out_code=NULL; +void code_message_inqueue_process() +{ + int size = 0; + updater_code_t out_code = NULL; - ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) ); - ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint8_t))) ); - ROS_INFO("[Debug] Updater received patch of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)+sizeof(uint8_t)) ) ); + ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode)); + ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)), + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); + ROS_INFO("[Debug] Updater received patch of size %u \n", + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); - if( *(int*) (updater->mode) == CODE_RUNNING){ - //fprintf(stdout,"[debug]Inside inmsg code running"); - if(*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE){ - size+=sizeof(uint8_t); - if( *(uint16_t*)(updater->inmsg_queue->queue+size) > *(uint16_t*) (updater->update_no) ){ - //fprintf(stdout,"[debug]Inside update number comparision"); - uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue+size); - size +=sizeof(uint16_t); - uint16_t update_patch_size =*(uint16_t*)(updater->inmsg_queue->queue+size); - size +=sizeof(uint16_t); - /*Generate patch*/ - std::string bzzfile_name(bzz_file); - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0,name1.find_last_of(".")); - if(test_patch(path, name1,update_patch_size,(updater->inmsg_queue->queue+size)) ){ - out_code = obtain_patched_bo(path, name1); - - //fprintf(stdout,"in queue process Update no %d\n", (int) update_no); - //fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); - //FILE *fp; - //fp=fopen("update.bo", "wb"); - //fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); - //fclose(fp); + if (*(int*)(updater->mode) == CODE_RUNNING) + { + // fprintf(stdout,"[debug]Inside inmsg code running"); + if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) + { + size += sizeof(uint8_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + { + // fprintf(stdout,"[debug]Inside update number comparision"); + uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + /*Generate patch*/ + std::string bzzfile_name(bzz_file); + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + { + out_code = obtain_patched_bo(path, name1); + // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); + // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); + // FILE *fp; + // fp=fopen("update.bo", "wb"); + // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); + // fclose(fp); - if(test_set_code( (out_code->bcode), - (char*) dbgf_name, (size_t) (*(uint16_t*)out_code->bcode_size) ) ){ - - printf("TEST PASSED!\n"); - *(uint16_t*)updater->update_no=update_no; - /*Clear exisiting patch if any*/ - delete_p(updater->patch); - /*copy the patch into the updater*/ - updater->patch = (uint8_t*)malloc(update_patch_size); - memcpy(updater->patch, (updater->inmsg_queue->queue+size), update_patch_size); - *(size_t*) (updater->patch_size) = update_patch_size; - //code_message_outqueue_append(); - neigh=1; - } - /*clear the temp code buff*/ - delete_p(out_code->bcode); - delete_p(out_code->bcode_size); - delete_p(out_code); - } - else{ - ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); - update_try_counter++; - outqueue_append_code_request(update_no); - } - - } - } - - } - size=0; - if(*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE){ - size+=sizeof(uint8_t); - if( *(uint16_t*)(updater->inmsg_queue->queue+size) == *(uint16_t*) (updater->update_no) ){ - size+=sizeof(uint16_t); - if(*(uint16_t*)(updater->inmsg_queue->queue+size) > update_try_counter){ - update_try_counter=*(uint16_t*)(updater->inmsg_queue->queue+size); - ROS_ERROR("Code appended! update try : %u \n",update_try_counter); - code_message_outqueue_append(); - - } - if(update_try_counter > MAX_UPDATE_TRY) ROS_ERROR("TODO: ROLL BACK !!"); - } - } - //fprintf(stdout,"in queue freed\n"); - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); + if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) + { + printf("TEST PASSED!\n"); + *(uint16_t*)updater->update_no = update_no; + /*Clear exisiting patch if any*/ + delete_p(updater->patch); + /*copy the patch into the updater*/ + updater->patch = (uint8_t*)malloc(update_patch_size); + memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); + *(size_t*)(updater->patch_size) = update_patch_size; + // code_message_outqueue_append(); + neigh = 1; + } + /*clear the temp code buff*/ + delete_p(out_code->bcode); + delete_p(out_code->bcode_size); + delete_p(out_code); + } + else + { + ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); + update_try_counter++; + outqueue_append_code_request(update_no); + } + } + } + } + size = 0; + if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) + { + size += sizeof(uint8_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) + { + size += sizeof(uint16_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) + { + update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); + ROS_ERROR("Code appended! update try : %u \n", update_try_counter); + code_message_outqueue_append(); + } + if (update_try_counter > MAX_UPDATE_TRY) + ROS_ERROR("TODO: ROLL BACK !!"); + } + } + // fprintf(stdout,"in queue freed\n"); + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); } +void create_update_patch() +{ + std::stringstream genpatch; + std::stringstream usepatch; + std::string bzzfile_name(bzz_file); + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + std::string name2 = name1 + "-update"; -void create_update_patch(){ - std::stringstream genpatch; - std::stringstream usepatch; + // CALL BSDIFF CMD + genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; + fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str()); + system(genpatch.str().c_str()); - std::string bzzfile_name(bzz_file); + // BETTER: CALL THE FUNCTION IN BSDIFF.CPP + // bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo"); - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0,name1.find_last_of(".")); + /* delete old files & rename new files */ - std::string name2 = name1 + "-update"; + remove((path + name1 + ".bo").c_str()); + remove((path + name1 + ".bdb").c_str()); - // CALL BSDIFF CMD - genpatch<< "bsdiff "<< path << name1 <<".bo "<< path << name2 <<".bo "<< path<<"diff.bo"; - fprintf(stdout,"Launching bsdiff command: %s \n", genpatch.str().c_str()); - system(genpatch.str().c_str()); + rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); + rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); - // BETTER: CALL THE FUNCTION IN BSDIFF.CPP - //bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo"); - + /*Read the diff file */ + std::stringstream patchfileName; + patchfileName << path << "diff.bo"; + uint8_t* patch_buff = 0; + FILE* fp = fopen(patchfileName.str().c_str(), "rb"); + if (!fp) + { + perror(patchfileName.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patch_size = ftell(fp); + rewind(fp); + patch_buff = (uint8_t*)malloc(patch_size); + if (fread(patch_buff, 1, patch_size, fp) < patch_size) + { + perror(patchfileName.str().c_str()); + fclose(fp); + } + fclose(fp); + delete_p(updater->patch); + updater->patch = patch_buff; + *(size_t*)(updater->patch_size) = patch_size; - /* delete old files & rename new files */ - - remove((path + name1 + ".bo").c_str()); - remove((path + name1 + ".bdb").c_str()); - - rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); - rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); - - - /*Read the diff file */ - std::stringstream patchfileName; - patchfileName<patch); - updater->patch = patch_buff; - *(size_t*) (updater->patch_size) = patch_size; - - /* Delete the diff file */ - remove(patchfileName.str().c_str()); + /* Delete the diff file */ + remove(patchfileName.str().c_str()); } +void update_routine() +{ + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); + if (*(int*)updater->mode == CODE_RUNNING) + { + buzzvm_function_call(VM, "updated_neigh", 0); + // Check for update + if (check_update()) + { + ROS_INFO("Update found \nUpdating script ...\n"); -void update_routine(){ - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); - //fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); - if(*(int*)updater->mode==CODE_RUNNING){ - buzzvm_function_call(VM, "updated_neigh", 0); - // Check for update - if(check_update()){ + if (compile_bzz(bzz_file)) + { + ROS_WARN("Errors in comipilg script so staying on old script\n"); + } + else + { + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0, name.find_last_of(".")); + bzzfile_in_compile << path << name << "-update.bo"; + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit + if (!fp) + { + perror(bzzfile_in_compile.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bcfname); + fclose(fp); + } + fclose(fp); + if (test_set_code(BO_BUF, dbgf_name, bcode_size)) + { + uint16_t update_no = *(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) = update_no + 1; - - ROS_INFO("Update found \nUpdating script ...\n"); - - if(compile_bzz(bzz_file)){ - ROS_WARN("Errors in comipilg script so staying on old script\n"); - } - else { - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0,name.find_last_of(".")); - bzzfile_in_compile<update_no); - *(uint16_t*)(updater->update_no) =update_no +1; - - create_update_patch(); - VM = buzz_utility::get_vm(); - ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); - neigh=-1; - ROS_INFO("Sending code... \n"); - code_message_outqueue_append(); - } - delete_p(BO_BUF); - } - - } - - } + create_update_patch(); + VM = buzz_utility::get_vm(); + ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + neigh = -1; + ROS_INFO("Sending code... \n"); + code_message_outqueue_append(); + } + delete_p(BO_BUF); + } + } + } - else{ - - - timer_steps++; - buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1)); - buzzvm_gload(VM); - buzzobj_t tObj = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - ROS_INFO("Barrier ..................... %i \n",tObj->i.value); - if(tObj->i.value==no_of_robot) { - *(int*)(updater->mode) = CODE_RUNNING; - gettimeofday(&t2, NULL); - //collect_data(); - buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); - //buzzvm_function_call(m_tBuzzVM, "updated", 0); - updated=1; - update_try_counter=0; - timer_steps=0; - } - else if (timer_steps>TIMEOUT_FOR_ROLLBACK){ - ROS_ERROR("TIME OUT Reached, decided to roll back"); - /*Time out hit deceided to roll back*/ - *(int*)(updater->mode) = CODE_RUNNING; - buzz_utility::buzz_script_set(old_bcfname, dbgf_name, - (int)VM->robot); - updated=1; - update_try_counter=0; - timer_steps=0; - } - - } - + else + { + timer_steps++; + buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); + buzzvm_gload(VM); + buzzobj_t tObj = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + ROS_INFO("Barrier ..................... %i \n", tObj->i.value); + if (tObj->i.value == no_of_robot) + { + *(int*)(updater->mode) = CODE_RUNNING; + gettimeofday(&t2, NULL); + // collect_data(); + buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); + // buzzvm_function_call(m_tBuzzVM, "updated", 0); + updated = 1; + update_try_counter = 0; + timer_steps = 0; + } + else if (timer_steps > TIMEOUT_FOR_ROLLBACK) + { + ROS_ERROR("TIME OUT Reached, decided to roll back"); + /*Time out hit deceided to roll back*/ + *(int*)(updater->mode) = CODE_RUNNING; + buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); + updated = 1; + update_try_counter = 0; + timer_steps = 0; + } + } } - - - -uint8_t* getupdater_out_msg(){ -return (uint8_t*)updater->outmsg_queue->queue; +uint8_t* getupdater_out_msg() +{ + return (uint8_t*)updater->outmsg_queue->queue; } -uint8_t* getupdate_out_msg_size(){ -//fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); -return (uint8_t*)updater->outmsg_queue->size; +uint8_t* getupdate_out_msg_size() +{ + // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); + return (uint8_t*)updater->outmsg_queue->size; } -int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){ - if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){ - ROS_WARN("Initializtion of script test passed\n"); - if(buzz_utility::update_step_test()){ - /*data logging*/ - old_byte_code_size=*(size_t*)updater->bcode_size; - /*data logging*/ - ROS_WARN("Step test passed\n"); - *(int*) (updater->mode) = CODE_STANDBY; - //fprintf(stdout,"updater value = %i\n",updater->mode); - delete_p(updater->bcode); - updater->bcode = (uint8_t*)malloc(bcode_size); - memcpy(updater->bcode, BO_BUF, bcode_size); - *(size_t*)updater->bcode_size = bcode_size; - buzz_utility::buzz_update_init_test((updater)->standby_bcode, - (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - gettimeofday(&t1, NULL); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - return 1; - } - /*Unable to step something wrong*/ - else{ - if(*(int*) (updater->mode) == CODE_RUNNING){ - ROS_ERROR("step test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t)*(size_t*)(updater->bcode_size)); - } - else{ - /*You will never reach here*/ - ROS_ERROR("step test failed, Return to stand by\n"); - buzz_utility::buzz_update_init_test((updater)->standby_bcode, - (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - - } - return 0; - - } - } - else { - if(*(int*) (updater->mode) == CODE_RUNNING){ - ROS_ERROR("Initialization test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname,(int)*(size_t*) (updater->bcode_size)); - } - else{ - /*You will never reach here*/ - ROS_ERROR("Initialization test failed, Return to stand by\n"); - buzz_utility::buzz_update_init_test((updater)->standby_bcode, - (char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - } - return 0; - } +int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) +{ + if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) + { + ROS_WARN("Initializtion of script test passed\n"); + if (buzz_utility::update_step_test()) + { + /*data logging*/ + old_byte_code_size = *(size_t*)updater->bcode_size; + /*data logging*/ + ROS_WARN("Step test passed\n"); + *(int*)(updater->mode) = CODE_STANDBY; + // fprintf(stdout,"updater value = %i\n",updater->mode); + delete_p(updater->bcode); + updater->bcode = (uint8_t*)malloc(bcode_size); + memcpy(updater->bcode, BO_BUF, bcode_size); + *(size_t*)updater->bcode_size = bcode_size; + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + gettimeofday(&t1, NULL); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + return 1; + } + /*Unable to step something wrong*/ + else + { + if (*(int*)(updater->mode) == CODE_RUNNING) + { + ROS_ERROR("step test failed, stick to old script\n"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); + } + else + { + /*You will never reach here*/ + ROS_ERROR("step test failed, Return to stand by\n"); + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + } + return 0; + } + } + else + { + if (*(int*)(updater->mode) == CODE_RUNNING) + { + ROS_ERROR("Initialization test failed, stick to old script\n"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); + } + else + { + /*You will never reach here*/ + ROS_ERROR("Initialization test failed, Return to stand by\n"); + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + } + return 0; + } } -void destroy_out_msg_queue(){ - //fprintf(stdout,"out queue freed\n"); - delete_p(updater->outmsg_queue->queue); - delete_p(updater->outmsg_queue->size); - delete_p(updater->outmsg_queue); - updater_msg_ready=0; +void destroy_out_msg_queue() +{ + // fprintf(stdout,"out queue freed\n"); + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + updater_msg_ready = 0; } -int get_update_status(){ -return updated; +int get_update_status() +{ + return updated; } -void set_read_update_status(){ - updated=0; +void set_read_update_status() +{ + updated = 0; } -int get_update_mode(){ -return (int)*(int*)(updater->mode); +int get_update_mode() +{ + return (int)*(int*)(updater->mode); } -int is_msg_present(){ -return updater_msg_ready; +int is_msg_present() +{ + return updater_msg_ready; } -buzz_updater_elem_t get_updater(){ -return updater; +buzz_updater_elem_t get_updater() +{ + return updater; } -void destroy_updater(){ - delete_p(updater->bcode); - delete_p(updater->bcode_size); - delete_p(updater->standby_bcode); - delete_p(updater->standby_bcode_size); - delete_p(updater->mode); - delete_p(updater->update_no); - if(updater->outmsg_queue){ - delete_p(updater->outmsg_queue->queue); - delete_p(updater->outmsg_queue->size); - delete_p(updater->outmsg_queue); - } - if(updater->inmsg_queue){ - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); - } - inotify_rm_watch(fd,wd); - close(fd); +void destroy_updater() +{ + delete_p(updater->bcode); + delete_p(updater->bcode_size); + delete_p(updater->standby_bcode); + delete_p(updater->standby_bcode_size); + delete_p(updater->mode); + delete_p(updater->update_no); + if (updater->outmsg_queue) + { + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + } + if (updater->inmsg_queue) + { + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + inotify_rm_watch(fd, wd); + close(fd); } -void set_bzz_file(const char* in_bzz_file){ - bzz_file=in_bzz_file; +void set_bzz_file(const char* in_bzz_file) +{ + bzz_file = in_bzz_file; } -void updates_set_robots(int robots){ - no_of_robot=robots; +void updates_set_robots(int robots) +{ + no_of_robot = robots; } /*-------------------------------------------------------- / Create Buzz bytecode from the bzz script inputed /-------------------------------------------------------*/ -int compile_bzz(std::string bzz_file){ - /*Compile the buzz code .bzz to .bo*/ - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0,name.find_last_of(".")); - bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<bcode_size<<"," - <<*(size_t*)updater->patch_size<<","<<(int)*(uint8_t*)updater->update_no<<","<<(int)packet_id_; - +void collect_data(std::ofstream& logger) +{ + double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; + time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; + // RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number, + // Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter + logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << "," + << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << "," + << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index bba7a5b..a8b19df 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -8,553 +8,582 @@ #include "buzz_utility.h" -namespace buzz_utility{ +namespace buzz_utility +{ +/****************************************/ +/****************************************/ - /****************************************/ - /****************************************/ +static buzzvm_t VM = 0; +static char* BO_FNAME = 0; +static uint8_t* BO_BUF = 0; +static buzzdebug_t DBG_INFO = 0; +static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets +static uint8_t Robot_id = 0; +static std::vector IN_MSG; +std::map users_map; - static buzzvm_t VM = 0; - static char* BO_FNAME = 0; - static uint8_t* BO_BUF = 0; - static buzzdebug_t DBG_INFO = 0; - static uint32_t MAX_MSG_SIZE = 250; // 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; +/****************************************/ - /****************************************/ - - /**************************************************************************/ - /*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */ - /**************************************************************************/ - uint16_t* u64_cvt_u16(uint64_t u64){ - uint16_t* out = new uint16_t[4]; - uint32_t int32_1 = u64 & 0xFFFFFFFF; - uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32; - out[0] = int32_1 & 0xFFFF; - out[1] = (int32_1 & (0xFFFF0000) ) >> 16; - out[2] = int32_2 & 0xFFFF; - out[3] = (int32_2 & (0xFFFF0000) ) >> 16; - //cout << " values " < 0 && unMsgSize <= size - tot ) { - buzzinmsg_queue_append(VM, - neigh_id, - buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize)); - tot += unMsgSize; - } - }while(size - tot > sizeof(uint16_t) && unMsgSize > 0); - free(first_INmsg); - IN_MSG.erase(IN_MSG.begin()); - } - /* Process messages VM call*/ - buzzvm_process_inmsgs(VM); - } - - /***************************************************/ - /*Obtains messages from buzz out message Queue*/ - /***************************************************/ - uint64_t* obt_out_msg(){ - /* Process out messages */ - buzzvm_process_outmsgs(VM); - uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE); - memset(buff_send, 0, MAX_MSG_SIZE); - /*Taking into consideration the sizes included at the end*/ - ssize_t tot = sizeof(uint16_t); - /* Send robot id */ - *(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot; - tot += sizeof(uint16_t); - /* Send messages from FIFO */ - do { - /* Are there more messages? */ - if(buzzoutmsg_queue_isempty(VM)) break; - /* Get first message */ - buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); - /* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */ - //ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t))); - if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) { - buzzmsg_payload_destroy(&m); - break; - } - - /* Add message length to data buffer */ - *(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m); - tot += sizeof(uint16_t); - - /* Add payload to data buffer */ - memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m)); - tot += buzzmsg_payload_size(m); - - /* Get rid of message */ - buzzoutmsg_queue_next(VM); - buzzmsg_payload_destroy(&m); - } while(1); - - uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t))); - *(uint16_t*)buff_send = (uint16_t) total_size; - - uint64_t* payload_64 = new uint64_t[total_size]; - - memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t)); - free(buff_send); - /*for(int i=0;ipc); - char* msg; - if(dbg != NULL) { - asprintf(&msg, - "%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n", - BO_FNAME, - dbg->fname, - dbg->line, - dbg->col, - VM->errormsg); - } - else { - asprintf(&msg, - "%s: execution terminated abnormally at bytecode offset %d: %s\n\n", - BO_FNAME, - VM->pc, - VM->errormsg); - } - return msg; - } - - /****************************************/ - /*Buzz hooks that can be used inside .bzz file*/ - /****************************************/ - - static int buzz_register_hooks() { - buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome)); - buzzvm_gstore(VM); - 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_targetrb", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus)); - buzzvm_gstore(VM); - - return VM->state; - } - - /**************************************************/ - /*Register dummy Buzz hooks for test during update*/ - /**************************************************/ - - static int testing_buzz_register_hooks() { - buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); - 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_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); - buzzvm_gstore(VM); - 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_targetrb", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); - buzzvm_gstore(VM); - buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); - buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); - buzzvm_gstore(VM); - - return VM->state; - } - - /****************************************/ - /*Sets the .bzz and .bdbg file*/ - /****************************************/ - int buzz_script_set(const char* bo_filename, - const char* bdbg_filename, int robot_id) { - ROS_INFO(" Robot ID: %i" , robot_id); - Robot_id=robot_id; - /* Read bytecode from file and fill the bo buffer*/ - FILE* fd = fopen(bo_filename, "rb"); - if(!fd) { - perror(bo_filename); - return 0; - } - fseek(fd, 0, SEEK_END); - size_t bcode_size = ftell(fd); - rewind(fd); - BO_BUF = (uint8_t*)malloc(bcode_size); - if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) { - perror(bo_filename); - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - fclose(fd); - return 0; - } - fclose(fd); - - /* Save bytecode file name */ - BO_FNAME = strdup(bo_filename); - - return buzz_update_set(BO_BUF, bdbg_filename, bcode_size); - } - - /****************************************/ - /*Sets a new update */ - /****************************************/ - int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ - // Reset the Buzz VM - if(VM) buzzvm_destroy(&VM); - VM = buzzvm_new(Robot_id); - // Get rid of debug info - if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); - DBG_INFO = buzzdebug_new(); - - // Read debug information - if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - perror(bdbg_filename); - return 0; - } - // Set byte code - if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id); - return 0; - } - // Register hook functions - if(buzz_register_hooks() != BUZZVM_STATE_READY) { - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("[%i] Error registering hooks (update)", Robot_id); - return 0; - } - - // Initialize UAVSTATE variable - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); - buzzvm_gstore(VM); - - // 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); - return 0; - } - // Call the Init() function - if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ - ROS_ERROR("Error in calling init, VM state : %i", VM->state); - return 0; - } - // All OK - return 1; - } - - /****************************************/ - /*Performs a initialization test */ - /****************************************/ - int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ - // Reset the Buzz VM - if(VM) buzzvm_destroy(&VM); - VM = buzzvm_new(Robot_id); - // Get rid of debug info - if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); - DBG_INFO = buzzdebug_new(); - - // Read debug information - if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - perror(bdbg_filename); - return 0; - } - // Set byte code - if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id); - return 0; - } - // Register hook functions - if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) { - buzzvm_destroy(&VM); - buzzdebug_destroy(&DBG_INFO); - ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id); - return 0; - } - - // Initialize UAVSTATE variable - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); - buzzvm_gstore(VM); - - // 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); - return 0; - } - // Call the Init() function - if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ - ROS_ERROR("Error in calling init, VM state : %i", VM->state); - return 0; - } - // All OK - return 1; - } - - /****************************************/ - /*Swarm struct*/ - /****************************************/ - - struct buzzswarm_elem_s { - buzzdarray_t swarms; - uint16_t age; - }; - typedef struct buzzswarm_elem_s* buzzswarm_elem_t; - - void check_swarm_members(const void* key, void* data, void* params) { - buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; - int* status = (int*)params; - if(*status == 3) return; - fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n", - buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age); - if(buzzdarray_size(e->swarms) != 1) { - fprintf(stderr, "Swarm list size is not 1\n"); - *status = 3; - } - else { - int sid = 1; - if(!buzzdict_isempty(VM->swarms)) { - if(*buzzdict_get(VM->swarms, &sid, uint8_t) && - buzzdarray_get(e->swarms, 0, uint16_t) != sid) { - fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", - sid, - buzzdarray_get(e->swarms, 0, uint16_t)); - *status = 3; - return; - } - } - if(buzzdict_size(VM->swarms)>1) { - sid = 2; - if(*buzzdict_get(VM->swarms, &sid, uint8_t) && - buzzdarray_get(e->swarms, 0, uint16_t) != sid) { - fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", - sid, - buzzdarray_get(e->swarms, 0, uint16_t)); - *status = 3; - return; - } - } - } - } - - /*Step through the buzz script*/ - 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); - buzzuav_closures::buzzuav_update_targets(VM); - //update_users(); - buzzuav_closures::buzzuav_update_flight_status(VM); - } - - void buzz_script_step() { - /*Process available messages*/ - in_message_process(); - /*Update sensors*/ - update_sensors(); - /* Call Buzz step() function */ - if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { - ROS_ERROR("%s: execution terminated abnormally: %s", - BO_FNAME, - buzz_error_info()); - buzzvm_dump(VM); - } - } - - /****************************************/ - /*Destroy the bvm and other resorces*/ - /****************************************/ - - void buzz_script_destroy() { - if(VM) { - if(VM->state != BUZZVM_STATE_READY) { - ROS_ERROR("%s: execution terminated abnormally: %s", - BO_FNAME, - buzz_error_info()); - buzzvm_dump(VM); - } - buzzvm_function_call(VM, "destroy", 0); - buzzvm_destroy(&VM); - free(BO_FNAME); - buzzdebug_destroy(&DBG_INFO); - } - ROS_INFO("Script execution stopped."); - } - - - /****************************************/ - /****************************************/ - - /****************************************/ - /*Execution completed*/ - /****************************************/ - - int buzz_script_done() { - return VM->state != BUZZVM_STATE_READY; - } - - int update_step_test() { - /*Process available messages*/ - in_message_process(); - buzzuav_closures::buzzuav_update_battery(VM); - buzzuav_closures::buzzuav_update_prox(VM); - buzzuav_closures::buzzuav_update_currentpos(VM); - buzzuav_closures::update_neighbors(VM); - buzzuav_closures::buzzuav_update_flight_status(VM); - - int a = buzzvm_function_call(VM, "step", 0); - - if(a!= BUZZVM_STATE_READY) { - ROS_ERROR("%s: execution terminated abnormally: %s\n\n", - BO_FNAME, - buzz_error_info()); - fprintf(stdout, "step test VM state %i\n",a); - } - - return a == BUZZVM_STATE_READY; - } - - buzzvm_t get_vm() { - return VM; - } - - void set_robot_var(int ROBOTS){ - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, ROBOTS); - buzzvm_gstore(VM); - } - - int get_inmsg_size(){ - return IN_MSG.size(); - } +/**************************************************************************/ +/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */ +/**************************************************************************/ +uint16_t* u64_cvt_u16(uint64_t u64) +{ + uint16_t* out = new uint16_t[4]; + uint32_t int32_1 = u64 & 0xFFFFFFFF; + uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000) >> 32; + out[0] = int32_1 & 0xFFFF; + out[1] = (int32_1 & (0xFFFF0000)) >> 16; + out[2] = int32_2 & 0xFFFF; + out[3] = (int32_2 & (0xFFFF0000)) >> 16; + // cout << " values " < 0 && unMsgSize <= size - tot) + { + buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize)); + tot += unMsgSize; + } + } while (size - tot > sizeof(uint16_t) && unMsgSize > 0); + free(first_INmsg); + IN_MSG.erase(IN_MSG.begin()); + } + /* Process messages VM call*/ + buzzvm_process_inmsgs(VM); +} + +/***************************************************/ +/*Obtains messages from buzz out message Queue*/ +/***************************************************/ +uint64_t* obt_out_msg() +{ + /* Process out messages */ + buzzvm_process_outmsgs(VM); + uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE); + memset(buff_send, 0, MAX_MSG_SIZE); + /*Taking into consideration the sizes included at the end*/ + ssize_t tot = sizeof(uint16_t); + /* Send robot id */ + *(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot; + tot += sizeof(uint16_t); + /* Send messages from FIFO */ + do + { + /* Are there more messages? */ + if (buzzoutmsg_queue_isempty(VM)) + break; + /* Get first message */ + buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); + /* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */ + // ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t))); + if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) + { + buzzmsg_payload_destroy(&m); + break; + } + + /* Add message length to data buffer */ + *(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m); + tot += sizeof(uint16_t); + + /* Add payload to data buffer */ + memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m)); + tot += buzzmsg_payload_size(m); + + /* Get rid of message */ + buzzoutmsg_queue_next(VM); + buzzmsg_payload_destroy(&m); + } while (1); + + uint16_t total_size = (ceil((float)tot / (float)sizeof(uint64_t))); + *(uint16_t*)buff_send = (uint16_t)total_size; + + uint64_t* payload_64 = new uint64_t[total_size]; + + memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); + free(buff_send); + /*for(int i=0;ipc); + char* msg; + if (dbg != NULL) + { + asprintf(&msg, "%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n", BO_FNAME, dbg->fname, + dbg->line, dbg->col, VM->errormsg); + } + else + { + asprintf(&msg, "%s: execution terminated abnormally at bytecode offset %d: %s\n\n", BO_FNAME, VM->pc, VM->errormsg); + } + return msg; +} + +/****************************************/ +/*Buzz hooks that can be used inside .bzz file*/ +/****************************************/ + +static int buzz_register_hooks() +{ + buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome)); + buzzvm_gstore(VM); + 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_targetrb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus)); + buzzvm_gstore(VM); + + return VM->state; +} + +/**************************************************/ +/*Register dummy Buzz hooks for test during update*/ +/**************************************************/ + +static int testing_buzz_register_hooks() +{ + buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + 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_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + 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_targetrb", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); + buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); + buzzvm_gstore(VM); + + return VM->state; +} + +/****************************************/ +/*Sets the .bzz and .bdbg file*/ +/****************************************/ +int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) +{ + ROS_INFO(" Robot ID: %i", robot_id); + Robot_id = robot_id; + /* Read bytecode from file and fill the bo buffer*/ + FILE* fd = fopen(bo_filename, "rb"); + if (!fd) + { + perror(bo_filename); + return 0; + } + fseek(fd, 0, SEEK_END); + size_t bcode_size = ftell(fd); + rewind(fd); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fd) < bcode_size) + { + perror(bo_filename); + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + fclose(fd); + return 0; + } + fclose(fd); + + /* Save bytecode file name */ + BO_FNAME = strdup(bo_filename); + + return buzz_update_set(BO_BUF, bdbg_filename, bcode_size); +} + +/****************************************/ +/*Sets a new update */ +/****************************************/ +int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size) +{ + // Reset the Buzz VM + if (VM) + buzzvm_destroy(&VM); + VM = buzzvm_new(Robot_id); + // Get rid of debug info + if (DBG_INFO) + buzzdebug_destroy(&DBG_INFO); + DBG_INFO = buzzdebug_new(); + + // Read debug information + if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + perror(bdbg_filename); + return 0; + } + // Set byte code + if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id); + return 0; + } + // Register hook functions + if (buzz_register_hooks() != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] Error registering hooks (update)", Robot_id); + return 0; + } + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + + // 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); + return 0; + } + // Call the Init() function + if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY) + { + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } + // All OK + return 1; +} + +/****************************************/ +/*Performs a initialization test */ +/****************************************/ +int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size) +{ + // Reset the Buzz VM + if (VM) + buzzvm_destroy(&VM); + VM = buzzvm_new(Robot_id); + // Get rid of debug info + if (DBG_INFO) + buzzdebug_destroy(&DBG_INFO); + DBG_INFO = buzzdebug_new(); + + // Read debug information + if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + perror(bdbg_filename); + return 0; + } + // Set byte code + if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id); + return 0; + } + // Register hook functions + if (testing_buzz_register_hooks() != BUZZVM_STATE_READY) + { + buzzvm_destroy(&VM); + buzzdebug_destroy(&DBG_INFO); + ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id); + return 0; + } + + // Initialize UAVSTATE variable + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1)); + buzzvm_gstore(VM); + + // 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); + return 0; + } + // Call the Init() function + if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY) + { + ROS_ERROR("Error in calling init, VM state : %i", VM->state); + return 0; + } + // All OK + return 1; +} + +/****************************************/ +/*Swarm struct*/ +/****************************************/ + +struct buzzswarm_elem_s +{ + buzzdarray_t swarms; + uint16_t age; +}; +typedef struct buzzswarm_elem_s* buzzswarm_elem_t; + +void check_swarm_members(const void* key, void* data, void* params) +{ + buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; + int* status = (int*)params; + if (*status == 3) + return; + fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n", buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, + e->age); + if (buzzdarray_size(e->swarms) != 1) + { + fprintf(stderr, "Swarm list size is not 1\n"); + *status = 3; + } + else + { + int sid = 1; + if (!buzzdict_isempty(VM->swarms)) + { + if (*buzzdict_get(VM->swarms, &sid, uint8_t) && buzzdarray_get(e->swarms, 0, uint16_t) != sid) + { + fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid, buzzdarray_get(e->swarms, 0, uint16_t)); + *status = 3; + return; + } + } + if (buzzdict_size(VM->swarms) > 1) + { + sid = 2; + if (*buzzdict_get(VM->swarms, &sid, uint8_t) && buzzdarray_get(e->swarms, 0, uint16_t) != sid) + { + fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid, buzzdarray_get(e->swarms, 0, uint16_t)); + *status = 3; + return; + } + } + } +} + +/*Step through the buzz script*/ +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); + buzzuav_closures::buzzuav_update_targets(VM); + // update_users(); + buzzuav_closures::buzzuav_update_flight_status(VM); +} + +void buzz_script_step() +{ + /*Process available messages*/ + in_message_process(); + /*Update sensors*/ + update_sensors(); + /* Call Buzz step() function */ + if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) + { + ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info()); + buzzvm_dump(VM); + } +} + +/****************************************/ +/*Destroy the bvm and other resorces*/ +/****************************************/ + +void buzz_script_destroy() +{ + if (VM) + { + if (VM->state != BUZZVM_STATE_READY) + { + ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info()); + buzzvm_dump(VM); + } + buzzvm_function_call(VM, "destroy", 0); + buzzvm_destroy(&VM); + free(BO_FNAME); + buzzdebug_destroy(&DBG_INFO); + } + ROS_INFO("Script execution stopped."); +} + +/****************************************/ +/****************************************/ + +/****************************************/ +/*Execution completed*/ +/****************************************/ + +int buzz_script_done() +{ + return VM->state != BUZZVM_STATE_READY; +} + +int update_step_test() +{ + /*Process available messages*/ + in_message_process(); + buzzuav_closures::buzzuav_update_battery(VM); + buzzuav_closures::buzzuav_update_prox(VM); + buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::update_neighbors(VM); + buzzuav_closures::buzzuav_update_flight_status(VM); + + int a = buzzvm_function_call(VM, "step", 0); + + if (a != BUZZVM_STATE_READY) + { + ROS_ERROR("%s: execution terminated abnormally: %s\n\n", BO_FNAME, buzz_error_info()); + fprintf(stdout, "step test VM state %i\n", a); + } + + return a == BUZZVM_STATE_READY; +} + +buzzvm_t get_vm() +{ + return VM; +} + +void set_robot_var(int ROBOTS) +{ + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, ROBOTS); + buzzvm_gstore(VM); +} + +int get_inmsg_size() +{ + return IN_MSG.size(); +} } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 180df2f..988b2d8 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -9,41 +9,42 @@ #include "buzzuav_closures.h" #include "math.h" -namespace buzzuav_closures{ - // TODO: Minimize the required global variables and put them in the header - //static const rosbzz_node::roscontroller* roscontroller_ptr; - /*forward declaration of ros controller ptr storing function*/ - //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin); - static double goto_pos[3]; - static double rc_goto_pos[3]; - static float rc_gimbal[4]; - 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 rc_id=-1; - static int buzz_cmd=0; - static float height=0; - static bool deque_full = false; - static int rssi = 0; - static float raw_packet_loss = 0.0; - static int filtered_packet_loss = 0; - static float api_rssi = 0.0; - string WPlistname = ""; +namespace buzzuav_closures +{ +// TODO: Minimize the required global variables and put them in the header +// static const rosbzz_node::roscontroller* roscontroller_ptr; +/*forward declaration of ros controller ptr storing function*/ +// void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin); +static double goto_pos[3]; +static double rc_goto_pos[3]; +static float rc_gimbal[4]; +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 rc_id = -1; +static int buzz_cmd = 0; +static float height = 0; +static bool deque_full = false; +static int rssi = 0; +static float raw_packet_loss = 0.0; +static int filtered_packet_loss = 0; +static float api_rssi = 0.0; +string WPlistname = ""; - std::map< int, buzz_utility::RB_struct> targets_map; - std::map< int, buzz_utility::RB_struct> wplist_map; - std::map< int, buzz_utility::Pos_struct> neighbors_map; - std::map< int, buzz_utility::neighbors_status> neighbors_status_map; +std::map targets_map; +std::map wplist_map; +std::map neighbors_map; +std::map neighbors_status_map; - /****************************************/ - /****************************************/ +/****************************************/ +/****************************************/ int buzzros_print(buzzvm_t vm) { - std::ostringstream buffer (std::ostringstream::ate); + std::ostringstream buffer(std::ostringstream::ate); buffer << buzz_utility::get_robotid(); for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index) { @@ -84,656 +85,696 @@ int buzzros_print(buzzvm_t vm) break; } } - ROS_INFO("%s",buffer.str().c_str()); + ROS_INFO("%s", buffer.str().c_str()); return buzzvm_ret0(vm); } - void setWPlist(string path){ - WPlistname = path + "include/graphs/waypointlist.csv"; - } - - /*----------------------------------------/ - / Compute GPS destination from current position and desired Range and Bearing - /----------------------------------------*/ - - void gps_from_rb(double range, double bearing, double out[3]) { - double lat = RAD2DEG(cur_pos[0]); - double lon = RAD2DEG(cur_pos[1]); - 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] = RAD2DEG(out[0]); - out[1] = RAD2DEG(out[1]); - 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] = constrainAngle(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}; - - void parse_gpslist() - { - // Open file: - ROS_INFO("WP list file: %s", WPlistname.c_str()); - std::ifstream fin(WPlistname.c_str()); // Open in text-mode. - - // Opening may fail, always check. - if (!fin) { - ROS_ERROR("GPS list parser, could not open file."); - return; - } - - // Prepare a C-string buffer to be used when reading lines. - const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need. - char buffer[MAX_LINE_LENGTH] = {}; - const char * DELIMS = "\t ,"; // Tab, space or comma. - - // Read the file and load the data: - double lat, lon; - int alt, tilt, tid; - buzz_utility::RB_struct RB_arr; - // Read one line at a time. - while ( fin.getline(buffer, MAX_LINE_LENGTH) ) { - // Extract the tokens: - tid = atoi(strtok( buffer, DELIMS )); - lon = atof(strtok( NULL, DELIMS )); - lat = atof(strtok( NULL, DELIMS )); - alt = atoi(strtok( NULL, DELIMS )); - tilt = atoi(strtok( NULL, DELIMS )); - //ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid); - RB_arr.latitude=lat; - RB_arr.longitude=lon; - RB_arr.altitude=alt; - // Insert elements. - map< int, buzz_utility::RB_struct >::iterator it = wplist_map.find(tid); - if(it!=wplist_map.end()) - wplist_map.erase(it); - wplist_map.insert(make_pair(tid, RB_arr)); - } - - ROS_INFO("----->Saved %i waypoints.", wplist_map.size()); - - // Close the file: - fin.close(); - } - - - /*----------------------------------------/ - / Buzz closure to move following a 2D vector - /----------------------------------------*/ - int buzzuav_moveto(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 3); - buzzvm_lload(vm, 1); /* dx */ - buzzvm_lload(vm, 2); /* dy */ - buzzvm_lload(vm, 3); /* dheight */ - buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - float dh = buzzvm_stack_at(vm, 1)->f.value; - float dy = buzzvm_stack_at(vm, 2)->f.value; - float dx = buzzvm_stack_at(vm, 3)->f.value; - goto_pos[0]=dx; - goto_pos[1]=dy; - goto_pos[2]=height+dh; - //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 buzzuav_update_targets(buzzvm_t vm) { - if(vm->state != BUZZVM_STATE_READY) return vm->state; - buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1)); - buzzvm_pusht(vm); - buzzobj_t targettbl = buzzvm_stack_at(vm, 1); - double rb[3], tmp[3]; - map< int, buzz_utility::RB_struct >::iterator it; - for (it=targets_map.begin(); it!=targets_map.end(); ++it){ - tmp[0]=(it->second).latitude; tmp[1]=(it->second).longitude; tmp[2]=height; - rb_from_gps(tmp, rb, cur_pos); - //ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]); - buzzvm_push(vm, targettbl); - // When we get here, the "targets" table is on top of the stack - //ROS_INFO("Buzz_utility will save user %i.", it->first); - // Push user id - buzzvm_pushi(vm, it->first); - // Create entry table - buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); - // Insert range - buzzvm_push(vm, entry); - buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1)); - buzzvm_pushf(vm, rb[0]); - buzzvm_tput(vm); - // Insert longitude - buzzvm_push(vm, entry); - buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1)); - buzzvm_pushf(vm, rb[1]); - buzzvm_tput(vm); - // Save entry into data table - buzzvm_push(vm, entry); - buzzvm_tput(vm); - } - buzzvm_gstore(vm); - - return vm->state; - } - - int buzzuav_addtargetRB(buzzvm_t vm) { - 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]); - buzz_utility::RB_struct RB_arr; - RB_arr.latitude=tmp[0]; - RB_arr.longitude=tmp[1]; - RB_arr.altitude=tmp[2]; - 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 - ROS_WARN(" ---------- Target too far %f",rb[0]); - - return 0; - } - - int buzzuav_addNeiStatus(buzzvm_t vm){ - buzzvm_lnum_assert(vm, 5); - buzzvm_lload(vm, 1); // fc - buzzvm_lload(vm, 2); // xbee - buzzvm_lload(vm, 3); // batt - buzzvm_lload(vm, 4); // gps - buzzvm_lload(vm, 5); // id - buzzvm_type_assert(vm, 5, BUZZTYPE_INT); - buzzvm_type_assert(vm, 4, BUZZTYPE_INT); - buzzvm_type_assert(vm, 3, BUZZTYPE_INT); - buzzvm_type_assert(vm, 2, BUZZTYPE_INT); - buzzvm_type_assert(vm, 1, BUZZTYPE_INT); - buzz_utility::neighbors_status newRS; - uint8_t id = buzzvm_stack_at(vm, 5)->i.value; - newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value; - newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value; - newRS.xbee= buzzvm_stack_at(vm, 2)->i.value; - newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value; - map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id); - if(it!=neighbors_status_map.end()) - neighbors_status_map.erase(it); - neighbors_status_map.insert(make_pair(id, newRS)); - return vm->state; - } - - mavros_msgs::Mavlink get_status(){ - mavros_msgs::Mavlink payload_out; - map< int, buzz_utility::neighbors_status >::iterator it; - for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++it){ - payload_out.payload64.push_back(it->first); - payload_out.payload64.push_back(it->second.gps_strenght); - payload_out.payload64.push_back(it->second.batt_lvl); - payload_out.payload64.push_back(it->second.xbee); - payload_out.payload64.push_back(it->second.flight_status); - } - /*Add Robot id and message number to the published message*/ - payload_out.sysid = (uint8_t)neighbors_status_map.size(); - /*payload_out.msgid = (uint32_t)message_number; - - message_number++;*/ - return payload_out; - } - /*----------------------------------------/ - / Buzz closure to take a picture here. - /----------------------------------------*/ - int buzzuav_takepicture(buzzvm_t vm) { - //cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; - buzz_cmd = COMMAND_PICTURE; - return buzzvm_ret0(vm); - } - - /*----------------------------------------/ - / Buzz closure to change locally the gimbal orientation - /----------------------------------------*/ - int buzzuav_setgimbal(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 4); - buzzvm_lload(vm, 1); // time - buzzvm_lload(vm, 2); // pitch - buzzvm_lload(vm, 3); // roll - buzzvm_lload(vm, 4); // yaw - buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); - buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - rc_gimbal[0] = buzzvm_stack_at(vm, 4)->f.value; - rc_gimbal[1] = buzzvm_stack_at(vm, 3)->f.value; - rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value; - rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value; - - ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)",rc_gimbal[0],rc_gimbal[1],rc_gimbal[2],rc_gimbal[3]); - buzz_cmd = COMMAND_GIMBAL; - return buzzvm_ret0(vm); - } - - /*----------------------------------------/ - / Buzz closure to store locally a GPS destination from the fleet - /----------------------------------------*/ - int buzzuav_storegoal(buzzvm_t vm) { - buzzvm_lnum_assert(vm, 3); - buzzvm_lload(vm, 1); // altitude - buzzvm_lload(vm, 2); // longitude - 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); - double goal[3]; - goal[0] = buzzvm_stack_at(vm, 3)->f.value; - goal[1] = buzzvm_stack_at(vm, 2)->f.value; - goal[2] = buzzvm_stack_at(vm, 1)->f.value; - if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){ - if(wplist_map.size()<=0) - parse_gpslist(); - goal[0] = wplist_map.begin()->second.latitude; - goal[1] = wplist_map.begin()->second.longitude; - goal[2] = wplist_map.begin()->second.altitude; - wplist_map.erase(wplist_map.begin()->first); - } - - double rb[3]; - - rb_from_gps(goal, rb, cur_pos); - if(fabs(rb[0])<150.0) - rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); - - ROS_WARN("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]); - 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 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_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; - } - - float* getgimbal() { - return rc_gimbal; - } - - string getuavstate() { - static buzzvm_t VM = buzz_utility::get_vm(); - std::stringstream state_buff; - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); - buzzvm_gload(VM); - buzzobj_t uav_state = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - return uav_state->s.value.str; - } - - int getcmd() { - return cur_cmd; - } - - void set_goto(double pos[]) { - goto_pos[0] = pos[0]; - goto_pos[1] = pos[1]; - goto_pos[2] = pos[2]; - - } - - int bzz_cmd() { - int cmd = buzz_cmd; - buzz_cmd = 0; - return cmd; - } - - void rc_set_goto(int id, double latitude, double longitude, double altitude) { - rc_id = id; - rc_goto_pos[0] = latitude; - rc_goto_pos[1] = longitude; - rc_goto_pos[2] = altitude; - - } - - void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) { - - rc_id = id; - rc_gimbal[0] = yaw; - rc_gimbal[1] = roll; - rc_gimbal[2] = pitch; - rc_gimbal[3] = t; - - } - - void rc_call(int rc_cmd_in) { - rc_cmd = rc_cmd_in; - } - - void set_obstacle_dist(float dist[]) { - for (int i = 0; i < 5; i++) - obst[i] = dist[i]; - } - - void set_battery(float voltage,float current,float remaining){ - batt[0]=voltage; - batt[1]=current; - batt[2]=remaining; - } - - int buzzuav_update_battery(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1)); - buzzvm_pushf(vm, batt[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); - buzzvm_pushf(vm, batt[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); - buzzvm_pushi(vm, (int)batt[2]); - buzzvm_tput(vm); - buzzvm_gstore(vm); - return vm->state; - } - - void set_deque_full(bool state) - { - deque_full = state; - } - - void set_rssi(float value) - { - rssi = round(value); - } - - void set_raw_packet_loss(float value) - { - raw_packet_loss = value; - } - - void set_filtered_packet_loss(float value) - { - filtered_packet_loss = round(100*value); - } - - void set_api_rssi(float value) - { - api_rssi = value; - } - - 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_pushi(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_pushi(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; - } - - /***************************************/ - /*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); - } - } - - /****************************************/ - int buzzuav_update_currentpos(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); - buzzvm_pushf(vm, cur_pos[0]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); - buzzvm_pushf(vm, cur_pos[1]); - buzzvm_tput(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); - buzzvm_pushf(vm, cur_pos[2]); - buzzvm_tput(vm); - buzzvm_gstore(vm); - return vm->state; - } - - void flight_status_update(uint8_t state){ - status=state; - } - - /*---------------------------------------------------- - / Create the generic robot table with status, remote controller current comand and destination - / and current position of the robot - / TODO: change global name for robot - /------------------------------------------------------*/ - int buzzuav_update_flight_status(buzzvm_t vm) { - buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1)); - buzzvm_pushi(vm, rc_cmd); - buzzvm_tput(vm); - buzzvm_dup(vm); - rc_cmd=0; - buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); - buzzvm_pushi(vm, status); - buzzvm_tput(vm); - buzzvm_gstore(vm); - //also set rc_controllers goto - buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); - buzzvm_pusht(vm); - buzzvm_dup(vm); - buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1)); - buzzvm_pushi(vm, rc_id); - buzzvm_tput(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 */ - /******************************************************/ - - int buzzuav_update_prox(buzzvm_t vm) { - - 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; - for(size_t i = 0; i < 4; ++i) { - /* Create table for i-th read */ - 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[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; - } - /* 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); - return vm->state; - } - - /**********************************************/ - /*Dummy closure for use during update testing */ - /**********************************************/ - - int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} - +void setWPlist(string path) +{ + WPlistname = path + "include/graphs/waypointlist.csv"; +} + +/*----------------------------------------/ +/ Compute GPS destination from current position and desired Range and Bearing +/----------------------------------------*/ + +void gps_from_rb(double range, double bearing, double out[3]) +{ + double lat = RAD2DEG(cur_pos[0]); + double lon = RAD2DEG(cur_pos[1]); + 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] = RAD2DEG(out[0]); + out[1] = RAD2DEG(out[1]); + 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] = constrainAngle(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 }; + +void parse_gpslist() +{ + // Open file: + ROS_INFO("WP list file: %s", WPlistname.c_str()); + std::ifstream fin(WPlistname.c_str()); // Open in text-mode. + + // Opening may fail, always check. + if (!fin) + { + ROS_ERROR("GPS list parser, could not open file."); + return; + } + + // Prepare a C-string buffer to be used when reading lines. + const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need. + char buffer[MAX_LINE_LENGTH] = {}; + const char* DELIMS = "\t ,"; // Tab, space or comma. + + // Read the file and load the data: + double lat, lon; + int alt, tilt, tid; + buzz_utility::RB_struct RB_arr; + // Read one line at a time. + while (fin.getline(buffer, MAX_LINE_LENGTH)) + { + // Extract the tokens: + tid = atoi(strtok(buffer, DELIMS)); + lon = atof(strtok(NULL, DELIMS)); + lat = atof(strtok(NULL, DELIMS)); + alt = atoi(strtok(NULL, DELIMS)); + tilt = atoi(strtok(NULL, DELIMS)); + // ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid); + RB_arr.latitude = lat; + RB_arr.longitude = lon; + RB_arr.altitude = alt; + // Insert elements. + map::iterator it = wplist_map.find(tid); + if (it != wplist_map.end()) + wplist_map.erase(it); + wplist_map.insert(make_pair(tid, RB_arr)); + } + + ROS_INFO("----->Saved %i waypoints.", wplist_map.size()); + + // Close the file: + fin.close(); +} + +/*----------------------------------------/ +/ Buzz closure to move following a 2D vector +/----------------------------------------*/ +int buzzuav_moveto(buzzvm_t vm) +{ + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); /* dx */ + buzzvm_lload(vm, 2); /* dy */ + buzzvm_lload(vm, 3); /* dheight */ + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + float dh = buzzvm_stack_at(vm, 1)->f.value; + float dy = buzzvm_stack_at(vm, 2)->f.value; + float dx = buzzvm_stack_at(vm, 3)->f.value; + goto_pos[0] = dx; + goto_pos[1] = dy; + goto_pos[2] = height + dh; + // 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 buzzuav_update_targets(buzzvm_t vm) +{ + if (vm->state != BUZZVM_STATE_READY) + return vm->state; + buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1)); + buzzvm_pusht(vm); + buzzobj_t targettbl = buzzvm_stack_at(vm, 1); + double rb[3], tmp[3]; + map::iterator it; + for (it = targets_map.begin(); it != targets_map.end(); ++it) + { + tmp[0] = (it->second).latitude; + tmp[1] = (it->second).longitude; + tmp[2] = height; + rb_from_gps(tmp, rb, cur_pos); + // ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]); + buzzvm_push(vm, targettbl); + // When we get here, the "targets" table is on top of the stack + // ROS_INFO("Buzz_utility will save user %i.", it->first); + // Push user id + buzzvm_pushi(vm, it->first); + // Create entry table + buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); + // Insert range + buzzvm_push(vm, entry); + buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1)); + buzzvm_pushf(vm, rb[0]); + buzzvm_tput(vm); + // Insert longitude + buzzvm_push(vm, entry); + buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1)); + buzzvm_pushf(vm, rb[1]); + buzzvm_tput(vm); + // Save entry into data table + buzzvm_push(vm, entry); + buzzvm_tput(vm); + } + buzzvm_gstore(vm); + + return vm->state; +} + +int buzzuav_addtargetRB(buzzvm_t vm) +{ + 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]); + buzz_utility::RB_struct RB_arr; + RB_arr.latitude = tmp[0]; + RB_arr.longitude = tmp[1]; + RB_arr.altitude = tmp[2]; + RB_arr.r = rb[0]; + RB_arr.b = rb[1]; + map::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 + ROS_WARN(" ---------- Target too far %f", rb[0]); + + return 0; +} + +int buzzuav_addNeiStatus(buzzvm_t vm) +{ + buzzvm_lnum_assert(vm, 5); + buzzvm_lload(vm, 1); // fc + buzzvm_lload(vm, 2); // xbee + buzzvm_lload(vm, 3); // batt + buzzvm_lload(vm, 4); // gps + buzzvm_lload(vm, 5); // id + buzzvm_type_assert(vm, 5, BUZZTYPE_INT); + buzzvm_type_assert(vm, 4, BUZZTYPE_INT); + buzzvm_type_assert(vm, 3, BUZZTYPE_INT); + buzzvm_type_assert(vm, 2, BUZZTYPE_INT); + buzzvm_type_assert(vm, 1, BUZZTYPE_INT); + buzz_utility::neighbors_status newRS; + uint8_t id = buzzvm_stack_at(vm, 5)->i.value; + newRS.gps_strenght = buzzvm_stack_at(vm, 4)->i.value; + newRS.batt_lvl = buzzvm_stack_at(vm, 3)->i.value; + newRS.xbee = buzzvm_stack_at(vm, 2)->i.value; + newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value; + map::iterator it = neighbors_status_map.find(id); + if (it != neighbors_status_map.end()) + neighbors_status_map.erase(it); + neighbors_status_map.insert(make_pair(id, newRS)); + return vm->state; +} + +mavros_msgs::Mavlink get_status() +{ + mavros_msgs::Mavlink payload_out; + map::iterator it; + for (it = neighbors_status_map.begin(); it != neighbors_status_map.end(); ++it) + { + payload_out.payload64.push_back(it->first); + payload_out.payload64.push_back(it->second.gps_strenght); + payload_out.payload64.push_back(it->second.batt_lvl); + payload_out.payload64.push_back(it->second.xbee); + payload_out.payload64.push_back(it->second.flight_status); + } + /*Add Robot id and message number to the published message*/ + payload_out.sysid = (uint8_t)neighbors_status_map.size(); + /*payload_out.msgid = (uint32_t)message_number; + + message_number++;*/ + return payload_out; +} +/*----------------------------------------/ +/ Buzz closure to take a picture here. +/----------------------------------------*/ +int buzzuav_takepicture(buzzvm_t vm) +{ + // cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; + buzz_cmd = COMMAND_PICTURE; + return buzzvm_ret0(vm); +} + +/*----------------------------------------/ +/ Buzz closure to change locally the gimbal orientation +/----------------------------------------*/ +int buzzuav_setgimbal(buzzvm_t vm) +{ + buzzvm_lnum_assert(vm, 4); + buzzvm_lload(vm, 1); // time + buzzvm_lload(vm, 2); // pitch + buzzvm_lload(vm, 3); // roll + buzzvm_lload(vm, 4); // yaw + buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); + buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); + rc_gimbal[0] = buzzvm_stack_at(vm, 4)->f.value; + rc_gimbal[1] = buzzvm_stack_at(vm, 3)->f.value; + rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value; + rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value; + + ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]); + buzz_cmd = COMMAND_GIMBAL; + return buzzvm_ret0(vm); +} + +/*----------------------------------------/ +/ Buzz closure to store locally a GPS destination from the fleet +/----------------------------------------*/ +int buzzuav_storegoal(buzzvm_t vm) +{ + buzzvm_lnum_assert(vm, 3); + buzzvm_lload(vm, 1); // altitude + buzzvm_lload(vm, 2); // longitude + 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); + double goal[3]; + goal[0] = buzzvm_stack_at(vm, 3)->f.value; + goal[1] = buzzvm_stack_at(vm, 2)->f.value; + goal[2] = buzzvm_stack_at(vm, 1)->f.value; + if (goal[0] == -1 && goal[1] == -1 && goal[2] == -1) + { + if (wplist_map.size() <= 0) + parse_gpslist(); + goal[0] = wplist_map.begin()->second.latitude; + goal[1] = wplist_map.begin()->second.longitude; + goal[2] = wplist_map.begin()->second.altitude; + wplist_map.erase(wplist_map.begin()->first); + } + + double rb[3]; + + rb_from_gps(goal, rb, cur_pos); + if (fabs(rb[0]) < 150.0) + rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); + + ROS_WARN("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); + 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 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_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; +} + +float* getgimbal() +{ + return rc_gimbal; +} + +string getuavstate() +{ + static buzzvm_t VM = buzz_utility::get_vm(); + std::stringstream state_buff; + buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_gload(VM); + buzzobj_t uav_state = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + return uav_state->s.value.str; +} + +int getcmd() +{ + return cur_cmd; +} + +void set_goto(double pos[]) +{ + goto_pos[0] = pos[0]; + goto_pos[1] = pos[1]; + goto_pos[2] = pos[2]; +} + +int bzz_cmd() +{ + int cmd = buzz_cmd; + buzz_cmd = 0; + return cmd; +} + +void rc_set_goto(int id, double latitude, double longitude, double altitude) +{ + rc_id = id; + rc_goto_pos[0] = latitude; + rc_goto_pos[1] = longitude; + rc_goto_pos[2] = altitude; +} + +void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) +{ + rc_id = id; + rc_gimbal[0] = yaw; + rc_gimbal[1] = roll; + rc_gimbal[2] = pitch; + rc_gimbal[3] = t; +} + +void rc_call(int rc_cmd_in) +{ + rc_cmd = rc_cmd_in; +} + +void set_obstacle_dist(float dist[]) +{ + for (int i = 0; i < 5; i++) + obst[i] = dist[i]; +} + +void set_battery(float voltage, float current, float remaining) +{ + batt[0] = voltage; + batt[1] = current; + batt[2] = remaining; +} + +int buzzuav_update_battery(buzzvm_t vm) +{ + buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1)); + buzzvm_pushf(vm, batt[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); + buzzvm_pushf(vm, batt[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); + buzzvm_pushi(vm, (int)batt[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} + +void set_deque_full(bool state) +{ + deque_full = state; +} + +void set_rssi(float value) +{ + rssi = round(value); +} + +void set_raw_packet_loss(float value) +{ + raw_packet_loss = value; +} + +void set_filtered_packet_loss(float value) +{ + filtered_packet_loss = round(100 * value); +} + +void set_api_rssi(float value) +{ + api_rssi = value; +} + +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_pushi(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_pushi(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; +} + +/***************************************/ +/*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::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::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; +} + +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, "id", 1)); + buzzvm_pushi(vm, rc_id); + buzzvm_tput(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 */ +/******************************************************/ + +int buzzuav_update_prox(buzzvm_t vm) +{ + 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; + for (size_t i = 0; i < 4; ++i) + { + /* Create table for i-th read */ + 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[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; + } + /* 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); + return vm->state; +} + +/**********************************************/ +/*Dummy closure for use during update testing */ +/**********************************************/ + +int dummy_closure(buzzvm_t vm) +{ + return buzzvm_ret0(vm); +} } diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index 4481734..195d672 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -1,7 +1,7 @@ /** @file rosbuzz.cpp - * @version 1.0 + * @version 1.0 * @date 27.09.2016 - * @brief Buzz Implementation as a node in ROS. + * @brief Buzz Implementation as a node in ROS. * @author Vivek Shankar Varadharajan and David St-Onge * @copyright 2016 MistLab. All rights reserved. */ @@ -11,18 +11,16 @@ /** * This program implements Buzz node in ros using mavros_msgs. */ - -int main(int argc, char **argv) + +int main(int argc, char** argv) { /*Initialize rosBuzz node*/ - ros::init(argc, argv, "rosBuzz"); + ros::init(argc, argv, "rosBuzz"); ros::NodeHandle nh_priv("~"); ros::NodeHandle nh; rosbzz_node::roscontroller RosController(nh, nh_priv); /*Register ros controller object inside buzz*/ - //buzzuav_closures::set_ros_controller_ptr(&RosController); - RosController.RosControllerRun(); + // buzzuav_closures::set_ros_controller_ptr(&RosController); + RosController.RosControllerRun(); return 0; } - - diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c388d0d..25d2bf9 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -5,18 +5,18 @@ * @author Vivek Shankar Varadharajan and David St-Onge * @copyright 2016 MistLab. All rights reserved. */ - + #include "roscontroller.h" #include -namespace rosbzz_node { - +namespace rosbzz_node +{ const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; const bool debug = false; /*--------------- /Constructor ---------------*/ -roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv) +roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv) { ROS_INFO("Buzz_node"); /*Obtain parameters from ros parameter server*/ @@ -45,15 +45,19 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv) cur_pos.latitude = 0; cur_pos.altitude = 0; - while (cur_pos.latitude == 0.0f) { + while (cur_pos.latitude == 0.0f) + { ROS_INFO("Waiting for GPS. "); ros::Duration(0.5).sleep(); ros::spinOnce(); } - if (xbeeplugged) { + if (xbeeplugged) + { GetRobotId(); - } else { + } + else + { robot_id = strtol(robot_name.c_str() + 5, NULL, 10); } } @@ -85,25 +89,31 @@ void roscontroller::GetRobotId() robot_id = robot_id_srv_response.value.integer; } -bool roscontroller::GetDequeFull(bool &result) +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;} + 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) +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;} + if (!xbeestatus_srv.call(srv_request, srv_response)) + { + return false; + } result = srv_response.value.real; return srv_response.success; @@ -112,7 +122,7 @@ bool roscontroller::GetRssi(float &result) bool roscontroller::TriggerAPIRssi(const uint8_t short_id) { mavros_msgs::ParamGet::Request srv_request; - if(short_id == 0xFF) + if (short_id == 0xFF) { srv_request.param_id = "trig_rssi_api_avg"; } @@ -121,15 +131,18 @@ 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; } -bool roscontroller::GetAPIRssi(const uint8_t short_id, float &result) +bool roscontroller::GetAPIRssi(const uint8_t short_id, float& result) { mavros_msgs::ParamGet::Request srv_request; - if(short_id == 0xFF) + if (short_id == 0xFF) { srv_request.param_id = "get_rssi_api_avg"; } @@ -138,16 +151,19 @@ 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; } -bool roscontroller::GetRawPacketLoss(const uint8_t short_id, float &result) +bool roscontroller::GetRawPacketLoss(const uint8_t short_id, float& result) { mavros_msgs::ParamGet::Request srv_request; - if(short_id == 0xFF) + if (short_id == 0xFF) { srv_request.param_id = "pl_raw_avg"; } @@ -156,16 +172,19 @@ 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; } -bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float &result) +bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float& result) { mavros_msgs::ParamGet::Request srv_request; - if(short_id == 0xFF) + if (short_id == 0xFF) { srv_request.param_id = "pl_filtered_avg"; } @@ -174,14 +193,18 @@ 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; } -void roscontroller::send_MPpayload(){ - MPpayload_pub.publish(buzzuav_closures::get_status()); +void roscontroller::send_MPpayload() +{ + MPpayload_pub.publish(buzzuav_closures::get_status()); } /*------------------------------------------------- @@ -189,41 +212,46 @@ void roscontroller::send_MPpayload(){ /--------------------------------------------------*/ void roscontroller::RosControllerRun() { - /* Set the Buzz bytecode */ - if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), - robot_id)) { + if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id)) + { ROS_INFO("[%i] INIT DONE!!!", robot_id); - int packet_loss_tmp,time_step=0; - double cur_packet_loss=0; + int packet_loss_tmp, time_step = 0; + double cur_packet_loss = 0; ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; /*Intialize the update monitor*/ - init_update_monitor(bcfname.c_str(), standby_bo.c_str(),dbgfname.c_str(),robot_id); + init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); /*loop rate of ros*/ ros::Rate loop_rate(BUZZRATE); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// - //ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); - while (ros::ok() && !buzz_utility::buzz_script_done()) { + // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); + while (ros::ok() && !buzz_utility::buzz_script_done()) + { /*Neighbours of the robot published with id in respective topic*/ neighbours_pos_publisher(); send_MPpayload(); /*Check updater state and step code*/ update_routine(); - if(time_step==BUZZRATE){ - time_step=0; - cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) ); - packet_loss_tmp=0; - if(cur_packet_loss<0) cur_packet_loss=0; - else if (cur_packet_loss>1) cur_packet_loss=1; + if (time_step == BUZZRATE) + { + time_step = 0; + cur_packet_loss = 1 - ((double)packet_loss_tmp / (BUZZRATE * ((int)no_of_robots - 1))); + packet_loss_tmp = 0; + if (cur_packet_loss < 0) + cur_packet_loss = 0; + else if (cur_packet_loss > 1) + cur_packet_loss = 1; } - else{ - packet_loss_tmp+=(int)buzz_utility::get_inmsg_size(); - time_step++; + else + { + packet_loss_tmp += (int)buzz_utility::get_inmsg_size(); + time_step++; } - if(debug) ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss); + if (debug) + ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); /*Step buzz script */ buzz_utility::buzz_script_step(); /*Prepare messages and publish them in respective topic*/ @@ -233,19 +261,20 @@ void roscontroller::RosControllerRun() /*Set multi message available after update*/ if (get_update_status()) { - set_read_update_status(); + set_read_update_status(); } /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ get_number_of_robots(); buzz_utility::set_robot_var(no_of_robots); updates_set_robots(no_of_robots); - //get_xbee_status(); // commented out because it may slow down the node too much, to be tested + // get_xbee_status(); // commented out because it may slow down the node too much, to be tested /*run once*/ ros::spinOnce(); loop_rate.sleep(); // make sure to sleep for the remainder of our cycle time if (loop_rate.cycleTime() > ros::Duration(1.0 / (float)BUZZRATE)) - ROS_WARN("ROSBuzz loop missed its desired rate of %iHz... the loop actually took %.4f seconds", BUZZRATE, loop_rate.cycleTime().toSec()); + ROS_WARN("ROSBuzz loop missed its desired rate of %iHz... the loop actually took %.4f seconds", BUZZRATE, + loop_rate.cycleTime().toSec()); if (fcu_timeout <= 0) buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); else @@ -261,9 +290,8 @@ void roscontroller::RosControllerRun() /*-------------------------------------------------------- / Get all required parameters from the ROS launch file /-------------------------------------------------------*/ -void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) +void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) { - /*Obtain .bzz file name from parameter server*/ if (n_c.getParam("bzzfile_name", bzzfile_name)) ; @@ -275,18 +303,21 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) /*Obtain rc service option from parameter server*/ if (n_c.getParam("rcclient", rcclient)) { - if (rcclient == true) { + 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) + } + else if (rcclient == false) { ROS_INFO("RC service is disabled"); } - } else + } + else { ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node"); @@ -305,7 +336,8 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node"); } - if (!xbeeplugged) { + if (!xbeeplugged) + { if (n_c.getParam("name", robot_name)) ; else @@ -313,10 +345,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node"); } - } else + } + else n_c.getParam("xbee_status_srv", xbeesrv_name); - if(!n_c.getParam("capture_image_srv", capture_srv_name)) + if (!n_c.getParam("capture_image_srv", capture_srv_name)) { capture_srv_name = CAPTURE_SRV_DEFAULT_NAME; } @@ -328,7 +361,7 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c) /Obtains publisher, subscriber and services from yml file based on the robot used /-----------------------------------------------------------------------------------*/ -void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle) +void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) { m_sMySubscriptions.clear(); std::string gps_topic, gps_type; @@ -405,7 +438,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle) /*-------------------------------------------------------- / Create all required subscribers, publishers and ROS service clients /-------------------------------------------------------*/ -void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c) +void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) { /*subscribers*/ @@ -439,20 +472,28 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c) /*--------------------------------------- /Robot independent subscribers /--------------------------------------*/ -void roscontroller::Subscribe(ros::NodeHandle &n_c) +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") { + for (std::map::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it) + { + if (it->second == "mavros_msgs/ExtendedState") + { flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this); - } else if (it->second == "mavros_msgs/State") { + } + else if (it->second == "mavros_msgs/State") + { flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this); - } else if (it->second == "mavros_msgs/BatteryStatus") { + } + else if (it->second == "mavros_msgs/BatteryStatus") + { battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this); - } else if (it->second == "sensor_msgs/NavSatFix") { + } + else if (it->second == "sensor_msgs/NavSatFix") + { current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this); - } else if (it->second == "std_msgs/Float64") { + } + else if (it->second == "std_msgs/Float64") + { relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this); } @@ -467,12 +508,10 @@ std::string roscontroller::Compile_bzz(std::string bzzfile_name) { /*Compile the buzz code .bzz to .bo*/ stringstream bzzfile_in_compile; - std::string path = - bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); name = name.substr(0, name.find_last_of(".")); - bzzfile_in_compile << "bzzc -I " << path - << "include/"; + bzzfile_in_compile << "bzzc -I " << path << "include/"; bzzfile_in_compile << " -b " << path << name << ".bo"; bzzfile_in_compile << " -d " << path << name << ".bdb "; bzzfile_in_compile << bzzfile_name; @@ -490,17 +529,17 @@ void roscontroller::neighbours_pos_publisher() { auto current_time = ros::Time::now(); map::iterator it; - rosbuzz::neigh_pos neigh_pos_array; // neigh_pos_array.clear(); + 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) { + 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.position_covariance_type = it->first; // custom robot id storage neigh_tmp.latitude = (it->second).x; neigh_tmp.longitude = (it->second).y; neigh_tmp.altitude = (it->second).z; @@ -517,12 +556,15 @@ void roscontroller::Arm() { mavros_msgs::CommandBool arming_message; arming_message.request.value = armstate; - if (arm_client.call(arming_message)) { + if (arm_client.call(arming_message)) + { if (arming_message.response.success == 1) ROS_WARN("FC Arm Service called!"); else ROS_WARN("FC Arm Service call failed!"); - } else { + } + else + { ROS_WARN("FC Arm Service call failed!"); } } @@ -546,7 +588,7 @@ with size......... | / void roscontroller::prepare_msg_and_publish() { /*obtain Pay load to be sent*/ - uint64_t *payload_out_ptr = buzz_utility::obt_out_msg(); + uint64_t* payload_out_ptr = buzz_utility::obt_out_msg(); uint64_t position[3]; /*Appened current position to message*/ double tmp[3]; @@ -560,8 +602,9 @@ void roscontroller::prepare_msg_and_publish() payload_out.payload64.push_back(position[1]); payload_out.payload64.push_back(position[2]); /*Append Buzz message*/ - uint16_t *out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]); - for (int i = 0; i < out[0]; i++) { + uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]); + for (int i = 0; i < out[0]; i++) + { payload_out.payload64.push_back(payload_out_ptr[i]); } /*Add Robot id and message number to the published message*/ @@ -579,32 +622,32 @@ void roscontroller::prepare_msg_and_publish() /*Check for updater message if present send*/ if (is_msg_present()) { - uint8_t *buff_send = 0; - uint16_t updater_msgSize = *(uint16_t *)(getupdate_out_msg_size()); + uint8_t* buff_send = 0; + uint16_t updater_msgSize = *(uint16_t*)(getupdate_out_msg_size()); ; int tot = 0; mavros_msgs::Mavlink update_packets; fprintf(stdout, "Appending code into message ...\n"); fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize); // allocate mem and clear it - buff_send = (uint8_t *)malloc(sizeof(uint16_t) + updater_msgSize); + buff_send = (uint8_t*)malloc(sizeof(uint16_t) + updater_msgSize); memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize); // Append updater msg size - *(uint16_t *)(buff_send + tot) = updater_msgSize; + *(uint16_t*)(buff_send + tot) = updater_msgSize; tot += sizeof(uint16_t); // Append updater msgs - memcpy(buff_send + tot, (uint8_t *)(getupdater_out_msg()), updater_msgSize); + memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); tot += updater_msgSize; // Destroy the updater out msg queue destroy_out_msg_queue(); uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t))); - uint64_t *payload_64 = new uint64_t[total_size]; - memcpy((void *)payload_64, (void *)buff_send, - total_size * sizeof(uint64_t)); + uint64_t* payload_64 = new uint64_t[total_size]; + memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); free(buff_send); // Send a constant number to differenciate updater msgs update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT); - for (int i = 0; i < total_size; i++) { + for (int i = 0; i < total_size; i++) + { update_packets.payload64.push_back(payload_64[i]); } // Add Robot id and message number to the published message @@ -615,7 +658,7 @@ void roscontroller::prepare_msg_and_publish() update_packets.sysid = (uint8_t)robot_id; update_packets.msgid = (uint32_t)message_number; payload_pub.publish(update_packets); - //multi_msg = false; + // multi_msg = false; delete[] payload_64; } } @@ -627,15 +670,16 @@ script void roscontroller::flight_controller_service_call() { /* flight controller client call if requested from Buzz*/ - double *goto_pos; - float *gimbal; - switch (buzzuav_closures::bzz_cmd()) { - + double* goto_pos; + float* gimbal; + switch (buzzuav_closures::bzz_cmd()) + { case buzzuav_closures::COMMAND_TAKEOFF: goto_pos = buzzuav_closures::getgoto(); cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); - if (!armstate) { + if (!armstate) + { SetMode("LOITER", 0); armstate = 1; Arm(); @@ -644,59 +688,72 @@ void roscontroller::flight_controller_service_call() home = cur_pos; } if (current_mode != "GUIDED") - SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it - // should always be in loiter after arm/disarm) - if (mav_client.call(cmd_srv)) { + SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it + // should always be in loiter after arm/disarm) + if (mav_client.call(cmd_srv)) + { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else + } + else ROS_ERROR("Failed to call service from flight controller"); break; case buzzuav_closures::COMMAND_LAND: cmd_srv.request.command = buzzuav_closures::getcmd(); - if (current_mode != "LAND") { + if (current_mode != "LAND") + { SetMode("LAND", 0); armstate = 0; Arm(); } - if (mav_client.call(cmd_srv)) { + if (mav_client.call(cmd_srv)) + { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else { + } + else + { ROS_ERROR("Failed to call service from flight controller"); } armstate = 0; break; - case buzzuav_closures::COMMAND_GOHOME: // NOT FULLY IMPLEMENTED/TESTED !!! - cmd_srv.request.param5 = home.latitude; - cmd_srv.request.param6 = home.longitude; - cmd_srv.request.param7 = home.altitude; - cmd_srv.request.command = buzzuav_closures::getcmd(); - if (mav_client.call(cmd_srv)) { - ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else - ROS_ERROR("Failed to call service from flight controller"); - break; + case buzzuav_closures::COMMAND_GOHOME: // NOT FULLY IMPLEMENTED/TESTED !!! + cmd_srv.request.param5 = home.latitude; + cmd_srv.request.param6 = home.longitude; + cmd_srv.request.param7 = home.altitude; + cmd_srv.request.command = buzzuav_closures::getcmd(); + if (mav_client.call(cmd_srv)) + { + ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); + } + else + ROS_ERROR("Failed to call service from flight controller"); + break; - case buzzuav_closures::COMMAND_GOTO: // NOT FULLY IMPLEMENTED/TESTED !!! + case buzzuav_closures::COMMAND_GOTO: // NOT FULLY IMPLEMENTED/TESTED !!! goto_pos = buzzuav_closures::getgoto(); cmd_srv.request.param5 = goto_pos[0]; cmd_srv.request.param6 = goto_pos[1]; cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.command = buzzuav_closures::getcmd(); - if (mav_client.call(cmd_srv)) { + if (mav_client.call(cmd_srv)) + { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else + } + else ROS_ERROR("Failed to call service from flight controller"); cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; - if (mav_client.call(cmd_srv)) { + if (mav_client.call(cmd_srv)) + { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else + } + else ROS_ERROR("Failed to call service from flight controller"); break; case buzzuav_closures::COMMAND_ARM: - if (!armstate) { + if (!armstate) + { SetMode("LOITER", 0); armstate = 1; Arm(); @@ -704,7 +761,8 @@ void roscontroller::flight_controller_service_call() break; case buzzuav_closures::COMMAND_DISARM: - if (armstate) { + if (armstate) + { armstate = 0; SetMode("LOITER", 0); Arm(); @@ -723,18 +781,22 @@ void roscontroller::flight_controller_service_call() cmd_srv.request.param3 = gimbal[2]; cmd_srv.request.param4 = gimbal[3]; cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; - if (mav_client.call(cmd_srv)) { + if (mav_client.call(cmd_srv)) + { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); - } else + } + else ROS_ERROR("Failed to call service from flight controller"); break; case buzzuav_closures::COMMAND_PICTURE: ROS_INFO("TAKING A PICTURE HERE!! --------------"); mavros_msgs::CommandBool capture_command; - if (capture_srv.call(capture_command)) { + if (capture_srv.call(capture_command)) + { ROS_INFO("Reply: %ld", (long int)capture_command.response.success); - } else + } + else ROS_ERROR("Failed to call service from camera streamer"); break; } @@ -743,8 +805,10 @@ void roscontroller::flight_controller_service_call() /*---------------------------------------------- /Refresh neighbours Position for every ten step /---------------------------------------------*/ -void roscontroller::maintain_pos(int tim_step) { - if (timer_step >= BUZZRATE) { +void roscontroller::maintain_pos(int tim_step) +{ + if (timer_step >= BUZZRATE) + { neighbours_pos_map.clear(); // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // have to clear ! @@ -756,8 +820,8 @@ void roscontroller::maintain_pos(int tim_step) { /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) { +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); @@ -766,10 +830,9 @@ void roscontroller::neighbours_pos_put(int id, /*----------------------------------------------------------------------------------- /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); +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)); @@ -778,8 +841,8 @@ void roscontroller::raw_neighbours_pos_put(int id, /*-------------------------------------------------------- /Set the current position of the robot callback /--------------------------------------------------------*/ -void roscontroller::set_cur_pos(double latitude, double longitude, - double altitude) { +void roscontroller::set_cur_pos(double latitude, double longitude, double altitude) +{ cur_pos.latitude = latitude; cur_pos.longitude = longitude; cur_pos.altitude = altitude; @@ -789,37 +852,39 @@ void roscontroller::set_cur_pos(double latitude, double longitude, / Compute Range and Bearing of a neighbor in a local reference frame / from GPS coordinates ----------------------------------------------------------- */ -float roscontroller::constrainAngle(float x) { - x = fmod(x,2*M_PI); +float roscontroller::constrainAngle(float x) +{ + x = fmod(x, 2 * M_PI); if (x < 0.0) - x += 2*M_PI; + x += 2 * M_PI; return x; - } +} -void roscontroller::gps_rb(GPS nei_pos, double out[]) { +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] = constrainAngle(atan2(ned_y, ned_x)); // out[1] = std::floor(out[1] * 1000000) / 1000000; out[2] = 0.0; } -void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t) { - gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, - cur_pos.latitude); +void roscontroller::gps_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_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) { +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; @@ -829,7 +894,8 @@ void roscontroller::gps_convert_ned(float &ned_x, float &ned_y, /*------------------------------------------------------ / Update battery status into BVM from subscriber /------------------------------------------------------*/ -void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr &msg) { +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); @@ -838,8 +904,8 @@ void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr &msg) { /*---------------------------------------------------------------------- /Update flight extended status into BVM from subscriber for solos /---------------------------------------------------------------------*/ -void roscontroller::flight_status_update( - const mavros_msgs::State::ConstPtr &msg) { +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; @@ -847,21 +913,22 @@ void roscontroller::flight_status_update( 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); //? + 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) { +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) { +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; @@ -871,14 +938,12 @@ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr &msg) { 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); + 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) { +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; @@ -887,21 +952,24 @@ void roscontroller::local_pos_callback( /*------------------------------------------------------------- / Update altitude into BVM from subscriber /-------------------------------------------------------------*/ -void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr &msg) { +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) { +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) { +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 @@ -910,7 +978,7 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) { moveMsg.header.seq = setpoint_counter++; moveMsg.header.frame_id = 1; - //ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y); + // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y); moveMsg.pose.position.x = local_pos_new[0] + y; moveMsg.pose.position.y = local_pos_new[1] + x; moveMsg.pose.position.z = z; @@ -927,9 +995,11 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) { // } } -void roscontroller::SetMode(std::string mode, int delay_miliseconds) { +void roscontroller::SetMode(std::string mode, int delay_miliseconds) +{ // wait if necessary - if (delay_miliseconds != 0) { + if (delay_miliseconds != 0) + { std::this_thread::sleep_for(std::chrono::milliseconds(delay_miliseconds)); } // set mode @@ -937,24 +1007,29 @@ void roscontroller::SetMode(std::string mode, int delay_miliseconds) { 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 { + 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) { +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)) { + 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"); + // ROS_INFO("Set stream rate call successful"); } /*------------------------------------------------------------- @@ -972,41 +1047,45 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off) { /*|_____|_____|_____|________________________________________________|______________________________| */ /*******************************************************************************************************/ -void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { +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) { + if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT) + { uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size()); uint64_t message_obt[obt_msg_size]; /* Go throught the obtained payload*/ - for (int i = 0; i < (int)msg->payload64.size(); i++) { + 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); + 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); + memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); + uint16_t unMsgSize = *(uint16_t*)(pl); fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); - if (unMsgSize > 0) { - code_message_inqueue_append((uint8_t *)(pl + sizeof(uint16_t)), - unMsgSize); + if (unMsgSize > 0) + { + code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); code_message_inqueue_process(); } free(pl); } /*BVM FIFO message*/ - else if (msg->payload64[0]==(uint64_t)XBEE_MESSAGE_CONSTANT) { - uint64_t message_obt[msg->payload64.size()-1]; + else if (msg->payload64[0] == (uint64_t)XBEE_MESSAGE_CONSTANT) + { + uint64_t message_obt[msg->payload64.size() - 1]; /* Go throught the obtained payload*/ - for (int i = 1; i < (int)msg->payload64.size(); i++) { - message_obt[i-1] = (uint64_t)msg->payload64[i]; + for (int i = 1; i < (int)msg->payload64.size(); i++) + { + message_obt[i - 1] = (uint64_t)msg->payload64[i]; } /* Extract neighbours position from payload*/ double neighbours_pos_payload[3]; memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); - buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0], - neighbours_pos_payload[1], + buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0], neighbours_pos_payload[1], neighbours_pos_payload[2]); GPS nei_pos; nei_pos.latitude = neighbours_pos_payload[0]; @@ -1015,18 +1094,17 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { double cvt_neighbours_pos_payload[3]; gps_rb(nei_pos, cvt_neighbours_pos_payload); /*Extract robot id of the neighbour*/ - uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); - if(debug) ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); + uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); + if (debug) + ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); /*pass neighbour position to local maintaner*/ - buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], - cvt_neighbours_pos_payload[1], + buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1], cvt_neighbours_pos_payload[2]); /*Put RID and pos*/ raw_neighbours_pos_put((int)out[1], raw_neigh_pos); /* TODO: remove roscontroller local map array for neighbors */ neighbours_pos_put((int)out[1], n_pos); - buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, - n_pos.z); + buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z); delete[] out; buzz_utility::in_msg_append((message_obt + 3)); } @@ -1036,10 +1114,11 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) { / Catch the ROS service call from a custom remote controller (Mission Planner) / and send the requested commands to Buzz ----------------------------------------------------------- */ -bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, - mavros_msgs::CommandLong::Response &res) { +bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res) +{ int rc_cmd; - switch (req.command) { + switch (req.command) + { case mavros_msgs::CommandCode::NAV_TAKEOFF: ROS_INFO("RC_call: TAKE OFF!!!!"); rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; @@ -1055,11 +1134,14 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM: rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; armstate = req.param1; - if (armstate) { + if (armstate) + { ROS_INFO("RC_Call: ARM!!!!"); buzzuav_closures::rc_call(rc_cmd); res.success = true; - } else { + } + else + { ROS_INFO("RC_Call: DISARM!!!!"); buzzuav_closures::rc_call(rc_cmd + 1); res.success = true; @@ -1073,20 +1155,20 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, break; case mavros_msgs::CommandCode::NAV_WAYPOINT: ROS_INFO("RC_Call: GO TO!!!! "); - buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7); + buzzuav_closures::rc_set_goto(req.param1, req.param5, req.param6, req.param7); rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: ROS_INFO("RC_Call: Gimbal!!!! "); - buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3,req.param4,req.param5); + buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; buzzuav_closures::rc_call(rc_cmd); res.success = true; break; case CMD_REQUEST_UPDATE: - //ROS_INFO("RC_Call: Update Fleet Status!!!!"); + // ROS_INFO("RC_Call: Update Fleet Status!!!!"); rc_cmd = CMD_REQUEST_UPDATE; buzzuav_closures::rc_call(rc_cmd); res.success = true; @@ -1100,23 +1182,31 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, return true; } -void roscontroller::get_number_of_robots() { +void roscontroller::get_number_of_robots() +{ int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1; - if (no_of_robots == 0) { + if (no_of_robots == 0) + { no_of_robots = cur_robots; - - } else { - if (no_of_robots != cur_robots && no_cnt == 0) { + } + 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) { + } + else if (no_cnt != 0 && old_val == cur_robots) + { no_cnt++; - if (no_cnt >= 150 || cur_robots > no_of_robots) { + if (no_cnt >= 150 || cur_robots > no_of_robots) + { no_of_robots = cur_robots; no_cnt = 0; } - } else { + } + else + { no_cnt = 0; } } @@ -1130,19 +1220,19 @@ void roscontroller::get_xbee_status() bool result_bool; float result_float; const uint8_t all_ids = 0xFF; - if(GetDequeFull(result_bool)) + if (GetDequeFull(result_bool)) { buzzuav_closures::set_deque_full(result_bool); } - if(GetRssi(result_float)) + if (GetRssi(result_float)) { buzzuav_closures::set_rssi(result_float); } - if(GetRawPacketLoss(all_ids, result_float)) + if (GetRawPacketLoss(all_ids, result_float)) { buzzuav_closures::set_raw_packet_loss(result_float); } - if(GetFilteredPacketLoss(all_ids, result_float)) + if (GetFilteredPacketLoss(all_ids, result_float)) { buzzuav_closures::set_filtered_packet_loss(result_float); } @@ -1155,5 +1245,4 @@ void roscontroller::get_xbee_status() * TriggerAPIRssi(all_ids); */ } - } diff --git a/src/uav_utility.cpp b/src/uav_utility.cpp index b116e92..98c6eeb 100644 --- a/src/uav_utility.cpp +++ b/src/uav_utility.cpp @@ -8,17 +8,17 @@ /*To do !*/ /****************************************/ -void uav_setup() { - +void uav_setup() +{ } /****************************************/ /*To do !*/ /****************************************/ -void uav_done() { - - fprintf(stdout, "Robot stopped.\n"); +void uav_done() +{ + fprintf(stdout, "Robot stopped.\n"); } /****************************************/