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

clean and comment

Browse Source
This commit is contained in:
David St-Onge 2017-02-27 18:39:23 -05:00
parent 6593215a57
commit 8d8928d914
3 changed files with 95 additions and 298 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
include/roscontroller.h
View File

@ -107,21 +107,9 @@ private:
void set_cur_pos(double latitude, void set_cur_pos(double latitude,
double longitude, double longitude,
double altitude); double altitude);
/*convert from cartesian to spherical coordinate system callback */
void cvt_spherical_coordinates(double neighbours_pos_payload [], double out[]);
/*convert from spherical to cartesian coordinate system callback */
void cvt_cartesian_coordinates(double neighbours_pos_payload [], double out[]);
/*convert from spherical to cartesian coordinate system callback */ /*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]); void cvt_rangebearing_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
/*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearingGB_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
/*convert from spherical to cartesian coordinate system callback */
void cvt_rangebearing2D_coordinates(double neighbours_pos_payload [], double out[], double pos[]);
/*battery status callback*/ /*battery status callback*/
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);

178
src/buzzuav_closures.cpp
View File

@ -9,6 +9,9 @@
#include "buzzuav_closures.h" #include "buzzuav_closures.h"
#include "buzz_utility.h" #include "buzz_utility.h"
namespace buzzuav_closures{ namespace buzzuav_closures{
// TODO: Minimize the required global variables and put them in the header
static double goto_pos[3]; static double goto_pos[3];
static double rc_goto_pos[3]; static double rc_goto_pos[3];
static float batt[3]; static float batt[3];
@ -62,42 +65,13 @@ int buzzros_print(buzzvm_t vm) {
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/****************************************/ /*----------------------------------------/
/****************************************/ / Compute GPS destination from current position and desired Range and Bearing
#define EARTH_RADIUS 6371000.0 /----------------------------------------*/
/*convert from spherical to cartesian coordinate system callback */ #define EARTH_RADIUS (double) 6371000.0
void cartesian_coordinates(double spherical_pos_payload [], double out[]){
double latitude, longitude, rho;
latitude = spherical_pos_payload[0] / 180.0 * M_PI;
longitude = spherical_pos_payload[1] / 180.0 * M_PI;
rho = spherical_pos_payload[2]+EARTH_RADIUS; //centered on Earth
try {
out[0] = cos(latitude) * cos(longitude) * rho;
out[1] = cos(latitude) * sin(longitude) * rho;
out[2] = sin(latitude) * rho; // z is 'up'
} catch (std::overflow_error e) {
// std::cout << e.what() << " Error in convertion to cartesian coordinate system "<<endl;
}
}
/*convert from cartesian to spherical coordinate system callback */
void spherical_coordinates(double cartesian_pos_payload [], double out[]){
double x, y, z;
x = cartesian_pos_payload[0];
y = cartesian_pos_payload[1];
z = cartesian_pos_payload[2];
try {
out[2]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0))-EARTH_RADIUS;
out[1]=atan2(y,x)*180.0/M_PI;
out[0]=atan2(z,(sqrt(pow(x,2.0)+pow(y,2.0))))*180.0/M_PI;
} catch (std::overflow_error e) {
// std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl;
}
}
void gps_from_rb(double range, double bearing, double out[3]) { void gps_from_rb(double range, double bearing, double out[3]) {
double lat = cur_pos[0]*M_PI/180.0; double lat = cur_pos[0]*M_PI/180.0;
double lon = cur_pos[1]*M_PI/180.0; double lon = cur_pos[1]*M_PI/180.0;
// bearing = bearing*M_PI/180.0;
out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing)); 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[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[0] = out[0]*180.0/M_PI;
@ -105,10 +79,14 @@ void gps_from_rb(double range, double bearing, double out[3]) {
out[2] = height; //constant height. 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 hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958}; double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677}; 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) { int buzzuav_moveto(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 2); buzzvm_lnum_assert(vm, 2);
buzzvm_lload(vm, 1); /* dx */ buzzvm_lload(vm, 1); /* dx */
@ -122,46 +100,16 @@ int buzzuav_moveto(buzzvm_t vm) {
double d = sqrt(dx*dx+dy*dy); //range double d = sqrt(dx*dx+dy*dy); //range
double b = atan2(dy,dx); //bearing double b = atan2(dy,dx); //bearing
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy); printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
//goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
//double cur_pos_cartesian[3];
//cartesian_coordinates(cur_pos,cur_pos_cartesian);
//double goto_cartesian[3];
//goto_cartesian[0] = dx + cur_pos_cartesian[0];
//goto_cartesian[1] = dy + cur_pos_cartesian[1];
//goto_cartesian[2] = cur_pos_cartesian[2];
//spherical_coordinates(goto_cartesian, goto_pos);
// goto_pos[0]=dx;
// goto_pos[1]=dy;
//goto_pos[2]=height; // force a constant altitude to avoid loop increases
gps_from_rb(d, b, goto_pos); gps_from_rb(d, b, goto_pos);
/* if(dx == 1.0){
if((int)buzz_utility::get_robotid()==1){
goto_pos[0]=hcpos1[0];goto_pos[1]=hcpos1[1];goto_pos[2]=height;
}
if((int)buzz_utility::get_robotid()==2){
goto_pos[0]=hcpos2[0];goto_pos[1]=hcpos2[1];goto_pos[2]=height;
}
if((int)buzz_utility::get_robotid()==3){
goto_pos[0]=hcpos3[0];goto_pos[1]=hcpos3[1];goto_pos[2]=height;
}
}
if(dx == 2.0){OB
if((int)buzz_utility::get_robotid()==1){
goto_pos[0]=hcpos1[2];goto_pos[1]=hcpos1[3];goto_pos[2]=height;
}
if((int)buzz_utility::get_robotid()==2){
goto_pos[0]=hcpos2[2];goto_pos[1]=hcpos2[3];goto_pos[2]=height;
}
if((int)buzz_utility::get_robotid()==3){
goto_pos[0]=hcpos3[2];goto_pos[1]=hcpos3[3];goto_pos[2]=height;
}
}*/
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; 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]); 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; buzz_cmd=2;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*----------------------------------------/
/ Buzz closure to go directly to a GPS destination from the Mission Planner
/----------------------------------------*/
int buzzuav_goto(buzzvm_t vm) { int buzzuav_goto(buzzvm_t vm) {
rc_goto_pos[2]=height; rc_goto_pos[2]=height;
set_goto(rc_goto_pos); set_goto(rc_goto_pos);
@ -171,6 +119,9 @@ int buzzuav_goto(buzzvm_t vm) {
return buzzvm_ret0(vm); 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) { int buzzuav_arm(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n"); printf(" Buzz requested Arm \n");
@ -183,12 +134,44 @@ int buzzuav_disarm(buzzvm_t vm) {
buzz_cmd=4; buzz_cmd=4;
return buzzvm_ret0(vm); 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_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() { double* getgoto() {
return goto_pos; return goto_pos;
} }
/******************************/
int getcmd() { int getcmd() {
return cur_cmd; return cur_cmd;
} }
@ -221,43 +204,11 @@ void set_obstacle_dist(float dist[]) {
obst[i] = dist[i]; obst[i] = dist[i];
} }
/****************************************/
/****************************************/
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_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);
}
/****************************************/
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]; //static char BATTERY_BUF[256];
@ -305,7 +256,10 @@ int buzzuav_update_currentpos(buzzvm_t vm) {
buzzvm_gstore(vm); buzzvm_gstore(vm);
return vm->state; return vm->state;
} }
/*obstacle*/ /*-----------------------------------------
/ Create an obstacle Buzz table from proximity sensors
/ TODO: swap to proximity function below
--------------------------------------------*/
int buzzuav_update_obstacle(buzzvm_t vm) { int buzzuav_update_obstacle(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "obstacle", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "obstacle", 1));
@ -333,24 +287,16 @@ int buzzuav_update_obstacle(buzzvm_t vm) {
buzzvm_gstore(vm); buzzvm_gstore(vm);
return vm->state; return vm->state;
} }
/**************************************/
/*Flight status ratinale*/
/**************************************/
/*****************************************************************
DJI_SDK -Mavlink ExtendedState landed_state
STATUS_GROUND_STANDBY = 1, 1
STATUS_TAKE_OFF = 2, 3
STATUS_SKY_STANDBY = 3, 2
STATUS_LANDING = 4, 4
STATUS_FINISHING_LANDING = 5 0
******************************************************************/
/****************************************/
/*flight status update*/
/***************************************/
void flight_status_update(uint8_t state){ void flight_status_update(uint8_t state){
status=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) { int buzzuav_update_flight_status(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);

203
src/roscontroller.cpp
View File

@ -13,6 +13,9 @@ namespace rosbzz_node{
/*Compile the .bzz file to .basm, .bo and .bdbg*/ /*Compile the .bzz file to .basm, .bo and .bdbg*/
Compile_bzz(); Compile_bzz();
set_bzz_file(bzzfile_name.c_str()); set_bzz_file(bzzfile_name.c_str());
/*Initialize variables*/
armstate = 0;
multi_msg = true;
} }
/***Destructor***/ /***Destructor***/
@ -82,6 +85,9 @@ namespace rosbzz_node{
} }
} }
/*--------------------------------------------------------
/ Get all required parameters from the ROS launch file
/-------------------------------------------------------*/
void roscontroller::Rosparameters_get(ros::NodeHandle n_c){ void roscontroller::Rosparameters_get(ros::NodeHandle n_c){
/*Obtain .bzz file name from parameter server*/ /*Obtain .bzz file name from parameter server*/
@ -145,6 +151,9 @@ namespace rosbzz_node{
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
} }
/*--------------------------------------------------------
/ Create all required subscribers, publishers and ROS service clients
/-------------------------------------------------------*/
void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){ void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
/*subscribers*/ /*subscribers*/
@ -187,6 +196,9 @@ namespace rosbzz_node{
} }
} }
/*--------------------------------------------------------
/ Create Buzz bytecode from the bzz script inputed
/-------------------------------------------------------*/
void roscontroller::Compile_bzz(){ void roscontroller::Compile_bzz(){
/*Compile the buzz code .bzz to .bo*/ /*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile; stringstream bzzfile_in_compile;
@ -211,6 +223,9 @@ namespace rosbzz_node{
} }
/*--------------------------------------------------------
/ Fonctions handling the local MAV ROS fligh controller
/-------------------------------------------------------*/
void roscontroller::Arm(){ void roscontroller::Arm(){
mavros_msgs::CommandBool arming_message; mavros_msgs::CommandBool arming_message;
arming_message.request.value = armstate; arming_message.request.value = armstate;
@ -236,7 +251,9 @@ namespace rosbzz_node{
} }
/*Prepare messages 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) */
/* _________________________________________________________________________________________________ */ /* _________________________________________________________________________________________________ */
@ -248,6 +265,8 @@ namespace rosbzz_node{
/* flight controller client call if requested from Buzz*/ /* flight controller client call if requested from Buzz*/
/*FC call for takeoff,land and gohome*/ /*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(); int tmp = buzzuav_closures::bzz_cmd();
double* goto_pos = buzzuav_closures::getgoto(); double* goto_pos = buzzuav_closures::getgoto();
if (tmp == 1){ if (tmp == 1){
@ -401,10 +420,12 @@ namespace rosbzz_node{
} }
#define EARTH_RADIUS 6371000.0 /*-----------------------------------------------------------
#define LAT_AVERAGE 45.564898 / Compute Range and Bearing of a neighbor in a local reference frame
/*rectangular projection range and bearing coordinate system callback */ / from GPS coordinates
void roscontroller::cvt_rangebearingGB_coordinates(double nei[], double out[], double cur[]){ ----------------------------------------------------------- */
#define EARTH_RADIUS (double) 6371000.0
void roscontroller::cvt_rangebearing_coordinates(double nei[], double out[], double cur[]){
double lat1 = cur[0]*M_PI/180.0; double lat1 = cur[0]*M_PI/180.0;
double lon1 = cur[1]*M_PI/180.0; double lon1 = cur[1]*M_PI/180.0;
double lat2 = nei[0]*M_PI/180.0; double lat2 = nei[0]*M_PI/180.0;
@ -416,145 +437,6 @@ namespace rosbzz_node{
out[2] = 0.0; out[2] = 0.0;
} }
/*rectangular projection range and bearing coordinate system callback */
void roscontroller::cvt_rangebearing2D_coordinates(double spherical_pos_payload [], double out[], double cur[]){
double nei_lat = spherical_pos_payload[0] / 180.0 * M_PI;
double nei_long = spherical_pos_payload[1] / 180.0 * M_PI;
double cur_lat = cur[0] / 180.0 * M_PI;
double cur_long = cur[1] / 180.0 * M_PI;
double rho = spherical_pos_payload[2]+EARTH_RADIUS; //centered on Earth
double x_nei = rho * nei_long * cos(LAT_AVERAGE);
double y_nei = rho * nei_lat;
double x_cur = rho * cur_long * cos(LAT_AVERAGE);
double y_cur = rho * cur_lat;
out[0] = sqrt(pow(x_nei-x_cur,2.0)+pow(y_nei-y_cur,2.0));
out[1] = atan2(y_nei-y_cur,x_nei-x_cur);
out[2] = 0;
}
/*convert spherical to cartesian coordinate system callback */
void roscontroller::cvt_cartesian_coordinates(double spherical_pos_payload [], double out[]){
double latitude = spherical_pos_payload[0] / 180.0 * M_PI;
double longitude = spherical_pos_payload[1] / 180.0 * M_PI;
double rho = spherical_pos_payload[2]+EARTH_RADIUS; //centered on Earth
try {
out[0] = cos(latitude) * cos(longitude) * rho;
out[1] = cos(latitude) * sin(longitude) * rho;
out[2] = sin(latitude) * rho; // z is 'up'
} catch (std::overflow_error e) {
std::cout << e.what() << " Error in convertion to cartesian coordinate system "<<endl;
}
}
/*convert from GPS delta to Range and Bearing */
void roscontroller::cvt_rangebearing_coordinates(double spherical_pos[], double out[], double pos[]){
//Earth ellipse parameteres
double f = 1.0/298.257223563;
double a = 6378137.0;
double b = 6356752.314245;
double a2b2b2 = (a*a-b*b)/(b*b);
// constants to enhance the computation
double omega = (spherical_pos[1]-pos[1])/180*M_PI;
double A, lambda0, sigma, deltasigma, lambda=omega;
double cosU2 = cos(atan((1-f)*tan(spherical_pos[0]/180*M_PI)));
double sinU2 = sin(atan((1-f)*tan(spherical_pos[0]/180*M_PI)));
double cosU1 = cos(atan((1-f)*tan(pos[0]/180*M_PI)));
double sinU1 = sin(atan((1-f)*tan(pos[0]/180*M_PI)));
try {
bool converged = 0;
for (int i = 0; i < 20; i++) {
lambda0=lambda;
// eq. 14
double sin2sigma = (cosU2 * sin(lambda) * cosU2 * sin(lambda)) + pow(cosU1*sinU2 - sinU1*cosU2 * cos(lambda), 2.0);
double sinsigma = sqrt(sin2sigma);
// eq. 15
double cossigma = sinU1*sinU2 + (cosU1*cosU2 * cos(lambda));
// eq. 16
sigma = atan2(sinsigma, cossigma);
// eq. 17 Careful! sin2sigma might be almost 0!
double sinalpha = (sin2sigma == 0) ? 0.0 : cosU1*cosU2 * sin(lambda) / sinsigma;
double alpha = asin(sinalpha);
double cos2alpha = cos(alpha) * cos(alpha);
// eq. 18 Careful! cos2alpha might be almost 0!
double cos2sigmam = cos2alpha == 0.0 ? 0.0 : cossigma - 2 * sinU1*sinU2 / cos2alpha;
double u2 = cos2alpha * a2b2b2;
double cos2sigmam2 = cos2sigmam * cos2sigmam;
// eq. 3
A = 1.0 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)));
// eq. 4
double B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2)));
// eq. 6
deltasigma = B * sinsigma * (cos2sigmam + B / 4 * (cossigma * (-1 + 2 * cos2sigmam2) - B / 6 * cos2sigmam * (-3 + 4 * sin2sigma) * (-3 + 4 * cos2sigmam2)));
// eq. 10
double C = f / 16 * cos2alpha * (4 + f * (4 - 3 * cos2alpha));
// eq. 11 (modified)
lambda = omega + (1 - C) * f * sinalpha * (sigma + C * sinsigma * (cos2sigmam + C * cossigma * (-1 + 2 * cos2sigmam2)));
// see how much improvement we got
double change = fabs((lambda - lambda0) / lambda);
if ((i > 1) && (change < 0.0000000000001)) {
//cout << "CONVERSION CONVERGED at " << i << " !!!!" << endl;
converged = 1;
break;
}
}
// eq. 19
out[0] = b * A * (sigma - deltasigma);
// didn't converge? must be N/S
if (!converged) {
if (pos[0] > spherical_pos[0]) {
out[1] = M_PI/2;
out[2] = 0;
} else if (pos[0] < spherical_pos[0]) {
out[1] = 0;
out[2] = M_PI/2;
}
} else { // else, it converged, so do the math
// eq. 20
out[1] = atan2(cosU2 * sin(lambda), (cosU1*sinU2 - sinU1*cosU2 * cos(lambda)));
if (out[1] < 0.0) out[1] += 2*M_PI;
// eq. 21
out[2] = atan2(cosU1 * sin(lambda), (-sinU1*cosU2 + cosU1*sinU2 * cos(lambda))) + 3.1416;
if (out[2] < 0.0) out[2] += 2*M_PI;
}
if (out[1] >= M_PI) out[1] -= M_PI;
if (out[2] >= M_PI) out[2] -= M_PI;
} catch (std::overflow_error e) {
std::cout << e.what() << " Error in convertion to range and bearing "<<endl;
}
}
/*convert from cartesian to spherical coordinate system callback */
void roscontroller::cvt_spherical_coordinates(double cartesian_pos_payload [], double out[]){
double x = cartesian_pos_payload[0];
double y = cartesian_pos_payload[1];
double z = cartesian_pos_payload[2];
try {
out[0]=sqrt(pow(x,2.0)+pow(y,2.0)+pow(z,2.0));
out[1]=atan2(y,x);
out[2]=atan2((sqrt(pow(x,2.0)+pow(y,2.0))),z);
} catch (std::overflow_error e) {
std::cout << e.what() << " Error in convertion to spherical coordinate system "<<endl;
}
}
/*battery status callback*/ /*battery status callback*/
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);
@ -649,23 +531,11 @@ namespace rosbzz_node{
memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t)); memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]); buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]);
// cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl; // cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl;
double cvt_neighbours_pos_test[3]; double cvt_neighbours_pos_payload[3];
cvt_rangebearingGB_coordinates(neighbours_pos_payload, cvt_neighbours_pos_test, cur_pos); cvt_rangebearing_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload, cur_pos);
/*Convert obtained position to relative CARTESIAN position*/
double cartesian_neighbours_pos[3], cartesian_cur_pos[3];
cvt_cartesian_coordinates(neighbours_pos_payload, cartesian_neighbours_pos);
cvt_cartesian_coordinates(cur_pos, cartesian_cur_pos);
/*Compute the relative position*/
for(int i=0;i<3;i++){
neighbours_pos_payload[i]=cartesian_neighbours_pos[i]-cartesian_cur_pos[i];
}
double *cvt_neighbours_pos_payload = cvt_neighbours_pos_test;
//double cvt_neighbours_pos_payload[3];
//cvt_spherical_coordinates(neighbours_pos_payload, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/ /*Extract robot id of the neighbour*/
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3)); uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl; cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;
// cout << "Rel Test Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_test[0] << ", "<< cvt_neighbours_pos_test[2] << ", "<< cvt_neighbours_pos_test[3] << endl;
/*pass neighbour position to local maintaner*/ /*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]); buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
/*Put RID and pos*/ /*Put RID and pos*/
@ -676,24 +546,17 @@ namespace rosbzz_node{
} }
} }
/* RC command service */ /*-----------------------------------------------------------
/ Catch the ROS service call from a custom remote controller (Mission Planner)
/ and send the requested commands to Buzz
----------------------------------------------------------- */
bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req, bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
mavros_msgs::CommandLong::Response &res){ mavros_msgs::CommandLong::Response &res){
int rc_cmd; int rc_cmd;
/* if(req.command==oldcmdID)
return true;
else oldcmdID=req.command;*/
switch(req.command){ switch(req.command){
case mavros_msgs::CommandCode::NAV_TAKEOFF: case mavros_msgs::CommandCode::NAV_TAKEOFF:
ROS_INFO("RC_call: TAKE OFF!!!!"); ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
/* arming */
SetMode();
if(!armstate) {
armstate =1;
Arm();
}
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
break; break;