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

Merge branch 'dev' 

Conflicts:
	src/roscontroller.cpp
This commit is contained in:
isvogor 2017-04-25 21:06:53 -04:00
parent e01e6e6c5e 52bed0536f
commit 187e05aca8
11 changed files with 668 additions and 234 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -54,7 +54,7 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
src/buzzuav_closures.cpp src/buzzuav_closures.cpp
src/uav_utility.cpp src/uav_utility.cpp
src/buzz_update.cpp) src/buzz_update.cpp)
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread) target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread)
add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp) add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)
# Executables and libraries for installation to do # Executables and libraries for installation to do

4
include/buzz_utility.h
View File

@ -30,6 +30,8 @@ int buzz_listen(const char* type,
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map); void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
int buzz_update_users_stigmergy(buzzobj_t tbl);
void in_msg_process(uint64_t* payload); void in_msg_process(uint64_t* payload);
uint64_t* out_msg_process(); uint64_t* out_msg_process();
@ -49,7 +51,7 @@ int buzz_script_done();
int update_step_test(); int update_step_test();
uint16_t get_robotid(); int get_robotid();
buzzvm_t get_vm(); buzzvm_t get_vm();

3
include/buzzuav_closures.h
View File

@ -46,6 +46,7 @@ void rc_call(int rc_cmd);
void set_battery(float voltage,float current,float remaining); void set_battery(float voltage,float current,float remaining);
/* sets current position */ /* sets current position */
void set_currentpos(double latitude, double longitude, double altitude); void set_currentpos(double latitude, double longitude, double altitude);
void set_userspos(double latitude, double longitude, double altitude);
/*retuns the current go to position */ /*retuns the current go to position */
double* getgoto(); double* getgoto();
/* updates flight status*/ /* updates flight status*/
@ -82,7 +83,7 @@ int buzzuav_update_battery(buzzvm_t vm);
* Updates current position in Buzz * Updates current position in Buzz
*/ */
int buzzuav_update_currentpos(buzzvm_t vm); int buzzuav_update_currentpos(buzzvm_t vm);
buzzobj_t buzzuav_update_userspos(buzzvm_t vm);
/* /*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it * Updates flight status and rc command in Buzz, put it in a tabel to acess it

24
include/roscontroller.h
View File

@ -46,7 +46,7 @@ namespace rosbzz_node{
class roscontroller{ class roscontroller{
public: public:
roscontroller(ros::NodeHandle n_c); roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
~roscontroller(); ~roscontroller();
//void RosControllerInit(); //void RosControllerInit();
void RosControllerRun(); void RosControllerRun();
@ -61,6 +61,14 @@ private:
}; typedef struct num_robot_count Num_robot_count ; }; typedef struct num_robot_count Num_robot_count ;
// WGS84 constants
double equatorial_radius = 6378137.0;
double flattening = 1.0/298.257223563;
double excentrity2 = 2*flattening - flattening*flattening;
// default reference position
double DEFAULT_REFERENCE_LATITUDE = 45.457817;
double DEFAULT_REFERENCE_LONGITUDE = -73.636075;
double cur_pos[3]; double cur_pos[3];
double home[3]; double home[3];
double cur_rel_altitude; double cur_rel_altitude;
@ -70,6 +78,7 @@ private:
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos; //std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step=0; int timer_step=0;
int robot_id=0; int robot_id=0;
std::string robot_name = "";
//int oldcmdID=0; //int oldcmdID=0;
int rc_cmd; int rc_cmd;
float fcu_timeout; float fcu_timeout;
@ -85,15 +94,17 @@ private:
std::string relative_altitude_sub_name; std::string relative_altitude_sub_name;
std::string setpoint_nonraw; std::string setpoint_nonraw;
bool rcclient; bool rcclient;
bool xbeeplugged = false;
bool multi_msg; bool multi_msg;
Num_robot_count count_robots; Num_robot_count count_robots;
ros::ServiceClient mav_client; ros::ServiceClient mav_client;
ros::ServiceClient xbeestatus_srv; ros::ServiceClient xbeestatus_srv;
ros::Publisher payload_pub; ros::Publisher payload_pub;
ros::Publisher neigh_pos_pub; ros::Publisher neigh_pos_pub;
ros::Publisher localsetpoint_pub; ros::Publisher localsetpoint_nonraw_pub;
ros::ServiceServer service; ros::ServiceServer service;
ros::Subscriber current_position_sub; ros::Subscriber current_position_sub;
ros::Subscriber users_sub;
ros::Subscriber battery_sub; ros::Subscriber battery_sub;
ros::Subscriber payload_sub; ros::Subscriber payload_sub;
ros::Subscriber flight_status_sub; ros::Subscriber flight_status_sub;
@ -118,12 +129,12 @@ private:
ros::ServiceClient mode_client; ros::ServiceClient mode_client;
/*Initialize publisher and subscriber, done in the constructor*/ /*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle n_c); 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*/ /*Obtain data from ros parameter server*/
void Rosparameters_get(ros::NodeHandle n_c); void Rosparameters_get(ros::NodeHandle& n_c_priv);
/*compiles buzz script from the specified .bzz file*/ /*compiles buzz script from the specified .bzz file*/
void Compile_bzz(); void Compile_bzz();
@ -166,6 +177,7 @@ private:
/*current position callback*/ /*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
void users_pos(const rosbuzz::neigh_pos msg);
/*current relative altitude callback*/ /*current relative altitude callback*/
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg); void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
@ -183,7 +195,7 @@ private:
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg); void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
/*Get publisher and subscriber from YML file*/ /*Get publisher and subscriber from YML file*/
void GetSubscriptionParameters(ros::NodeHandle node_handle); void GetSubscriptionParameters(ros::NodeHandle& node_handle);
/*Arm/disarm method that can be called from buzz*/ /*Arm/disarm method that can be called from buzz*/
void Arm(); void Arm();
@ -192,7 +204,7 @@ private:
void SetMode(std::string mode, int delay_miliseconds); void SetMode(std::string mode, int delay_miliseconds);
/*Robot independent subscribers*/ /*Robot independent subscribers*/
void Subscribe(ros::NodeHandle n_c); void Subscribe(ros::NodeHandle& n_c);
//void WaypointMissionSetup(float lat, float lng, float alt); //void WaypointMissionSetup(float lat, float lng, float alt);

