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

25
include/buzz_utility.h
View File

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

4
include/buzzuav_closures.h
View File

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

26
include/roscontroller.h
View File

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

1
include/uav_utility.h
View File

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

304
src/buzz_update.cpp
View File

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

221
src/buzz_utility.cpp
View File

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

175
src/buzzuav_closures.cpp
View File

@ -9,7 +9,8 @@
#include "buzzuav_closures.h"
#include "math.h"
namespace buzzuav_closures{
namespace buzzuav_closures
{
// TODO: Minimize the required global variables and put them in the header
// static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/
@ -88,7 +89,8 @@ int buzzros_print(buzzvm_t vm)
return buzzvm_ret0(vm);
}
void setWPlist(string path){
void setWPlist(string path)
{
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
/----------------------------------------*/
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 lon = RAD2DEG(cur_pos[1]);
out[0] = asin(sin(lat) * cos(range / EARTH_RADIUS) + cos(lat) * sin(range / EARTH_RADIUS) * cos(bearing));
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
out[1] = lon + atan2(sin(bearing) * sin(range / EARTH_RADIUS) * cos(lat),
cos(range / EARTH_RADIUS) - sin(lat) * sin(out[0]));
out[0] = RAD2DEG(out[0]);
out[1] = RAD2DEG(out[1]);
out[2] = height; // constant height.
}
float constrainAngle(float x){
float constrainAngle(float x)
{
x = fmod(x, 2 * M_PI);
if (x < 0.0)
x += 2 * M_PI;
return x;
}
void rb_from_gps(double nei[], double out[], double cur[]){
void rb_from_gps(double nei[], double out[], double cur[])
{
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
@ -135,7 +141,8 @@ int buzzros_print(buzzvm_t vm)
std::ifstream fin(WPlistname.c_str()); // Open in text-mode.
// Opening may fail, always check.
if (!fin) {
if (!fin)
{
ROS_ERROR("GPS list parser, could not open file.");
return;
}
@ -150,7 +157,8 @@ int buzzros_print(buzzvm_t vm)
int alt, tilt, tid;
buzz_utility::RB_struct RB_arr;
// Read one line at a time.
while ( fin.getline(buffer, MAX_LINE_LENGTH) ) {
while (fin.getline(buffer, MAX_LINE_LENGTH))
{
// Extract the tokens:
tid = atoi(strtok(buffer, DELIMS));
lon = atof(strtok(NULL, DELIMS));
@ -174,11 +182,11 @@ int buzzros_print(buzzvm_t vm)
fin.close();
}
/*----------------------------------------/
/ 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_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */
@ -192,20 +200,26 @@ int buzzros_print(buzzvm_t vm)
goto_pos[0] = dx;
goto_pos[1] = dy;
goto_pos[2] = height + dh;
//ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]);
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
// goto_pos[1], goto_pos[2]);
buzz_cmd = COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm);
}
int buzzuav_update_targets(buzzvm_t vm) {
if(vm->state != BUZZVM_STATE_READY) return vm->state;
int buzzuav_update_targets(buzzvm_t vm)
{
if (vm->state != BUZZVM_STATE_READY)
return vm->state;
buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1));
buzzvm_pusht(vm);
buzzobj_t targettbl = buzzvm_stack_at(vm, 1);
double rb[3], tmp[3];
map<int, buzz_utility::RB_struct>::iterator it;
for (it=targets_map.begin(); it!=targets_map.end(); ++it){
tmp[0]=(it->second).latitude; tmp[1]=(it->second).longitude; tmp[2]=height;
for (it = targets_map.begin(); it != targets_map.end(); ++it)
{
tmp[0] = (it->second).latitude;
tmp[1] = (it->second).longitude;
tmp[2] = height;
rb_from_gps(tmp, rb, cur_pos);
// ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
buzzvm_push(vm, targettbl);
@ -234,7 +248,8 @@ int buzzros_print(buzzvm_t vm)
return vm->state;
}
int buzzuav_addtargetRB(buzzvm_t vm) {
int buzzuav_addtargetRB(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // longitude
buzzvm_lload(vm, 2); // latitude
@ -250,7 +265,8 @@ int buzzros_print(buzzvm_t vm)
double rb[3];
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]);
buzz_utility::RB_struct RB_arr;
RB_arr.latitude = tmp[0];
@ -264,13 +280,15 @@ int buzzros_print(buzzvm_t vm)
targets_map.insert(make_pair(uid, RB_arr));
// ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
return vm->state;
} else
}
else
ROS_WARN(" ---------- Target too far %f", rb[0]);
return 0;
}
int buzzuav_addNeiStatus(buzzvm_t vm){
int buzzuav_addNeiStatus(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 5);
buzzvm_lload(vm, 1); // fc
buzzvm_lload(vm, 2); // xbee
@ -295,10 +313,12 @@ int buzzros_print(buzzvm_t vm)
return vm->state;
}
mavros_msgs::Mavlink get_status(){
mavros_msgs::Mavlink get_status()
{
mavros_msgs::Mavlink payload_out;
map<int, buzz_utility::neighbors_status>::iterator it;
for (it= neighbors_status_map.begin(); it!= neighbors_status_map.end(); ++it){
for (it = neighbors_status_map.begin(); it != neighbors_status_map.end(); ++it)
{
payload_out.payload64.push_back(it->first);
payload_out.payload64.push_back(it->second.gps_strenght);
payload_out.payload64.push_back(it->second.batt_lvl);
@ -315,7 +335,8 @@ int buzzros_print(buzzvm_t vm)
/*----------------------------------------/
/ 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;
buzz_cmd = COMMAND_PICTURE;
return buzzvm_ret0(vm);
@ -324,7 +345,8 @@ int buzzros_print(buzzvm_t vm)
/*----------------------------------------/
/ 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_lload(vm, 1); // time
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
/----------------------------------------*/
int buzzuav_storegoal(buzzvm_t vm) {
int buzzuav_storegoal(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // altitude
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[1] = buzzvm_stack_at(vm, 2)->f.value;
goal[2] = buzzvm_stack_at(vm, 1)->f.value;
if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){
if (goal[0] == -1 && goal[1] == -1 && goal[2] == -1)
{
if (wplist_map.size() <= 0)
parse_gpslist();
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
/----------------------------------------*/
int buzzuav_arm(buzzvm_t vm) {
int buzzuav_arm(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n");
buzz_cmd = COMMAND_ARM;
return buzzvm_ret0(vm);
}
int buzzuav_disarm(buzzvm_t vm) {
int buzzuav_disarm(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n");
buzz_cmd = COMMAND_DISARM;
@ -397,7 +423,8 @@ int buzzros_print(buzzvm_t vm)
/*---------------------------------------/
/ Buzz closure for basic UAV commands
/---------------------------------------*/
int buzzuav_takeoff(buzzvm_t vm) {
int buzzuav_takeoff(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); /* Altitude */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
@ -409,33 +436,37 @@ int buzzros_print(buzzvm_t vm)
return buzzvm_ret0(vm);
}
int buzzuav_land(buzzvm_t vm) {
int buzzuav_land(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
printf(" Buzz requested Land !!! \n");
buzz_cmd = COMMAND_LAND;
return buzzvm_ret0(vm);
}
int buzzuav_gohome(buzzvm_t vm) {
int buzzuav_gohome(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n");
buzz_cmd = COMMAND_GOHOME;
return buzzvm_ret0(vm);
}
/*-------------------------------
/ Get/Set to transfer variable from Roscontroller to buzzuav
/------------------------------------*/
double* getgoto() {
double* getgoto()
{
return goto_pos;
}
float* getgimbal() {
float* getgimbal()
{
return rc_gimbal;
}
string getuavstate() {
string getuavstate()
{
static buzzvm_t VM = buzz_utility::get_vm();
std::stringstream state_buff;
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;
}
int getcmd() {
int getcmd()
{
return cur_cmd;
}
void set_goto(double pos[]) {
void set_goto(double pos[])
{
goto_pos[0] = pos[0];
goto_pos[1] = pos[1];
goto_pos[2] = pos[2];
}
int bzz_cmd() {
int bzz_cmd()
{
int cmd = buzz_cmd;
buzz_cmd = 0;
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_goto_pos[0] = latitude;
rc_goto_pos[1] = longitude;
rc_goto_pos[2] = altitude;
}
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) {
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
{
rc_id = id;
rc_gimbal[0] = yaw;
rc_gimbal[1] = roll;
rc_gimbal[2] = pitch;
rc_gimbal[3] = t;
}
void rc_call(int rc_cmd_in) {
void rc_call(int 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++)
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[1] = current;
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_pusht(vm);
buzzvm_dup(vm);
@ -539,7 +575,8 @@ int buzzros_print(buzzvm_t vm)
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_pusht(vm);
buzzvm_dup(vm);
@ -569,13 +606,15 @@ int buzzros_print(buzzvm_t vm)
/***************************************/
/*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[1] = longitude;
cur_pos[2] = altitude;
}
/*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;
pos_arr.x = range;
pos_arr.y = bearing;
@ -587,22 +626,21 @@ int buzzros_print(buzzvm_t vm)
}
/* update at each step the VM table */
void update_neighbors(buzzvm_t vm){
void update_neighbors(buzzvm_t vm)
{
/* Reset neighbor information */
buzzneighbors_reset(vm);
/* Get robot id and update neighbor information */
map<int, buzz_utility::Pos_struct>::iterator it;
for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
buzzneighbors_add(vm,
it->first,
(it->second).x,
(it->second).y,
(it->second).z);
for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it)
{
buzzneighbors_add(vm, it->first, (it->second).x, (it->second).y, (it->second).z);
}
}
/****************************************/
int buzzuav_update_currentpos(buzzvm_t vm) {
int buzzuav_update_currentpos(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -621,7 +659,8 @@ int buzzros_print(buzzvm_t vm)
return vm->state;
}
void flight_status_update(uint8_t state){
void flight_status_update(uint8_t state)
{
status = state;
}
@ -630,7 +669,8 @@ int buzzros_print(buzzvm_t vm)
/ and current position of the 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_pusht(vm);
buzzvm_dup(vm);
@ -666,8 +706,6 @@ int buzzros_print(buzzvm_t vm)
return vm->state;
}
/******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script
@ -677,8 +715,8 @@ int buzzros_print(buzzvm_t vm)
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_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
@ -687,7 +725,8 @@ int buzzros_print(buzzvm_t vm)
/* Fill into the proximity table */
buzzobj_t tProxRead;
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 */
buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1);
@ -734,6 +773,8 @@ int buzzros_print(buzzvm_t vm)
/*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();
return 0;
}

369
src/roscontroller.cpp
View File

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

8
src/uav_utility.cpp
View File

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