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

reformat gimbal, uavstate, fc cmd and add warning for ros rate and enhance gps prec.

This commit is contained in:
dave 2017-09-08 09:53:20 -04:00
parent 7284565504 b41a086ede
commit 527569fe71
7 changed files with 205 additions and 190 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)))

20
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: ", TARGET_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,7 +48,7 @@ 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()
} }
@ -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,13 +86,12 @@ 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) 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)
@ -183,7 +182,6 @@ function LimitAngle(angle){
} }
function rb_from_gps(lat, lon) { function rb_from_gps(lat, lon) {
# print("gps2rb d: ",lat,lon) # print("gps2rb d: ",lat,lon)
# print("gps2rb h: ",position.latitude,position.longitude) # print("gps2rb h: ",position.latitude,position.longitude)
d_lon = lon - position.longitude d_lon = lon - position.longitude

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);

58
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.
} }
@ -321,11 +320,34 @@ namespace buzzuav_closures{
/ Buzz closure to take a picture here. / Buzz closure to take a picture here.
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_takepicture(buzzvm_t vm) { int buzzuav_takepicture(buzzvm_t vm) {
cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; //cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
buzz_cmd = COMMAND_PICTURE; buzz_cmd = COMMAND_PICTURE;
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;
} }

292
src/roscontroller.cpp
View File

@ -195,7 +195,9 @@ void roscontroller::RosControllerRun()
robot_id)) { robot_id)) {
ROS_INFO("[%i] Bytecode file found and set", robot_id); ROS_INFO("[%i] Bytecode file found and set", robot_id);
std::string standby_bo = Compile_bzz(stand_by) + ".bo"; std::string standby_bo = Compile_bzz(stand_by) + ".bo";
init_update_monitor(bcfname.c_str(), standby_bo.c_str()); //init_update_monitor(bcfname.c_str(), standby_bo.c_str());
/*loop rate of ros*/
ros::Rate loop_rate(BUZZRATE);
/////////////////////////////////////////////////////// ///////////////////////////////////////////////////////
// MAIN LOOP // MAIN LOOP
////////////////////////////////////////////////////// //////////////////////////////////////////////////////
@ -220,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
@ -236,7 +231,7 @@ void roscontroller::RosControllerRun()
neighbours_pos_publisher(); neighbours_pos_publisher();
send_MPpayload(); send_MPpayload();
/*Check updater state and step code*/ /*Check updater state and step code*/
update_routine(bcfname.c_str(), dbgfname.c_str()); // update_routine(bcfname.c_str(), dbgfname.c_str());
/*Log In Msg queue size*/ /*Log In Msg queue size*/
log<<(int)buzz_utility::get_inmsg_size()<<","; log<<(int)buzz_utility::get_inmsg_size()<<",";
/*Step buzz script */ /*Step buzz script */
@ -246,13 +241,13 @@ void roscontroller::RosControllerRun()
/*call flight controler service to set command long*/ /*call flight controler service to set command long*/
flight_controller_service_call(); flight_controller_service_call();
/*Set multi message available after update*/ /*Set multi message available after update*/
if (get_update_status()) //if (get_update_status())
{ //{
/* set_read_update_status(); /* set_read_update_status();
multi_msg = true; multi_msg = true;
log << cur_pos.latitude << "," << cur_pos.longitude << "," log << cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ","; << cur_pos.altitude << ",";
collect_data(log); collect_data(log);
map<int, buzz_utility::Pos_struct>::iterator it = map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin(); neighbours_pos_map.begin();
log << "," << neighbours_pos_map.size(); log << "," << neighbours_pos_map.size();
@ -262,40 +257,28 @@ void roscontroller::RosControllerRun()
<< "," << (double)it->second.z; << "," << (double)it->second.z;
} }
log << std::endl;*/ log << std::endl;*/
} //}
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/ /*Set ROBOTS variable for barrier in .bzz from neighbours count*/
// no_of_robots=get_number_of_robots(); // no_of_robots=get_number_of_robots();
get_number_of_robots(); get_number_of_robots();
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();
std::stringstream state_buff; log<<state_buff.str()<<std::endl;
//buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
// buzzvm_gload(VM);
// buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
// buzzvm_pop(VM);
// 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 of ros*/
ros::Rate loop_rate(BUZZRATE);
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
@ -633,7 +616,7 @@ void roscontroller::prepare_msg_and_publish()
delete[] out; delete[] out;
delete[] payload_out_ptr; delete[] payload_out_ptr;
/*Check for updater message if present send*/ /*Check for updater message if present send*/
if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && /* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 &&
multi_msg) multi_msg)
{ {
uint8_t *buff_send = 0; uint8_t *buff_send = 0;
@ -675,7 +658,7 @@ void roscontroller::prepare_msg_and_publish()
payload_pub.publish(update_packets); payload_pub.publish(update_packets);
multi_msg = false; multi_msg = false;
delete[] payload_64; delete[] payload_64;
} }*/
} }
/*--------------------------------------------------------------------------------- /*---------------------------------------------------------------------------------
/Flight controller service call every step if there is a command set from bzz /Flight controller service call every step if there is a command set from bzz
@ -685,95 +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 = 90; break;
cmd_srv.request.param2 = 0;
cmd_srv.request.param3 = 0; case buzzuav_closures::COMMAND_MOVETO:
cmd_srv.request.param4 = 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];
capture_srv.call(capture_command); cmd_srv.request.param2 = gimbal[1];
cmd_srv.request.param3 = gimbal[2];
cmd_srv.request.param4 = gimbal[3];
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
/---------------------------------------------*/ /---------------------------------------------*/
@ -823,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)
@ -989,18 +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[0], local_pos[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;
@ -1111,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],
@ -1175,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;