18
launch/launch_config/m100.yaml
View File

@ -1,13 +1,13 @@
topics: topics:
gps : /global_position gps : global_position
battery : /power_status battery : power_status
status : /flight_status status : flight_status
fcclient : /dji_mavcmd fcclient : dji_mavcmd
setpoint : /setpoint_position/local setpoint : setpoint_position/local
armclient: /dji_mavarm armclient: dji_mavarm
modeclient: /dji_mavmode modeclient: dji_mavmode
altitude: /rel_alt altitude: rel_alt
stream: /set_stream_rate stream: set_stream_rate
type: type:
gps : sensor_msgs/NavSatFix gps : sensor_msgs/NavSatFix

18
launch/launch_config/solo.yaml
View File

@ -1,13 +1,13 @@
topics: topics:
gps : /mavros/global_position/global gps : mavros/global_position/global
battery : /mavros/battery battery : mavros/battery
status : /mavros/state status : mavros/state
fcclient: /mavros/cmd/command fcclient: mavros/cmd/command
setpoint: /mavros/setpoint_position/local setpoint: mavros/setpoint_position/local
armclient: /mavros/cmd/arming armclient: mavros/cmd/arming
modeclient: /mavros/set_mode modeclient: mavros/set_mode
stream: /mavros/set_stream_rate stream: mavros/set_stream_rate
altitude: /mavros/global_position/rel_alt altitude: mavros/global_position/rel_alt
type: type:
gps : sensor_msgs/NavSatFix gps : sensor_msgs/NavSatFix
# for SITL Solo # for SITL Solo

222
src/buzz_utility.cpp
View File

