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

Merge branch 'dev' of https://github.com/MISTLab/ROSBuzz into dev 

Conflicts:
	src/roscontroller.cpp
	src/testflockfev.bzz

	modified:   include/buzzuav_closures.h
	modified:   include/roscontroller.h
	modified:   launch/rosbuzz_2_parallel.launch
	modified:   src/buzz_utility.cpp
	modified:   src/buzzuav_closures.cpp
	modified:   src/rosbuzz.cpp
	modified:   src/roscontroller.cpp
	modified:   src/testflockfev.bzz
This commit is contained in:
David St-Onge 2017-03-15 16:57:56 -04:00
parent abdef63da6 141f05807d
commit f4c4478e38
8 changed files with 505 additions and 541 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
include/buzzuav_closures.h
View File

@ -6,7 +6,8 @@
#include "uav_utility.h" #include "uav_utility.h"
#include "mavros_msgs/CommandCode.h" #include "mavros_msgs/CommandCode.h"
#include "ros/ros.h" #include "ros/ros.h"
#include "buzz_utility.h"
//#include "roscontroller.h"
namespace buzzuav_closures{ namespace buzzuav_closures{
/* /*
@ -46,8 +47,13 @@ void set_obstacle_dist(float dist[]);
* Commands the UAV to takeoff * Commands the UAV to takeoff
*/ */
int buzzuav_takeoff(buzzvm_t vm); int buzzuav_takeoff(buzzvm_t vm);
/*
* Arm command from Buzz
*/
int buzzuav_arm(buzzvm_t vm); 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 /* Commands the UAV to land
*/ */
@ -66,8 +72,6 @@ int buzzuav_update_battery(buzzvm_t vm);
*/ */
int buzzuav_update_currentpos(buzzvm_t vm); int buzzuav_update_currentpos(buzzvm_t vm);
/*update obstacles in Buzz*/
int buzzuav_update_obstacle(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

14
include/roscontroller.h
View File

@ -83,7 +83,8 @@ private:
mavros_msgs::SetMode m_cmdSetMode; mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client; ros::ServiceClient mode_client;
/*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle n_c); void Initialize_pub_sub(ros::NodeHandle n_c);
/*Obtain data from ros parameter server*/ /*Obtain data from ros parameter server*/
@ -92,6 +93,12 @@ private:
/*compiles buzz script from the specified .bzz file*/ /*compiles buzz script from the specified .bzz file*/
void Compile_bzz(); void Compile_bzz();
/*Flight controller service call*/
void flight_controler_service_call();
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*Prepare messages and publish*/ /*Prepare messages and publish*/
void prepare_msg_and_publish(); void prepare_msg_and_publish();
@ -133,14 +140,19 @@ private:
/*robot id sub callback*/ /*robot id sub callback*/
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg); void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
/*Obstacle distance table callback*/
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*/
void GetSubscriptionParameters(ros::NodeHandle node_handle); void GetSubscriptionParameters(ros::NodeHandle node_handle);
/*Arm/disarm method that can be called from buzz*/
void Arm(); void Arm();
/*set mode like guided for solo*/
void SetMode(); void SetMode();
/*Robot independent subscribers*/
void Subscribe(ros::NodeHandle n_c); void Subscribe(ros::NodeHandle n_c);
}; };

2
launch/rosbuzz_2_parallel.launch
View File

@ -2,7 +2,7 @@
<launch> <launch>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" > <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev.bzz" /> <param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev_2parallel.bzz" />
<rosparam file="/home/vivek/catkin_ws/src/rosbuzz/launch/launch_config/m100.yaml"/> <rosparam file="/home/vivek/catkin_ws/src/rosbuzz/launch/launch_config/m100.yaml"/>
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" /> <param name="rcservice_name" value="rc_cmd" />

51
src/buzz_utility.cpp
View File

