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

51
include/buzz_update.h
View File

@ -10,12 +10,17 @@
#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;
static const uint16_t UPDATE_CODE_HEADER_SIZE=5;
static const uint16_t TIMEOUT_FOR_ROLLBACK=50;
static const uint16_t CODE_REQUEST_PADDING = 250;
static const uint16_t MIN_UPDATE_PACKET = 251;
static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
/*********************/
/* Updater states */
/********************/
@ -23,7 +28,7 @@ static const uint16_t TIMEOUT_FOR_ROLLBACK=50;
typedef enum {
CODE_RUNNING = 0, // Code executing
CODE_STANDBY, // Standing by for others to update
} code_states_e;
} code_states_e;
/*********************/
/*Message types */
@ -32,31 +37,34 @@ typedef enum {
typedef enum {
SENT_CODE = 0, // Broadcast code
RESEND_CODE, // ReBroadcast request
} code_message_e;
} code_message_e;
/*************************/
/*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;
};
typedef struct updater_msgqueue_s* updater_msgqueue_t;
struct updater_code_s {
struct updater_code_s
{
uint8_t* bcode;
uint8_t* bcode_size;
} ;
};
typedef struct updater_code_s* updater_code_t;
/**************************/
/*Updater data*/
/**************************/
struct buzz_updater_elem_s {
struct buzz_updater_elem_s
{
/* robot id */
//uint16_t robotid;
// uint16_t robotid;
/*current Bytecode content */
uint8_t* bcode;
/*old Bytecode name */
@ -78,8 +86,8 @@ struct buzz_updater_elem_s {
/*Current state of the updater one in code_states_e ENUM*/
int* mode;
uint8_t* update_no;
};
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
};
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
/**************************************************************************/
/*Updater routine from msg processing to file checks to be called from main*/
@ -89,15 +97,13 @@ void update_routine();
/************************************************/
/*Initalizes the updater */
/************************************************/
void init_update_monitor(const char* bo_filename,const char* stand_by_script,
const char* dbgfname, int robot_id);
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
/*********************************************************/
/*Appends buffer of given size to in msg queue of updater*/
/*********************************************************/
void code_message_inqueue_append(uint8_t* msg,uint16_t size);
void code_message_inqueue_append(uint8_t* msg, uint16_t size);
/*********************************************************/
/*Processes messages inside the queue of the updater*/
@ -125,14 +131,13 @@ void destroy_out_msg_queue();
/***************************************************/
int get_update_mode();
buzz_updater_elem_t get_updater();
/***************************************************/
/*sets bzz file name*/
/***************************************************/
void set_bzz_file(const char* in_bzz_file);
int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size);
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
/****************************************************/
/*Destroys the updater*/
@ -152,5 +157,5 @@ void updates_set_robots(int robots);
void set_packet_id(int packet_id);
void collect_data(std::ofstream &logger);
void collect_data(std::ofstream& logger);
#endif

37
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(){}
double x, y, z;
pos_struct(double x, double y, double z) : x(x), y(y), z(z){};
pos_struct()
{
}
};
typedef struct pos_struct Pos_struct;
struct rb_struct
{
double r,b,latitude,longitude,altitude;
rb_struct(double la, double lo, double al, double r,double b):latitude(la),longitude(lo),altitude(al),r(r),b(b){};
rb_struct(){}
double r, b, latitude, longitude, altitude;
rb_struct(double la, double lo, double al, double r, double b)
: latitude(la), longitude(lo), altitude(al), r(r), b(b){};
rb_struct()
{
}
};
typedef struct rb_struct RB_struct;
@ -35,16 +40,15 @@ struct neiStatus
uint batt_lvl = 0;
uint xbee = 0;
uint flight_status = 0;
}; typedef struct neiStatus neighbors_status ;
};
typedef struct neiStatus neighbors_status;
uint16_t* u64_cvt_u16(uint64_t u64);
int buzz_listen(const char* type,
int msg_size);
int buzz_listen(const char* type, int msg_size);
int make_table(buzzobj_t* t);
int buzzusers_reset();
int create_stig_tables();
int create_stig_tables();
void in_msg_append(uint64_t* payload);
@ -52,12 +56,11 @@ uint64_t* obt_out_msg();
void update_sensors();
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id);
int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id);
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size);
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size);
void buzz_script_step();

18
include/buzzuav_closures.h
View File

