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

Merge branch 'dev' 

Conflicts:
	include/roscontroller.h
	src/buzz_utility.cpp
	src/roscontroller.cpp
This commit is contained in:
isvogor 2017-03-15 09:42:51 -04:00
parent 002baf636d 141f05807d
commit 0cd59a91f0
8 changed files with 528 additions and 521 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 "mavros_msgs/CommandCode.h"
#include "ros/ros.h"
#include "buzz_utility.h"
//#include "roscontroller.h"
namespace buzzuav_closures{
/*
@ -46,8 +47,13 @@ void set_obstacle_dist(float dist[]);
* Commands the UAV to takeoff
*/
int buzzuav_takeoff(buzzvm_t vm);
/*
* Arm command from Buzz
*/
int buzzuav_arm(buzzvm_t vm);
/*
* Disarm from buzz
*/
int buzzuav_disarm(buzzvm_t vm) ;
/* Commands the UAV to land
*/
@ -66,8 +72,6 @@ int buzzuav_update_battery(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

20
include/roscontroller.h
View File

@ -11,6 +11,7 @@
#include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/Mavlink.h"
#include "mavros_msgs/PositionTarget.h"
#include "sensor_msgs/NavSatStatus.h"
#include "mavros_msgs/WaypointPush.h"
#include "mavros_msgs/Waypoint.h"
@ -65,6 +66,7 @@ private:
ros::ServiceClient mav_client;
ros::Publisher payload_pub;
ros::Publisher neigh_pos_pub;
ros::Publisher localsetpoint_pub;
ros::ServiceServer service;
ros::Subscriber current_position_sub;
ros::Subscriber battery_sub;
@ -86,8 +88,9 @@ private:
mavros_msgs::SetMode m_cmdSetMode;
ros::ServiceClient mode_client;
ros::ServiceClient mission_client;
/*Initialize publisher and subscriber, done in the constructor*/
void Initialize_pub_sub(ros::NodeHandle n_c);
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
@ -99,6 +102,12 @@ private:
/*compiles buzz script from the specified .bzz file*/
void Compile_bzz();
/*Flight controller service call*/
void flight_controler_service_call();
/*Neighbours pos publisher*/
void neighbours_pos_publisher();
/*Prepare messages and publish*/
void prepare_msg_and_publish();
@ -140,14 +149,19 @@ private:
/*robot id sub callback*/
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
/*Obstacle distance table callback*/
void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
/*Get publisher and subscriber from YML file*/
void GetSubscriptionParameters(ros::NodeHandle node_handle);
/*Arm/disarm method that can be called from buzz*/
void Arm();
void SetMode(std::string mode, int delay_miliseconds);
/*set mode like guided for solo*/
void SetMode();
/*Robot independent subscribers*/
void Subscribe(ros::NodeHandle n_c);
void WaypointMissionSetup(float lat, float lng, float alt);

2
launch/rosbuzz_2_parallel.launch
View File

@ -2,7 +2,7 @@
<launch>
<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"/>
<param name="rcclient" value="true" />
<param name="rcservice_name" value="rc_cmd" />

18
src/buzz_utility.cpp
View File

@ -76,15 +76,9 @@ namespace buzz_utility{
memcpy(pl, payload ,size);
/*size and robot id read*/
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 */
//fprintf(stdout,"Total data size : utils : %d\n",(int)size);
uint16_t unMsgSize=0;
<<<<<<< HEAD
//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);
@ -102,6 +96,9 @@ namespace buzz_utility{
//}
//unMsgSize=0;
=======
>>>>>>> dev
/*Obtain Buzz messages only when they are present*/
do {
/* Get payload size */
@ -131,6 +128,7 @@ namespace buzz_utility{
/* Send robot id */
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
tot += sizeof(uint16_t);
<<<<<<< HEAD
//uint8_t updater_msg_pre = 0;
//uint16_t updater_msgSize= 0;
//if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){
@ -155,6 +153,8 @@ namespace buzz_utility{
// *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
// tot += sizeof(uint8_t);
//}
=======
>>>>>>> dev
/* Send messages from FIFO */
do {
/* Are there more messages? */
@ -507,7 +507,6 @@ namespace buzz_utility{
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
buzzuav_closures::buzzuav_update_obstacle(VM);
/*
* Call Buzz step() function
*/
@ -518,7 +517,7 @@ namespace buzz_utility{
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*/
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
@ -571,7 +570,6 @@ int update_step_test(){
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::buzzuav_update_flight_status(VM);
buzzuav_closures::buzzuav_update_obstacle(VM);
int a = buzzvm_function_call(VM, "step", 0);
if(a != BUZZVM_STATE_READY){

668
src/buzzuav_closures.cpp
View File

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

2
src/rosbuzz.cpp
View File

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

316
src/roscontroller.cpp
View File

@ -2,7 +2,9 @@
#include <thread>
namespace rosbzz_node{
/***Constructor***/
/*---------------
/Constructor
---------------*/
roscontroller::roscontroller(ros::NodeHandle n_c)
{
ROS_INFO("Buzz_node");
@ -20,7 +22,9 @@ namespace rosbzz_node{
multi_msg = true;
}
/***Destructor***/
/*---------------------
/Destructor
---------------------*/
roscontroller::~roscontroller()
{
/* All done */
@ -30,7 +34,9 @@ namespace rosbzz_node{
uav_done();
}
/*rosbuzz_node run*/
/*-------------------------------------------------
/rosbuzz_node loop method executed once every step
/--------------------------------------------------*/
void roscontroller::RosControllerRun(){
/* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
@ -40,34 +46,16 @@ namespace rosbzz_node{
{
/*Update neighbors position inside Buzz*/
buzz_utility::neighbour_pos_callback(neighbours_pos_map);
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);
/*Neighbours of the robot published with id in respective topic*/
neighbours_pos_publisher();
/*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
prepare_msg_and_publish();
/*call flight controler service to set command long*/
flight_controler_service_call();
/*Set multi message available after update*/
if(get_update_status()){
set_read_update_status();
@ -127,7 +115,9 @@ namespace rosbzz_node{
// initialize topics to null?
}
/*-----------------------------------------------------------------------------------
/Obtains publisher, subscriber and services from yml file based on the robot used
/-----------------------------------------------------------------------------------*/
void roscontroller::GetSubscriptionParameters(ros::NodeHandle node_handle){
//todo: make it as an array in yaml?
m_sMySubscriptions.clear();
@ -177,7 +167,8 @@ namespace rosbzz_node{
//mavros_msgs::PositionTarget message;
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);
/* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
@ -185,7 +176,9 @@ namespace rosbzz_node{
multi_msg=true;
}
/*---------------------------------------
/Robot independent subscribers
/--------------------------------------*/
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){
@ -210,6 +203,7 @@ namespace rosbzz_node{
/ Create Buzz bytecode from the bzz script inputed
/-------------------------------------------------------*/
void roscontroller::Compile_bzz(){
/*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
/*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile;
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
@ -219,7 +213,6 @@ namespace rosbzz_node{
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0,name.find_last_of("."));
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());
bzzfile_in_compile.str("");
bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
@ -232,9 +225,32 @@ namespace rosbzz_node{
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(){
mavros_msgs::CommandBool arming_message;
@ -248,80 +264,33 @@ namespace rosbzz_node{
ROS_INFO("FC Arm Service call failed!");
}
}
/* Prepare Buzz messages payload for each step and publish */
/*******************************************************************************************************/
/* 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......... | */
/*|_____|_____|_____|________________________________________________|______________________________| */
/*******************************************************************************************************/
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();
std::cout<< "Going: " << tmp << std::endl;
double* goto_pos = buzzuav_closures::getgoto();
// TODO: Make sure that land and takeoff use the right statuses in bzz
if (tmp == 1){
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl;
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode
switch(buzzuav_closures::getcmd()){
case mavros_msgs::CommandCode::NAV_LAND:
if(current_mode != "LAND")
SetMode("LAND", 0);
break;
case mavros_msgs::CommandCode::NAV_TAKEOFF:
if(current_mode != "GUIDED")
SetMode("GUIDED", 0); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
break;
}
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*/
// SOLO SPECIFIC: when you land, mode is LANDED, which is not armable, therefore change it to LOITER
SetMode("LOITER", 0);
armstate=1;
Arm();
} else if (tmp == 4){
SetMode("LOITER", 0);
armstate=0;
Arm();
/*---------------------------------------------------------
/ Set mode for the solos
/---------------------------------------------------------*/
void roscontroller::SetMode(){
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = "GUIDED"; // shit!
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
/*obtain Pay load to be sent*/
//fprintf(stdout, "before getting msg from utility\n");
}
/*-----------------------------------------------------------------------------------------------------
/Prepare Buzz messages payload for each step and publish
/-----------------------------------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------------------------------*/
/* 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......... | /
/|_____|_____|_____|________________________________________________|______________________________| */
/*----------------------------------------------------------------------------------------------------*/
void roscontroller::prepare_msg_and_publish(){
/*obtain Pay load to be sent*/
uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
uint64_t position[3];
/*Appened current position to message*/
@ -335,21 +304,6 @@ namespace rosbzz_node{
for(int i=0;i<out[0];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*/
if (message_number < 0) message_number=0;
else message_number++;
@ -360,8 +314,7 @@ namespace rosbzz_node{
payload_pub.publish(payload_out);
delete[] out;
delete[] payload_out_ptr;
/*Check for updater message if present send*/
if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
uint8_t* buff_send = 0;
uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
@ -399,36 +352,74 @@ namespace rosbzz_node{
multi_msg=false;
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;
//}
}
/*Refresh neighbours Position for every ten step*/
}
/*---------------------------------------------------------------------------------
/Flight controller service call every step if there is a command set from bzz script
/-------------------------------------------------------------------------------- */
void roscontroller::flight_controler_service_call(){
/* 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;
pt.position.x = goto_pos[0];
pt.position.y = goto_pos[1];
pt.position.z = goto_pos[2];
pt.yaw = 0;//goto_pos[3];
localsetpoint_pub.publish(pt);
}
}
/*----------------------------------------------
/Refresh neighbours Position for every ten step
/---------------------------------------------*/
void roscontroller::maintain_pos(int tim_step){
if(timer_step >=10){
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;
}
}
/*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 ){
map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id);
if(it!=neighbours_pos_map.end())
neighbours_pos_map.erase(it);
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 ){
map< int, buzz_utility::Pos_struct >::iterator it = raw_neighbours_pos_map.find(id);
if(it!=raw_neighbours_pos_map.end())
@ -436,14 +427,15 @@ namespace rosbzz_node{
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,
double longitude,
double altitude){
cur_pos [0] =latitude;
cur_pos [1] =longitude;
cur_pos [2] =altitude;
}
@ -464,13 +456,17 @@ namespace rosbzz_node{
out[2] = 0.0;
}
/*battery status callback*/
/*------------------------------------------------------
/ Update battery status into BVM from subscriber
/------------------------------------------------------*/
void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
//ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, msg->current, msg ->remaining);
}
/*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){
// http://wiki.ros.org/mavros/CustomModes
// TODO: Handle landing
@ -483,47 +479,47 @@ namespace rosbzz_node{
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){
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){
set_cur_pos(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){
float obst[5];
for(int i=0;i<5;i++)
obst[i]=msg->ranges[i];
buzzuav_closures::set_obstacle_dist(obst);
}
/*payload callback callback*/
/*-------------------------------------------------------------
/Push payload into BVM FIFO
/-------------------------------------------------------------*/
/*******************************************************************************************************/
/* 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......... | */
/*|_____|_____|_____|________________________________________________|______________________________| */
/*******************************************************************************************************/
/*******************************************************************************************************/
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
/*Ack from xbee about its transfer complete of multi packet*/
/*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){
/*Check for Updater message, if updater message push it into updater FIFO*/
if((uint64_t)msg->payload64[0]==(uint64_t)UPDATER_MESSAGE_CONSTANT && msg->payload64.size() > 5){
uint16_t obt_msg_size=sizeof(uint64_t)*(msg->payload64.size());
uint64_t message_obt[obt_msg_size];
/* Go throught the obtained payload*/
for(int i=0;i < (int)msg->payload64.size();i++){
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);
@ -533,7 +529,7 @@ namespace rosbzz_node{
uint16_t unMsgSize = *(uint16_t*)(pl);
//uint16_t tot;
//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){
code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)),unMsgSize);
//fprintf(stdout,"before in queue process : utils\n");
@ -541,10 +537,8 @@ namespace rosbzz_node{
//fprintf(stdout,"after in queue process : utils\n");
}
delete[] pl;
}
/*BVM FIFO message*/
else if(msg->payload64.size() > 3){
uint64_t message_obt[msg->payload64.size()];
/* Go throught the obtained payload*/
@ -636,6 +630,10 @@ namespace rosbzz_node{
}
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){
robot_id=(int)msg->data;

11
src/testflockfev.bzz
View File

@ -78,13 +78,20 @@ function barrier_ready() {
#
# Executes the barrier
#
WAIT_TIMEOUT = 100
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=hexagon #idle
timeW=0
}
timeW = timeW+1
}
# flight status
@ -144,7 +151,6 @@ function init() {
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
@ -166,6 +172,7 @@ function step() {
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}