@ -76,31 +76,8 @@ namespace buzz_utility{
memcpy(pl, payload ,size); memcpy(pl, payload ,size);
/*size and robot id read*/ /*size and robot id read*/
size_t tot = sizeof(uint32_t); size_t tot = sizeof(uint32_t);
/*for(int i=0;i<data[0];i++){
uint16_t* out = u64_cvt_u16(payload[i]);
for(int k=0;k<4;k++){
cout<<" [Debug inside buzz util: obt msg:] "<<out[k]<<endl;
}
}*/
/* Go through the messages until there's nothing else to read */ /* Go through the messages until there's nothing else to read */
//fprintf(stdout,"Total data size : utils : %d\n",(int)size);
uint16_t unMsgSize=0; uint16_t unMsgSize=0;
//uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
//tot+=sizeof(uint8_t);
//fprintf(stdout,"is_updater msg present : %i\n",(int)is_msg_pres);
//if(is_msg_pres){
//unMsgSize = *(uint16_t*)(pl + tot);
//tot+=sizeof(uint16_t);
// fprintf(stdout,"%u : read msg size : %u \n",m_unRobotId,unMsgSize);
// if(unMsgSize>0){
// code_message_inqueue_append((uint8_t*)(pl + tot),unMsgSize);
// tot+=unMsgSize;
//fprintf(stdout,"before in queue process : utils\n");
// code_message_inqueue_process();
//fprintf(stdout,"after in queue process : utils\n");
// }
//}
//unMsgSize=0;
/*Obtain Buzz messages only when they are present*/ /*Obtain Buzz messages only when they are present*/
do { do {
@ -131,30 +108,6 @@ namespace buzz_utility{
/* Send robot id */ /* 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); tot += sizeof(uint16_t);
//uint8_t updater_msg_pre = 0;
//uint16_t updater_msgSize= 0;
//if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){
// fprintf(stdout,"transfer code \n");
// updater_msg_pre =1;
//transfer_code=0;
// *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
// tot += sizeof(uint8_t);
/*Append updater msg size*/
//*(uint16_t*)(buff_send + tot) = *(uint16_t*) (getupdate_out_msg_size());
// updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());
// *(uint16_t*)(buff_send + tot)=updater_msgSize;
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
// tot += sizeof(uint16_t);
/*Append updater msgs*/
// memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
// tot += updater_msgSize;
/*destroy the updater out msg queue*/
// destroy_out_msg_queue();
//}
//else{
// *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
// tot += sizeof(uint8_t);
//}
/* Send messages from FIFO */ /* Send messages from FIFO */
do { do {
/* Are there more messages? */ /* Are there more messages? */
@ -507,7 +460,6 @@ namespace buzz_utility{
buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM); buzzuav_closures::buzzuav_update_flight_status(VM);
buzzuav_closures::buzzuav_update_obstacle(VM);
/* /*
* Call Buzz step() function * Call Buzz step() function
*/ */
@ -518,7 +470,7 @@ namespace buzz_utility{
buzzvm_dump(VM); buzzvm_dump(VM);
} }
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/* look into this currently we don't have swarm feature at all */ /* look into this currently we don't have swarm feature tested */
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/ /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/*Print swarm*/ /*Print swarm*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
@ -571,7 +523,6 @@ int update_step_test(){
buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM); buzzuav_closures::buzzuav_update_flight_status(VM);
buzzuav_closures::buzzuav_update_obstacle(VM);
int a = buzzvm_function_call(VM, "step", 0); int a = buzzvm_function_call(VM, "step", 0);
if(a != BUZZVM_STATE_READY){ if(a != BUZZVM_STATE_READY){

671
src/buzzuav_closures.cpp
View File

@ -7,365 +7,346 @@
*/ */
//#define _GNU_SOURCE //#define _GNU_SOURCE
#include "buzzuav_closures.h" #include "buzzuav_closures.h"
#include "buzz_utility.h" //#include "roscontroller.h"
namespace buzzuav_closures{ namespace buzzuav_closures{
// TODO: Minimize the required global variables and put them in the header // TODO: Minimize the required global variables and put them in the header
//static const rosbzz_node::roscontroller* roscontroller_ptr;
/*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 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 buzz_cmd=0;
static float height=0;
/****************************************/
/****************************************/
static double goto_pos[3]; int buzzros_print(buzzvm_t vm) {
static double rc_goto_pos[3]; int i;
static float batt[3]; for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
static float obst[5]={0,0,0,0,0}; buzzvm_lload(vm, i);
static double cur_pos[3]; buzzobj_t o = buzzvm_stack_at(vm, 1);
static uint8_t status; buzzvm_pop(vm);
static int cur_cmd = 0; switch(o->o.type) {
static int rc_cmd=0; case BUZZTYPE_NIL:
static int buzz_cmd=0; ROS_INFO("BUZZ - [nil]");
static float height=0; break;
/****************************************/ case BUZZTYPE_INT:
/****************************************/ ROS_INFO("%d", o->i.value);
//fprintf(stdout, "%d", o->i.value);
break;
case BUZZTYPE_FLOAT:
ROS_INFO("%f", o->f.value);
break;
case BUZZTYPE_TABLE:
ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value)));
break;
case BUZZTYPE_CLOSURE:
if(o->c.value.isnative)
ROS_INFO("[n-closure @%d]", o->c.value.ref);
else
ROS_INFO("[c-closure @%d]", o->c.value.ref);
break;
case BUZZTYPE_STRING:
ROS_INFO("%s", o->s.value.str);
break;
case BUZZTYPE_USERDATA:
ROS_INFO("[userdata @%p]", o->u.value);
break;
default:
break;
}
}
//fprintf(stdout, "\n");
return buzzvm_ret0(vm);
}
int buzzros_print(buzzvm_t vm) { /*----------------------------------------/
int i; / Compute GPS destination from current position and desired Range and Bearing
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { /----------------------------------------*/
buzzvm_lload(vm, i); #define EARTH_RADIUS (double) 6371000.0
buzzobj_t o = buzzvm_stack_at(vm, 1); void gps_from_rb(double range, double bearing, double out[3]) {
buzzvm_pop(vm); double lat = cur_pos[0]*M_PI/180.0;
switch(o->o.type) { double lon = cur_pos[1]*M_PI/180.0;
case BUZZTYPE_NIL: out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
ROS_INFO("BUZZ - [nil]"); out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
break; out[0] = out[0]*180.0/M_PI;
case BUZZTYPE_INT: out[1] = out[1]*180.0/M_PI;
ROS_INFO("%d", o->i.value); out[2] = height; //constant height.
//fprintf(stdout, "%d", o->i.value); }
break;
case BUZZTYPE_FLOAT:
ROS_INFO("%f", o->f.value);
break;
case BUZZTYPE_TABLE:
ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value)));
break;
case BUZZTYPE_CLOSURE:
if(o->c.value.isnative)
ROS_INFO("[n-closure @%d]", o->c.value.ref);
else
ROS_INFO("[c-closure @%d]", o->c.value.ref);
break;
case BUZZTYPE_STRING:
ROS_INFO("%s", o->s.value.str);
break;
case BUZZTYPE_USERDATA:
ROS_INFO("[userdata @%p]", o->u.value);
break;
default:
break;
}
}
//fprintf(stdout, "\n");
return buzzvm_ret0(vm);
}
/*----------------------------------------/ // Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
/ Compute GPS destination from current position and desired Range and Bearing double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
/----------------------------------------*/ double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
#define EARTH_RADIUS (double) 6371000.0 double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
void gps_from_rb(double range, double bearing, double out[3]) {
double lat = cur_pos[0]*M_PI/180.0;
double lon = cur_pos[1]*M_PI/180.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] = out[0]*180.0/M_PI;
out[1] = out[1]*180.0/M_PI;
out[2] = height; //constant height.
}
// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests /*----------------------------------------/
double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336}; / Buzz closure to move following a 2D vector
double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958}; /----------------------------------------*/
double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677}; int buzzuav_moveto(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2);
buzzvm_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */
//buzzvm_lload(vm, 3); /* Latitude */
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
float dy = buzzvm_stack_at(vm, 1)->f.value;
float dx = buzzvm_stack_at(vm, 2)->f.value;
double d = sqrt(dx*dx+dy*dy); //range
goto_pos[0]=dx;
goto_pos[1]=dy;
goto_pos[2]=0;
/*double b = atan2(dy,dx); //bearing
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]);
buzz_cmd=5;
return buzzvm_ret0(vm);
}
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to move following a 2D vector / Buzz closure to go directly to a GPS destination from the Mission Planner
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm) { int buzzuav_goto(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2); rc_goto_pos[2]=height;
buzzvm_lload(vm, 1); /* dx */ set_goto(rc_goto_pos);
buzzvm_lload(vm, 2); /* dy */ cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
//buzzvm_lload(vm, 3); /* Latitude */ printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzz_cmd=2;
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); return buzzvm_ret0(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); }
float dy = buzzvm_stack_at(vm, 1)->f.value;
float dx = buzzvm_stack_at(vm, 2)->f.value;
goto_pos[0]=dx;
goto_pos[1]=dy;
goto_pos[2]=0;
/*double d = sqrt(dx*dx+dy*dy); //range
double b = atan2(dy,dx); //bearing
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
gps_from_rb(d, b, goto_pos);
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]);
buzz_cmd=5;
return buzzvm_ret0(vm);
}
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to go directly to a GPS destination from the Mission Planner / Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_goto(buzzvm_t vm) { int buzzuav_arm(buzzvm_t vm) {
rc_goto_pos[2]=height; cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
set_goto(rc_goto_pos); printf(" Buzz requested Arm \n");
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; buzz_cmd=3;
printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); return buzzvm_ret0(vm);
buzz_cmd=2; }
return buzzvm_ret0(vm); int buzzuav_disarm(buzzvm_t vm) {
} cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n");
buzz_cmd=4;
return buzzvm_ret0(vm);
}
/*----------------------------------------/ /*---------------------------------------/
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and runing / Buss closure for basic UAV commands
/----------------------------------------*/ /---------------------------------------*/
int buzzuav_arm(buzzvm_t vm) { int buzzuav_takeoff(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; buzzvm_lnum_assert(vm, 1);
printf(" Buzz requested Arm \n"); buzzvm_lload(vm, 1); /* Altitude */
buzz_cmd=3; buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
return buzzvm_ret0(vm); goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value;
} height = goto_pos[2];
int buzzuav_disarm(buzzvm_t vm) { cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; printf(" Buzz requested Take off !!! \n");
printf(" Buzz requested Disarm \n"); buzz_cmd = 1;
buzz_cmd=4; return buzzvm_ret0(vm);
return buzzvm_ret0(vm); }
}
/*---------------------------------------/ int buzzuav_land(buzzvm_t vm) {
/ Buss closure for basic UAV commands cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
/---------------------------------------*/ printf(" Buzz requested Land !!! \n");
int buzzuav_takeoff(buzzvm_t vm) { buzz_cmd = 1;
buzzvm_lnum_assert(vm, 1); return buzzvm_ret0(vm);
buzzvm_lload(vm, 1); /* Altitude */ }
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value;
height = goto_pos[2];
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
printf(" Buzz requested Take off !!! \n");
buzz_cmd = 1;
return buzzvm_ret0(vm);
}
int buzzuav_land(buzzvm_t vm) { int buzzuav_gohome(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_LAND; cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested Land !!! \n"); printf(" Buzz requested gohome !!! \n");
buzz_cmd = 1; buzz_cmd = 1;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_gohome(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n");
buzz_cmd = 1;
return buzzvm_ret0(vm);
}
/*------------------------------- /*-------------------------------
/ Get/Set to transfer variable from Roscontroller to buzzuav / Get/Set to transfer variable from Roscontroller to buzzuav
/------------------------------------*/ /------------------------------------*/
double* getgoto() { double* getgoto() {
return goto_pos; return goto_pos;
} }
int getcmd() { int getcmd() {
return cur_cmd; return cur_cmd;
} }
void set_goto(double pos[]) { void set_goto(double pos[]) {
goto_pos[0] = pos[0]; goto_pos[0] = pos[0];
goto_pos[1] = pos[1]; goto_pos[1] = pos[1];
goto_pos[2] = pos[2]; goto_pos[2] = pos[2];
} }
int bzz_cmd() { int bzz_cmd() {
int cmd = buzz_cmd; int cmd = buzz_cmd;
buzz_cmd = 0; buzz_cmd = 0;
return cmd; return cmd;
} }
void rc_set_goto(double pos[]) { void rc_set_goto(double pos[]) {
rc_goto_pos[0] = pos[0]; rc_goto_pos[0] = pos[0];
rc_goto_pos[1] = pos[1]; rc_goto_pos[1] = pos[1];
rc_goto_pos[2] = pos[2]; rc_goto_pos[2] = pos[2];
} }
void rc_call(int rc_cmd_in) { void rc_call(int rc_cmd_in) {
rc_cmd = rc_cmd_in; rc_cmd = rc_cmd_in;
} }
void set_obstacle_dist(float dist[]) { void set_obstacle_dist(float dist[]) {
for (int i = 0; i < 5; i++) for (int i = 0; i < 5; i++)
obst[i] = dist[i]; obst[i] = dist[i];
} }
void set_battery(float voltage,float current,float remaining){ void set_battery(float voltage,float current,float remaining){
batt[0]=voltage; batt[0]=voltage;
batt[1]=current; batt[1]=current;
batt[2]=remaining; batt[2]=remaining;
} }
int buzzuav_update_battery(buzzvm_t vm) { int buzzuav_update_battery(buzzvm_t vm) {
//static char BATTERY_BUF[256]; buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); buzzvm_pusht(vm);
buzzvm_pusht(vm); buzzvm_dup(vm);
buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1)); buzzvm_pushf(vm, batt[0]);
buzzvm_pushf(vm, batt[0]); buzzvm_tput(vm);
buzzvm_tput(vm); buzzvm_dup(vm);
buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1)); buzzvm_pushf(vm, batt[1]);
buzzvm_pushf(vm, batt[1]); buzzvm_tput(vm);
buzzvm_tput(vm); buzzvm_dup(vm);
buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1)); buzzvm_pushf(vm, batt[2]);
buzzvm_pushf(vm, batt[2]); buzzvm_tput(vm);
buzzvm_tput(vm); buzzvm_gstore(vm);
buzzvm_gstore(vm); return vm->state;
return vm->state; }
} /****************************************/
/****************************************/ /*current pos update*/
/*current pos update*/ /***************************************/
/***************************************/ void set_currentpos(double latitude, double longitude, double altitude){
void set_currentpos(double latitude, double longitude, double altitude){ cur_pos[0]=latitude;
cur_pos[0]=latitude; cur_pos[1]=longitude;
cur_pos[1]=longitude; cur_pos[2]=altitude;
cur_pos[2]=altitude; }
} /****************************************/
/****************************************/ int buzzuav_update_currentpos(buzzvm_t vm) {
int buzzuav_update_currentpos(buzzvm_t vm) { buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); buzzvm_pusht(vm);
buzzvm_pusht(vm); buzzvm_dup(vm);
buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); buzzvm_pushf(vm, cur_pos[0]);
buzzvm_pushf(vm, cur_pos[0]); buzzvm_tput(vm);
buzzvm_tput(vm); buzzvm_dup(vm);
buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); buzzvm_pushf(vm, cur_pos[1]);
buzzvm_pushf(vm, cur_pos[1]); buzzvm_tput(vm);
buzzvm_tput(vm); buzzvm_dup(vm);
buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); buzzvm_pushf(vm, cur_pos[2]);
buzzvm_pushf(vm, cur_pos[2]); buzzvm_tput(vm);
buzzvm_tput(vm); buzzvm_gstore(vm);
buzzvm_gstore(vm); return vm->state;
return vm->state; }
}
/*----------------------------------------- void flight_status_update(uint8_t state){
/ Create an obstacle Buzz table from proximity sensors status=state;
/ TODO: swap to proximity function below }
--------------------------------------------*/
/*----------------------------------------------------
int buzzuav_update_obstacle(buzzvm_t vm) { / Create the generic robot table with status, remote controller current comand and destination
buzzvm_pushs(vm, buzzvm_string_register(vm, "obstacle", 1)); / and current position of the robot
buzzvm_pusht(vm); / TODO: change global name for robot
buzzvm_dup(vm); /------------------------------------------------------*/
buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1)); int buzzuav_update_flight_status(buzzvm_t vm) {
buzzvm_pushf(vm, obst[0]); buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_tput(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1));
buzzvm_pushf(vm, obst[1]); buzzvm_pushi(vm, rc_cmd);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1)); rc_cmd=0;
buzzvm_pushf(vm, obst[2]); buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
buzzvm_tput(vm); buzzvm_pushi(vm, status);
buzzvm_dup(vm); buzzvm_tput(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1)); buzzvm_gstore(vm);
buzzvm_pushf(vm, obst[3]); //also set rc_controllers goto
buzzvm_tput(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
buzzvm_dup(vm); buzzvm_pusht(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1)); buzzvm_dup(vm);
buzzvm_pushf(vm, obst[4]); buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
buzzvm_tput(vm); buzzvm_pushf(vm, rc_goto_pos[0]);
buzzvm_gstore(vm); buzzvm_tput(vm);
return vm->state; buzzvm_dup(vm);
} buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
buzzvm_pushf(vm, rc_goto_pos[1]);
void flight_status_update(uint8_t state){ buzzvm_tput(vm);
status=state; buzzvm_dup(vm);
} buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
buzzvm_pushf(vm, rc_goto_pos[2]);
/*---------------------------------------------------- buzzvm_tput(vm);
/ Create the generic robot table with status, remote controller current comand and destination buzzvm_gstore(vm);
/ and current position of the robot return vm->state;
/ 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); /*Create an obstacle Buzz table from proximity sensors*/
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1)); /******************************************************/
buzzvm_pushi(vm, rc_cmd);
buzzvm_tput(vm); int buzzuav_update_prox(buzzvm_t vm) {
buzzvm_dup(vm); buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
rc_cmd=0; buzzvm_pusht(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); buzzvm_dup(vm);
buzzvm_pushi(vm, status); buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
buzzvm_tput(vm); buzzvm_pushf(vm, obst[0]);
buzzvm_gstore(vm); buzzvm_tput(vm);
//also set rc_controllers goto buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
buzzvm_pusht(vm); buzzvm_pushf(vm, obst[1]);
buzzvm_dup(vm); buzzvm_tput(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); buzzvm_dup(vm);
buzzvm_pushf(vm, rc_goto_pos[0]); buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
buzzvm_tput(vm); buzzvm_pushf(vm, obst[2]);
buzzvm_dup(vm); buzzvm_tput(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); buzzvm_dup(vm);
buzzvm_pushf(vm, rc_goto_pos[1]); buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
buzzvm_tput(vm); buzzvm_pushf(vm, obst[3]);
buzzvm_dup(vm); buzzvm_tput(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); buzzvm_dup(vm);
buzzvm_pushf(vm, rc_goto_pos[2]); buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
buzzvm_tput(vm); buzzvm_pushf(vm, obst[4]);
buzzvm_gstore(vm); buzzvm_tput(vm);
return vm->state; buzzvm_gstore(vm);
} return vm->state;
}
/**********************************************/
/****************************************/ /*Dummy closure for use during update testing */
/*To do !!!!!*/ /**********************************************/
/****************************************/
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
int buzzuav_update_prox(buzzvm_t vm) {
/* static char PROXIMITY_BUF[256]; /***********************************************/
int i; /* Store Ros controller object pointer */
//kh4_proximity_ir(PROXIMITY_BUF, DSPIC); /***********************************************/
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity_ir", 1));
buzzvm_pusht(vm); //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
for(i = 0; i < 8; i++) { //roscontroller_ptr = roscontroller_ptrin;
buzzvm_dup(vm); //}
buzzvm_pushi(vm, i+1);
buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8));
buzzvm_tput(vm);
}
buzzvm_gstore(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "ground_ir", 1));
buzzvm_pusht(vm);
for(i = 8; i < 12; i++) {
buzzvm_dup(vm);
buzzvm_pushi(vm, i-7);
buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8));
buzzvm_tput(vm);
}
buzzvm_gstore(vm);*/
return vm->state;
}
/****************************************/
/****************************************/
int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
} }

