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

Merge commit '527569fe7125de85f983482af5656267f701b69e' into ros_drones_ws

This commit is contained in:
David St-Onge 2017-09-08 11:59:04 -04:00
parent 663bd5e4ed 527569fe71
commit aa94ef6b59
7 changed files with 212 additions and 198 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
buzz_scripts/graphformGPS.bzz
View File

@ -567,7 +567,6 @@ function set_rc_goto() {
m_cMeToPred.Bearing=m_MessageBearing[IDofPred] m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing) var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
var S2Target=math.vec2.add(S2Pred,P2Target) var S2Target=math.vec2.add(S2Pred,P2Target)
gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x))) gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))

29
buzz_scripts/include/uavstates.bzz
View File

@ -5,7 +5,7 @@
######################################## ########################################
TARGET_ALTITUDE = 5.0 # m. TARGET_ALTITUDE = 5.0 # m.
UAVSTATE = "TURNEDOFF" UAVSTATE = "TURNEDOFF"
PICTURE_WAIT = 40 # steps PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 2 # m/steps GOTO_MAXVEL = 2 # m/steps
GOTO_MAXDIST = 150 # m. GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5 # m. GOTODIST_TOL = 0.5 # m.
@ -35,10 +35,10 @@ function takeoff() {
barrier_set(ROBOTS, action, land, -1) barrier_set(ROBOTS, action, land, -1)
barrier_ready() barrier_ready()
} else { } else {
log("Altitude: ", position.altitude) log("Altitude: ", position.altitude)
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE) uav_takeoff(TARGET_ALTITUDE)
} }
} }
function land() { function land() {
@ -48,8 +48,8 @@ function land() {
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
uav_land() uav_land()
if(flight.status != 2 and flight.status != 3){ if(flight.status != 2 and flight.status != 3) {
barrier_set(ROBOTS,turnedoff,land, 21) barrier_set(ROBOTS,turnedoff,land, 21)
barrier_ready() barrier_ready()
} }
} }
@ -57,14 +57,14 @@ function land() {
function set_goto(transf) { function set_goto(transf) {
UAVSTATE = "GOTOGPS" UAVSTATE = "GOTOGPS"
statef=function() { statef=function() {
gotoWP(transf); gotoWP(transf);
} }
if(rc_goto.id==id){ if(rc_goto.id==id){
if(s!=nil){ if(s!=nil){
if(s.in()) if(s.in())
s.leave() s.leave()
} }
} else { } else {
neighbors.broadcast("cmd", 16) neighbors.broadcast("cmd", 16)
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
@ -75,10 +75,10 @@ ptime=0
function picture() { function picture() {
statef=picture statef=picture
UAVSTATE="PICTURE" UAVSTATE="PICTURE"
#TODO: change gimbal orientation uav_setgimbal(0.0, 0.0, -90.0, 20.0)
if(ptime==PICTURE_WAIT/2) if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize
uav_takepicture() uav_takepicture()
else if(ptime>=PICTURE_WAIT) { } else if(ptime>=PICTURE_WAIT) { # wait for the picture
statef=action statef=action
ptime=0 ptime=0
} }
@ -86,14 +86,13 @@ function picture() {
} }
function gotoWP(transf) { function gotoWP(transf) {
# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
rb_from_gps(rc_goto.latitude, rc_goto.longitude) rb_from_gps(rc_goto.latitude, rc_goto.longitude)
print(" has to move ", goal.range, goal.bearing) print(" has to move ", goal.range, goal.bearing)
m_navigation = math.vec2.newp(goal.range, goal.bearing) m_navigation = math.vec2.newp(goal.range, goal.bearing)
if(math.vec2.length(m_navigation)>GOTO_MAXDIST) if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far.") log("Sorry this is too far.")
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
} else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination } else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination

9
include/buzzuav_closures.h
View File

@ -10,7 +10,8 @@
#include "buzz_utility.h" #include "buzz_utility.h"
#define EARTH_RADIUS (double) 6371000.0 #define EARTH_RADIUS (double) 6371000.0
#define DEG2RAD(DEG) ((DEG)*((M_PI)/(180.0))) #define DEG2RAD(DEG) (double) ((DEG)*((M_PI)/(180.0)))
#define RAD2DEG(RAD) (double) ((RAD)*((180.0)/(M_PI)))
namespace buzzuav_closures{ namespace buzzuav_closures{
typedef enum { typedef enum {
@ -23,6 +24,7 @@ namespace buzzuav_closures{
COMMAND_GOTO, COMMAND_GOTO,
COMMAND_MOVETO, COMMAND_MOVETO,
COMMAND_PICTURE, COMMAND_PICTURE,
COMMAND_GIMBAL,
} Custom_CommandCode; } Custom_CommandCode;
/* /*
@ -38,6 +40,7 @@ void setWPlist(std::string path);
*/ */
int buzzuav_moveto(buzzvm_t vm); int buzzuav_moveto(buzzvm_t vm);
int buzzuav_storegoal(buzzvm_t vm); int buzzuav_storegoal(buzzvm_t vm);
int buzzuav_setgimbal(buzzvm_t vm);
void parse_gpslist(); void parse_gpslist();
int buzzuav_takepicture(buzzvm_t vm); int buzzuav_takepicture(buzzvm_t vm);
/* Returns the current command from local variable*/ /* Returns the current command from local variable*/
@ -47,7 +50,7 @@ void set_goto(double pos[]);
/*Sets goto position from rc client*/ /*Sets goto position from rc client*/
void rc_set_goto(int id, double latitude, double longitude, double altitude); void rc_set_goto(int id, double latitude, double longitude, double altitude);
/*Sets gimbal orientation from rc client*/ /*Sets gimbal orientation from rc client*/
void rc_set_gimbal(int id, float yaw, float pitch); void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t);
/*sets rc requested command */ /*sets rc requested command */
void rc_call(int rc_cmd); void rc_call(int rc_cmd);
/* sets the battery state */ /* sets the battery state */
@ -61,6 +64,8 @@ void set_api_rssi(float value);
void set_currentpos(double latitude, double longitude, double altitude); void set_currentpos(double latitude, double longitude, double altitude);
/*retuns the current go to position */ /*retuns the current go to position */
double* getgoto(); double* getgoto();
std::string getuavstate();
float* getgimbal();
/* updates flight status*/ /* updates flight status*/
void flight_status_update(uint8_t state); void flight_status_update(uint8_t state);
/* Update neighbors table */ /* Update neighbors table */

2
include/roscontroller.h
View File

@ -71,7 +71,7 @@ private:
double longitude=0.0; double longitude=0.0;
double latitude=0.0; double latitude=0.0;
float altitude=0.0; float altitude=0.0;
}; typedef struct gps GPS ; // not useful in cpp }; typedef struct gps GPS ;
GPS target, home, cur_pos; GPS target, home, cur_pos;
double cur_rel_altitude; double cur_rel_altitude;

9
src/buzz_utility.cpp
View File

@ -307,6 +307,9 @@ void in_message_process(){
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
buzzvm_gstore(VM); buzzvm_gstore(VM);
@ -352,6 +355,12 @@ void in_message_process(){
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM); buzzvm_gstore(VM);

56
src/buzzuav_closures.cpp
View File

@ -16,7 +16,7 @@ namespace buzzuav_closures{
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin); //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
static double goto_pos[3]; static double goto_pos[3];
static double rc_goto_pos[3]; static double rc_goto_pos[3];
static float rc_gimbal[2]; static float rc_gimbal[4];
static float batt[3]; static float batt[3];
static float obst[5]={0,0,0,0,0}; static float obst[5]={0,0,0,0,0};
static double cur_pos[3]; static double cur_pos[3];
@ -28,11 +28,10 @@ namespace buzzuav_closures{
static float height=0; static float height=0;
static bool deque_full = false; static bool deque_full = false;
static int rssi = 0; static int rssi = 0;
static int message_number = 0;
static float raw_packet_loss = 0.0; static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0; static int filtered_packet_loss = 0;
static float api_rssi = 0.0; static float api_rssi = 0.0;
string WPlistname = ""; string WPlistname = "";
std::map< int, buzz_utility::RB_struct> targets_map; std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::RB_struct> wplist_map; std::map< int, buzz_utility::RB_struct> wplist_map;
@ -94,12 +93,12 @@ namespace buzzuav_closures{
/----------------------------------------*/ /----------------------------------------*/
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 = RAD2DEG(cur_pos[0]);
double lon = cur_pos[1]*M_PI/180.0; double lon = RAD2DEG(cur_pos[1]);
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] = RAD2DEG(out[0]);
out[1] = out[1]*180.0/M_PI; out[1] = RAD2DEG(out[1]);
out[2] = height; //constant height. out[2] = height; //constant height.
} }
@ -326,6 +325,29 @@ namespace buzzuav_closures{
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*----------------------------------------/
/ Buzz closure to change locally the gimbal orientation
/----------------------------------------*/
int buzzuav_setgimbal(buzzvm_t vm) {
buzzvm_lnum_assert(vm, 4);
buzzvm_lload(vm, 1); // time
buzzvm_lload(vm, 2); // pitch
buzzvm_lload(vm, 3); // roll
buzzvm_lload(vm, 4); // yaw
buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
rc_gimbal[0] = buzzvm_stack_at(vm, 4)->f.value;
rc_gimbal[1] = buzzvm_stack_at(vm, 3)->f.value;
rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value;
rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)",rc_gimbal[0],rc_gimbal[1],rc_gimbal[2],rc_gimbal[3]);
buzz_cmd = COMMAND_GIMBAL;
return buzzvm_ret0(vm);
}
/*----------------------------------------/ /*----------------------------------------/
/ Buzz closure to store locally a GPS destination from the fleet / Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/ /----------------------------------------*/
@ -413,6 +435,20 @@ namespace buzzuav_closures{
return goto_pos; return goto_pos;
} }
float* getgimbal() {
return rc_gimbal;
}
string getuavstate() {
static buzzvm_t VM = buzz_utility::get_vm();
std::stringstream state_buff;
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
buzzvm_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
return uav_state->s.value.str;
}
int getcmd() { int getcmd() {
return cur_cmd; return cur_cmd;
} }
@ -438,11 +474,13 @@ namespace buzzuav_closures{
} }
void rc_set_gimbal(int id, float yaw, float pitch) { void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) {
rc_id = id; rc_id = id;
rc_gimbal[0] = yaw; rc_gimbal[0] = yaw;
rc_gimbal[1] = pitch; rc_gimbal[1] = roll;
rc_gimbal[2] = pitch;
rc_gimbal[3] = t;
} }

304
src/roscontroller.cpp
View File

@ -222,14 +222,7 @@ void roscontroller::RosControllerRun()
} }
const uint8_t shrt_id= 0xFF; const uint8_t shrt_id= 0xFF;
float result; float result;
//if ( GetAPIRssi(shrt_id, result) )
// log<<result<<",";
//else
// log<<"0,";
// if ( GetRawPacketLoss(shrt_id, result) )
// log<<result<<",";
//else
// log<<"0,";
if ( GetFilteredPacketLoss(shrt_id, result) ) if ( GetFilteredPacketLoss(shrt_id, result) )
log<<result<<","; log<<result<<",";
else else
@ -269,33 +262,23 @@ void roscontroller::RosControllerRun()
// no_of_robots=get_number_of_robots(); // no_of_robots=get_number_of_robots();
get_number_of_robots(); get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots); buzz_utility::set_robot_var(no_of_robots);
/*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF /*Retrive the state of the graph and uav and log*/
SCRIPT IS NOT graphform.bzz*/ std::stringstream state_buff;
static buzzvm_t VM = buzz_utility::get_vm(); state_buff << buzzuav_closures::getuavstate();
//buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); log<<state_buff.str()<<std::endl;
// buzzvm_gload(VM);
// buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
// buzzvm_pop(VM);
std::stringstream state_buff;
// state_buff<< graph_state->s.value.str<<",";
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
buzzvm_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
state_buff<< uav_state->s.value.str;
log<<state_buff.str()<<std::endl;
// if(neighbours_pos_map.size() >0) no_of_robots // if(neighbours_pos_map.size() >0) no_of_robots
// =neighbours_pos_map.size()+1; // =neighbours_pos_map.size()+1;
// buzz_utility::set_robot_var(no_of_robots); // buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates TODO only when not updating*/ /*Set no of robots for updates TODO only when not updating*/
// if(multi_msg) // if(multi_msg)
//updates_set_robots(no_of_robots); //updates_set_robots(no_of_robots);
// ROS_INFO("ROBOTS: %i , acutal : //get_xbee_status(); // commented out because it may slow down the node too much, to be tested
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
get_xbee_status();
/*run once*/ /*run once*/
ros::spinOnce(); ros::spinOnce();
loop_rate.sleep(); loop_rate.sleep();
// make sure to sleep for the remainder of our cycle time
if (loop_rate.cycleTime() > ros::Duration(1.0 / (float)BUZZRATE))
ROS_WARN("ROSBuzz loop missed its desired rate of %iHz... the loop actually took %.4f seconds", BUZZRATE, loop_rate.cycleTime().toSec());
if (fcu_timeout <= 0) if (fcu_timeout <= 0)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND); buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
else else
@ -685,98 +668,119 @@ script
void roscontroller::flight_controller_service_call() void roscontroller::flight_controller_service_call()
{ {
/* flight controller client call if requested from Buzz*/ /* flight controller client call if requested from Buzz*/
/*FC call for takeoff,land, gohome and arm/disarm*/ double *goto_pos;
int tmp = buzzuav_closures::bzz_cmd(); float *gimbal;
double *goto_pos = buzzuav_closures::getgoto(); switch (buzzuav_closures::bzz_cmd()) {
if (tmp == buzzuav_closures::COMMAND_TAKEOFF ||
tmp == buzzuav_closures::COMMAND_LAND ||
tmp == buzzuav_closures::COMMAND_GOHOME) {
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);
armstate = 0;
Arm();
}
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");
}
armstate = 0;
break;
case mavros_msgs::CommandCode::NAV_TAKEOFF:
if (!armstate) {
SetMode("LOITER", 0); case buzzuav_closures::COMMAND_TAKEOFF:
armstate = 1; goto_pos = buzzuav_closures::getgoto();
Arm(); cmd_srv.request.param7 = goto_pos[2];
ros::Duration(0.5).sleep(); cmd_srv.request.command = buzzuav_closures::getcmd();
// Registering HOME POINT. if (!armstate) {
home = cur_pos; SetMode("LOITER", 0);
} armstate = 1;
if (current_mode != "GUIDED") Arm();
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it ros::Duration(0.5).sleep();
// should always be in loiter after arm/disarm) // Registering HOME POINT.
home = cur_pos;
}
if (current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it
// should always be in loiter after arm/disarm)
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");
break;
case buzzuav_closures::COMMAND_LAND:
cmd_srv.request.command = buzzuav_closures::getcmd();
if (current_mode != "LAND") {
SetMode("LAND", 0);
armstate = 0;
Arm();
}
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");
}
armstate = 0;
break;
case buzzuav_closures::COMMAND_GOHOME: // NOT FULLY IMPLEMENTED/TESTED !!!
cmd_srv.request.param5 = home.latitude;
cmd_srv.request.param6 = home.longitude;
cmd_srv.request.param7 = home.altitude;
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)) { if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else } else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
break; break;
}
} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/ case buzzuav_closures::COMMAND_GOTO: // NOT FULLY IMPLEMENTED/TESTED !!!
/*prepare the goto publish message */ goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param5 = goto_pos[0]; cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1]; cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)) { if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else } else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
if (mav_client.call(cmd_srv)) { if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else } else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/ break;
if (!armstate) {
SetMode("LOITER", 0); case buzzuav_closures::COMMAND_ARM:
armstate = 1; if (!armstate) {
Arm(); SetMode("LOITER", 0);
} armstate = 1;
} else if (tmp == buzzuav_closures::COMMAND_DISARM) { Arm();
if (armstate) { }
armstate = 0; break;
SetMode("LOITER", 0);
Arm(); case buzzuav_closures::COMMAND_DISARM:
} if (armstate) {
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ armstate = 0;
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); SetMode("LOITER", 0);
} else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ Arm();
ROS_INFO("TAKING A PICTURE HERE!! --------------"); }
cmd_srv.request.param1 = 0.0; break;
cmd_srv.request.param2 = 0.0;
cmd_srv.request.param3 = -90.0; case buzzuav_closures::COMMAND_MOVETO:
cmd_srv.request.param4 = 0.0; goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
if (mav_client.call(cmd_srv)) { break;
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else case buzzuav_closures::COMMAND_GIMBAL:
ROS_ERROR("Failed to call service from flight controller"); gimbal = buzzuav_closures::getgimbal();
mavros_msgs::CommandBool capture_command; cmd_srv.request.param1 = gimbal[0];
if (capture_srv.call(capture_command)) { cmd_srv.request.param2 = gimbal[1];
ROS_INFO("Reply: %ld", (long int)capture_command.response.success); cmd_srv.request.param3 = gimbal[2];
} else cmd_srv.request.param4 = gimbal[3];
ROS_ERROR("Failed to call service from camera streamer"); cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
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");
break;
case buzzuav_closures::COMMAND_PICTURE:
ROS_INFO("TAKING A PICTURE HERE!! --------------");
mavros_msgs::CommandBool capture_command;
if (capture_srv.call(capture_command)) {
ROS_INFO("Reply: %ld", (long int)capture_command.response.success);
} else
ROS_ERROR("Failed to call service from camera streamer");
break;
} }
} }
/*---------------------------------------------- /*----------------------------------------------
/Refresh neighbours Position for every ten step /Refresh neighbours Position for every ten step
/---------------------------------------------*/ /---------------------------------------------*/
@ -826,39 +830,6 @@ void roscontroller::set_cur_pos(double latitude, double longitude,
/ Compute Range and Bearing of a neighbor in a local reference frame / Compute Range and Bearing of a neighbor in a local reference frame
/ from GPS coordinates / from GPS coordinates
----------------------------------------------------------- */ ----------------------------------------------------------- */
void ecef2ned_matrix(const double ref_ecef[3], double M[3][3]) {
double hyp_az, hyp_el;
double sin_el, cos_el, sin_az, cos_az;
/* Convert reference point to spherical earth centered coordinates. */
hyp_az = sqrt(ref_ecef[0] * ref_ecef[0] + ref_ecef[1] * ref_ecef[1]);
hyp_el = sqrt(hyp_az * hyp_az + ref_ecef[2] * ref_ecef[2]);
sin_el = ref_ecef[2] / hyp_el;
cos_el = hyp_az / hyp_el;
sin_az = ref_ecef[1] / hyp_az;
cos_az = ref_ecef[0] / hyp_az;
M[0][0] = -sin_el * cos_az;
M[0][1] = -sin_el * sin_az;
M[0][2] = cos_el;
M[1][0] = -sin_az;
M[1][1] = cos_az;
M[1][2] = 0.0;
M[2][0] = -cos_el * cos_az;
M[2][1] = -cos_el * sin_az;
M[2][2] = -sin_el;
}
void matrix_multiply(uint32_t n, uint32_t m, uint32_t p, const double *a,
const double *b, double *c) {
uint32_t i, j, k;
for (i = 0; i < n; i++)
for (j = 0; j < p; j++) {
c[p * i + j] = 0;
for (k = 0; k < m; k++)
c[p * i + j] += a[m * i + k] * b[p * k + j];
}
}
float roscontroller::constrainAngle(float x) { float roscontroller::constrainAngle(float x) {
x = fmod(x,2*M_PI); x = fmod(x,2*M_PI);
if (x < 0.0) if (x < 0.0)
@ -992,17 +963,10 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) {
moveMsg.header.stamp = ros::Time::now(); moveMsg.header.stamp = ros::Time::now();
moveMsg.header.seq = setpoint_counter++; moveMsg.header.seq = setpoint_counter++;
moveMsg.header.frame_id = 1; moveMsg.header.frame_id = 1;
// float ned_x, ned_y;
// gps_ned_home(ned_x, ned_y);
// ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0],
// home[1]);
// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos_new[0], local_pos_new[1]);
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD //ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y);
* (then converted to NED)*/ moveMsg.pose.position.x = local_pos_new[0] + y;
// target[0]+=y; target[1]+=x; moveMsg.pose.position.y = local_pos_new[1] + x;
moveMsg.pose.position.x = local_pos_new[0] + y; // ned_y+y;
moveMsg.pose.position.y = local_pos_new[1] + x; // ned_x+x;
moveMsg.pose.position.z = z; moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0; moveMsg.pose.orientation.x = 0;
@ -1113,7 +1077,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
gps_rb(nei_pos, cvt_neighbours_pos_payload); gps_rb(nei_pos, 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));
// ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]); ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
/*pass neighbour position to local maintaner*/ /*pass neighbour position to local maintaner*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0], buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],
cvt_neighbours_pos_payload[1], cvt_neighbours_pos_payload[1],
@ -1177,7 +1141,7 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
break; break;
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
ROS_INFO("RC_Call: Gimbal!!!! "); ROS_INFO("RC_Call: Gimbal!!!! ");
buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3); buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3,req.param4,req.param5);
rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
@ -1260,22 +1224,22 @@ void roscontroller::get_xbee_status()
bool result_bool; bool result_bool;
float result_float; float result_float;
const uint8_t all_ids = 0xFF; const uint8_t all_ids = 0xFF;
//if(GetDequeFull(result_bool)) if(GetDequeFull(result_bool))
//{ {
// buzzuav_closures::set_deque_full(result_bool); buzzuav_closures::set_deque_full(result_bool);
//} }
//if(GetRssi(result_float)) if(GetRssi(result_float))
//{ {
// buzzuav_closures::set_rssi(result_float); buzzuav_closures::set_rssi(result_float);
//} }
//if(GetRawPacketLoss(all_ids, result_float)) if(GetRawPacketLoss(all_ids, result_float))
//{ {
// buzzuav_closures::set_raw_packet_loss(result_float); buzzuav_closures::set_raw_packet_loss(result_float);
//} }
//if(GetFilteredPacketLoss(all_ids, result_float)) if(GetFilteredPacketLoss(all_ids, result_float))
//{ {
// buzzuav_closures::set_filtered_packet_loss(result_float); buzzuav_closures::set_filtered_packet_loss(result_float);
//} }
// This part needs testing since it can overload the xbee module // This part needs testing since it can overload the xbee module
/* /*
* if(GetAPIRssi(all_ids, result_float)) * if(GetAPIRssi(all_ids, result_float))