Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

beautified

This commit is contained in:
dave 2017-12-08 18:53:32 -05:00
parent 72e51f3c82
commit 74f7f740e0
11 changed files with 2384 additions and 2182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
include/buzz_update.h
View File

@ -10,7 +10,12 @@
#include <buzz/buzzdarray.h> #include <buzz/buzzdarray.h>
#include <buzz/buzzvstig.h> #include <buzz/buzzvstig.h>
#include <fstream> #include <fstream>
#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 CODE_REQUEST_PADDING = 250;
static const uint16_t MIN_UPDATE_PACKET = 251; static const uint16_t MIN_UPDATE_PACKET = 251;
@ -38,13 +43,15 @@ typedef enum {
/*Updater message queue */ /*Updater message queue */
/*************************/ /*************************/
struct updater_msgqueue_s { struct updater_msgqueue_s
{
uint8_t* queue; uint8_t* queue;
uint8_t* size; uint8_t* size;
}; };
typedef struct updater_msgqueue_s* updater_msgqueue_t; typedef struct updater_msgqueue_s* updater_msgqueue_t;
struct updater_code_s { struct updater_code_s
{
uint8_t* bcode; uint8_t* bcode;
uint8_t* bcode_size; uint8_t* bcode_size;
}; };
@ -54,7 +61,8 @@ typedef struct updater_code_s* updater_code_t;
/*Updater data*/ /*Updater data*/
/**************************/ /**************************/
struct buzz_updater_elem_s { struct buzz_updater_elem_s
{
/* robot id */ /* robot id */
// uint16_t robotid; // uint16_t robotid;
/*current Bytecode content */ /*current Bytecode content */
@ -89,9 +97,7 @@ void update_routine();
/************************************************/ /************************************************/
/*Initalizes the updater */ /*Initalizes the updater */
/************************************************/ /************************************************/
void init_update_monitor(const char* bo_filename,const char* stand_by_script, void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
const char* dbgfname, int robot_id);
/*********************************************************/ /*********************************************************/
/*Appends buffer of given size to in msg queue of updater*/ /*Appends buffer of given size to in msg queue of updater*/
@ -125,7 +131,6 @@ void destroy_out_msg_queue();
/***************************************************/ /***************************************************/
int get_update_mode(); int get_update_mode();
buzz_updater_elem_t get_updater(); buzz_updater_elem_t get_updater();
/***************************************************/ /***************************************************/
/*sets bzz file name*/ /*sets bzz file name*/

25
include/buzz_utility.h
View File

@ -12,20 +12,25 @@
#include <map> #include <map>
using namespace std; using namespace std;
namespace buzz_utility{ namespace buzz_utility
{
struct pos_struct struct pos_struct
{ {
double x, y, z; double x, y, z;
pos_struct(double x, double y, double z) : x(x), y(y), z(z){}; pos_struct(double x, double y, double z) : x(x), y(y), z(z){};
pos_struct(){} pos_struct()
{
}
}; };
typedef struct pos_struct Pos_struct; typedef struct pos_struct Pos_struct;
struct rb_struct struct rb_struct
{ {
double r, b, latitude, longitude, altitude; 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 la, double lo, double al, double r, double b)
rb_struct(){} : latitude(la), longitude(lo), altitude(al), r(r), b(b){};
rb_struct()
{
}
}; };
typedef struct rb_struct RB_struct; typedef struct rb_struct RB_struct;
@ -35,13 +40,12 @@ struct neiStatus
uint batt_lvl = 0; uint batt_lvl = 0;
uint xbee = 0; uint xbee = 0;
uint flight_status = 0; uint flight_status = 0;
}; typedef struct neiStatus neighbors_status ; };
typedef struct neiStatus neighbors_status;
uint16_t* u64_cvt_u16(uint64_t u64); uint16_t* u64_cvt_u16(uint64_t u64);
int buzz_listen(const char* type, int buzz_listen(const char* type, int msg_size);
int msg_size);
int make_table(buzzobj_t* t); int make_table(buzzobj_t* t);
int buzzusers_reset(); int buzzusers_reset();
int create_stig_tables(); int create_stig_tables();
@ -52,8 +56,7 @@ uint64_t* obt_out_msg();
void update_sensors(); void update_sensors();
int buzz_script_set(const char* bo_filename, int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id);
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);

4
include/buzzuav_closures.h
View File

@ -12,7 +12,8 @@
#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0))) #define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))
#define RAD2DEG(RAD) (double)((RAD) * ((180.0) / (M_PI))) #define RAD2DEG(RAD) (double)((RAD) * ((180.0) / (M_PI)))
namespace buzzuav_closures{ namespace buzzuav_closures
{
typedef enum { typedef enum {
COMMAND_NIL = 0, // Dummy command COMMAND_NIL = 0, // Dummy command
COMMAND_TAKEOFF, // Take off COMMAND_TAKEOFF, // Take off
@ -126,7 +127,6 @@ int buzzuav_update_prox(buzzvm_t vm);
int bzz_cmd(); int bzz_cmd();
int dummy_closure(buzzvm_t vm); int dummy_closure(buzzvm_t vm);
//#endif //#endif

26
include/roscontroller.h
View File

@ -45,10 +45,8 @@ using namespace std;
namespace rosbzz_node namespace rosbzz_node
{ {
class roscontroller class roscontroller
{ {
public: public:
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller(); ~roscontroller();
@ -63,15 +61,19 @@ private:
uint8_t history[10]; uint8_t history[10];
uint8_t index = 0; uint8_t index = 0;
uint8_t current = 0; uint8_t current = 0;
num_robot_count(){} num_robot_count()
}; typedef struct num_robot_count Num_robot_count ; // not useful in cpp {
}
};
typedef struct num_robot_count Num_robot_count; // not useful in cpp
struct gps struct gps
{ {
double longitude = 0.0; double longitude = 0.0;
double latitude = 0.0; double latitude = 0.0;
float altitude = 0.0; float altitude = 0.0;
}; typedef struct gps GPS ; };
typedef struct gps GPS;
GPS target, home, cur_pos; GPS target, home, cur_pos;
double cur_rel_altitude; double cur_rel_altitude;
@ -136,7 +138,6 @@ private:
ros::Subscriber local_pos_sub; ros::Subscriber local_pos_sub;
double local_pos_new[3]; double local_pos_new[3];
ros::ServiceClient stream_client; ros::ServiceClient stream_client;
int setpoint_counter; int setpoint_counter;
@ -178,7 +179,6 @@ private:
/*Prepare messages and publish*/ /*Prepare messages and publish*/
void prepare_msg_and_publish(); void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/ /*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step); void maintain_pos(int tim_step);
@ -189,17 +189,14 @@ private:
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*/ /*Set the current position of the robot callback*/
void set_cur_pos(double latitude, void set_cur_pos(double latitude, double longitude, double altitude);
double longitude,
double altitude);
/*convert from spherical to cartesian coordinate system callback */ /*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_rb(GPS nei_pos, double out[]);
void gps_ned_cur(float& ned_x, float& ned_y, GPS t); void gps_ned_cur(float& ned_x, float& ned_y, GPS t);
void gps_ned_home(float& ned_x, float& ned_y); void gps_ned_home(float& ned_x, float& ned_y);
void gps_convert_ned(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_t_lon, double gps_t_lat, double gps_r_lat);
double gps_r_lon, double gps_r_lat);
/*battery status callback */ /*battery status callback */
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
@ -213,7 +210,6 @@ private:
/*current position callback*/ /*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*current relative altitude callback*/ /*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
@ -264,7 +260,5 @@ private:
bool GetFilteredPacketLoss(const uint8_t short_id, float& result); bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
void get_xbee_status(); void get_xbee_status();
}; };
} }

1
include/uav_utility.h
View File

@ -5,5 +5,4 @@ extern void uav_setup();
extern void uav_done(); extern void uav_done();
#endif #endif

304
src/buzz_update.cpp
View File

@ -9,7 +9,6 @@
#include <buzz/buzzdebug.h> #include <buzz/buzzdebug.h>
#include <sys/time.h> #include <sys/time.h>
/*Temp for data collection*/ /*Temp for data collection*/
// static int neigh=-1; // static int neigh=-1;
static struct timeval t1, t2; static struct timeval t1, t2;
@ -34,38 +33,42 @@ static const uint16_t MAX_UPDATE_TRY=5;
static int packet_id_ = 0; static int packet_id_ = 0;
static size_t old_byte_code_size = 0; static size_t old_byte_code_size = 0;
/*Initialize updater*/ /*Initialize updater*/
void init_update_monitor(const char* bo_filename, const char* stand_by_script, void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id)
const char* dbgfname, int robot_id){ {
Robot_id = robot_id; Robot_id = robot_id;
dbgf_name = dbgfname; dbgf_name = dbgfname;
bcfname = bo_filename; bcfname = bo_filename;
ROS_INFO("Initializing file monitor..."); ROS_INFO("Initializing file monitor...");
fd = inotify_init1(IN_NONBLOCK); fd = inotify_init1(IN_NONBLOCK);
if ( fd < 0 ) { if (fd < 0)
{
perror("inotify_init error"); perror("inotify_init error");
} }
/*If simulation set the file monitor only for robot 1*/ /*If simulation set the file monitor only for robot 1*/
if(SIMULATION==1 && robot_id==0){ if (SIMULATION == 1 && robot_id == 0)
{
/* watch /.bzz file for any activity and report it back to update */ /* watch /.bzz file for any activity and report it back to update */
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
} }
else if (SIMULATION==0){ else if (SIMULATION == 0)
{
/* watch /.bzz file for any activity and report it back to update */ /* watch /.bzz file for any activity and report it back to update */
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
} }
/*load the .bo under execution into the updater*/ /*load the .bo under execution into the updater*/
uint8_t* BO_BUF = 0; uint8_t* BO_BUF = 0;
FILE* fp = fopen(bo_filename, "rb"); FILE* fp = fopen(bo_filename, "rb");
if(!fp) { if (!fp)
{
perror(bo_filename); perror(bo_filename);
} }
fseek(fp, 0, SEEK_END); fseek(fp, 0, SEEK_END);
size_t bcode_size = ftell(fp); size_t bcode_size = ftell(fp);
rewind(fp); rewind(fp);
BO_BUF = (uint8_t*)malloc(bcode_size); BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size)
{
perror(bo_filename); perror(bo_filename);
// fclose(fp); // fclose(fp);
// return 0; // return 0;
@ -74,15 +77,16 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script,
/*Load stand_by .bo file into the updater*/ /*Load stand_by .bo file into the updater*/
uint8_t* STD_BO_BUF = 0; uint8_t* STD_BO_BUF = 0;
fp = fopen(stand_by_script, "rb"); fp = fopen(stand_by_script, "rb");
if(!fp) { if (!fp)
{
perror(stand_by_script); perror(stand_by_script);
} }
fseek(fp, 0, SEEK_END); fseek(fp, 0, SEEK_END);
size_t stdby_bcode_size = ftell(fp); size_t stdby_bcode_size = ftell(fp);
rewind(fp); rewind(fp);
STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size);
if(fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) { if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size)
{
perror(stand_by_script); perror(stand_by_script);
// fclose(fp); // fclose(fp);
// return 0; // return 0;
@ -113,20 +117,22 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script,
fp = fopen("old_bcode.bo", "wb"); fp = fopen("old_bcode.bo", "wb");
fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp);
fclose(fp); fclose(fp);
} }
/*Check for .bzz file chages*/ /*Check for .bzz file chages*/
int check_update(){ int check_update()
{
struct inotify_event* event; struct inotify_event* event;
char buf[1024]; char buf[1024];
int check = 0; int check = 0;
int i = 0; int i = 0;
int len = read(fd, buf, 1024); int len = read(fd, buf, 1024);
while(i<len){ while (i < len)
{
event = (struct inotify_event*)&buf[i]; event = (struct inotify_event*)&buf[i];
/* file was modified this flag is true in nano and self delet in gedit and other editors */ /* file was modified this flag is true in nano and self delet in gedit and other editors */
// fprintf(stdout,"inside file monitor.\n"); // fprintf(stdout,"inside file monitor.\n");
if(event->mask & (IN_MODIFY| IN_DELETE_SELF)){ if (event->mask & (IN_MODIFY | IN_DELETE_SELF))
{
/*respawn watch if the file is self deleted */ /*respawn watch if the file is self deleted */
inotify_rm_watch(fd, wd); inotify_rm_watch(fd, wd);
close(fd); close(fd);
@ -134,7 +140,8 @@ int check_update(){
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
// fprintf(stdout,"event.\n"); // fprintf(stdout,"event.\n");
/* To mask multiple writes from editors*/ /* To mask multiple writes from editors*/
if(!old_update){ if (!old_update)
{
check = 1; check = 1;
old_update = 1; old_update = 1;
} }
@ -142,15 +149,19 @@ int check_update(){
/* update index to start of next event */ /* update index to start of next event */
i += sizeof(struct inotify_event) + event->len; i += sizeof(struct inotify_event) + event->len;
} }
if (!check) old_update=0; if (!check)
old_update = 0;
return check; return check;
} }
int test_patch(std::string path, std::string name1,size_t update_patch_size, uint8_t* patch){ int test_patch(std::string path, std::string name1, size_t update_patch_size, uint8_t* patch)
if(SIMULATION==1){ {
if (SIMULATION == 1)
{
return 1; return 1;
} }
else { else
{
/*Patch the old bo with new patch*/ /*Patch the old bo with new patch*/
std::stringstream patch_writefile; std::stringstream patch_writefile;
patch_writefile << path << name1 << "tmp_patch.bo"; patch_writefile << path << name1 << "tmp_patch.bo";
@ -159,29 +170,36 @@ int test_patch(std::string path, std::string name1,size_t update_patch_size, uin
fwrite(patch, update_patch_size, 1, fp); fwrite(patch, update_patch_size, 1, fp);
fclose(fp); fclose(fp);
std::stringstream patch_exsisting; std::stringstream patch_exsisting;
patch_exsisting<< "bspatch "<< path << name1<<".bo "<< path<<name1 << "-patched.bo "<< path<<name1<<"tmp_patch.bo"; patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1
<< "tmp_patch.bo";
fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str()); fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str());
if(system(patch_exsisting.str().c_str()) ) return 0; if (system(patch_exsisting.str().c_str()))
else return 1; return 0;
else
return 1;
} }
} }
updater_code_t obtain_patched_bo(std::string path, std::string name1){ updater_code_t obtain_patched_bo(std::string path, std::string name1)
if(SIMULATION==1){ {
if (SIMULATION == 1)
{
/*Read the exsisting file to simulate the patching*/ /*Read the exsisting file to simulate the patching*/
std::stringstream read_patched; std::stringstream read_patched;
read_patched << path << name1 << ".bo"; read_patched << path << name1 << ".bo";
fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); fprintf(stdout, "read file name %s \n", read_patched.str().c_str());
uint8_t* patched_BO_Buf = 0; uint8_t* patched_BO_Buf = 0;
FILE* fp = fopen(read_patched.str().c_str(), "rb"); FILE* fp = fopen(read_patched.str().c_str(), "rb");
if(!fp) { if (!fp)
{
perror(read_patched.str().c_str()); perror(read_patched.str().c_str());
} }
fseek(fp, 0, SEEK_END); fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp); size_t patched_size = ftell(fp);
rewind(fp); rewind(fp);
patched_BO_Buf = (uint8_t*)malloc(patched_size); patched_BO_Buf = (uint8_t*)malloc(patched_size);
if(fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) { if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
{
perror(read_patched.str().c_str()); perror(read_patched.str().c_str());
fclose(fp); fclose(fp);
} }
@ -195,22 +213,24 @@ updater_code_t obtain_patched_bo(std::string path, std::string name1){
return update_code; return update_code;
} }
else{ else
{
/*Read the new patched file*/ /*Read the new patched file*/
std::stringstream read_patched; std::stringstream read_patched;
read_patched << path << name1 << "-patched.bo"; read_patched << path << name1 << "-patched.bo";
fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); fprintf(stdout, "read file name %s \n", read_patched.str().c_str());
uint8_t* patched_BO_Buf = 0; uint8_t* patched_BO_Buf = 0;
FILE* fp = fopen(read_patched.str().c_str(), "rb"); FILE* fp = fopen(read_patched.str().c_str(), "rb");
if(!fp) { if (!fp)
{
perror(read_patched.str().c_str()); perror(read_patched.str().c_str());
} }
fseek(fp, 0, SEEK_END); fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp); size_t patched_size = ftell(fp);
rewind(fp); rewind(fp);
patched_BO_Buf = (uint8_t*)malloc(patched_size); patched_BO_Buf = (uint8_t*)malloc(patched_size);
if(fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) { if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
{
perror(read_patched.str().c_str()); perror(read_patched.str().c_str());
fclose(fp); fclose(fp);
} }
@ -230,7 +250,8 @@ updater_code_t obtain_patched_bo(std::string path, std::string name1){
} }
} }
void code_message_outqueue_append(){ void code_message_outqueue_append()
{
updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
/* if size less than 250 Pad with zeors to assure transmission*/ /* 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 size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size);
@ -254,7 +275,8 @@ void code_message_outqueue_append(){
updater_msg_ready = 1; updater_msg_ready = 1;
} }
void outqueue_append_code_request(uint16_t update_no){ void outqueue_append_code_request(uint16_t update_no)
{
updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
uint16_t size = 0; uint16_t size = 0;
updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
@ -273,7 +295,8 @@ void outqueue_append_code_request(uint16_t update_no){
ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter); 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){ void code_message_inqueue_append(uint8_t* msg, uint16_t size)
{
updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
// ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); // ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size);
updater->inmsg_queue->queue = (uint8_t*)malloc(size); updater->inmsg_queue->queue = (uint8_t*)malloc(size);
@ -282,23 +305,30 @@ void code_message_inqueue_append(uint8_t* msg,uint16_t size){
*(uint16_t*)(updater->inmsg_queue->size) = size; *(uint16_t*)(updater->inmsg_queue->size) = size;
} }
void set_packet_id(int packet_id){ void set_packet_id(int packet_id)
{
/*Used for data logging*/ /*Used for data logging*/
packet_id_ = packet_id; packet_id_ = packet_id;
} }
void code_message_inqueue_process(){ void code_message_inqueue_process()
{
int size = 0; int size = 0;
updater_code_t out_code = NULL; updater_code_t out_code = NULL;
ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode)); 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] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)),
ROS_INFO("[Debug] Updater received patch of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)+sizeof(uint8_t)) ) ); (*(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){ if (*(int*)(updater->mode) == CODE_RUNNING)
{
// fprintf(stdout,"[debug]Inside inmsg code running"); // fprintf(stdout,"[debug]Inside inmsg code running");
if(*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE){ if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE)
{
size += sizeof(uint8_t); size += sizeof(uint8_t);
if( *(uint16_t*)(updater->inmsg_queue->queue+size) > *(uint16_t*) (updater->update_no) ){ if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no))
{
// fprintf(stdout,"[debug]Inside update number comparision"); // fprintf(stdout,"[debug]Inside update number comparision");
uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size);
size += sizeof(uint16_t); size += sizeof(uint16_t);
@ -309,7 +339,8 @@ void code_message_inqueue_process(){
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 name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name1 = name1.substr(0, name1.find_last_of(".")); name1 = name1.substr(0, name1.find_last_of("."));
if(test_patch(path, name1,update_patch_size,(updater->inmsg_queue->queue+size)) ){ if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size)))
{
out_code = obtain_patched_bo(path, name1); out_code = obtain_patched_bo(path, name1);
// fprintf(stdout,"in queue process Update no %d\n", (int) update_no); // fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
@ -319,10 +350,8 @@ void code_message_inqueue_process(){
// fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
// fclose(fp); // fclose(fp);
if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size)))
if(test_set_code( (out_code->bcode), {
(char*) dbgf_name, (size_t) (*(uint16_t*)out_code->bcode_size) ) ){
printf("TEST PASSED!\n"); printf("TEST PASSED!\n");
*(uint16_t*)updater->update_no = update_no; *(uint16_t*)updater->update_no = update_no;
/*Clear exisiting patch if any*/ /*Clear exisiting patch if any*/
@ -339,28 +368,30 @@ void code_message_inqueue_process(){
delete_p(out_code->bcode_size); delete_p(out_code->bcode_size);
delete_p(out_code); delete_p(out_code);
} }
else{ else
{
ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n");
update_try_counter++; update_try_counter++;
outqueue_append_code_request(update_no); outqueue_append_code_request(update_no);
} }
} }
} }
} }
size = 0; size = 0;
if(*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE){ if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE)
{
size += sizeof(uint8_t); size += sizeof(uint8_t);
if( *(uint16_t*)(updater->inmsg_queue->queue+size) == *(uint16_t*) (updater->update_no) ){ if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no))
{
size += sizeof(uint16_t); size += sizeof(uint16_t);
if(*(uint16_t*)(updater->inmsg_queue->queue+size) > update_try_counter){ if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter)
{
update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size);
ROS_ERROR("Code appended! update try : %u \n", update_try_counter); ROS_ERROR("Code appended! update try : %u \n", update_try_counter);
code_message_outqueue_append(); code_message_outqueue_append();
} }
if(update_try_counter > MAX_UPDATE_TRY) ROS_ERROR("TODO: ROLL BACK !!"); if (update_try_counter > MAX_UPDATE_TRY)
ROS_ERROR("TODO: ROLL BACK !!");
} }
} }
// fprintf(stdout,"in queue freed\n"); // fprintf(stdout,"in queue freed\n");
@ -369,12 +400,8 @@ void code_message_inqueue_process(){
delete_p(updater->inmsg_queue); delete_p(updater->inmsg_queue);
} }
void create_update_patch()
{
void create_update_patch(){
std::stringstream genpatch; std::stringstream genpatch;
std::stringstream usepatch; std::stringstream usepatch;
@ -395,8 +422,6 @@ void create_update_patch(){
// BETTER: CALL THE FUNCTION IN BSDIFF.CPP // BETTER: CALL THE FUNCTION IN BSDIFF.CPP
// bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo"); // bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo");
/* delete old files & rename new files */ /* delete old files & rename new files */
remove((path + name1 + ".bo").c_str()); remove((path + name1 + ".bo").c_str());
@ -405,21 +430,22 @@ void create_update_patch(){
rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str());
rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str());
/*Read the diff file */ /*Read the diff file */
std::stringstream patchfileName; std::stringstream patchfileName;
patchfileName << path << "diff.bo"; patchfileName << path << "diff.bo";
uint8_t* patch_buff = 0; uint8_t* patch_buff = 0;
FILE* fp = fopen(patchfileName.str().c_str(), "rb"); FILE* fp = fopen(patchfileName.str().c_str(), "rb");
if(!fp) { if (!fp)
{
perror(patchfileName.str().c_str()); perror(patchfileName.str().c_str());
} }
fseek(fp, 0, SEEK_END); fseek(fp, 0, SEEK_END);
size_t patch_size = ftell(fp); size_t patch_size = ftell(fp);
rewind(fp); rewind(fp);
patch_buff = (uint8_t*)malloc(patch_size); patch_buff = (uint8_t*)malloc(patch_size);
if(fread(patch_buff, 1, patch_size, fp) < patch_size) { if (fread(patch_buff, 1, patch_size, fp) < patch_size)
{
perror(patchfileName.str().c_str()); perror(patchfileName.str().c_str());
fclose(fp); fclose(fp);
} }
@ -432,25 +458,27 @@ void create_update_patch(){
remove(patchfileName.str().c_str()); remove(patchfileName.str().c_str());
} }
void update_routine()
void update_routine(){ {
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM); buzzvm_gstore(VM);
// fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if(*(int*)updater->mode==CODE_RUNNING){ if (*(int*)updater->mode == CODE_RUNNING)
{
buzzvm_function_call(VM, "updated_neigh", 0); buzzvm_function_call(VM, "updated_neigh", 0);
// Check for update // Check for update
if(check_update()){ if (check_update())
{
ROS_INFO("Update found \nUpdating script ...\n"); ROS_INFO("Update found \nUpdating script ...\n");
if(compile_bzz(bzz_file)){ if (compile_bzz(bzz_file))
{
ROS_WARN("Errors in comipilg script so staying on old script\n"); ROS_WARN("Errors in comipilg script so staying on old script\n");
} }
else { else
{
std::string bzzfile_name(bzz_file); std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile; 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("\\/")) + "/";
@ -459,19 +487,22 @@ void update_routine(){
bzzfile_in_compile << path << name << "-update.bo"; bzzfile_in_compile << path << name << "-update.bo";
uint8_t* BO_BUF = 0; uint8_t* BO_BUF = 0;
FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit
if(!fp) { if (!fp)
{
perror(bzzfile_in_compile.str().c_str()); perror(bzzfile_in_compile.str().c_str());
} }
fseek(fp, 0, SEEK_END); fseek(fp, 0, SEEK_END);
size_t bcode_size = ftell(fp); size_t bcode_size = ftell(fp);
rewind(fp); rewind(fp);
BO_BUF = (uint8_t*)malloc(bcode_size); BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fp) < bcode_size) { if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size)
{
perror(bcfname); perror(bcfname);
fclose(fp); fclose(fp);
} }
fclose(fp); fclose(fp);
if(test_set_code(BO_BUF, dbgf_name,bcode_size)){ if (test_set_code(BO_BUF, dbgf_name, bcode_size))
{
uint16_t update_no = *(uint16_t*)(updater->update_no); uint16_t update_no = *(uint16_t*)(updater->update_no);
*(uint16_t*)(updater->update_no) = update_no + 1; *(uint16_t*)(updater->update_no) = update_no + 1;
@ -487,21 +518,19 @@ void update_routine(){
} }
delete_p(BO_BUF); delete_p(BO_BUF);
} }
}
} }
} else
{
else{
timer_steps++; timer_steps++;
buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1));
buzzvm_gload(VM); buzzvm_gload(VM);
buzzobj_t tObj = buzzvm_stack_at(VM, 1); buzzobj_t tObj = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM); buzzvm_pop(VM);
ROS_INFO("Barrier ..................... %i \n", tObj->i.value); ROS_INFO("Barrier ..................... %i \n", tObj->i.value);
if(tObj->i.value==no_of_robot) { if (tObj->i.value == no_of_robot)
{
*(int*)(updater->mode) = CODE_RUNNING; *(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL); gettimeofday(&t2, NULL);
// collect_data(); // collect_data();
@ -511,37 +540,37 @@ void update_routine(){
update_try_counter = 0; update_try_counter = 0;
timer_steps = 0; timer_steps = 0;
} }
else if (timer_steps>TIMEOUT_FOR_ROLLBACK){ else if (timer_steps > TIMEOUT_FOR_ROLLBACK)
{
ROS_ERROR("TIME OUT Reached, decided to roll back"); ROS_ERROR("TIME OUT Reached, decided to roll back");
/*Time out hit deceided to roll back*/ /*Time out hit deceided to roll back*/
*(int*)(updater->mode) = CODE_RUNNING; *(int*)(updater->mode) = CODE_RUNNING;
buzz_utility::buzz_script_set(old_bcfname, dbgf_name, buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot);
(int)VM->robot);
updated = 1; updated = 1;
update_try_counter = 0; update_try_counter = 0;
timer_steps = 0; timer_steps = 0;
} }
}
} }
} uint8_t* getupdater_out_msg()
{
uint8_t* getupdater_out_msg(){
return (uint8_t*)updater->outmsg_queue->queue; return (uint8_t*)updater->outmsg_queue->queue;
} }
uint8_t* getupdate_out_msg_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); // 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; return (uint8_t*)updater->outmsg_queue->size;
} }
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)
if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){ {
if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size))
{
ROS_WARN("Initializtion of script test passed\n"); ROS_WARN("Initializtion of script test passed\n");
if(buzz_utility::update_step_test()){ if (buzz_utility::update_step_test())
{
/*data logging*/ /*data logging*/
old_byte_code_size = *(size_t*)updater->bcode_size; old_byte_code_size = *(size_t*)updater->bcode_size;
/*data logging*/ /*data logging*/
@ -552,8 +581,8 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
updater->bcode = (uint8_t*)malloc(bcode_size); updater->bcode = (uint8_t*)malloc(bcode_size);
memcpy(updater->bcode, BO_BUF, bcode_size); memcpy(updater->bcode, BO_BUF, bcode_size);
*(size_t*)updater->bcode_size = bcode_size; *(size_t*)updater->bcode_size = bcode_size;
buzz_utility::buzz_update_init_test((updater)->standby_bcode, buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); (size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
gettimeofday(&t1, NULL); gettimeofday(&t1, NULL);
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
@ -562,36 +591,40 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
return 1; return 1;
} }
/*Unable to step something wrong*/ /*Unable to step something wrong*/
else{ else
if(*(int*) (updater->mode) == CODE_RUNNING){ {
if (*(int*)(updater->mode) == CODE_RUNNING)
{
ROS_ERROR("step test failed, stick to old script\n"); 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)); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size));
} }
else{ else
{
/*You will never reach here*/ /*You will never reach here*/
ROS_ERROR("step test failed, Return to stand by\n"); ROS_ERROR("step test failed, Return to stand by\n");
buzz_utility::buzz_update_init_test((updater)->standby_bcode, buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); (size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, no_of_robot); buzzvm_pushi(VM, no_of_robot);
buzzvm_gstore(VM); buzzvm_gstore(VM);
} }
return 0; return 0;
} }
} }
else { else
if(*(int*) (updater->mode) == CODE_RUNNING){ {
if (*(int*)(updater->mode) == CODE_RUNNING)
{
ROS_ERROR("Initialization test failed, stick to old script\n"); 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)); buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size));
} }
else{ else
{
/*You will never reach here*/ /*You will never reach here*/
ROS_ERROR("Initialization test failed, Return to stand by\n"); ROS_ERROR("Initialization test failed, Return to stand by\n");
buzz_utility::buzz_update_init_test((updater)->standby_bcode, buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size)); (size_t) * (size_t*)(updater->standby_bcode_size));
buzzvm_t VM = buzz_utility::get_vm(); buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, no_of_robot); buzzvm_pushi(VM, no_of_robot);
@ -601,42 +634,51 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
} }
} }
void destroy_out_msg_queue(){ void destroy_out_msg_queue()
{
// fprintf(stdout,"out queue freed\n"); // fprintf(stdout,"out queue freed\n");
delete_p(updater->outmsg_queue->queue); delete_p(updater->outmsg_queue->queue);
delete_p(updater->outmsg_queue->size); delete_p(updater->outmsg_queue->size);
delete_p(updater->outmsg_queue); delete_p(updater->outmsg_queue);
updater_msg_ready = 0; updater_msg_ready = 0;
} }
int get_update_status(){ int get_update_status()
{
return updated; return updated;
} }
void set_read_update_status(){ void set_read_update_status()
{
updated = 0; updated = 0;
} }
int get_update_mode(){ int get_update_mode()
{
return (int)*(int*)(updater->mode); return (int)*(int*)(updater->mode);
} }
int is_msg_present(){ int is_msg_present()
{
return updater_msg_ready; return updater_msg_ready;
} }
buzz_updater_elem_t get_updater(){ buzz_updater_elem_t get_updater()
{
return updater; return updater;
} }
void destroy_updater(){ void destroy_updater()
{
delete_p(updater->bcode); delete_p(updater->bcode);
delete_p(updater->bcode_size); delete_p(updater->bcode_size);
delete_p(updater->standby_bcode); delete_p(updater->standby_bcode);
delete_p(updater->standby_bcode_size); delete_p(updater->standby_bcode_size);
delete_p(updater->mode); delete_p(updater->mode);
delete_p(updater->update_no); delete_p(updater->update_no);
if(updater->outmsg_queue){ if (updater->outmsg_queue)
{
delete_p(updater->outmsg_queue->queue); delete_p(updater->outmsg_queue->queue);
delete_p(updater->outmsg_queue->size); delete_p(updater->outmsg_queue->size);
delete_p(updater->outmsg_queue); delete_p(updater->outmsg_queue);
} }
if(updater->inmsg_queue){ if (updater->inmsg_queue)
{
delete_p(updater->inmsg_queue->queue); delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size); delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue); delete_p(updater->inmsg_queue);
@ -645,18 +687,21 @@ void destroy_updater(){
close(fd); close(fd);
} }
void set_bzz_file(const char* in_bzz_file){ void set_bzz_file(const char* in_bzz_file)
{
bzz_file = in_bzz_file; bzz_file = in_bzz_file;
} }
void updates_set_robots(int robots){ void updates_set_robots(int robots)
{
no_of_robot = robots; no_of_robot = robots;
} }
/*-------------------------------------------------------- /*--------------------------------------------------------
/ Create Buzz bytecode from the bzz script inputed / Create Buzz bytecode from the bzz script inputed
/-------------------------------------------------------*/ /-------------------------------------------------------*/
int compile_bzz(std::string bzz_file){ int compile_bzz(std::string bzz_file)
{
/*Compile the buzz code .bzz to .bo*/ /*Compile the buzz code .bzz to .bo*/
std::string bzzfile_name(bzz_file); std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile; stringstream bzzfile_in_compile;
@ -671,14 +716,13 @@ int compile_bzz(std::string bzz_file){
return system(bzzfile_in_compile.str().c_str()); return system(bzzfile_in_compile.str().c_str());
} }
void collect_data(std::ofstream& logger)
{
void collect_data(std::ofstream &logger){
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; 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; 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, // 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 // 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<<"," logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << ","
<<*(size_t*)updater->patch_size<<","<<(int)*(uint8_t*)updater->update_no<<","<<(int)packet_id_; << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << ","
<< (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_;
} }

221
src/buzz_utility.cpp
View File

@ -8,8 +8,8 @@
#include "buzz_utility.h" #include "buzz_utility.h"
namespace buzz_utility{ namespace buzz_utility
{
/****************************************/ /****************************************/
/****************************************/ /****************************************/
@ -27,7 +27,8 @@ namespace buzz_utility{
/**************************************************************************/ /**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */ /*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/ /**************************************************************************/
uint16_t* u64_cvt_u16(uint64_t u64){ uint16_t* u64_cvt_u16(uint64_t u64)
{
uint16_t* out = new uint16_t[4]; uint16_t* out = new uint16_t[4];
uint32_t int32_1 = u64 & 0xFFFFFFFF; uint32_t int32_1 = u64 & 0xFFFFFFFF;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000) >> 32; uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000) >> 32;
@ -39,7 +40,8 @@ namespace buzz_utility{
return out; return out;
} }
int get_robotid() { int get_robotid()
{
return Robot_id; return Robot_id;
} }
/***************************************************/ /***************************************************/
@ -54,8 +56,8 @@ namespace buzz_utility{
/*|__________________________________________________________________________|____________________________________|*/ /*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/ /*******************************************************************************************************************/
void in_msg_append(uint64_t* payload){ void in_msg_append(uint64_t* payload)
{
/* Go through messages and append them to the vector */ /* Go through messages and append them to the vector */
uint16_t* data = u64_cvt_u16((uint64_t)payload[0]); uint16_t* data = u64_cvt_u16((uint64_t)payload[0]);
/*Size is at first 2 bytes*/ /*Size is at first 2 bytes*/
@ -65,11 +67,12 @@ namespace buzz_utility{
/* Copy packet into temporary buffer */ /* Copy packet into temporary buffer */
memcpy(pl, payload, size); memcpy(pl, payload, size);
IN_MSG.push_back(pl); IN_MSG.push_back(pl);
} }
void in_message_process(){ void in_message_process()
while(!IN_MSG.empty()){ {
while (!IN_MSG.empty())
{
/* Go through messages and append them to the FIFO */ /* Go through messages and append them to the FIFO */
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
size_t tot = 0; size_t tot = 0;
@ -82,15 +85,15 @@ void in_message_process(){
/* Go through the messages until there's nothing else to read */ /* Go through the messages until there's nothing else to read */
uint16_t unMsgSize = 0; uint16_t unMsgSize = 0;
/*Obtain Buzz messages push it into queue*/ /*Obtain Buzz messages push it into queue*/
do { do
{
/* Get payload size */ /* Get payload size */
unMsgSize = *(uint16_t*)(first_INmsg + tot); unMsgSize = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */ /* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= size - tot ) { if (unMsgSize > 0 && unMsgSize <= size - tot)
buzzinmsg_queue_append(VM, {
neigh_id, buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize));
buzzmsg_payload_frombuffer(first_INmsg +tot, unMsgSize));
tot += unMsgSize; tot += unMsgSize;
} }
} while (size - tot > sizeof(uint16_t) && unMsgSize > 0); } while (size - tot > sizeof(uint16_t) && unMsgSize > 0);
@ -104,7 +107,8 @@ void in_message_process(){
/***************************************************/ /***************************************************/
/*Obtains messages from buzz out message Queue*/ /*Obtains messages from buzz out message Queue*/
/***************************************************/ /***************************************************/
uint64_t* obt_out_msg(){ uint64_t* obt_out_msg()
{
/* Process out messages */ /* Process out messages */
buzzvm_process_outmsgs(VM); buzzvm_process_outmsgs(VM);
uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE); uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE);
@ -115,14 +119,17 @@ void in_message_process(){
*(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot; *(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot;
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/* Send messages from FIFO */ /* Send messages from FIFO */
do { do
{
/* Are there more messages? */ /* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM)) break; if (buzzoutmsg_queue_isempty(VM))
break;
/* Get first message */ /* Get first message */
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */ /* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */
// ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t))); // 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) { if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE)
{
buzzmsg_payload_destroy(&m); buzzmsg_payload_destroy(&m);
break; break;
} }
@ -158,24 +165,18 @@ void in_message_process(){
/*Buzz script not able to load*/ /*Buzz script not able to load*/
/****************************************/ /****************************************/
static const char* buzz_error_info() { static const char* buzz_error_info()
{
buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc); buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
char* msg; char* msg;
if(dbg != NULL) { if (dbg != NULL)
asprintf(&msg, {
"%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n", asprintf(&msg, "%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n", BO_FNAME, dbg->fname,
BO_FNAME, dbg->line, dbg->col, VM->errormsg);
dbg->fname,
dbg->line,
dbg->col,
VM->errormsg);
} }
else { else
asprintf(&msg, {
"%s: execution terminated abnormally at bytecode offset %d: %s\n\n", asprintf(&msg, "%s: execution terminated abnormally at bytecode offset %d: %s\n\n", BO_FNAME, VM->pc, VM->errormsg);
BO_FNAME,
VM->pc,
VM->errormsg);
} }
return msg; return msg;
} }
@ -184,7 +185,8 @@ void in_message_process(){
/*Buzz hooks that can be used inside .bzz file*/ /*Buzz hooks that can be used inside .bzz file*/
/****************************************/ /****************************************/
static int buzz_register_hooks() { static int buzz_register_hooks()
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM); buzzvm_gstore(VM);
@ -235,7 +237,8 @@ void in_message_process(){
/*Register dummy Buzz hooks for test during update*/ /*Register dummy Buzz hooks for test during update*/
/**************************************************/ /**************************************************/
static int testing_buzz_register_hooks() { static int testing_buzz_register_hooks()
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM); buzzvm_gstore(VM);
@ -285,13 +288,14 @@ void in_message_process(){
/****************************************/ /****************************************/
/*Sets the .bzz and .bdbg file*/ /*Sets the .bzz and .bdbg file*/
/****************************************/ /****************************************/
int buzz_script_set(const char* bo_filename, int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id)
const char* bdbg_filename, int robot_id) { {
ROS_INFO(" Robot ID: %i", robot_id); ROS_INFO(" Robot ID: %i", robot_id);
Robot_id = robot_id; Robot_id = robot_id;
/* Read bytecode from file and fill the bo buffer*/ /* Read bytecode from file and fill the bo buffer*/
FILE* fd = fopen(bo_filename, "rb"); FILE* fd = fopen(bo_filename, "rb");
if(!fd) { if (!fd)
{
perror(bo_filename); perror(bo_filename);
return 0; return 0;
} }
@ -299,7 +303,8 @@ void in_message_process(){
size_t bcode_size = ftell(fd); size_t bcode_size = ftell(fd);
rewind(fd); rewind(fd);
BO_BUF = (uint8_t*)malloc(bcode_size); BO_BUF = (uint8_t*)malloc(bcode_size);
if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) { if (fread(BO_BUF, 1, bcode_size, fd) < bcode_size)
{
perror(bo_filename); perror(bo_filename);
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -317,30 +322,36 @@ void in_message_process(){
/****************************************/ /****************************************/
/*Sets a new update */ /*Sets a new update */
/****************************************/ /****************************************/
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)
{
// Reset the Buzz VM // Reset the Buzz VM
if(VM) buzzvm_destroy(&VM); if (VM)
buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id); VM = buzzvm_new(Robot_id);
// Get rid of debug info // Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if (DBG_INFO)
buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new(); DBG_INFO = buzzdebug_new();
// Read debug information // Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename))
{
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename); perror(bdbg_filename);
return 0; return 0;
} }
// Set byte code // Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id); ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id);
return 0; return 0;
} }
// Register hook functions // Register hook functions
if(buzz_register_hooks() != BUZZVM_STATE_READY) { if (buzz_register_hooks() != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error registering hooks (update)", Robot_id); ROS_ERROR("[%i] Error registering hooks (update)", Robot_id);
@ -353,12 +364,14 @@ void in_message_process(){
buzzvm_gstore(VM); buzzvm_gstore(VM);
// Execute the global part of the script // Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ if (buzzvm_execute_script(VM) != BUZZVM_STATE_DONE)
{
ROS_ERROR("Error executing global part, VM state : %i", VM->state); ROS_ERROR("Error executing global part, VM state : %i", VM->state);
return 0; return 0;
} }
// Call the Init() function // Call the Init() function
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY)
{
ROS_ERROR("Error in calling init, VM state : %i", VM->state); ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0; return 0;
} }
@ -369,30 +382,36 @@ void in_message_process(){
/****************************************/ /****************************************/
/*Performs a initialization test */ /*Performs a initialization test */
/****************************************/ /****************************************/
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)
{
// Reset the Buzz VM // Reset the Buzz VM
if(VM) buzzvm_destroy(&VM); if (VM)
buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id); VM = buzzvm_new(Robot_id);
// Get rid of debug info // Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if (DBG_INFO)
buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new(); DBG_INFO = buzzdebug_new();
// Read debug information // Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { if (!buzzdebug_fromfile(DBG_INFO, bdbg_filename))
{
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename); perror(bdbg_filename);
return 0; return 0;
} }
// Set byte code // Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id); ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id);
return 0; return 0;
} }
// Register hook functions // Register hook functions
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) { if (testing_buzz_register_hooks() != BUZZVM_STATE_READY)
{
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id); ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id);
@ -405,12 +424,14 @@ void in_message_process(){
buzzvm_gstore(VM); buzzvm_gstore(VM);
// Execute the global part of the script // Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ if (buzzvm_execute_script(VM) != BUZZVM_STATE_DONE)
{
ROS_ERROR("Error executing global part, VM state : %i", VM->state); ROS_ERROR("Error executing global part, VM state : %i", VM->state);
return 0; return 0;
} }
// Call the Init() function // Call the Init() function
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){ if (buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY)
{
ROS_ERROR("Error in calling init, VM state : %i", VM->state); ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0; return 0;
} }
@ -422,41 +443,44 @@ void in_message_process(){
/*Swarm struct*/ /*Swarm struct*/
/****************************************/ /****************************************/
struct buzzswarm_elem_s { struct buzzswarm_elem_s
{
buzzdarray_t swarms; buzzdarray_t swarms;
uint16_t age; uint16_t age;
}; };
typedef struct buzzswarm_elem_s* buzzswarm_elem_t; typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
void check_swarm_members(const void* key, void* data, void* params) { void check_swarm_members(const void* key, void* data, void* params)
{
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params; int* status = (int*)params;
if(*status == 3) return; if (*status == 3)
fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n", return;
buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age); fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n", buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key,
if(buzzdarray_size(e->swarms) != 1) { e->age);
if (buzzdarray_size(e->swarms) != 1)
{
fprintf(stderr, "Swarm list size is not 1\n"); fprintf(stderr, "Swarm list size is not 1\n");
*status = 3; *status = 3;
} }
else { else
{
int sid = 1; int sid = 1;
if(!buzzdict_isempty(VM->swarms)) { if (!buzzdict_isempty(VM->swarms))
if(*buzzdict_get(VM->swarms, &sid, uint8_t) && {
buzzdarray_get(e->swarms, 0, uint16_t) != sid) { 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, fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid, buzzdarray_get(e->swarms, 0, uint16_t));
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3; *status = 3;
return; return;
} }
} }
if(buzzdict_size(VM->swarms)>1) { if (buzzdict_size(VM->swarms) > 1)
{
sid = 2; sid = 2;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) && if (*buzzdict_get(VM->swarms, &sid, uint8_t) && buzzdarray_get(e->swarms, 0, uint16_t) != sid)
buzzdarray_get(e->swarms, 0, uint16_t) != sid) { {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid, buzzdarray_get(e->swarms, 0, uint16_t));
sid,
buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3; *status = 3;
return; return;
} }
@ -465,7 +489,8 @@ void in_message_process(){
} }
/*Step through the buzz script*/ /*Step through the buzz script*/
void update_sensors(){ void update_sensors()
{
/* Update sensors*/ /* Update sensors*/
buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_xbee_status(VM); buzzuav_closures::buzzuav_update_xbee_status(VM);
@ -477,16 +502,16 @@ void in_message_process(){
buzzuav_closures::buzzuav_update_flight_status(VM); buzzuav_closures::buzzuav_update_flight_status(VM);
} }
void buzz_script_step() { void buzz_script_step()
{
/*Process available messages*/ /*Process available messages*/
in_message_process(); in_message_process();
/*Update sensors*/ /*Update sensors*/
update_sensors(); update_sensors();
/* Call Buzz step() function */ /* Call Buzz step() function */
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY)
ROS_ERROR("%s: execution terminated abnormally: %s", {
BO_FNAME, ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info());
buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
} }
} }
@ -495,12 +520,13 @@ void in_message_process(){
/*Destroy the bvm and other resorces*/ /*Destroy the bvm and other resorces*/
/****************************************/ /****************************************/
void buzz_script_destroy() { void buzz_script_destroy()
if(VM) { {
if(VM->state != BUZZVM_STATE_READY) { if (VM)
ROS_ERROR("%s: execution terminated abnormally: %s", {
BO_FNAME, if (VM->state != BUZZVM_STATE_READY)
buzz_error_info()); {
ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
} }
buzzvm_function_call(VM, "destroy", 0); buzzvm_function_call(VM, "destroy", 0);
@ -511,7 +537,6 @@ void in_message_process(){
ROS_INFO("Script execution stopped."); ROS_INFO("Script execution stopped.");
} }
/****************************************/ /****************************************/
/****************************************/ /****************************************/
@ -519,11 +544,13 @@ void in_message_process(){
/*Execution completed*/ /*Execution completed*/
/****************************************/ /****************************************/
int buzz_script_done() { int buzz_script_done()
{
return VM->state != BUZZVM_STATE_READY; return VM->state != BUZZVM_STATE_READY;
} }
int update_step_test() { int update_step_test()
{
/*Process available messages*/ /*Process available messages*/
in_message_process(); in_message_process();
buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_battery(VM);
@ -534,27 +561,29 @@ void in_message_process(){
int a = buzzvm_function_call(VM, "step", 0); int a = buzzvm_function_call(VM, "step", 0);
if(a!= BUZZVM_STATE_READY) { if (a != BUZZVM_STATE_READY)
ROS_ERROR("%s: execution terminated abnormally: %s\n\n", {
BO_FNAME, ROS_ERROR("%s: execution terminated abnormally: %s\n\n", BO_FNAME, buzz_error_info());
buzz_error_info());
fprintf(stdout, "step test VM state %i\n", a); fprintf(stdout, "step test VM state %i\n", a);
} }
return a == BUZZVM_STATE_READY; return a == BUZZVM_STATE_READY;
} }
buzzvm_t get_vm() { buzzvm_t get_vm()
{
return VM; return VM;
} }
void set_robot_var(int ROBOTS){ void set_robot_var(int ROBOTS)
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, ROBOTS); buzzvm_pushi(VM, ROBOTS);
buzzvm_gstore(VM); buzzvm_gstore(VM);
} }
int get_inmsg_size(){ int get_inmsg_size()
{
return IN_MSG.size(); return IN_MSG.size();
} }
} }

175
src/buzzuav_closures.cpp
View File

@ -9,7 +9,8 @@
#include "buzzuav_closures.h" #include "buzzuav_closures.h"
#include "math.h" #include "math.h"
namespace buzzuav_closures{ namespace buzzuav_closures
{
// TODO: Minimize the required global variables and put them in the header // TODO: Minimize the required global variables and put them in the header
// static const rosbzz_node::roscontroller* roscontroller_ptr; // static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/ /*forward declaration of ros controller ptr storing function*/
@ -88,7 +89,8 @@ int buzzros_print(buzzvm_t vm)
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
void setWPlist(string path){ void setWPlist(string path)
{
WPlistname = path + "include/graphs/waypointlist.csv"; WPlistname = path + "include/graphs/waypointlist.csv";
} }
@ -96,24 +98,28 @@ int buzzros_print(buzzvm_t vm)
/ Compute GPS destination from current position and desired Range and Bearing / Compute GPS destination from current position and desired Range and Bearing
/----------------------------------------*/ /----------------------------------------*/
void gps_from_rb(double range, double bearing, double out[3]) { void gps_from_rb(double range, double bearing, double out[3])
{
double lat = RAD2DEG(cur_pos[0]); double lat = RAD2DEG(cur_pos[0]);
double lon = RAD2DEG(cur_pos[1]); double lon = RAD2DEG(cur_pos[1]);
out[0] = asin(sin(lat) * cos(range / EARTH_RADIUS) + cos(lat) * sin(range / EARTH_RADIUS) * cos(bearing)); 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[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[0] = RAD2DEG(out[0]);
out[1] = RAD2DEG(out[1]); out[1] = RAD2DEG(out[1]);
out[2] = height; // constant height. out[2] = height; // constant height.
} }
float constrainAngle(float x){ float constrainAngle(float x)
{
x = fmod(x, 2 * M_PI); x = fmod(x, 2 * M_PI);
if (x < 0.0) if (x < 0.0)
x += 2 * M_PI; x += 2 * M_PI;
return x; return x;
} }
void rb_from_gps(double nei[], double out[], double cur[]){ void rb_from_gps(double nei[], double out[], double cur[])
{
double d_lon = nei[1] - cur[1]; double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0]; double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
@ -135,7 +141,8 @@ int buzzros_print(buzzvm_t vm)
std::ifstream fin(WPlistname.c_str()); // Open in text-mode. std::ifstream fin(WPlistname.c_str()); // Open in text-mode.
// Opening may fail, always check. // Opening may fail, always check.
if (!fin) { if (!fin)
{
ROS_ERROR("GPS list parser, could not open file."); ROS_ERROR("GPS list parser, could not open file.");
return; return;
} }
@ -150,7 +157,8 @@ int buzzros_print(buzzvm_t vm)
int alt, tilt, tid; int alt, tilt, tid;
buzz_utility::RB_struct RB_arr; buzz_utility::RB_struct RB_arr;
// Read one line at a time. // Read one line at a time.
while ( fin.getline(buffer, MAX_LINE_LENGTH) ) { while (fin.getline(buffer, MAX_LINE_LENGTH))
{
// Extract the tokens: // Extract the tokens:
tid = atoi(strtok(buffer, DELIMS)); tid = atoi(strtok(buffer, DELIMS));
lon = atof(strtok(NULL, DELIMS)); lon = atof(strtok(NULL, DELIMS));
@ -174,11 +182,11 @@ int buzzros_print(buzzvm_t vm)
fin.close(); fin.close();
} }
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to move following a 2D vector / Buzz closure to move following a 2D vector
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm) { int buzzuav_moveto(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3); buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* dx */ buzzvm_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */ buzzvm_lload(vm, 2); /* dy */
@ -192,20 +200,26 @@ int buzzros_print(buzzvm_t vm)
goto_pos[0] = dx; goto_pos[0] = dx;
goto_pos[1] = dy; goto_pos[1] = dy;
goto_pos[2] = height + dh; 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]); // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
// goto_pos[1], goto_pos[2]);
buzz_cmd = COMMAND_MOVETO; // TO DO what should we use buzz_cmd = COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_update_targets(buzzvm_t vm) { int buzzuav_update_targets(buzzvm_t vm)
if(vm->state != BUZZVM_STATE_READY) return vm->state; {
if (vm->state != BUZZVM_STATE_READY)
return vm->state;
buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzobj_t targettbl = buzzvm_stack_at(vm, 1); buzzobj_t targettbl = buzzvm_stack_at(vm, 1);
double rb[3], tmp[3]; double rb[3], tmp[3];
map<int, buzz_utility::RB_struct>::iterator it; map<int, buzz_utility::RB_struct>::iterator it;
for (it=targets_map.begin(); it!=targets_map.end(); ++it){ for (it = targets_map.begin(); it != targets_map.end(); ++it)
tmp[0]=(it->second).latitude; tmp[1]=(it->second).longitude; tmp[2]=height; {
tmp[0] = (it->second).latitude;
tmp[1] = (it->second).longitude;
tmp[2] = height;
rb_from_gps(tmp, rb, cur_pos); rb_from_gps(tmp, rb, cur_pos);
// ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]); // ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
buzzvm_push(vm, targettbl); buzzvm_push(vm, targettbl);
@ -234,7 +248,8 @@ int buzzros_print(buzzvm_t vm)
return vm->state; return vm->state;
} }
int buzzuav_addtargetRB(buzzvm_t vm) { int buzzuav_addtargetRB(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3); buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // longitude buzzvm_lload(vm, 1); // longitude
buzzvm_lload(vm, 2); // latitude buzzvm_lload(vm, 2); // latitude
@ -250,7 +265,8 @@ int buzzros_print(buzzvm_t vm)
double rb[3]; double rb[3];
rb_from_gps(tmp, rb, cur_pos); rb_from_gps(tmp, rb, cur_pos);
if(fabs(rb[0])<100.0) { if (fabs(rb[0]) < 100.0)
{
// printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]); // printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
buzz_utility::RB_struct RB_arr; buzz_utility::RB_struct RB_arr;
RB_arr.latitude = tmp[0]; RB_arr.latitude = tmp[0];
@ -264,13 +280,15 @@ int buzzros_print(buzzvm_t vm)
targets_map.insert(make_pair(uid, RB_arr)); targets_map.insert(make_pair(uid, RB_arr));
// ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); // ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
return vm->state; return vm->state;
} else }
else
ROS_WARN(" ---------- Target too far %f", rb[0]); ROS_WARN(" ---------- Target too far %f", rb[0]);
return 0; return 0;
} }
int buzzuav_addNeiStatus(buzzvm_t vm){ int buzzuav_addNeiStatus(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 5); buzzvm_lnum_assert(vm, 5);
buzzvm_lload(vm, 1); // fc buzzvm_lload(vm, 1); // fc
buzzvm_lload(vm, 2); // xbee buzzvm_lload(vm, 2); // xbee
@ -295,10 +313,12 @@ int buzzros_print(buzzvm_t vm)
return vm->state; return vm->state;
} }
mavros_msgs::Mavlink get_status(){ mavros_msgs::Mavlink get_status()
{
mavros_msgs::Mavlink payload_out; mavros_msgs::Mavlink payload_out;
map<int, buzz_utility::neighbors_status>::iterator it; map<int, buzz_utility::neighbors_status>::iterator it;
for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++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->first);
payload_out.payload64.push_back(it->second.gps_strenght); 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.batt_lvl);
@ -315,7 +335,8 @@ int buzzros_print(buzzvm_t vm)
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to take a picture here. / Buzz closure to take a picture here.
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_takepicture(buzzvm_t vm) { int buzzuav_takepicture(buzzvm_t vm)
{
// cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; // cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
buzz_cmd = COMMAND_PICTURE; buzz_cmd = COMMAND_PICTURE;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
@ -324,7 +345,8 @@ int buzzros_print(buzzvm_t vm)
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to change locally the gimbal orientation / Buzz closure to change locally the gimbal orientation
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_setgimbal(buzzvm_t vm) { int buzzuav_setgimbal(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 4); buzzvm_lnum_assert(vm, 4);
buzzvm_lload(vm, 1); // time buzzvm_lload(vm, 1); // time
buzzvm_lload(vm, 2); // pitch buzzvm_lload(vm, 2); // pitch
@ -347,7 +369,8 @@ int buzzros_print(buzzvm_t vm)
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to store locally a GPS destination from the fleet / Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_storegoal(buzzvm_t vm) { int buzzuav_storegoal(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3); buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // altitude buzzvm_lload(vm, 1); // altitude
buzzvm_lload(vm, 2); // longitude buzzvm_lload(vm, 2); // longitude
@ -359,7 +382,8 @@ int buzzros_print(buzzvm_t vm)
goal[0] = buzzvm_stack_at(vm, 3)->f.value; goal[0] = buzzvm_stack_at(vm, 3)->f.value;
goal[1] = buzzvm_stack_at(vm, 2)->f.value; goal[1] = buzzvm_stack_at(vm, 2)->f.value;
goal[2] = buzzvm_stack_at(vm, 1)->f.value; goal[2] = buzzvm_stack_at(vm, 1)->f.value;
if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){ if (goal[0] == -1 && goal[1] == -1 && goal[2] == -1)
{
if (wplist_map.size() <= 0) if (wplist_map.size() <= 0)
parse_gpslist(); parse_gpslist();
goal[0] = wplist_map.begin()->second.latitude; goal[0] = wplist_map.begin()->second.latitude;
@ -381,13 +405,15 @@ int buzzros_print(buzzvm_t vm)
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running / 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) { int buzzuav_arm(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n"); printf(" Buzz requested Arm \n");
buzz_cmd = COMMAND_ARM; buzz_cmd = COMMAND_ARM;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_disarm(buzzvm_t vm) { int buzzuav_disarm(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n"); printf(" Buzz requested Disarm \n");
buzz_cmd = COMMAND_DISARM; buzz_cmd = COMMAND_DISARM;
@ -397,7 +423,8 @@ int buzzros_print(buzzvm_t vm)
/*---------------------------------------/ /*---------------------------------------/
/ Buzz closure for basic UAV commands / Buzz closure for basic UAV commands
/---------------------------------------*/ /---------------------------------------*/
int buzzuav_takeoff(buzzvm_t vm) { int buzzuav_takeoff(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 1); buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); /* Altitude */ buzzvm_lload(vm, 1); /* Altitude */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
@ -409,33 +436,37 @@ int buzzros_print(buzzvm_t vm)
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_land(buzzvm_t vm) { int buzzuav_land(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::NAV_LAND; cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
printf(" Buzz requested Land !!! \n"); printf(" Buzz requested Land !!! \n");
buzz_cmd = COMMAND_LAND; buzz_cmd = COMMAND_LAND;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_gohome(buzzvm_t vm) { int buzzuav_gohome(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n"); printf(" Buzz requested gohome !!! \n");
buzz_cmd = COMMAND_GOHOME; buzz_cmd = COMMAND_GOHOME;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*------------------------------- /*-------------------------------
/ Get/Set to transfer variable from Roscontroller to buzzuav / Get/Set to transfer variable from Roscontroller to buzzuav
/------------------------------------*/ /------------------------------------*/
double* getgoto() { double* getgoto()
{
return goto_pos; return goto_pos;
} }
float* getgimbal() { float* getgimbal()
{
return rc_gimbal; return rc_gimbal;
} }
string getuavstate() { string getuavstate()
{
static buzzvm_t VM = buzz_utility::get_vm(); static buzzvm_t VM = buzz_utility::get_vm();
std::stringstream state_buff; std::stringstream state_buff;
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
@ -445,57 +476,62 @@ int buzzros_print(buzzvm_t vm)
return uav_state->s.value.str; return uav_state->s.value.str;
} }
int getcmd() { int getcmd()
{
return cur_cmd; return cur_cmd;
} }
void set_goto(double pos[]) { void set_goto(double pos[])
{
goto_pos[0] = pos[0]; goto_pos[0] = pos[0];
goto_pos[1] = pos[1]; goto_pos[1] = pos[1];
goto_pos[2] = pos[2]; goto_pos[2] = pos[2];
} }
int bzz_cmd() { int bzz_cmd()
{
int cmd = buzz_cmd; int cmd = buzz_cmd;
buzz_cmd = 0; buzz_cmd = 0;
return cmd; return cmd;
} }
void rc_set_goto(int id, double latitude, double longitude, double altitude) { void rc_set_goto(int id, double latitude, double longitude, double altitude)
{
rc_id = id; rc_id = id;
rc_goto_pos[0] = latitude; rc_goto_pos[0] = latitude;
rc_goto_pos[1] = longitude; rc_goto_pos[1] = longitude;
rc_goto_pos[2] = altitude; rc_goto_pos[2] = altitude;
} }
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) { void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
{
rc_id = id; rc_id = id;
rc_gimbal[0] = yaw; rc_gimbal[0] = yaw;
rc_gimbal[1] = roll; rc_gimbal[1] = roll;
rc_gimbal[2] = pitch; rc_gimbal[2] = pitch;
rc_gimbal[3] = t; rc_gimbal[3] = t;
} }
void rc_call(int rc_cmd_in) { void rc_call(int rc_cmd_in)
{
rc_cmd = rc_cmd_in; rc_cmd = rc_cmd_in;
} }
void set_obstacle_dist(float dist[]) { void set_obstacle_dist(float dist[])
{
for (int i = 0; i < 5; i++) for (int i = 0; i < 5; i++)
obst[i] = dist[i]; obst[i] = dist[i];
} }
void set_battery(float voltage,float current,float remaining){ void set_battery(float voltage, float current, float remaining)
{
batt[0] = voltage; batt[0] = voltage;
batt[1] = current; batt[1] = current;
batt[2] = remaining; batt[2] = remaining;
} }
int buzzuav_update_battery(buzzvm_t vm) { int buzzuav_update_battery(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
@ -539,7 +575,8 @@ int buzzros_print(buzzvm_t vm)
api_rssi = value; api_rssi = value;
} }
int buzzuav_update_xbee_status(buzzvm_t vm) { int buzzuav_update_xbee_status(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
@ -569,13 +606,15 @@ int buzzros_print(buzzvm_t vm)
/***************************************/ /***************************************/
/*current pos update*/ /*current pos update*/
/***************************************/ /***************************************/
void set_currentpos(double latitude, double longitude, double altitude){ void set_currentpos(double latitude, double longitude, double altitude)
{
cur_pos[0] = latitude; cur_pos[0] = latitude;
cur_pos[1] = longitude; cur_pos[1] = longitude;
cur_pos[2] = altitude; cur_pos[2] = altitude;
} }
/*adds neighbours position*/ /*adds neighbours position*/
void neighbour_pos_callback(int id, float range, float bearing, float elevation){ void neighbour_pos_callback(int id, float range, float bearing, float elevation)
{
buzz_utility::Pos_struct pos_arr; buzz_utility::Pos_struct pos_arr;
pos_arr.x = range; pos_arr.x = range;
pos_arr.y = bearing; pos_arr.y = bearing;
@ -587,22 +626,21 @@ int buzzros_print(buzzvm_t vm)
} }
/* update at each step the VM table */ /* update at each step the VM table */
void update_neighbors(buzzvm_t vm){ void update_neighbors(buzzvm_t vm)
{
/* Reset neighbor information */ /* Reset neighbor information */
buzzneighbors_reset(vm); buzzneighbors_reset(vm);
/* Get robot id and update neighbor information */ /* Get robot id and update neighbor information */
map<int, buzz_utility::Pos_struct>::iterator it; map<int, buzz_utility::Pos_struct>::iterator it;
for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){ for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it)
buzzneighbors_add(vm, {
it->first, buzzneighbors_add(vm, it->first, (it->second).x, (it->second).y, (it->second).z);
(it->second).x,
(it->second).y,
(it->second).z);
} }
} }
/****************************************/ /****************************************/
int buzzuav_update_currentpos(buzzvm_t vm) { int buzzuav_update_currentpos(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
@ -621,7 +659,8 @@ int buzzros_print(buzzvm_t vm)
return vm->state; return vm->state;
} }
void flight_status_update(uint8_t state){ void flight_status_update(uint8_t state)
{
status = state; status = state;
} }
@ -630,7 +669,8 @@ int buzzros_print(buzzvm_t vm)
/ and current position of the robot / and current position of the robot
/ TODO: change global name for robot / TODO: change global name for robot
/------------------------------------------------------*/ /------------------------------------------------------*/
int buzzuav_update_flight_status(buzzvm_t vm) { int buzzuav_update_flight_status(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
@ -666,8 +706,6 @@ int buzzros_print(buzzvm_t vm)
return vm->state; return vm->state;
} }
/******************************************************/ /******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/ /*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script /* Acessing proximity in buzz script
@ -677,8 +715,8 @@ int buzzros_print(buzzvm_t vm)
proximity[4].angle = -1 and proximity[4].value -bottom */ proximity[4].angle = -1 and proximity[4].value -bottom */
/******************************************************/ /******************************************************/
int buzzuav_update_prox(buzzvm_t vm) { int buzzuav_update_prox(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1); buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
@ -687,7 +725,8 @@ int buzzros_print(buzzvm_t vm)
/* Fill into the proximity table */ /* Fill into the proximity table */
buzzobj_t tProxRead; buzzobj_t tProxRead;
float angle = 0; float angle = 0;
for(size_t i = 0; i < 4; ++i) { for (size_t i = 0; i < 4; ++i)
{
/* Create table for i-th read */ /* Create table for i-th read */
buzzvm_pusht(vm); buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1); tProxRead = buzzvm_stack_at(vm, 1);
@ -734,6 +773,8 @@ int buzzros_print(buzzvm_t vm)
/*Dummy closure for use during update testing */ /*Dummy closure for use during update testing */
/**********************************************/ /**********************************************/
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);} int dummy_closure(buzzvm_t vm)
{
return buzzvm_ret0(vm);
}
} }

2
src/rosbuzz.cpp
View File

@ -24,5 +24,3 @@ int main(int argc, char **argv)
RosController.RosControllerRun(); RosController.RosControllerRun();
return 0; return 0;
} }

369
src/roscontroller.cpp
View File

@ -8,8 +8,8 @@
#include "roscontroller.h" #include "roscontroller.h"
#include <thread> #include <thread>
namespace rosbzz_node { namespace rosbzz_node
{
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
const bool debug = false; const bool debug = false;
@ -45,15 +45,19 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv)
cur_pos.latitude = 0; cur_pos.latitude = 0;
cur_pos.altitude = 0; cur_pos.altitude = 0;
while (cur_pos.latitude == 0.0f) { while (cur_pos.latitude == 0.0f)
{
ROS_INFO("Waiting for GPS. "); ROS_INFO("Waiting for GPS. ");
ros::Duration(0.5).sleep(); ros::Duration(0.5).sleep();
ros::spinOnce(); ros::spinOnce();
} }
if (xbeeplugged) { if (xbeeplugged)
{
GetRobotId(); GetRobotId();
} else { }
else
{
robot_id = strtol(robot_name.c_str() + 5, NULL, 10); robot_id = strtol(robot_name.c_str() + 5, NULL, 10);
} }
} }
@ -91,7 +95,10 @@ bool roscontroller::GetDequeFull(bool &result)
srv_request.param_id = "deque_full"; srv_request.param_id = "deque_full";
mavros_msgs::ParamGet::Response srv_response; 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; result = (srv_response.value.integer == 1) ? true : false;
return srv_response.success; return srv_response.success;
@ -103,7 +110,10 @@ bool roscontroller::GetRssi(float &result)
srv_request.param_id = "rssi"; srv_request.param_id = "rssi";
mavros_msgs::ParamGet::Response srv_response; 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; result = srv_response.value.real;
return srv_response.success; return srv_response.success;
@ -121,7 +131,10 @@ bool roscontroller::TriggerAPIRssi(const uint8_t short_id)
srv_request.param_id = "trig_rssi_api_" + std::to_string(short_id); srv_request.param_id = "trig_rssi_api_" + std::to_string(short_id);
} }
mavros_msgs::ParamGet::Response srv_response; 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; return srv_response.success;
} }
@ -138,7 +151,10 @@ bool roscontroller::GetAPIRssi(const uint8_t short_id, float &result)
srv_request.param_id = "get_rssi_api_" + std::to_string(short_id); srv_request.param_id = "get_rssi_api_" + std::to_string(short_id);
} }
mavros_msgs::ParamGet::Response srv_response; 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; result = srv_response.value.real;
return srv_response.success; return srv_response.success;
@ -156,7 +172,10 @@ bool roscontroller::GetRawPacketLoss(const uint8_t short_id, float &result)
srv_request.param_id = "pl_raw_" + std::to_string(short_id); srv_request.param_id = "pl_raw_" + std::to_string(short_id);
} }
mavros_msgs::ParamGet::Response srv_response; 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; result = srv_response.value.real;
return srv_response.success; return srv_response.success;
@ -174,13 +193,17 @@ bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float &result)
srv_request.param_id = "pl_filtered_" + std::to_string(short_id); srv_request.param_id = "pl_filtered_" + std::to_string(short_id);
} }
mavros_msgs::ParamGet::Response srv_response; 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; result = srv_response.value.real;
return srv_response.success; return srv_response.success;
} }
void roscontroller::send_MPpayload(){ void roscontroller::send_MPpayload()
{
MPpayload_pub.publish(buzzuav_closures::get_status()); MPpayload_pub.publish(buzzuav_closures::get_status());
} }
@ -189,10 +212,9 @@ void roscontroller::send_MPpayload(){
/--------------------------------------------------*/ /--------------------------------------------------*/
void roscontroller::RosControllerRun() void roscontroller::RosControllerRun()
{ {
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id))
robot_id)) { {
ROS_INFO("[%i] INIT DONE!!!", robot_id); ROS_INFO("[%i] INIT DONE!!!", robot_id);
int packet_loss_tmp, time_step = 0; int packet_loss_tmp, time_step = 0;
double cur_packet_loss = 0; double cur_packet_loss = 0;
@ -206,24 +228,30 @@ void roscontroller::RosControllerRun()
// MAIN LOOP // MAIN LOOP
////////////////////////////////////////////////////// //////////////////////////////////////////////////////
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
while (ros::ok() && !buzz_utility::buzz_script_done()) { while (ros::ok() && !buzz_utility::buzz_script_done())
{
/*Neighbours of the robot published with id in respective topic*/ /*Neighbours of the robot published with id in respective topic*/
neighbours_pos_publisher(); neighbours_pos_publisher();
send_MPpayload(); send_MPpayload();
/*Check updater state and step code*/ /*Check updater state and step code*/
update_routine(); update_routine();
if(time_step==BUZZRATE){ if (time_step == BUZZRATE)
{
time_step = 0; time_step = 0;
cur_packet_loss = 1 - ((double)packet_loss_tmp / (BUZZRATE * ((int)no_of_robots - 1))); cur_packet_loss = 1 - ((double)packet_loss_tmp / (BUZZRATE * ((int)no_of_robots - 1)));
packet_loss_tmp = 0; packet_loss_tmp = 0;
if(cur_packet_loss<0) cur_packet_loss=0; if (cur_packet_loss < 0)
else if (cur_packet_loss>1) cur_packet_loss=1; cur_packet_loss = 0;
else if (cur_packet_loss > 1)
cur_packet_loss = 1;
} }
else{ else
{
packet_loss_tmp += (int)buzz_utility::get_inmsg_size(); packet_loss_tmp += (int)buzz_utility::get_inmsg_size();
time_step++; 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 */ /*Step buzz script */
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/ /*Prepare messages and publish them in respective topic*/
@ -245,7 +273,8 @@ void roscontroller::RosControllerRun()
loop_rate.sleep(); loop_rate.sleep();
// make sure to sleep for the remainder of our cycle time // make sure to sleep for the remainder of our cycle time
if (loop_rate.cycleTime() > ros::Duration(1.0 / (float)BUZZRATE)) 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) if (fcu_timeout <= 0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
else else
@ -263,7 +292,6 @@ void roscontroller::RosControllerRun()
/-------------------------------------------------------*/ /-------------------------------------------------------*/
void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
{ {
/*Obtain .bzz file name from parameter server*/ /*Obtain .bzz file name from parameter server*/
if (n_c.getParam("bzzfile_name", bzzfile_name)) 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*/ /*Obtain rc service option from parameter server*/
if (n_c.getParam("rcclient", rcclient)) if (n_c.getParam("rcclient", rcclient))
{ {
if (rcclient == true) { if (rcclient == true)
{
/*Service*/ /*Service*/
if (!n_c.getParam("rcservice_name", rcservice_name)) if (!n_c.getParam("rcservice_name", rcservice_name))
{ {
ROS_ERROR("Provide a name topic name for rc service in Launch file"); ROS_ERROR("Provide a name topic name for rc service in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
} else if (rcclient == false) }
else if (rcclient == false)
{ {
ROS_INFO("RC service is disabled"); ROS_INFO("RC service is disabled");
} }
} else }
else
{ {
ROS_ERROR("Provide a rc client option: yes or no in Launch file"); ROS_ERROR("Provide a rc client option: yes or no in Launch file");
system("rosnode kill rosbuzz_node"); 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"); ROS_ERROR("Provide the xbee plugged boolean in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
if (!xbeeplugged) { if (!xbeeplugged)
{
if (n_c.getParam("name", robot_name)) if (n_c.getParam("name", robot_name))
; ;
else else
@ -313,7 +345,8 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
ROS_ERROR("Provide the xbee plugged boolean in Launch file"); ROS_ERROR("Provide the xbee plugged boolean in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
} else }
else
n_c.getParam("xbee_status_srv", xbeesrv_name); 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))
@ -441,18 +474,26 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
/--------------------------------------*/ /--------------------------------------*/
void roscontroller::Subscribe(ros::NodeHandle& n_c) void roscontroller::Subscribe(ros::NodeHandle& n_c)
{ {
for (std::map<std::string, std::string>::iterator it = for (std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it)
m_smTopic_infos.begin(); {
it != m_smTopic_infos.end(); ++it) { if (it->second == "mavros_msgs/ExtendedState")
if (it->second == "mavros_msgs/ExtendedState") { {
flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this); 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); 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); 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); 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); 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*/ /*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile; stringstream bzzfile_in_compile;
std::string path = std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0, name.find_last_of(".")); name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << "bzzc -I " << path bzzfile_in_compile << "bzzc -I " << path << "include/";
<< "include/";
bzzfile_in_compile << " -b " << path << name << ".bo"; bzzfile_in_compile << " -b " << path << name << ".bo";
bzzfile_in_compile << " -d " << path << name << ".bdb "; bzzfile_in_compile << " -d " << path << name << ".bdb ";
bzzfile_in_compile << bzzfile_name; bzzfile_in_compile << bzzfile_name;
@ -493,8 +532,8 @@ void roscontroller::neighbours_pos_publisher()
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.frame_id = "/world";
neigh_pos_array.header.stamp = current_time; neigh_pos_array.header.stamp = current_time;
for (it = raw_neighbours_pos_map.begin(); it != raw_neighbours_pos_map.end(); for (it = raw_neighbours_pos_map.begin(); it != raw_neighbours_pos_map.end(); ++it)
++it) { {
sensor_msgs::NavSatFix neigh_tmp; sensor_msgs::NavSatFix neigh_tmp;
// cout<<"iterator it val: "<< it-> first << " After convertion: " // cout<<"iterator it val: "<< it-> first << " After convertion: "
// <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl; // <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;
@ -517,12 +556,15 @@ void roscontroller::Arm()
{ {
mavros_msgs::CommandBool arming_message; mavros_msgs::CommandBool arming_message;
arming_message.request.value = armstate; arming_message.request.value = armstate;
if (arm_client.call(arming_message)) { if (arm_client.call(arming_message))
{
if (arming_message.response.success == 1) if (arming_message.response.success == 1)
ROS_WARN("FC Arm Service called!"); ROS_WARN("FC Arm Service called!");
else else
ROS_WARN("FC Arm Service call failed!"); ROS_WARN("FC Arm Service call failed!");
} else { }
else
{
ROS_WARN("FC Arm Service call failed!"); ROS_WARN("FC Arm Service call failed!");
} }
} }
@ -561,7 +603,8 @@ void roscontroller::prepare_msg_and_publish()
payload_out.payload64.push_back(position[2]); payload_out.payload64.push_back(position[2]);
/*Append Buzz message*/ /*Append Buzz message*/
uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]); uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]);
for (int i = 0; i < out[0]; i++) { for (int i = 0; i < out[0]; i++)
{
payload_out.payload64.push_back(payload_out_ptr[i]); payload_out.payload64.push_back(payload_out_ptr[i]);
} }
/*Add Robot id and message number to the published message*/ /*Add Robot id and message number to the published message*/
@ -599,12 +642,12 @@ void roscontroller::prepare_msg_and_publish()
destroy_out_msg_queue(); destroy_out_msg_queue();
uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t))); uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size]; uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void *)payload_64, (void *)buff_send, memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t));
total_size * sizeof(uint64_t));
free(buff_send); free(buff_send);
// Send a constant number to differenciate updater msgs // Send a constant number to differenciate updater msgs
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT); 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]); update_packets.payload64.push_back(payload_64[i]);
} }
// Add Robot id and message number to the published message // Add Robot id and message number to the published message
@ -629,13 +672,14 @@ void roscontroller::flight_controller_service_call()
/* flight controller client call if requested from Buzz*/ /* flight controller client call if requested from Buzz*/
double* goto_pos; double* goto_pos;
float* gimbal; float* gimbal;
switch (buzzuav_closures::bzz_cmd()) { switch (buzzuav_closures::bzz_cmd())
{
case buzzuav_closures::COMMAND_TAKEOFF: case buzzuav_closures::COMMAND_TAKEOFF:
goto_pos = buzzuav_closures::getgoto(); goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (!armstate) { if (!armstate)
{
SetMode("LOITER", 0); SetMode("LOITER", 0);
armstate = 1; armstate = 1;
Arm(); Arm();
@ -646,22 +690,28 @@ void roscontroller::flight_controller_service_call()
if (current_mode != "GUIDED") if (current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it
// should always be in loiter after arm/disarm) // should always be in loiter after arm/disarm)
if (mav_client.call(cmd_srv)) { if (mav_client.call(cmd_srv))
{
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else }
else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
break; break;
case buzzuav_closures::COMMAND_LAND: case buzzuav_closures::COMMAND_LAND:
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (current_mode != "LAND") { if (current_mode != "LAND")
{
SetMode("LAND", 0); SetMode("LAND", 0);
armstate = 0; armstate = 0;
Arm(); Arm();
} }
if (mav_client.call(cmd_srv)) { if (mav_client.call(cmd_srv))
{
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else { }
else
{
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
} }
armstate = 0; armstate = 0;
@ -672,9 +722,11 @@ void roscontroller::flight_controller_service_call()
cmd_srv.request.param6 = home.longitude; cmd_srv.request.param6 = home.longitude;
cmd_srv.request.param7 = home.altitude; cmd_srv.request.param7 = home.altitude;
cmd_srv.request.command = buzzuav_closures::getcmd(); 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); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else }
else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
break; break;
@ -684,19 +736,24 @@ void roscontroller::flight_controller_service_call()
cmd_srv.request.param6 = goto_pos[1]; cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd(); 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); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else }
else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; 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); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else }
else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
break; break;
case buzzuav_closures::COMMAND_ARM: case buzzuav_closures::COMMAND_ARM:
if (!armstate) { if (!armstate)
{
SetMode("LOITER", 0); SetMode("LOITER", 0);
armstate = 1; armstate = 1;
Arm(); Arm();
@ -704,7 +761,8 @@ void roscontroller::flight_controller_service_call()
break; break;
case buzzuav_closures::COMMAND_DISARM: case buzzuav_closures::COMMAND_DISARM:
if (armstate) { if (armstate)
{
armstate = 0; armstate = 0;
SetMode("LOITER", 0); SetMode("LOITER", 0);
Arm(); Arm();
@ -723,18 +781,22 @@ void roscontroller::flight_controller_service_call()
cmd_srv.request.param3 = gimbal[2]; cmd_srv.request.param3 = gimbal[2];
cmd_srv.request.param4 = gimbal[3]; cmd_srv.request.param4 = gimbal[3];
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; 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); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else }
else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
break; break;
case buzzuav_closures::COMMAND_PICTURE: case buzzuav_closures::COMMAND_PICTURE:
ROS_INFO("TAKING A PICTURE HERE!! --------------"); ROS_INFO("TAKING A PICTURE HERE!! --------------");
mavros_msgs::CommandBool capture_command; 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); ROS_INFO("Reply: %ld", (long int)capture_command.response.success);
} else }
else
ROS_ERROR("Failed to call service from camera streamer"); ROS_ERROR("Failed to call service from camera streamer");
break; break;
} }
@ -743,8 +805,10 @@ void roscontroller::flight_controller_service_call()
/*---------------------------------------------- /*----------------------------------------------
/Refresh neighbours Position for every ten step /Refresh neighbours Position for every ten step
/---------------------------------------------*/ /---------------------------------------------*/
void roscontroller::maintain_pos(int tim_step) { void roscontroller::maintain_pos(int tim_step)
if (timer_step >= BUZZRATE) { {
if (timer_step >= BUZZRATE)
{
neighbours_pos_map.clear(); neighbours_pos_map.clear();
// raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
// have to clear ! // 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 /Puts neighbours position into local struct storage that is cleared every 10
step step
/---------------------------------------------------------------------------------*/ /---------------------------------------------------------------------------------*/
void roscontroller::neighbours_pos_put(int id, void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr)
buzz_utility::Pos_struct pos_arr) { {
map<int, buzz_utility::Pos_struct>::iterator it = neighbours_pos_map.find(id); map<int, buzz_utility::Pos_struct>::iterator it = neighbours_pos_map.find(id);
if (it != neighbours_pos_map.end()) if (it != neighbours_pos_map.end())
neighbours_pos_map.erase(it); 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 /Puts raw neighbours position into local storage for neighbours pos publisher
/-----------------------------------------------------------------------------------*/ /-----------------------------------------------------------------------------------*/
void roscontroller::raw_neighbours_pos_put(int id, void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr)
buzz_utility::Pos_struct pos_arr) { {
map<int, buzz_utility::Pos_struct>::iterator it = map<int, buzz_utility::Pos_struct>::iterator it = raw_neighbours_pos_map.find(id);
raw_neighbours_pos_map.find(id);
if (it != raw_neighbours_pos_map.end()) if (it != raw_neighbours_pos_map.end())
raw_neighbours_pos_map.erase(it); raw_neighbours_pos_map.erase(it);
raw_neighbours_pos_map.insert(make_pair(id, pos_arr)); 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 /Set the current position of the robot callback
/--------------------------------------------------------*/ /--------------------------------------------------------*/
void roscontroller::set_cur_pos(double latitude, double longitude, void roscontroller::set_cur_pos(double latitude, double longitude, double altitude)
double altitude) { {
cur_pos.latitude = latitude; cur_pos.latitude = latitude;
cur_pos.longitude = longitude; cur_pos.longitude = longitude;
cur_pos.altitude = altitude; cur_pos.altitude = altitude;
@ -789,14 +852,16 @@ void roscontroller::set_cur_pos(double latitude, double longitude,
/ Compute Range and Bearing of a neighbor in a local reference frame / Compute Range and Bearing of a neighbor in a local reference frame
/ from GPS coordinates / from GPS coordinates
----------------------------------------------------------- */ ----------------------------------------------------------- */
float roscontroller::constrainAngle(float x) { float roscontroller::constrainAngle(float x)
{
x = fmod(x, 2 * M_PI); x = fmod(x, 2 * M_PI);
if (x < 0.0) if (x < 0.0)
x += 2 * M_PI; x += 2 * M_PI;
return x; 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; float ned_x = 0.0, ned_y = 0.0;
gps_ned_cur(ned_x, ned_y, nei_pos); gps_ned_cur(ned_x, ned_y, nei_pos);
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y); out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
@ -807,19 +872,19 @@ void roscontroller::gps_rb(GPS nei_pos, double out[]) {
out[2] = 0.0; out[2] = 0.0;
} }
void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t) { 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); 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) { 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); 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, 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_t_lon, double gps_t_lat, double gps_r_lat)
double gps_r_lon, double gps_r_lat) { {
double d_lon = gps_t_lon - gps_r_lon; double d_lon = gps_t_lon - gps_r_lon;
double d_lat = gps_t_lat - gps_r_lat; double d_lat = gps_t_lat - gps_r_lat;
ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; 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 / 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); buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining);
// ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, // ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage,
// msg->current, msg ->remaining); // 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 /Update flight extended status into BVM from subscriber for solos
/---------------------------------------------------------------------*/ /---------------------------------------------------------------------*/
void roscontroller::flight_status_update( void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg)
const mavros_msgs::State::ConstPtr &msg) { {
// http://wiki.ros.org/mavros/CustomModes // http://wiki.ros.org/mavros/CustomModes
// TODO: Handle landing // TODO: Handle landing
std::cout << "Message: " << msg->mode << std::endl; std::cout << "Message: " << msg->mode << std::endl;
@ -854,14 +920,15 @@ void roscontroller::flight_status_update(
/*------------------------------------------------------------ /*------------------------------------------------------------
/Update flight extended status into BVM from subscriber /Update flight extended status into BVM from subscriber
------------------------------------------------------------*/ ------------------------------------------------------------*/
void roscontroller::flight_extended_status_update( void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg)
const mavros_msgs::ExtendedState::ConstPtr &msg) { {
buzzuav_closures::flight_status_update(msg->landed_state); buzzuav_closures::flight_status_update(msg->landed_state);
} }
/*------------------------------------------------------------- /*-------------------------------------------------------------
/ Update current position into BVM from subscriber / 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); // ROS_INFO("Altitude out: %f", cur_rel_altitude);
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
// double lat = std::floor(msg->latitude * 1000000) / 1000000; // 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[1]=lon;
home[2]=cur_rel_altitude; home[2]=cur_rel_altitude;
}*/ }*/
set_cur_pos(msg->latitude, msg->longitude, set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude);
cur_rel_altitude); // msg->altitude); buzzuav_closures::set_currentpos(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( void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose)
const geometry_msgs::PoseStamped::ConstPtr &pose) { {
local_pos_new[0] = pose->pose.position.x; local_pos_new[0] = pose->pose.position.x;
local_pos_new[1] = pose->pose.position.y; local_pos_new[1] = pose->pose.position.y;
local_pos_new[2] = pose->pose.position.z; local_pos_new[2] = pose->pose.position.z;
@ -887,21 +952,24 @@ void roscontroller::local_pos_callback(
/*------------------------------------------------------------- /*-------------------------------------------------------------
/ Update altitude into BVM from subscriber / 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); // ROS_INFO("Altitude in: %f", msg->data);
cur_rel_altitude = (double)msg->data; cur_rel_altitude = (double)msg->data;
} }
/*------------------------------------------------------------- /*-------------------------------------------------------------
/Set obstacle Obstacle distance table into BVM from subscriber /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]; float obst[5];
for (int i = 0; i < 5; i++) for (int i = 0; i < 5; i++)
obst[i] = msg->ranges[i]; obst[i] = msg->ranges[i];
buzzuav_closures::set_obstacle_dist(obst); 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://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 // http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
@ -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 // wait if necessary
if (delay_miliseconds != 0) { if (delay_miliseconds != 0)
{
std::this_thread::sleep_for(std::chrono::milliseconds(delay_miliseconds)); std::this_thread::sleep_for(std::chrono::milliseconds(delay_miliseconds));
} }
// set mode // set mode
@ -937,20 +1007,25 @@ void roscontroller::SetMode(std::string mode, int delay_miliseconds) {
set_mode_message.request.base_mode = 0; set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = mode; set_mode_message.request.custom_mode = mode;
current_mode = mode; current_mode = mode;
if (mode_client.call(set_mode_message)) { if (mode_client.call(set_mode_message))
{
; // ROS_INFO("Set Mode Service call successful!"); ; // ROS_INFO("Set Mode Service call successful!");
} else { }
else
{
ROS_INFO("Set Mode Service call failed!"); 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; mavros_msgs::StreamRate message;
message.request.stream_id = id; message.request.stream_id = id;
message.request.message_rate = rate; message.request.message_rate = rate;
message.request.on_off = on_off; 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_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
} }
@ -972,13 +1047,16 @@ 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*/ /*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()); uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size());
uint64_t message_obt[obt_msg_size]; uint64_t message_obt[obt_msg_size];
/* Go throught the obtained payload*/ /* 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]; message_obt[i] = (uint64_t)msg->payload64[i];
} }
@ -988,25 +1066,26 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size); memcpy((void*)pl, (void*)(message_obt + 1), obt_msg_size);
uint16_t unMsgSize = *(uint16_t*)(pl); uint16_t unMsgSize = *(uint16_t*)(pl);
fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize);
if (unMsgSize > 0) { if (unMsgSize > 0)
code_message_inqueue_append((uint8_t *)(pl + sizeof(uint16_t)), {
unMsgSize); code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize);
code_message_inqueue_process(); code_message_inqueue_process();
} }
free(pl); free(pl);
} }
/*BVM FIFO message*/ /*BVM FIFO message*/
else if (msg->payload64[0]==(uint64_t)XBEE_MESSAGE_CONSTANT) { else if (msg->payload64[0] == (uint64_t)XBEE_MESSAGE_CONSTANT)
{
uint64_t message_obt[msg->payload64.size() - 1]; uint64_t message_obt[msg->payload64.size() - 1];
/* Go throught the obtained payload*/ /* Go throught the obtained payload*/
for (int i = 1; i < (int)msg->payload64.size(); i++) { for (int i = 1; i < (int)msg->payload64.size(); i++)
{
message_obt[i - 1] = (uint64_t)msg->payload64[i]; message_obt[i - 1] = (uint64_t)msg->payload64[i];
} }
/* Extract neighbours position from payload*/ /* Extract neighbours position from payload*/
double neighbours_pos_payload[3]; double neighbours_pos_payload[3];
memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t));
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0], buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0], neighbours_pos_payload[1],
neighbours_pos_payload[1],
neighbours_pos_payload[2]); neighbours_pos_payload[2]);
GPS nei_pos; GPS nei_pos;
nei_pos.latitude = neighbours_pos_payload[0]; nei_pos.latitude = neighbours_pos_payload[0];
@ -1016,17 +1095,16 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
gps_rb(nei_pos, cvt_neighbours_pos_payload); gps_rb(nei_pos, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/ /*Extract robot id of the neighbour*/
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); 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]); 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*/ /*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1],
cvt_neighbours_pos_payload[1],
cvt_neighbours_pos_payload[2]); cvt_neighbours_pos_payload[2]);
/*Put RID and pos*/ /*Put RID and pos*/
raw_neighbours_pos_put((int)out[1], raw_neigh_pos); raw_neighbours_pos_put((int)out[1], raw_neigh_pos);
/* TODO: remove roscontroller local map array for neighbors */ /* TODO: remove roscontroller local map array for neighbors */
neighbours_pos_put((int)out[1], n_pos); neighbours_pos_put((int)out[1], n_pos);
buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z);
n_pos.z);
delete[] out; delete[] out;
buzz_utility::in_msg_append((message_obt + 3)); 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) / Catch the ROS service call from a custom remote controller (Mission Planner)
/ and send the requested commands to Buzz / and send the requested commands to Buzz
----------------------------------------------------------- */ ----------------------------------------------------------- */
bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res)
mavros_msgs::CommandLong::Response &res) { {
int rc_cmd; int rc_cmd;
switch (req.command) { switch (req.command)
{
case mavros_msgs::CommandCode::NAV_TAKEOFF: case mavros_msgs::CommandCode::NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!"); ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF; 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: case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
armstate = req.param1; armstate = req.param1;
if (armstate) { if (armstate)
{
ROS_INFO("RC_Call: ARM!!!!"); ROS_INFO("RC_Call: ARM!!!!");
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
} else { }
else
{
ROS_INFO("RC_Call: DISARM!!!!"); ROS_INFO("RC_Call: DISARM!!!!");
buzzuav_closures::rc_call(rc_cmd + 1); buzzuav_closures::rc_call(rc_cmd + 1);
res.success = true; res.success = true;
@ -1100,23 +1182,31 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
return true; 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; 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; no_of_robots = cur_robots;
}
} else { else
if (no_of_robots != cur_robots && no_cnt == 0) { {
if (no_of_robots != cur_robots && no_cnt == 0)
{
no_cnt++; no_cnt++;
old_val = cur_robots; old_val = cur_robots;
}
} else if (no_cnt != 0 && old_val == cur_robots) { else if (no_cnt != 0 && old_val == cur_robots)
{
no_cnt++; 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_of_robots = cur_robots;
no_cnt = 0; no_cnt = 0;
} }
} else { }
else
{
no_cnt = 0; no_cnt = 0;
} }
} }
@ -1155,5 +1245,4 @@ void roscontroller::get_xbee_status()
* TriggerAPIRssi(all_ids); * TriggerAPIRssi(all_ids);
*/ */
} }
} }

8
src/uav_utility.cpp
View File

@ -8,16 +8,16 @@
/*To do !*/ /*To do !*/
/****************************************/ /****************************************/
void uav_setup() { void uav_setup()
{
} }
/****************************************/ /****************************************/
/*To do !*/ /*To do !*/
/****************************************/ /****************************************/
void uav_done() { void uav_done()
{
fprintf(stdout, "Robot stopped.\n"); fprintf(stdout, "Robot stopped.\n");
} }