@ -20,6 +20,8 @@ namespace buzz_utility{
static uint8_t MSG_SIZE = 50; // Only 100 bytes of Buzz messages every step static uint8_t MSG_SIZE = 50; // Only 100 bytes of Buzz messages every step
static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static int Robot_id = 0; static int Robot_id = 0;
buzzobj_t usersvstig;
buzzobj_t key;
/****************************************/ /****************************************/
/*adds neighbours position*/ /*adds neighbours position*/
@ -51,6 +53,9 @@ namespace buzz_utility{
return out; return out;
} }
int get_robotid() {
return Robot_id;
}
/***************************************************/ /***************************************************/
/*Appends obtained messages to buzz in message Queue*/ /*Appends obtained messages to buzz in message Queue*/
/***************************************************/ /***************************************************/
@ -252,18 +257,10 @@ namespace buzz_utility{
int buzz_script_set(const char* bo_filename, int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) { const char* bdbg_filename, int robot_id) {
//cout<<"bo file name"<<bo_filename;
/* Get hostname */
//char hstnm[30];
//char* hstnm ="M1003";
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm+4, NULL, 10);
cout << " Robot ID: " <<robot_id<< endl; cout << " Robot ID: " <<robot_id<< endl;
/* Reset the Buzz VM */ /* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
Robot_id =robot_id; Robot_id = robot_id;
VM = buzzvm_new((int)robot_id); VM = buzzvm_new((int)robot_id);
/* Get rid of debug info */ /* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
@ -287,69 +284,102 @@ namespace buzz_utility{
} }
fclose(fd); fclose(fd);
/* Read debug information */ /* 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, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
// buzzvm_destroy(&VM);
// buzzdebug_destroy(&DBG_INFO);
// fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
// return 0;
//}
/* Register hook functions */
/*if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename); perror(bdbg_filename);
return 0; return 0;
}*/ }
/* Save bytecode file name */ /* Set byte code */
//BO_FNAME = strdup(bo_filename); if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
/* Execute the global part of the script */ buzzvm_destroy(&VM);
//buzzvm_execute_script(VM); buzzdebug_destroy(&DBG_INFO);
/* Call the Init() function */ fprintf(stderr, "%s: Error loading Buzz script\n\n", bo_filename);
//buzzvm_function_call(VM, "init", 0); return 0;
/* All OK */ }
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size); /* Register hook functions */
} if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename);
return 0;
}
buzzvm_dup(VM);
// usersvstig = stigmergy.create(123)
buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
// value of the stigmergy id
buzzvm_pushi(VM, 5);
// get the stigmergy table from the global scope
// buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
// buzzvm_gload(VM);
// get the create method from the stigmergy table
// buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
// buzzvm_tget(VM);
// call the stigmergy.create() method
// buzzvm_closure_call(VM, 1);
// now the stigmergy is at the top of the stack - save it for future reference
// usersvstig = buzzvm_stack_at(VM, 0);
//buzzvm_pop(VM);
// assign the virtual stigmergy to the global symbol v
// create also a global variable for it, so the garbage collector does not remove it
//buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
//buzzvm_push(VM, usersvstig);
buzzvm_gstore(VM);
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */
buzzvm_execute_script(VM);
/* Call the Init() function */
buzzvm_function_call(VM, "init", 0);
/* All OK */
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
int buzz_update_users_stigmergy(buzzobj_t tbl){
// push the key (here it's an int with value 46)
buzzvm_pushi(VM, 46);
// push the table
buzzvm_push(VM, tbl);
// the stigmergy is stored in the vstig variable
// let's push it on the stack
buzzvm_push(VM, usersvstig);
// get the put method from myvstig
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM);
// call the vstig.put() method
buzzvm_closure_call(VM, 2);
return 1;
}
/****************************************/ /****************************************/
/*Sets a new update */ /*Sets a new update */
/****************************************/ /****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */ /* // Reset the Buzz VM
//char hstnm[30];
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm + 4, NULL, 10);
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id); VM = buzzvm_new(Robot_id);
/* Get rid of debug info */ // Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new(); DBG_INFO = buzzdebug_new();
/* Read debug information */ // Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename); perror(bdbg_filename);
return 0; return 0;
} }
/* Set byte code */ // Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0; return 0;
} }
/* Register hook functions */ // Register hook functions
if(buzz_register_hooks() != BUZZVM_STATE_READY) { if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -357,46 +387,40 @@ namespace buzz_utility{
return 0; return 0;
} }
/* Execute the global part of the script */ // Execute the global part of the script
buzzvm_execute_script(VM); buzzvm_execute_script(VM);
/* Call the Init() function */ // Call the Init() function
buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
/* All OK */ // All OK
return 1; */ return 1;
} }
/****************************************/ /****************************************/
/*Performs a initialization test */ /*Performs a initialization test */
/****************************************/ /****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */ /* // Reset the Buzz VM
//char hstnm[30];
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm + 4, NULL, 10);
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id); VM = buzzvm_new(Robot_id);
/* Get rid of debug info */ // Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new(); DBG_INFO = buzzdebug_new();
/* Read debug information */ // Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename); perror(bdbg_filename);
return 0; return 0;
} }
/* Set byte code */ // Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0; return 0;
} }
/* Register hook functions */ // Register hook functions
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) { if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -404,12 +428,12 @@ namespace buzz_utility{
return 0; return 0;
} }
/* Execute the global part of the script */ // Execute the global part of the script
buzzvm_execute_script(VM); buzzvm_execute_script(VM);
/* Call the Init() function */ // Call the Init() function
buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
/* All OK */ // All OK
return 1; */ return 1;
} }
/****************************************/ /****************************************/
@ -425,31 +449,35 @@ namespace buzz_utility{
void check_swarm_members(const void* key, void* data, void* params) { void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params; int* status = (int*)params;
fprintf(stderr, "CHECKING SWARM MEMBERS\n");
if(*status == 3) return; if(*status == 3) return;
fprintf(stderr, "CHECKING SWARM MEMBERS:%i\n",buzzdarray_get(e->swarms, 0, uint16_t));
if(buzzdarray_size(e->swarms) != 1) { if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n"); fprintf(stderr, "Swarm list size is not 1\n");
*status = 3; *status = 3;
} }
else { else {
int sid = 1; int sid = 1;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) && if(!buzzdict_isempty(VM->swarms)) {
buzzdarray_get(e->swarms, 0, uint16_t) != sid) { if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
sid, fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
buzzdarray_get(e->swarms, 0, uint16_t)); sid,
*status = 3; buzzdarray_get(e->swarms, 0, uint16_t));
return; *status = 3;
} return;
sid = 2; }
if(*buzzdict_get(VM->swarms, &sid, uint8_t) && }
buzzdarray_get(e->swarms, 0, uint16_t) != sid) { if(buzzdict_size(VM->swarms)>1) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid = 2;
sid, if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t)); buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
*status = 3; fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
return; sid,
} buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
} }
} }
/*Step through the buzz script*/ /*Step through the buzz script*/
@ -461,7 +489,12 @@ namespace buzz_utility{
buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_currentpos(VM);
buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM); buzzuav_closures::buzzuav_update_flight_status(VM);
//buzz_update_users_stigmergy(tbl);
/* /*
* Call Buzz step() function * Call Buzz step() function
*/ */
@ -471,21 +504,16 @@ namespace buzz_utility{
buzz_error_info()); buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
} }
// usleep(10000); //buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step
//usleep(10000);
/*Print swarm*/ /*Print swarm*/
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
int SwarmSize = buzzdict_size(VM->swarmmembers)+1; //int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/* Check swarm state -- SOMETHING CRASHING HERE!! */ /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
int status = 1; // int status = 1;
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); // buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
/* if(status == 1 &&
buzzdict_size(VM->swarmmembers) < 9)
status = 2;*/
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
buzzvm_pushi(VM, status);
buzzvm_gstore(VM);
} }
/****************************************/ /****************************************/