2
src/rosbuzz.cpp
View File

@ -18,6 +18,8 @@ int main(int argc, char **argv)
ros::init(argc, argv, "rosBuzz"); ros::init(argc, argv, "rosBuzz");
ros::NodeHandle n_c("~"); ros::NodeHandle n_c("~");
rosbzz_node::roscontroller RosController(n_c); rosbzz_node::roscontroller RosController(n_c);
/*Register ros controller object inside buzz*/
//buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun(); RosController.RosControllerRun();
return 0; return 0;
} }

283
src/roscontroller.cpp
View File

@ -2,7 +2,9 @@
namespace rosbzz_node{ namespace rosbzz_node{
/***Constructor***/ /*---------------
/Constructor
---------------*/
roscontroller::roscontroller(ros::NodeHandle n_c) roscontroller::roscontroller(ros::NodeHandle n_c)
{ {
ROS_INFO("Buzz_node"); ROS_INFO("Buzz_node");
@ -18,7 +20,9 @@ namespace rosbzz_node{
multi_msg = true; multi_msg = true;
} }
/***Destructor***/ /*---------------------
/Destructor
---------------------*/
roscontroller::~roscontroller() roscontroller::~roscontroller()
{ {
/* All done */ /* All done */
@ -28,7 +32,9 @@ namespace rosbzz_node{
uav_done(); uav_done();
} }
/*rosbuzz_node run*/ /*-------------------------------------------------
/rosbuzz_node loop method executed once every step
/--------------------------------------------------*/
void roscontroller::RosControllerRun(){ void roscontroller::RosControllerRun(){
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
@ -38,33 +44,16 @@ namespace rosbzz_node{
{ {
/*Update neighbors position inside Buzz*/ /*Update neighbors position inside Buzz*/
buzz_utility::neighbour_pos_callback(neighbours_pos_map); buzz_utility::neighbour_pos_callback(neighbours_pos_map);
auto current_time = ros::Time::now(); /*Neighbours of the robot published with id in respective topic*/
map< int, buzz_utility::Pos_struct >::iterator it; neighbours_pos_publisher();
rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear();
neigh_pos_array.header.frame_id = "/world";
neigh_pos_array.header.stamp = current_time;
for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){
sensor_msgs::NavSatFix neigh_tmp;
//cout<<"iterator it val: "<< it-> first << " After convertion: " <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;
neigh_tmp.header.stamp = current_time;
neigh_tmp.header.frame_id = "/world";
neigh_tmp.position_covariance_type=it->first; //custom robot id storage
neigh_tmp.latitude=(it->second).x;
neigh_tmp.longitude=(it->second).y;
neigh_tmp.altitude=(it->second).z;
neigh_pos_array.pos_neigh.push_back(neigh_tmp);
//std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
}
neigh_pos_pub.publish(neigh_pos_array);
/*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*/
prepare_msg_and_publish(); prepare_msg_and_publish();
/*call flight controler service to set command long*/
flight_controler_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();
@ -122,7 +111,9 @@ namespace rosbzz_node{
// initialize topics to null? // initialize topics to null?
} }
/*-----------------------------------------------------------------------------------
/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();
@ -167,8 +158,8 @@ namespace rosbzz_node{
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<mavros_msgs::PositionTarget>("/setpoint_raw/local",1000); localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>("/setpoint_raw/local",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);
@ -176,7 +167,9 @@ namespace rosbzz_node{
multi_msg=true; multi_msg=true;
} }
/*---------------------------------------
/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){
@ -201,6 +194,7 @@ namespace rosbzz_node{
/ Create Buzz bytecode from the bzz script inputed / Create Buzz bytecode from the bzz script inputed
/-------------------------------------------------------*/ /-------------------------------------------------------*/
void roscontroller::Compile_bzz(){ void roscontroller::Compile_bzz(){
/*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
/*Compile the buzz code .bzz to .bo*/ /*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile; stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")); std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
@ -210,7 +204,6 @@ namespace rosbzz_node{
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of(".")); name = name.substr(0,name.find_last_of("."));
bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm"; bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm";
//system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
system(bzzfile_in_compile.str().c_str()); system(bzzfile_in_compile.str().c_str());
bzzfile_in_compile.str(""); bzzfile_in_compile.str("");
bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg"; bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
@ -223,9 +216,32 @@ namespace rosbzz_node{
dbgfname = bzzfile_in_compile.str(); dbgfname = bzzfile_in_compile.str();
} }
/*----------------------------------------------------
/ Publish neighbours pos and id in neighbours pos topic
/----------------------------------------------------*/
void roscontroller::neighbours_pos_publisher(){
auto current_time = ros::Time::now();
map< int, buzz_utility::Pos_struct >::iterator it;
rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear();
neigh_pos_array.header.frame_id = "/world";
neigh_pos_array.header.stamp = current_time;
for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){
sensor_msgs::NavSatFix neigh_tmp;
//cout<<"iterator it val: "<< it-> first << " After convertion: " <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;
neigh_tmp.header.stamp = current_time;
neigh_tmp.header.frame_id = "/world";
neigh_tmp.position_covariance_type=it->first; //custom robot id storage
neigh_tmp.latitude=(it->second).x;
neigh_tmp.longitude=(it->second).y;
neigh_tmp.altitude=(it->second).z;
neigh_pos_array.pos_neigh.push_back(neigh_tmp);
//std::cout<<"long obt"<<neigh_tmp.longitude<<endl;
}
neigh_pos_pub.publish(neigh_pos_array);
}
/*-------------------------------------------------------- /*--------------------------------------------------------
/ Fonctions handling the local MAV ROS fligh controller / Functions handling the local MAV ROS flight controller
/-------------------------------------------------------*/ /-------------------------------------------------------*/
void roscontroller::Arm(){ void roscontroller::Arm(){
mavros_msgs::CommandBool arming_message; mavros_msgs::CommandBool arming_message;
@ -239,7 +255,9 @@ namespace rosbzz_node{
ROS_INFO("FC Arm Service call failed!"); ROS_INFO("FC Arm Service call failed!");
} }
} }
/*---------------------------------------------------------
/ Set mode for the solos
/---------------------------------------------------------*/
void roscontroller::SetMode(){ void roscontroller::SetMode(){
mavros_msgs::SetMode set_mode_message; mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0; set_mode_message.request.base_mode = 0;
@ -252,61 +270,18 @@ namespace rosbzz_node{
} }
/*----------------------------------------------------------------- /*-----------------------------------------------------------------------------------------------------
/Prepare Buzz messages payload for each step and publish /Prepare Buzz messages payload for each step and publish
/ /-----------------------------------------------------------------------------------------------------*/
/*******************************************************************************************************/ /*----------------------------------------------------------------------------------------------------*/
/* Message format of payload (Each slot is uint64_t) */ /* Message format of payload (Each slot is uint64_t) /
/* _________________________________________________________________________________________________ */ / _________________________________________________________________________________________________ /
/*| | | | | | */ /| | | | | | /
/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */ /|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | /
/*|_____|_____|_____|________________________________________________|______________________________| */ /|_____|_____|_____|________________________________________________|______________________________| */
/*******************************************************************************************************/ /*----------------------------------------------------------------------------------------------------*/
void roscontroller::prepare_msg_and_publish(){ void roscontroller::prepare_msg_and_publish(){
/* flight controller client call if requested from Buzz*/
/*FC call for takeoff,land and gohome*/
/* TODO: this should go in a separate function and be called by the main Buzz step */
int tmp = buzzuav_closures::bzz_cmd();
double* goto_pos = buzzuav_closures::getgoto();
if (tmp == 1){
cmd_srv.request.param7 = goto_pos[2];
//cmd_srv.request.z = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == 2) { /*FC call for goto*/
/*prepare the goto publish message */
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == 3) { /*FC call for arm*/
armstate=1;
Arm();
} else if (tmp == 4){
armstate=0;
Arm();
} else if (tmp == 5) { /*Buzz call for moveto*/
/*prepare the goto publish message */
mavros_msgs::PositionTarget pt;
pt.type_mask = mavros_msgs::PositionTarget::IGNORE_VX && mavros_msgs::PositionTarget::IGNORE_VY && mavros_msgs::PositionTarget::IGNORE_VZ && mavros_msgs::PositionTarget::IGNORE_AFX && mavros_msgs::PositionTarget::IGNORE_AFY && mavros_msgs::PositionTarget::IGNORE_AFZ && mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
pt.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
pt.position.x = goto_pos[0];
pt.position.y = goto_pos[1];
pt.position.z = goto_pos[2];
pt.yaw = 0;//goto_pos[3];
ROS_INFO("Sending local setpoint: %.2f, %.2f, %.2f",pt.position.x,pt.position.y,pt.position.z);
localsetpoint_pub.publish(pt);
}
/*obtain Pay load to be sent*/ /*obtain Pay load to be sent*/
//fprintf(stdout, "before getting msg from utility\n");
uint64_t* payload_out_ptr= buzz_utility::out_msg_process(); uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
uint64_t position[3]; uint64_t position[3];
/*Appened current position to message*/ /*Appened current position to message*/
@ -320,21 +295,6 @@ namespace rosbzz_node{
for(int i=0;i<out[0];i++){ for(int i=0;i<out[0];i++){
payload_out.payload64.push_back(payload_out_ptr[i]); payload_out.payload64.push_back(payload_out_ptr[i]);
} }
/*int i=0;
uint64_t message_obt[payload_out.payload64.size()];
for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin();
it != payload_out.payload64.end(); ++it){
message_obt[i] =(uint64_t) *it;
i++;
}*/
/*for(int i=0;i<payload_out.payload64.size();i++){
cout<<" [Debug:] sent message "<<payload_out.payload64[i]<<endl;
out = buzz_utility::u64_cvt_u16(message_obt[i]);
for(int k=0;k<4;k++){
cout<<" [Debug:] sent message "<<out[k]<<endl;
}
}*/
/*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++;
@ -345,8 +305,7 @@ namespace rosbzz_node{
payload_pub.publish(payload_out); payload_pub.publish(payload_out);
delete[] out; delete[] out;
delete[] payload_out_ptr; delete[] payload_out_ptr;
/*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());;
@ -384,36 +343,75 @@ namespace rosbzz_node{
multi_msg=false; multi_msg=false;
delete[] payload_64; delete[] payload_64;
} }
/*Request xbee to stop any multi transmission updated not needed any more*/
//if(get_update_status()){
// set_read_update_status();
// mavros_msgs::Mavlink stop_req_packet;
// stop_req_packet.payload64.push_back((uint64_t) XBEE_STOP_TRANSMISSION);
// payload_pub.publish(stop_req_packet);
// std::cout << "request xbee to stop multi-packet transmission" << std::endl;
//} }
/*---------------------------------------------------------------------------------
} /Flight controller service call every step if there is a command set from bzz script
/-------------------------------------------------------------------------------- */
void roscontroller::flight_controler_service_call(){
/*Refresh neighbours Position for every ten step*/ /* flight controller client call if requested from Buzz*/
/*FC call for takeoff,land, gohome and arm/disarm*/
int tmp = buzzuav_closures::bzz_cmd();
double* goto_pos = buzzuav_closures::getgoto();
if (tmp == 1){
cmd_srv.request.param7 = goto_pos[2];
//cmd_srv.request.z = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == 2) { /*FC call for goto*/
/*prepare the goto publish message */
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == 3) { /*FC call for arm*/
armstate=1;
Arm();
} else if (tmp == 4){
armstate=0;
Arm();
} else if (tmp == 5) { /*Buzz call for moveto*/
/*prepare the goto publish message */
mavros_msgs::PositionTarget pt;
pt.type_mask = mavros_msgs::PositionTarget::IGNORE_VX | mavros_msgs::PositionTarget::IGNORE_VY | mavros_msgs::PositionTarget::IGNORE_VZ | mavros_msgs::PositionTarget::IGNORE_AFX | mavros_msgs::PositionTarget::IGNORE_AFY | mavros_msgs::PositionTarget::IGNORE_AFZ | mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
pt.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED_OFFSET;
pt.position.x = goto_pos[0];
pt.position.y = goto_pos[1];
pt.position.z = goto_pos[2];
pt.yaw = 0;//goto_pos[3];
ROS_INFO("Sending local setpoint: %.2f, %.2f, %.2f",pt.position.x,pt.position.y,pt.position.z);
localsetpoint_pub.publish(pt);
}
}
/*----------------------------------------------
/Refresh neighbours Position for every ten step
/---------------------------------------------*/
void roscontroller::maintain_pos(int tim_step){ void roscontroller::maintain_pos(int tim_step){
if(timer_step >=10){ if(timer_step >=10){
neighbours_pos_map.clear(); neighbours_pos_map.clear();
//raw_neighbours_pos_map.clear(); //raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but have to clear !
timer_step=0; timer_step=0;
} }
} }
/*Puts neighbours position*/ /*---------------------------------------------------------------------------------
/Puts neighbours position into local struct storage that is cleared every 10 step
/---------------------------------------------------------------------------------*/
void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){ void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){
map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id); map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id);
if(it!=neighbours_pos_map.end()) if(it!=neighbours_pos_map.end())
neighbours_pos_map.erase(it); neighbours_pos_map.erase(it);
neighbours_pos_map.insert(make_pair(id, pos_arr)); neighbours_pos_map.insert(make_pair(id, pos_arr));
} }
/*Puts raw neighbours position*/ /*-----------------------------------------------------------------------------------
/Puts raw neighbours position into local storage for neighbours pos publisher
/-----------------------------------------------------------------------------------*/
void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){ void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){
map< int, buzz_utility::Pos_struct >::iterator it = raw_neighbours_pos_map.find(id); map< int, buzz_utility::Pos_struct >::iterator it = raw_neighbours_pos_map.find(id);
if(it!=raw_neighbours_pos_map.end()) if(it!=raw_neighbours_pos_map.end())
@ -421,14 +419,15 @@ namespace rosbzz_node{
raw_neighbours_pos_map.insert(make_pair(id, pos_arr)); raw_neighbours_pos_map.insert(make_pair(id, pos_arr));
} }
/*Set the current position of the robot callback*/ /*--------------------------------------------------------
/Set the current position of the robot callback
/--------------------------------------------------------*/
void roscontroller::set_cur_pos(double latitude, void roscontroller::set_cur_pos(double latitude,
double longitude, double longitude,
double altitude){ double altitude){
cur_pos [0] =latitude; cur_pos [0] =latitude;
cur_pos [1] =longitude; cur_pos [1] =longitude;
cur_pos [2] =altitude; cur_pos [2] =altitude;
} }
@ -449,13 +448,17 @@ namespace rosbzz_node{
out[2] = 0.0; out[2] = 0.0;
} }
/*battery status callback*/ /*------------------------------------------------------
/ Update battery status into BVM from subscriber
/------------------------------------------------------*/
void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){ void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining); buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
//ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, msg->current, msg ->remaining); //ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, msg->current, msg ->remaining);
} }
/*flight extended status update*/ /*----------------------------------------------------------------------
/Update flight extended status into BVM from subscriber for solos
/---------------------------------------------------------------------*/
void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg){ void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg){
// http://wiki.ros.org/mavros/CustomModes // http://wiki.ros.org/mavros/CustomModes
// TODO: Handle landing // TODO: Handle landing
@ -468,47 +471,47 @@ namespace rosbzz_node{
buzzuav_closures::flight_status_update(1);//? buzzuav_closures::flight_status_update(1);//?
} }
/*flight extended status update*/ /*------------------------------------------------------------
/Update flight extended status into BVM from subscriber
------------------------------------------------------------*/
void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){ void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
buzzuav_closures::flight_status_update(msg->landed_state); buzzuav_closures::flight_status_update(msg->landed_state);
} }
/*current position callback*/ /*-------------------------------------------------------------
/ Update current position into BVM from subscriber
/-------------------------------------------------------------*/
void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
set_cur_pos(msg->latitude,msg->longitude,msg->altitude); set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude); buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude);
} }
/*Obstacle distance call back*/ /*-------------------------------------------------------------
/Set obstacle Obstacle distance table into BVM from subscriber
/-------------------------------------------------------------*/
void roscontroller::obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg){ void roscontroller::obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg){
float obst[5]; float obst[5];
for(int i=0;i<5;i++) for(int i=0;i<5;i++)
obst[i]=msg->ranges[i]; obst[i]=msg->ranges[i];
buzzuav_closures::set_obstacle_dist(obst); buzzuav_closures::set_obstacle_dist(obst);
} }
/*payload callback callback*/
/*-------------------------------------------------------------
/Push payload into BVM FIFO
/-------------------------------------------------------------*/
/*******************************************************************************************************/ /*******************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */ /* Message format of payload (Each slot is uint64_t) */
/* _________________________________________________________________________________________________ */ /* _________________________________________________________________________________________________ */
/*| | | | | | */ /*| | | | | | */
/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */ /*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */
/*|_____|_____|_____|________________________________________________|______________________________| */ /*|_____|_____|_____|________________________________________________|______________________________| */
/*******************************************************************************************************/ /*******************************************************************************************************/
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
/*Check for Updater message, if updater message push it into updater FIFO*/
/*Ack from xbee about its transfer complete of multi packet*/ if((uint64_t)msg->payload64[0]==(uint64_t)UPDATER_MESSAGE_CONSTANT && msg->payload64.size() > 5){
/*if((uint64_t)msg->payload64[0]==(uint64_t)XBEE_MESSAGE_CONSTANT && msg->payload64.size() == 1){
multi_msg=true;
std::cout << "ACK From xbee after transimssion of code " << std::endl;
}*/
if((uint64_t)msg->payload64[0]==(uint64_t)UPDATER_MESSAGE_CONSTANT && msg->payload64.size() > 10){
uint16_t obt_msg_size=sizeof(uint64_t)*(msg->payload64.size()); uint16_t obt_msg_size=sizeof(uint64_t)*(msg->payload64.size());
uint64_t message_obt[obt_msg_size]; uint64_t message_obt[obt_msg_size];
/* Go throught the obtained payload*/ /* Go throught the obtained payload*/
for(int i=0;i < (int)msg->payload64.size();i++){ for(int i=0;i < (int)msg->payload64.size();i++){
message_obt[i] =(uint64_t)msg->payload64[i]; message_obt[i] =(uint64_t)msg->payload64[i];
//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
//i++;
} }
uint8_t* pl =(uint8_t*)malloc(obt_msg_size); uint8_t* pl =(uint8_t*)malloc(obt_msg_size);
@ -518,7 +521,7 @@ namespace rosbzz_node{
uint16_t unMsgSize = *(uint16_t*)(pl); uint16_t unMsgSize = *(uint16_t*)(pl);
//uint16_t tot; //uint16_t tot;
//tot+=sizeof(uint16_t); //tot+=sizeof(uint16_t);
fprintf(stdout,"Update packet read msg size : %u \n",unMsgSize); fprintf(stdout,"Update packet, read msg size : %u \n",unMsgSize);
if(unMsgSize>0){ if(unMsgSize>0){
code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)),unMsgSize); code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)),unMsgSize);
//fprintf(stdout,"before in queue process : utils\n"); //fprintf(stdout,"before in queue process : utils\n");
@ -526,10 +529,8 @@ namespace rosbzz_node{
//fprintf(stdout,"after in queue process : utils\n"); //fprintf(stdout,"after in queue process : utils\n");
} }
delete[] pl; delete[] pl;
} }
/*BVM FIFO message*/
else if(msg->payload64.size() > 3){ else if(msg->payload64.size() > 3){
uint64_t message_obt[msg->payload64.size()]; uint64_t message_obt[msg->payload64.size()];
/* Go throught the obtained payload*/ /* Go throught the obtained payload*/
@ -617,6 +618,10 @@ namespace rosbzz_node{
} }
return true; return true;
} }
/*-----------------------------------------------------
/Obtain robot id by subscribing to xbee robot id topic
/ TODO: check for integrity of this subscriber call back
/----------------------------------------------------*/
void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){ void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
robot_id=(int)msg->data; robot_id=(int)msg->data;

11
src/testflockfev.bzz
View File

@ -86,12 +86,21 @@ function barrier_wait(threshold, transf) {
if(barrier.size() >= threshold) { if(barrier.size() >= threshold) {
barrier = nil barrier = nil
transf() transf()
<<<<<<< HEAD
} else if(timeW>=WAIT_TIMEOUT) { } else if(timeW>=WAIT_TIMEOUT) {
barrier = nil barrier = nil
statef=land statef=land
timeW=0 timeW=0
} }
timeW = timeW+1 timeW = timeW+1
=======
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=hexagon #idle
timeW=0
}
timeW = timeW+1
>>>>>>> 141f05807d744ec9db040287ff04b06db10d6e8e
} }
# flight status # flight status
@ -150,7 +159,6 @@ function init() {
# Executed at each time step. # Executed at each time step.
function step() { function step() {
if(flight.rc_cmd==22) { if(flight.rc_cmd==22) {
log("cmd 22") log("cmd 22")
flight.rc_cmd=0 flight.rc_cmd=0
@ -173,6 +181,7 @@ function step() {
uav_arm() uav_arm()
neighbors.broadcast("cmd", 400) neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){ } else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm() uav_disarm()
neighbors.broadcast("cmd", 401) neighbors.broadcast("cmd", 401)
} }