@ -8,12 +8,13 @@
#include "ros/ros.h"
#include "buzz_utility.h"
#define EARTH_RADIUS (double) 6371000.0
#define DEG2RAD(DEG) (double) ((DEG)*((M_PI)/(180.0)))
#define RAD2DEG(RAD) (double) ((RAD)*((180.0)/(M_PI)))
#define EARTH_RADIUS (double)6371000.0
#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))
#define RAD2DEG(RAD) (double)((RAD) * ((180.0) / (M_PI)))
namespace buzzuav_closures{
typedef enum {
namespace buzzuav_closures
{
typedef enum {
COMMAND_NIL = 0, // Dummy command
COMMAND_TAKEOFF, // Take off
COMMAND_LAND,
@ -24,7 +25,7 @@ namespace buzzuav_closures{
COMMAND_MOVETO,
COMMAND_PICTURE,
COMMAND_GIMBAL,
} Custom_CommandCode;
} Custom_CommandCode;
/*
* prextern int() function in Buzz
@ -54,7 +55,7 @@ void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t);
/*sets rc requested command */
void rc_call(int rc_cmd);
/* sets the battery state */
void set_battery(float voltage,float current,float remaining);
void set_battery(float voltage, float current, float remaining);
void set_deque_full(bool state);
void set_rssi(float value);
void set_raw_packet_loss(float value);
@ -88,7 +89,7 @@ int buzzuav_arm(buzzvm_t vm);
/*
* Disarm from buzz
*/
int buzzuav_disarm(buzzvm_t vm) ;
int buzzuav_disarm(buzzvm_t vm);
/* Commands the UAV to land
*/
int buzzuav_land(buzzvm_t vm);
@ -126,7 +127,6 @@ int buzzuav_update_prox(buzzvm_t vm);
int bzz_cmd();
int dummy_closure(buzzvm_t vm);
//#endif

80
include/roscontroller.h
View File

@ -45,14 +45,12 @@ using namespace std;
namespace rosbzz_node
{
class roscontroller
{
public:
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller();
//void RosControllerInit();
// void RosControllerInit();
void RosControllerRun();
static const string CAPTURE_SRV_DEFAULT_NAME;
@ -63,43 +61,47 @@ private:
uint8_t history[10];
uint8_t index = 0;
uint8_t current = 0;
num_robot_count(){}
}; typedef struct num_robot_count Num_robot_count ; // not useful in cpp
num_robot_count()
{
}
};
typedef struct num_robot_count Num_robot_count; // not useful in cpp
struct gps
{
double longitude=0.0;
double latitude=0.0;
float altitude=0.0;
}; typedef struct gps GPS ;
double longitude = 0.0;
double latitude = 0.0;
float altitude = 0.0;
};
typedef struct gps GPS;
GPS target, home, cur_pos;
double cur_rel_altitude;
uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step=0;
int robot_id=0;
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
// std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step = 0;
int robot_id = 0;
std::string robot_name = "";
//int oldcmdID=0;
// int oldcmdID=0;
int rc_cmd;
float fcu_timeout;
int armstate;
int barrier;
int message_number=0;
uint8_t no_of_robots=0;
int message_number = 0;
uint8_t no_of_robots = 0;
/*tmp to be corrected*/
uint8_t no_cnt=0;
uint8_t old_val=0 ;
uint8_t no_cnt = 0;
uint8_t old_val = 0;
std::string bzzfile_name;
std::string fcclient_name;
std::string armclient;
std::string modeclient;
std::string rcservice_name;
std::string bcfname,dbgfname;
std::string bcfname, dbgfname;
std::string out_payload;
std::string in_payload;
std::string obstacles_topic;
@ -136,7 +138,6 @@ private:
ros::Subscriber local_pos_sub;
double local_pos_new[3];
ros::ServiceClient stream_client;
int setpoint_counter;
@ -145,7 +146,7 @@ private:
std::ofstream log;
/*Commands for flight controller*/
//mavros_msgs::CommandInt cmd_srv;
// mavros_msgs::CommandInt cmd_srv;
mavros_msgs::CommandLong cmd_srv;
std::vector<std::string> m_sMySubscriptions;
std::map<std::string, std::string> m_smTopic_infos;
@ -178,28 +179,24 @@ private:
/*Prepare messages and publish*/
void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step);
/*Puts neighbours position inside neigbours_pos_map*/
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude,
double longitude,
double altitude);
void set_cur_pos(double latitude, double longitude, double altitude);
/*convert from spherical to cartesian coordinate system callback */
float constrainAngle(float x);
void gps_rb(GPS nei_pos, double out[]);
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
void gps_ned_home(float &ned_x, float &ned_y);
void gps_convert_ned(float &ned_x, float &ned_y,
double gps_t_lon, double gps_t_lat,
double gps_r_lon, double gps_r_lat);
void gps_ned_cur(float& ned_x, float& ned_y, GPS t);
void gps_ned_home(float& ned_x, float& ned_y);
void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
double gps_r_lat);
/*battery status callback */
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
@ -213,7 +210,6 @@ private:
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
@ -221,7 +217,7 @@ private:
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/* RC commands service */
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
bool rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res);
/*robot id sub callback*/
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
@ -243,7 +239,7 @@ private:
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
//void WaypointMissionSetup(float lat, float lng, float alt);
// void WaypointMissionSetup(float lat, float lng, float alt);
void fc_command_setup();
@ -256,15 +252,13 @@ private:
void get_number_of_robots();
void GetRobotId();
bool GetDequeFull(bool &result);
bool GetRssi(float &result);
bool GetDequeFull(bool& result);
bool GetRssi(float& result);
bool TriggerAPIRssi(const uint8_t short_id);
bool GetAPIRssi(const uint8_t short_id, float &result);
bool GetRawPacketLoss(const uint8_t short_id, float &result);
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
bool GetAPIRssi(const uint8_t short_id, float& result);
bool GetRawPacketLoss(const uint8_t short_id, float& result);
bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
void get_xbee_status();
};
}

1
include/uav_utility.h
View File

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

632
src/buzz_update.cpp
View File

