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

111
include/buzz_update.h
View File

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

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();

38
include/buzzuav_closures.h
View File

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

86
include/roscontroller.h
View File

@ -37,7 +37,7 @@
#define UPDATER_MESSAGE_CONSTANT 987654321
#define XBEE_MESSAGE_CONSTANT 586782343
#define XBEE_STOP_TRANSMISSION 4355356352
#define TIMEOUT 60
#define TIMEOUT 60
#define BUZZRATE 10
#define CMD_REQUEST_UPDATE 666
@ -45,14 +45,12 @@ using namespace std;
namespace rosbzz_node
{
class roscontroller
{
public:
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller();
//void RosControllerInit();
// void RosControllerInit();
void RosControllerRun();
static const string CAPTURE_SRV_DEFAULT_NAME;
@ -63,43 +61,47 @@ private:
uint8_t history[10];
uint8_t index = 0;
uint8_t current = 0;
num_robot_count(){}
}; typedef struct num_robot_count Num_robot_count ; // not useful in cpp
num_robot_count()
{
}
};
typedef struct num_robot_count Num_robot_count; // not useful in cpp
struct gps
{
double longitude=0.0;
double latitude=0.0;
float altitude=0.0;
}; typedef struct gps GPS ;
double longitude = 0.0;
double latitude = 0.0;
float altitude = 0.0;
};
typedef struct gps GPS;
GPS target, home, cur_pos;
double cur_rel_altitude;
uint64_t payload;
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step=0;
int robot_id=0;
std::map<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;
@ -159,7 +160,7 @@ private:
/*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle& n_c);
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
/*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle& n_c_priv);
@ -178,28 +179,24 @@ private:
/*Prepare messages and publish*/
void prepare_msg_and_publish();
/*Refresh neighbours Position for every ten step*/
void maintain_pos(int tim_step);
/*Puts neighbours position inside neigbours_pos_map*/
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude,
double longitude,
double altitude);
void set_cur_pos(double latitude, double longitude, double altitude);
/*convert from spherical to cartesian coordinate system callback */
float constrainAngle(float x);
float constrainAngle(float x);
void gps_rb(GPS nei_pos, double out[]);
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
void gps_ned_home(float &ned_x, float &ned_y);
void gps_convert_ned(float &ned_x, float &ned_y,
double gps_t_lon, double gps_t_lat,
double gps_r_lon, double gps_r_lat);
void gps_ned_cur(float& ned_x, float& ned_y, GPS t);
void gps_ned_home(float& ned_x, float& ned_y);
void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
double gps_r_lat);
/*battery status callback */
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
@ -213,7 +210,6 @@ private:
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
/*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
@ -221,7 +217,7 @@ private:
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
/* RC commands service */
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
bool rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res);
/*robot id sub callback*/
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
@ -243,7 +239,7 @@ private:
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
//void WaypointMissionSetup(float lat, float lng, float alt);
// void WaypointMissionSetup(float lat, float lng, float alt);
void fc_command_setup();
@ -256,15 +252,13 @@ private:
void get_number_of_robots();
void GetRobotId();
bool GetDequeFull(bool &result);
bool GetRssi(float &result);
bool GetDequeFull(bool& result);
bool GetRssi(float& result);
bool TriggerAPIRssi(const uint8_t short_id);
bool GetAPIRssi(const uint8_t short_id, float &result);
bool GetRawPacketLoss(const uint8_t short_id, float &result);
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
bool GetAPIRssi(const uint8_t short_id, float& result);
bool GetRawPacketLoss(const uint8_t short_id, float& result);
bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
void get_xbee_status();
};
}

1
include/uav_utility.h
View File

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

1230
src/buzz_update.cpp
View File

File diff suppressed because it is too large Load Diff

1121
src/buzz_utility.cpp
View File

File diff suppressed because it is too large Load Diff

1399
src/buzzuav_closures.cpp
View File

File diff suppressed because it is too large Load Diff

16
src/rosbuzz.cpp
View File

@ -1,7 +1,7 @@
/** @file rosbuzz.cpp
* @version 1.0
* @version 1.0
* @date 27.09.2016
* @brief Buzz Implementation as a node in ROS.
* @brief Buzz Implementation as a node in ROS.
* @author Vivek Shankar Varadharajan and David St-Onge
* @copyright 2016 MistLab. All rights reserved.
*/
@ -11,18 +11,16 @@
/**
* This program implements Buzz node in ros using mavros_msgs.
*/
int main(int argc, char **argv)
int main(int argc, char** argv)
{
/*Initialize rosBuzz node*/
ros::init(argc, argv, "rosBuzz");
ros::init(argc, argv, "rosBuzz");
ros::NodeHandle nh_priv("~");
ros::NodeHandle nh;
rosbzz_node::roscontroller RosController(nh, nh_priv);
/*Register ros controller object inside buzz*/
//buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun();
// buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun();
return 0;
}

517
src/roscontroller.cpp
View File

File diff suppressed because it is too large Load Diff

10
src/uav_utility.cpp
View File

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