27
src/buzzuav_closures.cpp
View File

@ -19,6 +19,7 @@ namespace buzzuav_closures{
static float batt[3]; static float batt[3];
static float obst[5]={0,0,0,0,0}; static float obst[5]={0,0,0,0,0};
static double cur_pos[3]; static double cur_pos[3];
static double users_pos[3];
static uint8_t status; static uint8_t status;
static int cur_cmd = 0; static int cur_cmd = 0;
static int rc_cmd=0; static int rc_cmd=0;
@ -30,6 +31,7 @@ namespace buzzuav_closures{
int buzzros_print(buzzvm_t vm) { int buzzros_print(buzzvm_t vm) {
int i; int i;
char buffer [50] = ""; char buffer [50] = "";
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
buzzvm_lload(vm, i); buzzvm_lload(vm, i);
buzzobj_t o = buzzvm_stack_at(vm, 1); buzzobj_t o = buzzvm_stack_at(vm, 1);
@ -244,6 +246,11 @@ namespace buzzuav_closures{
cur_pos[1]=longitude; cur_pos[1]=longitude;
cur_pos[2]=altitude; cur_pos[2]=altitude;
} }
void set_userspos(double latitude, double longitude, double altitude){
users_pos[0]=latitude;
users_pos[1]=longitude;
users_pos[2]=altitude;
}
/****************************************/ /****************************************/
int buzzuav_update_currentpos(buzzvm_t vm) { int buzzuav_update_currentpos(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
@ -263,6 +270,26 @@ namespace buzzuav_closures{
buzzvm_gstore(vm); buzzvm_gstore(vm);
return vm->state; return vm->state;
} }
buzzobj_t buzzuav_update_userspos(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, users_pos[0]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, users_pos[1]);
buzzvm_tput(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1));
buzzvm_pushf(vm, users_pos[2]);
buzzvm_tput(vm);
buzzvm_gstore(vm);
return buzzvm_stack_at(vm, 0);
//return vm->state;
}
void flight_status_update(uint8_t state){ void flight_status_update(uint8_t state){
status=state; status=state;

5
src/rosbuzz.cpp
View File

@ -16,8 +16,9 @@ int main(int argc, char **argv)
{ {
/*Initialize rosBuzz node*/ /*Initialize rosBuzz node*/
ros::init(argc, argv, "rosBuzz"); ros::init(argc, argv, "rosBuzz");
ros::NodeHandle n_c("~"); ros::NodeHandle nh_priv("~");
rosbzz_node::roscontroller RosController(n_c); ros::NodeHandle nh;
rosbzz_node::roscontroller RosController(nh, nh_priv);
/*Register ros controller object inside buzz*/ /*Register ros controller object inside buzz*/
//buzzuav_closures::set_ros_controller_ptr(&RosController); //buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun(); RosController.RosControllerRun();

366
src/roscontroller.cpp
View File

@ -5,11 +5,11 @@ namespace rosbzz_node{
/*--------------- /*---------------
/Constructor /Constructor
---------------*/ ---------------*/
roscontroller::roscontroller(ros::NodeHandle n_c) roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
{ {
ROS_INFO("Buzz_node"); ROS_INFO("Buzz_node");
/*Obtain parameters from ros parameter server*/ /*Obtain parameters from ros parameter server*/
Rosparameters_get(n_c); Rosparameters_get(n_c_priv);
/*Initialize publishers, subscribers and client*/ /*Initialize publishers, subscribers and client*/
Initialize_pub_sub(n_c); Initialize_pub_sub(n_c);
/*Compile the .bzz file to .basm, .bo and .bdbg*/ /*Compile the .bzz file to .basm, .bo and .bdbg*/
@ -23,12 +23,13 @@ namespace rosbzz_node{
// set stream rate - wait for the FC to be started // set stream rate - wait for the FC to be started
SetStreamRate(0, 10, 1); SetStreamRate(0, 10, 1);
/// Get Robot Id - wait for Xbee to be started /// Get Robot Id - wait for Xbee to be started
GetRobotId(); if(xbeeplugged)
GetRobotId();
else
robot_id=strtol(robot_name.c_str() + 5, NULL, 10);;
setpoint_counter = 0; setpoint_counter = 0;
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
home[0] = 0.0; home[0]=0.0;home[1]=0.0;home[2]=0.0;
home[1] = 0.0;
home[2] = 0.0;
} }
/*--------------------- /*---------------------
@ -67,7 +68,10 @@ namespace rosbzz_node{
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n"); fprintf(stdout, "Bytecode file found and set\n");
init_update_monitor(bcfname.c_str(),stand_by.c_str()); //init_update_monitor(bcfname.c_str(),stand_by.c_str());
///////////////////////////////////////////////////////
// MAIN LOOP
//////////////////////////////////////////////////////
while (ros::ok() && !buzz_utility::buzz_script_done()) while (ros::ok() && !buzz_utility::buzz_script_done())
{ {
/*Update neighbors position inside Buzz*/ /*Update neighbors position inside Buzz*/
@ -75,7 +79,7 @@ namespace rosbzz_node{
/*Neighbours of the robot published with id in respective topic*/ /*Neighbours of the robot published with id in respective topic*/
neighbours_pos_publisher(); neighbours_pos_publisher();
/*Check updater state and step code*/ /*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str()); //update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */ /*Step buzz script */
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/ /*Prepare messages and publish them in respective topic*/
@ -83,10 +87,10 @@ namespace rosbzz_node{
/*call flight controler service to set command long*/ /*call flight controler service to set command long*/
flight_controller_service_call(); flight_controller_service_call();
/*Set multi message available after update*/ /*Set multi message available after update*/
if(get_update_status()){ /*if(get_update_status()){
set_read_update_status(); set_read_update_status();
multi_msg=true; multi_msg=true;
} }*/
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/ /*Set ROBOTS variable for barrier in .bzz from neighbours count*/
//no_of_robots=get_number_of_robots(); //no_of_robots=get_number_of_robots();
get_number_of_robots(); get_number_of_robots();
@ -117,7 +121,7 @@ namespace rosbzz_node{
/*-------------------------------------------------------- /*--------------------------------------------------------
/ Get all required parameters from the ROS launch file / Get all required parameters from the ROS launch file
/-------------------------------------------------------*/ /-------------------------------------------------------*/
void roscontroller::Rosparameters_get(ros::NodeHandle n_c){ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c){
/*Obtain .bzz file name from parameter server*/ /*Obtain .bzz file name from parameter server*/
if(n_c.getParam("bzzfile_name", bzzfile_name)); if(n_c.getParam("bzzfile_name", bzzfile_name));
@ -126,11 +130,8 @@ namespace rosbzz_node{
if(n_c.getParam("rcclient", rcclient)){ if(n_c.getParam("rcclient", rcclient)){
if(rcclient==true){ if(rcclient==true){
/*Service*/ /*Service*/
if(n_c.getParam("rcservice_name", rcservice_name)){ if(!n_c.getParam("rcservice_name", rcservice_name))
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this); {ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
ROS_INFO("Ready to receive Mav Commands from RC client");
}
else{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
} }
else if(rcclient==false){ROS_INFO("RC service is disabled");} else if(rcclient==false){ROS_INFO("RC service is disabled");}
} }
@ -144,7 +145,13 @@ namespace rosbzz_node{
n_c.getParam("in_payload", in_payload); n_c.getParam("in_payload", in_payload);
/*Obtain standby script to run during update*/ /*Obtain standby script to run during update*/
n_c.getParam("stand_by", stand_by); n_c.getParam("stand_by", stand_by);
n_c.getParam("xbee_status_srv", xbeesrv_name);
if(n_c.getParam("xbee_plugged", xbeeplugged));
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
if(!xbeeplugged){
if(n_c.getParam("name", robot_name));
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
}
GetSubscriptionParameters(n_c); GetSubscriptionParameters(n_c);
// initialize topics to null? // initialize topics to null?
@ -153,11 +160,12 @@ namespace rosbzz_node{
/*----------------------------------------------------------------------------------- /*-----------------------------------------------------------------------------------
/Obtains publisher, subscriber and services from yml file based on the robot used /Obtains publisher, subscriber and services from yml file based on the robot used
/-----------------------------------------------------------------------------------*/ /-----------------------------------------------------------------------------------*/
void roscontroller::GetSubscriptionParameters(ros::NodeHandle node_handle){ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle){
//todo: make it as an array in yaml? //todo: make it as an array in yaml?
m_sMySubscriptions.clear(); m_sMySubscriptions.clear();
std::string gps_topic, gps_type; std::string gps_topic, gps_type;
node_handle.getParam("topics/gps", gps_topic); if(node_handle.getParam("topics/gps", gps_topic));
else {ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node");}
node_handle.getParam("type/gps", gps_type); node_handle.getParam("type/gps", gps_type);
m_smTopic_infos.insert(pair <std::string, std::string>(gps_topic, gps_type)); m_smTopic_infos.insert(pair <std::string, std::string>(gps_topic, gps_type));
@ -192,7 +200,7 @@ namespace rosbzz_node{
/*-------------------------------------------------------- /*--------------------------------------------------------
/ Create all required subscribers, publishers and ROS service clients / Create all required subscribers, publishers and ROS service clients
/-------------------------------------------------------*/ /-------------------------------------------------------*/
void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c){
/*subscribers*/ /*subscribers*/
Subscribe(n_c); Subscribe(n_c);
@ -202,24 +210,29 @@ namespace rosbzz_node{
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this); payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this); //flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
//Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this); //Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this); obstacle_sub= n_c.subscribe("guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
/*publishers*/ /*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000); payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000); neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos",1000);
localsetpoint_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000); localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000);
/* Service Clients*/ /* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient); arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient); mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name); mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
if(rcclient==true)
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name); xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name); stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos,this);
multi_msg=true; multi_msg=true;
} }
/*--------------------------------------- /*---------------------------------------
/Robot independent subscribers /Robot independent subscribers
/--------------------------------------*/ /--------------------------------------*/
void roscontroller::Subscribe(ros::NodeHandle n_c){ void roscontroller::Subscribe(ros::NodeHandle& n_c){
for(std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){ for(std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){
if(it->second == "mavros_msgs/ExtendedState"){ if(it->second == "mavros_msgs/ExtendedState"){
@ -345,35 +358,35 @@ namespace rosbzz_node{
delete[] out; delete[] out;
delete[] payload_out_ptr; delete[] payload_out_ptr;
/*Check for updater message if present send*/ /*Check for updater message if present send*/
if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){ /*if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
uint8_t* buff_send = 0; uint8_t* buff_send = 0;
uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());; uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
int tot=0; int tot=0;
mavros_msgs::Mavlink update_packets; mavros_msgs::Mavlink update_packets;
fprintf(stdout,"Transfering code \n"); fprintf(stdout,"Transfering code \n");
fprintf(stdout,"Sent Update packet Size: %u \n",updater_msgSize); fprintf(stdout,"Sent Update packet Size: %u \n",updater_msgSize);
/*allocate mem and clear it*/ // allocate mem and clear it
buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize); buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize); memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
/*Append updater msg size*/ // Append updater msg size
*(uint16_t*)(buff_send + tot)=updater_msgSize; *(uint16_t*)(buff_send + tot)=updater_msgSize;
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize); //fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/*Append updater msgs*/ // Append updater msgs
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
tot += updater_msgSize; tot += updater_msgSize;
/*Destroy the updater out msg queue*/ // Destroy the updater out msg queue
destroy_out_msg_queue(); destroy_out_msg_queue();
uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t))); uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size]; uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t)); memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
free(buff_send); free(buff_send);
/*Send a constant number to differenciate updater msgs*/ // Send a constant number to differenciate updater msgs
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT); update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
for(int i=0;i<total_size;i++){ for(int i=0;i<total_size;i++){
update_packets.payload64.push_back(payload_64[i]); update_packets.payload64.push_back(payload_64[i]);
} }
/*Add Robot id and message number to the published message*/ // Add Robot id and message number to the published message
if (message_number < 0) message_number=0; if (message_number < 0) message_number=0;
else message_number++; else message_number++;
update_packets.sysid=(uint8_t)robot_id; update_packets.sysid=(uint8_t)robot_id;
@ -381,7 +394,7 @@ namespace rosbzz_node{
payload_pub.publish(update_packets); payload_pub.publish(update_packets);
multi_msg=false; multi_msg=false;
delete[] payload_64; delete[] payload_64;
} }*/
} }
/*--------------------------------------------------------------------------------- /*---------------------------------------------------------------------------------
@ -399,8 +412,11 @@ namespace rosbzz_node{
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode // SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode
switch(buzzuav_closures::getcmd()){ switch(buzzuav_closures::getcmd()){
case mavros_msgs::CommandCode::NAV_LAND: case mavros_msgs::CommandCode::NAV_LAND:
if(current_mode != "LAND") if(current_mode != "LAND") {
{SetMode("LAND", 0);} SetMode("LAND", 0);
armstate = 0;
Arm();
}
if (mav_client.call(cmd_srv)){ if (mav_client.call(cmd_srv)){
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else } else
@ -413,6 +429,8 @@ namespace rosbzz_node{
armstate = 1; armstate = 1;
Arm(); Arm();
ros::Duration(0.5).sleep(); ros::Duration(0.5).sleep();
// Registering HOME POINT.
home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2];
} }
if(current_mode != "GUIDED") if(current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm) SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@ -451,8 +469,7 @@ namespace rosbzz_node{
Arm(); Arm();
} }
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
//roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0); //roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
} }
@ -504,22 +521,133 @@ namespace rosbzz_node{
----------------------------------------------------------- */ ----------------------------------------------------------- */
#define EARTH_RADIUS (double) 6371000.0 #define EARTH_RADIUS (double) 6371000.0
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0))) #define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0)))
void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) {
double hyp_az, hyp_el;
double sin_el, cos_el, sin_az, cos_az;
/* Convert reference point to spherical earth centered coordinates. */
hyp_az = sqrt(ref_ecef[0]*ref_ecef[0] + ref_ecef[1]*ref_ecef[1]);
hyp_el = sqrt(hyp_az*hyp_az + ref_ecef[2]*ref_ecef[2]);
sin_el = ref_ecef[2] / hyp_el;
cos_el = hyp_az / hyp_el;
sin_az = ref_ecef[1] / hyp_az;
cos_az = ref_ecef[0] / hyp_az;
M[0][0] = -sin_el * cos_az;
M[0][1] = -sin_el * sin_az;
M[0][2] = cos_el;
M[1][0] = -sin_az;
M[1][1] = cos_az;
M[1][2] = 0.0;
M[2][0] = -cos_el * cos_az;
M[2][1] = -cos_el * sin_az;
M[2][2] = -sin_el;
}
void matrix_multiply(uint32_t n, uint32_t m, uint32_t p, const double *a,
const double *b, double *c)
{
uint32_t i, j, k;
for (i = 0; i < n; i++)
for (j = 0; j < p; j++) {
c[p*i + j] = 0;
for (k = 0; k < m; k++)
c[p*i + j] += a[m*i+k] * b[p*k + j];
}
}
void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){ void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){
double d_lon = nei[1] - cur[1];
// calculate earth radii
double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
double prime_vertical_radius = equatorial_radius * sqrt(temp);
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
/*double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned[3];
ned[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
ned[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
double ecef[3];
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double ref_ecef[3];
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
d = WGS84_E * sin(llh[0]);
N = WGS84_A / sqrt(1. - d*d);
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
double ned[3];
matrix_multiply(3, 3, 1, (double *)M, ecef, ned);
out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]);
out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned[1],ned[0]);
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;*/
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0]; double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
out[0] = sqrt(ned_x*ned_x+ned_y*ned_y); out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
//out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned_y,ned_x); out[1] = atan2(ned_y,ned_x);
//out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0; out[2] = 0.0;
} }
void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){ void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
// calculate earth radii
/*double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
double prime_vertical_radius = equatorial_radius * sqrt(temp);
double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
double radius_east = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
double d_lon = nei[1] - cur[1]; double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0]; double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS; out[0] = DEG2RAD(d_lat) * radius_north;//EARTH_RADIUS;
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0])); out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = -DEG2RAD(d_lon) * radius_east; //EARTH_RADIUS
out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = cur[2]; out[2] = cur[2];
// Using functions of the library Swift Nav (https://github.com/swift-nav/libswiftnav)
double ecef[3];
double llh[3];llh[0]=DEG2RAD(cur[0]);llh[1]=DEG2RAD(cur[1]);llh[2]=cur[2];
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double ref_ecef[3];
llh[0]=DEG2RAD(nei[0]);llh[1]=DEG2RAD(nei[1]);llh[2]=nei[2];
d = WGS84_E * sin(llh[0]);
N = WGS84_A / sqrt(1. - d*d);
ref_ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ref_ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
double M[3][3];
ecef2ned_matrix(ref_ecef, M);
matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
//out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
//out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0;
} }
/*------------------------------------------------------ /*------------------------------------------------------
@ -557,13 +685,36 @@ namespace rosbzz_node{
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
//ROS_INFO("Altitude out: %f", cur_rel_altitude); //ROS_INFO("Altitude out: %f", cur_rel_altitude);
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
if(home[0]==0){ //double lat = std::floor(msg->latitude * 1000000) / 1000000;
home[0]=msg->latitude; //double lon = std::floor(msg->longitude * 1000000) / 1000000;
home[1]=msg->longitude; /*if(cur_rel_altitude<1.2){
home[0]=lat;
home[1]=lon;
home[2]=cur_rel_altitude; home[2]=cur_rel_altitude;
} }*/
set_cur_pos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude); set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude);
buzzuav_closures::set_currentpos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude); buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude);
}
void roscontroller::users_pos(const rosbuzz::neigh_pos data){
//ROS_INFO("Altitude out: %f", cur_rel_altitude);
double us[3];
int n = data.pos_neigh.size();
// ROS_INFO("Neighbors array size: %i\n", n);
if(n>0)
{
for(int it=0; it<n; ++it)
{
us[0] = data.pos_neigh[it].latitude;
us[1] = data.pos_neigh[it].longitude;
us[2] = data.pos_neigh[it].altitude;
double out[3];
cvt_rangebearing_coordinates(us, out, cur_pos);
buzzuav_closures::set_userspos(out[0], out[1], out[2]);
}
}
} }
/*------------------------------------------------------------- /*-------------------------------------------------------------
/ Update altitude into BVM from subscriber / Update altitude into BVM from subscriber
@ -582,6 +733,66 @@ namespace rosbzz_node{
obst[i]=msg->ranges[i]; obst[i]=msg->ranges[i];
buzzuav_closures::set_obstacle_dist(obst); buzzuav_closures::set_obstacle_dist(obst);
} }
void roscontroller::SetLocalPosition(float x, float y, float z, float yaw){
// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
geometry_msgs::PoseStamped moveMsg;
moveMsg.header.stamp = ros::Time::now();
moveMsg.header.seq = setpoint_counter++;
moveMsg.header.frame_id = 1;
double local_pos[3];
cvt_ned_coordinates(cur_pos,local_pos,home);
ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]);
ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
moveMsg.pose.position.x = local_pos[1]+y;
moveMsg.pose.position.y = local_pos[0]+x;
moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0;
moveMsg.pose.orientation.y = 0;
moveMsg.pose.orientation.z = 0;
moveMsg.pose.orientation.w = 1;
// To prevent drifting from stable position.
if(fabs(x)>0.1 || fabs(y)>0.1) {
localsetpoint_nonraw_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!");
}
}
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
// wait if necessary
if (delay_miliseconds != 0){
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
}
// set mode
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = mode;
current_mode = mode;
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
}
void roscontroller::SetStreamRate(int id, int rate, int on_off){
mavros_msgs::StreamRate message;
message.request.stream_id = id;
message.request.message_rate = rate;
message.request.on_off = on_off;
while(!stream_client.call(message)){
ROS_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep();
}
ROS_INFO("Set stream rate call successful");
}
/*------------------------------------------------------------- /*-------------------------------------------------------------
/Push payload into BVM FIFO /Push payload into BVM FIFO
@ -777,66 +988,5 @@ namespace rosbzz_node{
} }
*/ */
} }
/*
* SOLO SPECIFIC FUNCTIONS
*/
void roscontroller::SetLocalPosition(float x, float y, float z, float yaw){
// http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html
// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
geometry_msgs::PoseStamped moveMsg;
moveMsg.header.stamp = ros::Time::now();
moveMsg.header.seq = setpoint_counter++;
moveMsg.header.frame_id = 1;
double local_pos[3];
cvt_ned_coordinates(cur_pos,local_pos,home);
moveMsg.pose.position.x = local_pos[0]+x;
moveMsg.pose.position.y = local_pos[1]+y;
moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0;
moveMsg.pose.orientation.y = 0;
moveMsg.pose.orientation.z = 0;
moveMsg.pose.orientation.w = 1;
std::cout<<"Home (0,1,2): " << home[0] <<", " << home[1] << ", " << home[2] <<std::endl;
localsetpoint_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!");
}
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
// wait if necessary
if (delay_miliseconds != 0){
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
}
// set mode
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = mode;
current_mode = mode;
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
}
void roscontroller::SetStreamRate(int id, int rate, int on_off){
mavros_msgs::StreamRate message;
message.request.stream_id = id;
message.request.message_rate = rate;
message.request.on_off = on_off;
while(!stream_client.call(message)){
ROS_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep();
}
ROS_INFO("Set stream rate call successful");
}
} }

213
src/testflocksim.bzz Normal file
View File

@ -0,0 +1,213 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 10.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 12.0
EPSILON = 12.0
# Lennard-Jones interaction magnitude
function lj_magnitude(dist, target, epsilon) {
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
}
# Neighbor data to LJ interaction vector
function lj_vector(rid, data) {
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
}
# Accumulator of neighbor LJ interactions
function lj_sum(rid, data, accum) {
return math.vec2.add(data, accum)
}
# Calculates and actuates the flocking interaction
function hexagon() {
statef=hexagon
CURSTATE = "HEXAGON"
# Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
math.vec2.scale(accum, 1.0 / neighbors.count())
# Move according to vector
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x,accum.y)
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else {
# timeW = timeW+1
# uav_moveto(0.0,0.0)
# }
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
timeW=0
barrier = nil
statef=idle
}
}
# Executed once at init time.
function init() {
s = swarm.create(1)
# s.select(1)
s.join()
statef=idle
CURSTATE = "IDLE"
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
uav_goto()
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
log("User position: ", users.range)
# Read a value from the structure
#x = usersvstig.get(46)
# Get the number of keys in the structure
#log("The vstig has ", usersvstig.size(), " elements")
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}