@ -9,83 +9,87 @@
#include <buzz/buzzdebug.h>
#include <sys/time.h>
/*Temp for data collection*/
//static int neigh=-1;
// static int neigh=-1;
static struct timeval t1, t2;
static int timer_steps=0;
//static clock_t end;
static int timer_steps = 0;
// static clock_t end;
/*Temp end */
static int fd,wd =0;
static int old_update =0;
static int fd, wd = 0;
static int old_update = 0;
static buzz_updater_elem_t updater;
static int no_of_robot;
static const char* dbgf_name;
static const char* bcfname;
static const char* old_bcfname="old_bcode.bo";
static const char* old_bcfname = "old_bcode.bo";
static const char* bzz_file;
static int Robot_id=0;
static int neigh=-1;
static int updater_msg_ready ;
static uint16_t update_try_counter=0;
static int updated=0;
static const uint16_t MAX_UPDATE_TRY=5;
static int packet_id_=0;
static size_t old_byte_code_size=0;
static int Robot_id = 0;
static int neigh = -1;
static int updater_msg_ready;
static uint16_t update_try_counter = 0;
static int updated = 0;
static const uint16_t MAX_UPDATE_TRY = 5;
static int packet_id_ = 0;
static size_t old_byte_code_size = 0;
/*Initialize updater*/
void init_update_monitor(const char* bo_filename, const char* stand_by_script,
const char* dbgfname, int robot_id){
Robot_id=robot_id;
dbgf_name=dbgfname;
bcfname=bo_filename;
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id)
{
Robot_id = robot_id;
dbgf_name = dbgfname;
bcfname = bo_filename;
ROS_INFO("Initializing file monitor...");
fd=inotify_init1(IN_NONBLOCK);
if ( fd < 0 ) {
perror( "inotify_init error" );
fd = inotify_init1(IN_NONBLOCK);
if (fd < 0)
{
perror("inotify_init error");
}
/*If simulation set the file monitor only for robot 1*/
if(SIMULATION==1 && robot_id==0){
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 );
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 );
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;
// fclose(fp);
// return 0;
}
fclose(fp);
/*Load stand_by .bo file into the updater*/
uint8_t* STD_BO_BUF = 0;
fp = fopen(stand_by_script, "rb");
if(!fp) {
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;
// fclose(fp);
// return 0;
}
fclose(fp);
/*Create the updater*/
@ -96,92 +100,106 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script,
updater->outmsg_queue = NULL;
updater->inmsg_queue = NULL;
updater->patch = NULL;
updater->patch_size = (size_t*) malloc(sizeof(size_t));
updater->bcode_size = (size_t*) malloc(sizeof(size_t));
updater->update_no = (uint8_t*) malloc(sizeof(uint16_t));
*(uint16_t*)(updater->update_no) =0;
*(size_t*)(updater->bcode_size)=bcode_size;
updater->patch_size = (size_t*)malloc(sizeof(size_t));
updater->bcode_size = (size_t*)malloc(sizeof(size_t));
updater->update_no = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(updater->update_no) = 0;
*(size_t*)(updater->bcode_size) = bcode_size;
updater->standby_bcode = STD_BO_BUF;
updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t));
*(size_t*)(updater->standby_bcode_size)=stdby_bcode_size;
updater->mode=(int*)malloc(sizeof(int));
*(int*)(updater->mode)=CODE_RUNNING;
//no_of_robot=barrier;
updater_msg_ready=0;
*(size_t*)(updater->standby_bcode_size) = stdby_bcode_size;
updater->mode = (int*)malloc(sizeof(int));
*(int*)(updater->mode) = CODE_RUNNING;
// no_of_robot=barrier;
updater_msg_ready = 0;
/*Write the bcode to a file for rollback*/
fp=fopen("old_bcode.bo", "wb");
fp = fopen("old_bcode.bo", "wb");
fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp);
fclose(fp);
}
/*Check for .bzz file chages*/
int check_update(){
struct inotify_event *event;
int check_update()
{
struct inotify_event* event;
char buf[1024];
int check =0;
int i=0;
int len=read(fd,buf,1024);
while(i<len){
event=(struct inotify_event *) &buf[i];
int check = 0;
int i = 0;
int len = read(fd, buf, 1024);
while (i < len)
{
event = (struct inotify_event*)&buf[i];
/* file was modified this flag is true in nano and self delet in gedit and other editors */
//fprintf(stdout,"inside file monitor.\n");
if(event->mask & (IN_MODIFY| IN_DELETE_SELF)){
// fprintf(stdout,"inside file monitor.\n");
if (event->mask & (IN_MODIFY | IN_DELETE_SELF))
{
/*respawn watch if the file is self deleted */
inotify_rm_watch(fd,wd);
inotify_rm_watch(fd, wd);
close(fd);
fd=inotify_init1(IN_NONBLOCK);
wd=inotify_add_watch(fd,bzz_file,IN_ALL_EVENTS);
//fprintf(stdout,"event.\n");
fd = inotify_init1(IN_NONBLOCK);
wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS);
// fprintf(stdout,"event.\n");
/* To mask multiple writes from editors*/
if(!old_update){
check=1;
old_update =1;
if (!old_update)
{
check = 1;
old_update = 1;
}
}
/* 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;
return check;
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";
patch_writefile << path << name1 << "tmp_patch.bo";
/*Write the patch to a file*/
FILE *fp=fopen(patch_writefile.str().c_str(), "wb");
FILE* fp = fopen(patch_writefile.str().c_str(), "wb");
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";
fprintf(stdout,"Launching bspatch command: %s \n", patch_exsisting.str().c_str());
if(system(patch_exsisting.str().c_str()) ) return 0;
else return 1;
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;
}
}
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());
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) {
FILE* fp = fopen(read_patched.str().c_str(), "rb");
if (!fp)
{
perror(read_patched.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp);
rewind(fp);
patched_BO_Buf = (uint8_t*)malloc(patched_size);
if(fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) {
if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
{
perror(read_patched.str().c_str());
fclose(fp);
}
@ -189,28 +207,30 @@ updater_code_t obtain_patched_bo(std::string path, std::string name1){
/*Write the patched to a code struct and return*/
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
update_code->bcode = patched_BO_Buf;
update_code->bcode_size = (uint8_t*) malloc(sizeof(uint16_t));
*(uint16_t*) (update_code->bcode_size) = patched_size;
update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(update_code->bcode_size) = patched_size;
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());
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) {
FILE* fp = fopen(read_patched.str().c_str(), "rb");
if (!fp)
{
perror(read_patched.str().c_str());
}
fseek(fp, 0, SEEK_END);
size_t patched_size = ftell(fp);
rewind(fp);
patched_BO_Buf = (uint8_t*)malloc(patched_size);
if(fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) {
if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
{
perror(read_patched.str().c_str());
fclose(fp);
}
@ -223,158 +243,165 @@ updater_code_t obtain_patched_bo(std::string path, std::string name1){
/*Write the patched to a code struct and return*/
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
update_code->bcode = patched_BO_Buf;
update_code->bcode_size = (uint8_t*) malloc(sizeof(uint16_t));
*(uint16_t*) (update_code->bcode_size) = patched_size;
update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(update_code->bcode_size) = patched_size;
return update_code;
}
}
void code_message_outqueue_append(){
updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
void code_message_outqueue_append()
{
updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
/* if size less than 250 Pad with zeors to assure transmission*/
uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size);
uint16_t padding_size= ( size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET-size;
size +=padding_size;
uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size;
size += padding_size;
updater->outmsg_queue->queue = (uint8_t*)malloc(size);
memset(updater->outmsg_queue->queue, 0, size);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
*(uint16_t*)(updater->outmsg_queue->size)=size;
size=0;
*(uint16_t*)(updater->outmsg_queue->size) = size;
size = 0;
/*Append message type*/
*(uint8_t*)(updater->outmsg_queue->queue+size) = SENT_CODE;
size+=sizeof(uint8_t);
*(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE;
size += sizeof(uint8_t);
/*Append the update no, code size and code to out msg*/
*(uint16_t*)(updater->outmsg_queue->queue+size) = *(uint16_t*) (updater->update_no);
size+=sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue+size) =(uint16_t) *(size_t*) (updater->patch_size);
size+=sizeof(uint16_t);
memcpy(updater->outmsg_queue->queue+size, updater->patch, *(size_t*)(updater->patch_size));
size+=(uint16_t)*(size_t*)(updater->patch_size);
updater_msg_ready=1;
*(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no);
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size);
size += sizeof(uint16_t);
memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size));
size += (uint16_t) * (size_t*)(updater->patch_size);
updater_msg_ready = 1;
}
void outqueue_append_code_request(uint16_t update_no){
updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
uint16_t size =0;
updater->outmsg_queue->queue = (uint8_t*)malloc(2*sizeof(uint16_t) + sizeof(uint8_t) +CODE_REQUEST_PADDING);
void outqueue_append_code_request(uint16_t update_no)
{
updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
uint16_t size = 0;
updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) +CODE_REQUEST_PADDING);
memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING);
/*Append message type*/
*(uint8_t*)(updater->outmsg_queue->queue+size) = RESEND_CODE;
size+=sizeof(uint8_t);
*(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE;
size += sizeof(uint8_t);
/*Append the update no, code size and code to out msg*/
*(uint16_t*)(updater->outmsg_queue->queue+size) = update_no;
size+=sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue+size) = update_try_counter;
size+=sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->size)=size+CODE_REQUEST_PADDING;
updater_msg_ready=1;
ROS_INFO("[Debug] Requested update no. %u with try: %u \n",update_no,update_try_counter);
*(uint16_t*)(updater->outmsg_queue->queue + size) = update_no;
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter;
size += sizeof(uint16_t);
*(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING;
updater_msg_ready = 1;
ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter);
}
void code_message_inqueue_append(uint8_t* msg,uint16_t size){
updater->inmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
//ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size);
void code_message_inqueue_append(uint8_t* msg, uint16_t size)
{
updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
// ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size);
updater->inmsg_queue->queue = (uint8_t*)malloc(size);
updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
memcpy(updater->inmsg_queue->queue, msg, size);
*(uint16_t*)(updater->inmsg_queue->size) = size;
}
void set_packet_id(int packet_id){
/*Used for data logging*/
packet_id_=packet_id;
void set_packet_id(int packet_id)
{
/*Used for data logging*/
packet_id_ = packet_id;
}
void code_message_inqueue_process(){
int size=0;
updater_code_t out_code=NULL;
void code_message_inqueue_process()
{
int size = 0;
updater_code_t out_code = NULL;
ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) );
ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint8_t))) );
ROS_INFO("[Debug] Updater received patch of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)+sizeof(uint8_t)) ) );
ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode));
ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)),
(*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t))));
ROS_INFO("[Debug] Updater received patch of size %u \n",
(*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t))));
if( *(int*) (updater->mode) == CODE_RUNNING){
//fprintf(stdout,"[debug]Inside inmsg code running");
if(*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE){
size+=sizeof(uint8_t);
if( *(uint16_t*)(updater->inmsg_queue->queue+size) > *(uint16_t*) (updater->update_no) ){
//fprintf(stdout,"[debug]Inside update number comparision");
uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t);
uint16_t update_patch_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
size +=sizeof(uint16_t);
if (*(int*)(updater->mode) == CODE_RUNNING)
{
// fprintf(stdout,"[debug]Inside inmsg code running");
if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE)
{
size += sizeof(uint8_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no))
{
// fprintf(stdout,"[debug]Inside update number comparision");
uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size);
size += sizeof(uint16_t);
uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size);
size += sizeof(uint16_t);
/*Generate patch*/
std::string bzzfile_name(bzz_file);
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name1 = name1.substr(0,name1.find_last_of("."));
if(test_patch(path, name1,update_patch_size,(updater->inmsg_queue->queue+size)) ){
name1 = name1.substr(0, name1.find_last_of("."));
if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size)))
{
out_code = obtain_patched_bo(path, name1);
//fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
//fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
//FILE *fp;
//fp=fopen("update.bo", "wb");
//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
//fclose(fp);
if(test_set_code( (out_code->bcode),
(char*) dbgf_name, (size_t) (*(uint16_t*)out_code->bcode_size) ) ){
// fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
// fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
// FILE *fp;
// fp=fopen("update.bo", "wb");
// fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
// fclose(fp);
if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size)))
{
printf("TEST PASSED!\n");
*(uint16_t*)updater->update_no=update_no;
*(uint16_t*)updater->update_no = update_no;
/*Clear exisiting patch if any*/
delete_p(updater->patch);
/*copy the patch into the updater*/
updater->patch = (uint8_t*)malloc(update_patch_size);
memcpy(updater->patch, (updater->inmsg_queue->queue+size), update_patch_size);
*(size_t*) (updater->patch_size) = update_patch_size;
//code_message_outqueue_append();
neigh=1;
memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size);
*(size_t*)(updater->patch_size) = update_patch_size;
// code_message_outqueue_append();
neigh = 1;
}
/*clear the temp code buff*/
delete_p(out_code->bcode);
delete_p(out_code->bcode_size);
delete_p(out_code);
}
else{
else
{
ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n");
update_try_counter++;
outqueue_append_code_request(update_no);
}
}
}
}
size=0;
if(*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE){
size+=sizeof(uint8_t);
if( *(uint16_t*)(updater->inmsg_queue->queue+size) == *(uint16_t*) (updater->update_no) ){
size+=sizeof(uint16_t);
if(*(uint16_t*)(updater->inmsg_queue->queue+size) > update_try_counter){
update_try_counter=*(uint16_t*)(updater->inmsg_queue->queue+size);
ROS_ERROR("Code appended! update try : %u \n",update_try_counter);
size = 0;
if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE)
{
size += sizeof(uint8_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no))
{
size += sizeof(uint16_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter)
{
update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size);
ROS_ERROR("Code appended! update try : %u \n", update_try_counter);
code_message_outqueue_append();
}
if(update_try_counter > MAX_UPDATE_TRY) ROS_ERROR("TODO: ROLL BACK !!");
if (update_try_counter > MAX_UPDATE_TRY)
ROS_ERROR("TODO: ROLL BACK !!");
}
}
//fprintf(stdout,"in queue freed\n");
// fprintf(stdout,"in queue freed\n");
delete_p(updater->inmsg_queue->queue);
delete_p(updater->inmsg_queue->size);
delete_p(updater->inmsg_queue);
}
void create_update_patch(){
void create_update_patch()
{
std::stringstream genpatch;
std::stringstream usepatch;
@ -383,19 +410,17 @@ void create_update_patch(){
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("."));
name1 = name1.substr(0, name1.find_last_of("."));
std::string name2 = name1 + "-update";
// CALL BSDIFF CMD
genpatch<< "bsdiff "<< path << name1 <<".bo "<< path << name2 <<".bo "<< path<<"diff.bo";
fprintf(stdout,"Launching bsdiff command: %s \n", genpatch.str().c_str());
genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo";
fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str());
system(genpatch.str().c_str());
// 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 */
@ -405,75 +430,81 @@ 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";
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);
}
fclose(fp);
delete_p(updater->patch);
updater->patch = patch_buff;
*(size_t*) (updater->patch_size) = patch_size;
*(size_t*)(updater->patch_size) = patch_size;
/* Delete the diff file */
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){
// fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if (*(int*)updater->mode == CODE_RUNNING)
{
buzzvm_function_call(VM, "updated_neigh", 0);
// Check for update
if(check_update()){
if (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("\\/")) + "/";
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of("."));
bzzfile_in_compile<<path<<name<<"-update.bo";
name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << path << name << "-update.bo";
uint8_t* BO_BUF = 0;
FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit
if(!fp) {
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)){
uint16_t update_no =*(uint16_t*)(updater->update_no);
*(uint16_t*)(updater->update_no) =update_no +1;
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;
create_update_patch();
VM = buzz_utility::get_vm();
@ -481,79 +512,77 @@ void update_routine(){
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
buzzvm_gstore(VM);
neigh=-1;
neigh = -1;
ROS_INFO("Sending code... \n");
code_message_outqueue_append();
}
delete_p(BO_BUF);
}
}
}
}
else{
else
{
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);
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) {
ROS_INFO("Barrier ..................... %i \n", tObj->i.value);
if (tObj->i.value == no_of_robot)
{
*(int*)(updater->mode) = CODE_RUNNING;
gettimeofday(&t2, NULL);
//collect_data();
// collect_data();
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size));
//buzzvm_function_call(m_tBuzzVM, "updated", 0);
updated=1;
update_try_counter=0;
timer_steps=0;
// buzzvm_function_call(m_tBuzzVM, "updated", 0);
updated = 1;
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);
updated=1;
update_try_counter=0;
timer_steps=0;
buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot);
updated = 1;
update_try_counter = 0;
timer_steps = 0;
}
}
}
uint8_t* getupdater_out_msg(){
return (uint8_t*)updater->outmsg_queue->queue;
uint8_t* getupdater_out_msg()
{
return (uint8_t*)updater->outmsg_queue->queue;
}
uint8_t* getupdate_out_msg_size(){
//fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
return (uint8_t*)updater->outmsg_queue->size;
uint8_t* getupdate_out_msg_size()
{
// fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size);
return (uint8_t*)updater->outmsg_queue->size;
}
int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
if(buzz_utility::buzz_update_init_test(BO_BUF, dbgfname,bcode_size)){
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;
old_byte_code_size = *(size_t*)updater->bcode_size;
/*data logging*/
ROS_WARN("Step test passed\n");
*(int*) (updater->mode) = CODE_STANDBY;
//fprintf(stdout,"updater value = %i\n",updater->mode);
*(int*)(updater->mode) = CODE_STANDBY;
// fprintf(stdout,"updater value = %i\n",updater->mode);
delete_p(updater->bcode);
updater->bcode = (uint8_t*)malloc(bcode_size);
memcpy(updater->bcode, BO_BUF, bcode_size);
*(size_t*)updater->bcode_size = bcode_size;
buzz_utility::buzz_update_init_test((updater)->standby_bcode,
(char*)dbgfname,(size_t) *(size_t*)(updater->standby_bcode_size));
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));
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));
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,68 +634,80 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
}
}
void destroy_out_msg_queue(){
//fprintf(stdout,"out queue freed\n");
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;
updater_msg_ready = 0;
}
int get_update_status(){
return updated;
int get_update_status()
{
return updated;
}
void set_read_update_status(){
updated=0;
void set_read_update_status()
{
updated = 0;
}
int get_update_mode(){
return (int)*(int*)(updater->mode);
int get_update_mode()
{
return (int)*(int*)(updater->mode);
}
int is_msg_present(){
return updater_msg_ready;
int is_msg_present()
{
return updater_msg_ready;
}
buzz_updater_elem_t get_updater(){
return updater;
buzz_updater_elem_t get_updater()
{
return updater;
}
void destroy_updater(){
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);
}
inotify_rm_watch(fd,wd);
inotify_rm_watch(fd, wd);
close(fd);
}
void set_bzz_file(const char* in_bzz_file){
bzz_file=in_bzz_file;
void set_bzz_file(const char* in_bzz_file)
{
bzz_file = in_bzz_file;
}
void updates_set_robots(int robots){
no_of_robot=robots;
void updates_set_robots(int robots)
{
no_of_robot = robots;
}
/*--------------------------------------------------------
/ Create Buzz bytecode from the bzz script inputed
/-------------------------------------------------------*/
int compile_bzz(std::string bzz_file){
int compile_bzz(std::string bzz_file)
{
/*Compile the buzz code .bzz to .bo*/
std::string bzzfile_name(bzz_file);
stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of("."));
name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<<path<< name<<".basm";
bzzfile_in_compile << " -b " << path << name << "-update.bo";
bzzfile_in_compile << " -d " << path << name << "-update.bdb ";
@ -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_;
// 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_;
}

437
src/buzz_utility.cpp
View File

@ -8,121 +8,128 @@
#include "buzz_utility.h"
namespace buzz_utility{
namespace buzz_utility
{
/****************************************/
/****************************************/
/****************************************/
/****************************************/
static buzzvm_t VM = 0;
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
std::map<int, Pos_struct> users_map;
static buzzvm_t VM = 0;
static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint32_t MAX_MSG_SIZE = 250; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
std::map< int, Pos_struct> users_map;
/****************************************/
/****************************************/
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
uint16_t* u64_cvt_u16(uint64_t u64){
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
uint16_t* u64_cvt_u16(uint64_t u64)
{
uint16_t* out = new uint16_t[4];
uint32_t int32_1 = u64 & 0xFFFFFFFF;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000) >> 32;
out[0] = int32_1 & 0xFFFF;
out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
out[1] = (int32_1 & (0xFFFF0000)) >> 16;
out[2] = int32_2 & 0xFFFF;
out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
//cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
out[3] = (int32_2 & (0xFFFF0000)) >> 16;
// cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
return out;
}
}
int get_robotid() {
int get_robotid()
{
return Robot_id;
}
/***************************************************/
/*Appends obtained messages to buzz in message Queue*/
/***************************************************/
}
/***************************************************/
/*Appends obtained messages to buzz in message Queue*/
/***************************************************/
/*******************************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _______________________________________________________________________________________________________________ */
/*| | |*/
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
/*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/
void in_msg_append(uint64_t* payload){
/*******************************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _______________________________________________________________________________________________________________ */
/*| | |*/
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
/*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/
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]);
uint16_t* data = u64_cvt_u16((uint64_t)payload[0]);
/*Size is at first 2 bytes*/
uint16_t size=data[0]*sizeof(uint64_t);
uint16_t size = data[0] * sizeof(uint64_t);
delete[] data;
uint8_t* pl =(uint8_t*)malloc(size);
uint8_t* pl = (uint8_t*)malloc(size);
/* Copy packet into temporary buffer */
memcpy(pl, payload ,size);
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;
size_t tot = 0;
/*Size is at first 2 bytes*/
uint16_t size=(*(uint16_t*)first_INmsg)*sizeof(uint64_t);
uint16_t size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t);
tot += sizeof(uint16_t);
/*Decode neighbor Id*/
uint16_t neigh_id =*(uint16_t*)(first_INmsg+tot);
tot+=sizeof(uint16_t);
uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t);
/* 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*/
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);
} while (size - tot > sizeof(uint16_t) && unMsgSize > 0);
free(first_INmsg);
IN_MSG.erase(IN_MSG.begin());
}
/* Process messages VM call*/
buzzvm_process_inmsgs(VM);
}
}
/***************************************************/
/*Obtains messages from buzz out message Queue*/
/***************************************************/
uint64_t* obt_out_msg(){
/***************************************************/
/*Obtains messages from buzz out message Queue*/
/***************************************************/
uint64_t* obt_out_msg()
{
/* Process out messages */
buzzvm_process_outmsgs(VM);
uint8_t* buff_send =(uint8_t*)malloc(MAX_MSG_SIZE);
uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE);
/*Taking into consideration the sizes included at the end*/
ssize_t tot = sizeof(uint16_t);
/* Send robot id */
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
*(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) {
// ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)));
if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE)
{
buzzmsg_payload_destroy(&m);
break;
}
@ -138,53 +145,48 @@ void in_message_process(){
/* Get rid of message */
buzzoutmsg_queue_next(VM);
buzzmsg_payload_destroy(&m);
} while(1);
} while (1);
uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
*(uint16_t*)buff_send = (uint16_t) total_size;
uint16_t total_size = (ceil((float)tot / (float)sizeof(uint64_t)));
*(uint16_t*)buff_send = (uint16_t)total_size;
uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t));
free(buff_send);
/*for(int i=0;i<total_size;i++){
cout<<" payload from out msg "<<*(payload_64+i)<<endl;
}*/
/* Send message */
return payload_64;
}
}
/****************************************/
/*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);
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;
}
}
/****************************************/
/*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_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
@ -229,13 +231,14 @@ void in_message_process(){
buzzvm_gstore(VM);
return VM->state;
}
}
/**************************************************/
/*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_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
@ -280,18 +283,19 @@ void in_message_process(){
buzzvm_gstore(VM);
return VM->state;
}
}
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) {
ROS_INFO(" Robot ID: %i" , robot_id);
Robot_id=robot_id;
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id)
{
ROS_INFO(" Robot ID: %i", robot_id);
Robot_id = robot_id;
/* Read bytecode from file and fill the bo buffer*/
FILE* fd = fopen(bo_filename, "rb");
if(!fd) {
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);
@ -312,35 +317,41 @@ void in_message_process(){
BO_FNAME = strdup(bo_filename);
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
}
/****************************************/
/*Sets a new update */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/****************************************/
/*Sets a new update */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size)
{
// Reset the Buzz VM
if(VM) buzzvm_destroy(&VM);
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,46 +364,54 @@ void in_message_process(){
buzzvm_gstore(VM);
// Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
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;
}
// All OK
return 1;
}
}
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size)
{
// Reset the Buzz VM
if(VM) buzzvm_destroy(&VM);
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,67 +424,73 @@ void in_message_process(){
buzzvm_gstore(VM);
// Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
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;
}
// All OK
return 1;
}
}
/****************************************/
/*Swarm struct*/
/****************************************/
/****************************************/
/*Swarm struct*/
/****************************************/
struct buzzswarm_elem_s {
struct buzzswarm_elem_s
{
buzzdarray_t swarms;
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;
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;
}
}
}
}
}
/*Step through the buzz script*/
void update_sensors(){
/*Step through the buzz script*/
void update_sensors()
{
/* Update sensors*/
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_xbee_status(VM);
@ -473,34 +498,35 @@ void in_message_process(){
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);
buzzuav_closures::buzzuav_update_targets(VM);
//update_users();
// update_users();
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);
}
}
}
/****************************************/
/*Destroy the bvm and other resorces*/
/****************************************/
/****************************************/
/*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);
@ -509,21 +535,22 @@ void in_message_process(){
buzzdebug_destroy(&DBG_INFO);
}
ROS_INFO("Script execution stopped.");
}
}
/****************************************/
/****************************************/
/****************************************/
/****************************************/
/****************************************/
/*Execution completed*/
/****************************************/
/****************************************/
/*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());
fprintf(stdout, "step test VM state %i\n",a);
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(){
return IN_MSG.size();
}
}
int get_inmsg_size()
{
return IN_MSG.size();
}
}

591
src/buzzuav_closures.cpp
View File

@ -9,41 +9,42 @@
#include "buzzuav_closures.h"
#include "math.h"
namespace buzzuav_closures{
// TODO: Minimize the required global variables and put them in the header
//static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
static double goto_pos[3];
static double rc_goto_pos[3];
static float rc_gimbal[4];
static float batt[3];
static float obst[5]={0,0,0,0,0};
static double cur_pos[3];
static uint8_t status;
static int cur_cmd = 0;
static int rc_cmd=0;
static int rc_id=-1;
static int buzz_cmd=0;
static float height=0;
static bool deque_full = false;
static int rssi = 0;
static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
string WPlistname = "";
namespace buzzuav_closures
{
// TODO: Minimize the required global variables and put them in the header
// static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/
// void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
static double goto_pos[3];
static double rc_goto_pos[3];
static float rc_gimbal[4];
static float batt[3];
static float obst[5] = { 0, 0, 0, 0, 0 };
static double cur_pos[3];
static uint8_t status;
static int cur_cmd = 0;
static int rc_cmd = 0;
static int rc_id = -1;
static int buzz_cmd = 0;
static float height = 0;
static bool deque_full = false;
static int rssi = 0;
static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
string WPlistname = "";
std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::RB_struct> wplist_map;
std::map< int, buzz_utility::Pos_struct> neighbors_map;
std::map< int, buzz_utility::neighbors_status> neighbors_status_map;
std::map<int, buzz_utility::RB_struct> targets_map;
std::map<int, buzz_utility::RB_struct> wplist_map;
std::map<int, buzz_utility::Pos_struct> neighbors_map;
std::map<int, buzz_utility::neighbors_status> neighbors_status_map;
/****************************************/
/****************************************/
/****************************************/
/****************************************/
int buzzros_print(buzzvm_t vm)
{
std::ostringstream buffer (std::ostringstream::ate);
std::ostringstream buffer(std::ostringstream::ate);
buffer << buzz_utility::get_robotid();
for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
{
@ -84,58 +85,64 @@ int buzzros_print(buzzvm_t vm)
break;
}
}
ROS_INFO("%s",buffer.str().c_str());
ROS_INFO("%s", buffer.str().c_str());
return buzzvm_ret0(vm);
}
void setWPlist(string path){
void setWPlist(string path)
{
WPlistname = path + "include/graphs/waypointlist.csv";
}
}
/*----------------------------------------/
/ 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 lon = RAD2DEG(cur_pos[1]);
out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
out[0] = asin(sin(lat) * cos(range / EARTH_RADIUS) + cos(lat) * sin(range / EARTH_RADIUS) * cos(bearing));
out[1] = lon + atan2(sin(bearing) * sin(range / EARTH_RADIUS) * cos(lat),
cos(range / EARTH_RADIUS) - sin(lat) * sin(out[0]));
out[0] = RAD2DEG(out[0]);
out[1] = RAD2DEG(out[1]);
out[2] = height; //constant height.
}
out[2] = height; // constant height.
}
float constrainAngle(float x){
x = fmod(x,2*M_PI);
float constrainAngle(float x)
{
x = fmod(x, 2 * M_PI);
if (x < 0.0)
x += 2*M_PI;
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;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
out[1] = constrainAngle(atan2(ned_y,ned_x));
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
out[1] = constrainAngle(atan2(ned_y, ned_x));
out[2] = 0.0;
}
}
// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
double hcpos1[4] = { 45.564489, -73.562537, 45.564140, -73.562336 };
double hcpos2[4] = { 45.564729, -73.562060, 45.564362, -73.562958 };
double hcpos3[4] = { 45.564969, -73.562838, 45.564636, -73.563677 };
void parse_gpslist()
{
void parse_gpslist()
{
// Open file:
ROS_INFO("WP list file: %s", WPlistname.c_str());
std::ifstream fin(WPlistname.c_str()); // Open in text-mode.
// Opening may fail, always check.
if (!fin) {
if (!fin)
{
ROS_ERROR("GPS list parser, could not open file.");
return;
}
@ -143,27 +150,28 @@ int buzzros_print(buzzvm_t vm)
// Prepare a C-string buffer to be used when reading lines.
const int MAX_LINE_LENGTH = 1024; // Choose this large enough for your need.
char buffer[MAX_LINE_LENGTH] = {};
const char * DELIMS = "\t ,"; // Tab, space or comma.
const char* DELIMS = "\t ,"; // Tab, space or comma.
// Read the file and load the data:
double lat, lon;
int alt, tilt, tid;
buzz_utility::RB_struct RB_arr;
// Read one line at a time.
while ( fin.getline(buffer, MAX_LINE_LENGTH) ) {
while (fin.getline(buffer, MAX_LINE_LENGTH))
{
// Extract the tokens:
tid = atoi(strtok( buffer, DELIMS ));
lon = atof(strtok( NULL, DELIMS ));
lat = atof(strtok( NULL, DELIMS ));
alt = atoi(strtok( NULL, DELIMS ));
tilt = atoi(strtok( NULL, DELIMS ));
//ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
RB_arr.latitude=lat;
RB_arr.longitude=lon;
RB_arr.altitude=alt;
tid = atoi(strtok(buffer, DELIMS));
lon = atof(strtok(NULL, DELIMS));
lat = atof(strtok(NULL, DELIMS));
alt = atoi(strtok(NULL, DELIMS));
tilt = atoi(strtok(NULL, DELIMS));
// ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
RB_arr.latitude = lat;
RB_arr.longitude = lon;
RB_arr.altitude = alt;
// Insert elements.
map< int, buzz_utility::RB_struct >::iterator it = wplist_map.find(tid);
if(it!=wplist_map.end())
map<int, buzz_utility::RB_struct>::iterator it = wplist_map.find(tid);
if (it != wplist_map.end())
wplist_map.erase(it);
wplist_map.insert(make_pair(tid, RB_arr));
}
@ -172,13 +180,13 @@ int buzzros_print(buzzvm_t vm)
// Close the file:
fin.close();
}
}
/*----------------------------------------/
/ Buzz closure to move following a 2D vector
/----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm) {
/*----------------------------------------/
/ Buzz closure to move following a 2D vector
/----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */
@ -189,28 +197,34 @@ int buzzros_print(buzzvm_t vm)
float dh = buzzvm_stack_at(vm, 1)->f.value;
float dy = buzzvm_stack_at(vm, 2)->f.value;
float dx = buzzvm_stack_at(vm, 3)->f.value;
goto_pos[0]=dx;
goto_pos[1]=dy;
goto_pos[2]=height+dh;
//ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]);
goto_pos[0] = dx;
goto_pos[1] = dy;
goto_pos[2] = height + dh;
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
// goto_pos[1], goto_pos[2]);
buzz_cmd = COMMAND_MOVETO; // TO DO what should we use
return buzzvm_ret0(vm);
}
}
int buzzuav_update_targets(buzzvm_t vm) {
if(vm->state != BUZZVM_STATE_READY) return vm->state;
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;
map<int, buzz_utility::RB_struct>::iterator it;
for (it = targets_map.begin(); it != targets_map.end(); ++it)
{
tmp[0] = (it->second).latitude;
tmp[1] = (it->second).longitude;
tmp[2] = height;
rb_from_gps(tmp, rb, cur_pos);
//ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
// ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
buzzvm_push(vm, targettbl);
// When we get here, the "targets" table is on top of the stack
//ROS_INFO("Buzz_utility will save user %i.", it->first);
// ROS_INFO("Buzz_utility will save user %i.", it->first);
// Push user id
buzzvm_pushi(vm, it->first);
// Create entry table
@ -232,9 +246,10 @@ int buzzros_print(buzzvm_t vm)
buzzvm_gstore(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,27 +265,30 @@ int buzzros_print(buzzvm_t vm)
double rb[3];
rb_from_gps(tmp, rb, cur_pos);
if(fabs(rb[0])<100.0) {
//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
if (fabs(rb[0]) < 100.0)
{
// printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
buzz_utility::RB_struct RB_arr;
RB_arr.latitude=tmp[0];
RB_arr.longitude=tmp[1];
RB_arr.altitude=tmp[2];
RB_arr.r=rb[0];
RB_arr.b=rb[1];
map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(uid);
if(it!=targets_map.end())
RB_arr.latitude = tmp[0];
RB_arr.longitude = tmp[1];
RB_arr.altitude = tmp[2];
RB_arr.r = rb[0];
RB_arr.b = rb[1];
map<int, buzz_utility::RB_struct>::iterator it = targets_map.find(uid);
if (it != targets_map.end())
targets_map.erase(it);
targets_map.insert(make_pair(uid, RB_arr));
//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
// ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
return vm->state;
} else
ROS_WARN(" ---------- Target too far %f",rb[0]);
}
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
@ -284,21 +302,23 @@ int buzzros_print(buzzvm_t vm)
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzz_utility::neighbors_status newRS;
uint8_t id = buzzvm_stack_at(vm, 5)->i.value;
newRS.gps_strenght= buzzvm_stack_at(vm, 4)->i.value;
newRS.batt_lvl= buzzvm_stack_at(vm, 3)->i.value;
newRS.xbee= buzzvm_stack_at(vm, 2)->i.value;
newRS.flight_status= buzzvm_stack_at(vm, 1)->i.value;
map< int, buzz_utility::neighbors_status >::iterator it = neighbors_status_map.find(id);
if(it!=neighbors_status_map.end())
newRS.gps_strenght = buzzvm_stack_at(vm, 4)->i.value;
newRS.batt_lvl = buzzvm_stack_at(vm, 3)->i.value;
newRS.xbee = buzzvm_stack_at(vm, 2)->i.value;
newRS.flight_status = buzzvm_stack_at(vm, 1)->i.value;
map<int, buzz_utility::neighbors_status>::iterator it = neighbors_status_map.find(id);
if (it != neighbors_status_map.end())
neighbors_status_map.erase(it);
neighbors_status_map.insert(make_pair(id, newRS));
return vm->state;
}
}
mavros_msgs::Mavlink get_status(){
mavros_msgs::Mavlink 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){
map<int, buzz_utility::neighbors_status>::iterator it;
for (it = neighbors_status_map.begin(); it != neighbors_status_map.end(); ++it)
{
payload_out.payload64.push_back(it->first);
payload_out.payload64.push_back(it->second.gps_strenght);
payload_out.payload64.push_back(it->second.batt_lvl);
@ -311,20 +331,22 @@ int buzzros_print(buzzvm_t vm)
message_number++;*/
return payload_out;
}
/*----------------------------------------/
/ Buzz closure to take a picture here.
/----------------------------------------*/
int buzzuav_takepicture(buzzvm_t vm) {
//cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
}
/*----------------------------------------/
/ Buzz closure to take a picture here.
/----------------------------------------*/
int buzzuav_takepicture(buzzvm_t vm)
{
// cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
buzz_cmd = COMMAND_PICTURE;
return buzzvm_ret0(vm);
}
}
/*----------------------------------------/
/ Buzz closure to change locally the gimbal orientation
/----------------------------------------*/
int buzzuav_setgimbal(buzzvm_t vm) {
/*----------------------------------------/
/ Buzz closure to change locally the gimbal orientation
/----------------------------------------*/
int buzzuav_setgimbal(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 4);
buzzvm_lload(vm, 1); // time
buzzvm_lload(vm, 2); // pitch
@ -339,15 +361,16 @@ int buzzros_print(buzzvm_t vm)
rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value;
rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)",rc_gimbal[0],rc_gimbal[1],rc_gimbal[2],rc_gimbal[3]);
ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]);
buzz_cmd = COMMAND_GIMBAL;
return buzzvm_ret0(vm);
}
}
/*----------------------------------------/
/ Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/
int buzzuav_storegoal(buzzvm_t vm) {
/*----------------------------------------/
/ Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/
int buzzuav_storegoal(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // altitude
buzzvm_lload(vm, 2); // longitude
@ -359,8 +382,9 @@ 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(wplist_map.size()<=0)
if (goal[0] == -1 && goal[1] == -1 && goal[2] == -1)
{
if (wplist_map.size() <= 0)
parse_gpslist();
goal[0] = wplist_map.begin()->second.latitude;
goal[1] = wplist_map.begin()->second.longitude;
@ -371,131 +395,143 @@ int buzzros_print(buzzvm_t vm)
double rb[3];
rb_from_gps(goal, rb, cur_pos);
if(fabs(rb[0])<150.0)
if (fabs(rb[0]) < 150.0)
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
ROS_WARN("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]);
ROS_WARN("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
return buzzvm_ret0(vm);
}
}
/*----------------------------------------/
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
/----------------------------------------*/
int buzzuav_arm(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
/*----------------------------------------/
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
/----------------------------------------*/
int buzzuav_arm(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n");
buzz_cmd=COMMAND_ARM;
buzz_cmd = COMMAND_ARM;
return buzzvm_ret0(vm);
}
int buzzuav_disarm(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
}
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;
buzz_cmd = COMMAND_DISARM;
return buzzvm_ret0(vm);
}
}
/*---------------------------------------/
/ Buzz closure for basic UAV commands
/---------------------------------------*/
int buzzuav_takeoff(buzzvm_t vm) {
/*---------------------------------------/
/ Buzz closure for basic UAV commands
/---------------------------------------*/
int buzzuav_takeoff(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); /* Altitude */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value;
goto_pos[2] = buzzvm_stack_at(vm, 1)->f.value;
height = goto_pos[2];
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
cur_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
printf(" Buzz requested Take off !!! \n");
buzz_cmd = COMMAND_TAKEOFF;
return buzzvm_ret0(vm);
}
}
int buzzuav_land(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
int buzzuav_land(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
printf(" Buzz requested Land !!! \n");
buzz_cmd = COMMAND_LAND;
return buzzvm_ret0(vm);
}
}
int buzzuav_gohome(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
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() {
/*-------------------------------
/ Get/Set to transfer variable from Roscontroller to buzzuav
/------------------------------------*/
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));
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
buzzvm_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
return uav_state->s.value.str;
}
}
int getcmd() {
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){
batt[0]=voltage;
batt[1]=current;
batt[2]=remaining;
}
void set_battery(float voltage, float current, float remaining)
{
batt[0] = voltage;
batt[1] = current;
batt[2] = remaining;
}
int buzzuav_update_battery(buzzvm_t vm) {
int buzzuav_update_battery(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -512,34 +548,35 @@ int buzzros_print(buzzvm_t vm)
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
}
void set_deque_full(bool state)
{
void set_deque_full(bool state)
{
deque_full = state;
}
}
void set_rssi(float value)
{
void set_rssi(float value)
{
rssi = round(value);
}
}
void set_raw_packet_loss(float value)
{
void set_raw_packet_loss(float value)
{
raw_packet_loss = value;
}
}
void set_filtered_packet_loss(float value)
{
filtered_packet_loss = round(100*value);
}
void set_filtered_packet_loss(float value)
{
filtered_packet_loss = round(100 * value);
}
void set_api_rssi(float value)
{
void set_api_rssi(float 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_pusht(vm);
buzzvm_dup(vm);
@ -564,45 +601,46 @@ int buzzros_print(buzzvm_t vm)
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
}
/***************************************/
/*current pos update*/
/***************************************/
void set_currentpos(double latitude, double longitude, double altitude){
cur_pos[0]=latitude;
cur_pos[1]=longitude;
cur_pos[2]=altitude;
}
/*adds neighbours position*/
void neighbour_pos_callback(int id, float range, float bearing, float elevation){
/***************************************/
/*current pos update*/
/***************************************/
void set_currentpos(double latitude, double longitude, double altitude)
{
cur_pos[0] = latitude;
cur_pos[1] = longitude;
cur_pos[2] = altitude;
}
/*adds neighbours position*/
void neighbour_pos_callback(int id, float range, float bearing, float elevation)
{
buzz_utility::Pos_struct pos_arr;
pos_arr.x=range;
pos_arr.y=bearing;
pos_arr.z=elevation;
map< int, buzz_utility::Pos_struct >::iterator it = neighbors_map.find(id);
if(it!=neighbors_map.end())
pos_arr.x = range;
pos_arr.y = bearing;
pos_arr.z = elevation;
map<int, buzz_utility::Pos_struct>::iterator it = neighbors_map.find(id);
if (it != neighbors_map.end())
neighbors_map.erase(it);
neighbors_map.insert(make_pair(id, pos_arr));
}
}
/* update at each step the VM table */
void update_neighbors(buzzvm_t vm){
/* update at each step the VM table */
void update_neighbors(buzzvm_t vm)
{
/* Reset neighbor information */
buzzneighbors_reset(vm);
/* Get robot id and update neighbor information */
map< int, buzz_utility::Pos_struct >::iterator it;
for (it=neighbors_map.begin(); it!=neighbors_map.end(); ++it){
buzzneighbors_add(vm,
it->first,
(it->second).x,
(it->second).y,
(it->second).z);
}
map<int, buzz_utility::Pos_struct>::iterator it;
for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it)
{
buzzneighbors_add(vm, it->first, (it->second).x, (it->second).y, (it->second).z);
}
}
/****************************************/
int buzzuav_update_currentpos(buzzvm_t vm) {
/****************************************/
int buzzuav_update_currentpos(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -619,18 +657,20 @@ int buzzros_print(buzzvm_t vm)
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
}
void flight_status_update(uint8_t state){
status=state;
}
void flight_status_update(uint8_t state)
{
status = state;
}
/*----------------------------------------------------
/ Create the generic robot table with status, remote controller current comand and destination
/ and current position of the robot
/ TODO: change global name for robot
/------------------------------------------------------*/
int buzzuav_update_flight_status(buzzvm_t vm) {
/*----------------------------------------------------
/ Create the generic robot table with status, remote controller current comand and destination
/ and current position of the robot
/ TODO: change global name for robot
/------------------------------------------------------*/
int buzzuav_update_flight_status(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -638,12 +678,12 @@ int buzzros_print(buzzvm_t vm)
buzzvm_pushi(vm, rc_cmd);
buzzvm_tput(vm);
buzzvm_dup(vm);
rc_cmd=0;
rc_cmd = 0;
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
buzzvm_pushi(vm, status);
buzzvm_tput(vm);
buzzvm_gstore(vm);
//also set rc_controllers goto
// also set rc_controllers goto
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -664,21 +704,19 @@ int buzzros_print(buzzvm_t vm)
buzzvm_tput(vm);
buzzvm_gstore(vm);
return vm->state;
}
}
/******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script
/******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script
proximity[0].angle and proximity[0].value - front
"" "" "" - right and back
proximity[3].angle and proximity[3].value - left
proximity[4].angle = -1 and proximity[4].value -bottom */
/******************************************************/
int buzzuav_update_prox(buzzvm_t vm) {
/******************************************************/
int buzzuav_update_prox(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
@ -686,8 +724,9 @@ 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) {
float angle = 0;
for (size_t i = 0; i < 4; ++i)
{
/* Create table for i-th read */
buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1);
@ -695,7 +734,7 @@ int buzzros_print(buzzvm_t vm)
/* Fill in the read */
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[i+1]);
buzzvm_pushf(vm, obst[i + 1]);
buzzvm_tput(vm);
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
@ -706,10 +745,10 @@ int buzzros_print(buzzvm_t vm)
buzzvm_pushi(vm, i);
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
angle+=1.5708;
angle += 1.5708;
}
/* Create table for bottom read */
angle =-1;
angle = -1;
buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
@ -728,12 +767,14 @@ int buzzros_print(buzzvm_t vm)
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
return vm->state;
}
/**********************************************/
/*Dummy closure for use during update testing */
/**********************************************/
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
}
/**********************************************/
/*Dummy closure for use during update testing */
/**********************************************/
int dummy_closure(buzzvm_t vm)
{
return buzzvm_ret0(vm);
}
}

6
src/rosbuzz.cpp
View File

@ -12,7 +12,7 @@
* This program implements Buzz node in ros using mavros_msgs.
*/
int main(int argc, char **argv)
int main(int argc, char** argv)
{
/*Initialize rosBuzz node*/
ros::init(argc, argv, "rosBuzz");
@ -20,9 +20,7 @@ int main(int argc, char **argv)
ros::NodeHandle nh;
rosbzz_node::roscontroller RosController(nh, nh_priv);
/*Register ros controller object inside buzz*/
//buzzuav_closures::set_ros_controller_ptr(&RosController);
// buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun();
return 0;
}

479
src/roscontroller.cpp
View File

File diff suppressed because it is too large Load Diff

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");
}