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]
var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
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)))

22
buzz_scripts/include/uavstates.bzz
View File

@ -5,7 +5,7 @@
########################################
TARGET_ALTITUDE = 5.0 # m.
UAVSTATE = "TURNEDOFF"
PICTURE_WAIT = 40 # steps
PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 2 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5 # m.
@ -35,10 +35,10 @@ function takeoff() {
barrier_set(ROBOTS, action, land, -1)
barrier_ready()
} else {
log("Altitude: ", TARGET_ALTITUDE)
log("Altitude: ", position.altitude)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
}
function land() {
@ -48,7 +48,7 @@ function land() {
neighbors.broadcast("cmd", 21)
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_ready()
}
@ -63,7 +63,7 @@ function set_goto(transf) {
if(rc_goto.id==id){
if(s!=nil){
if(s.in())
s.leave()
s.leave()
}
} else {
neighbors.broadcast("cmd", 16)
@ -75,10 +75,10 @@ ptime=0
function picture() {
statef=picture
UAVSTATE="PICTURE"
#TODO: change gimbal orientation
if(ptime==PICTURE_WAIT/2)
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize
uav_takepicture()
else if(ptime>=PICTURE_WAIT) {
} else if(ptime>=PICTURE_WAIT) { # wait for the picture
statef=action
ptime=0
}
@ -86,13 +86,12 @@ function picture() {
}
function gotoWP(transf) {
print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
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)
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
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)
@ -183,7 +182,6 @@ function LimitAngle(angle){
}
function rb_from_gps(lat, lon) {
# print("gps2rb d: ",lat,lon)
# print("gps2rb h: ",position.latitude,position.longitude)
d_lon = lon - position.longitude

9
include/buzzuav_closures.h
View File

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

2
include/roscontroller.h
View File

@ -71,7 +71,7 @@ private:
double longitude=0.0;
double latitude=0.0;
float altitude=0.0;
}; typedef struct gps GPS ; // not useful in cpp
}; typedef struct gps GPS ;
GPS target, home, cur_pos;
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_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
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_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
buzzvm_gstore(VM);
@ -352,6 +355,12 @@ void in_message_process(){
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
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_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
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);
static double 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 obst[5]={0,0,0,0,0};
static double cur_pos[3];
@ -28,11 +28,10 @@ namespace buzzuav_closures{
static float height=0;
static bool deque_full = false;
static int rssi = 0;
static int message_number = 0;
static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 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> wplist_map;
@ -94,12 +93,12 @@ namespace buzzuav_closures{
/----------------------------------------*/
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;
double lat = RAD2DEG(cur_pos[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[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[0] = RAD2DEG(out[0]);
out[1] = RAD2DEG(out[1]);
out[2] = height; //constant height.
}
@ -321,11 +320,34 @@ namespace buzzuav_closures{
/ Buzz closure to take a picture here.
/----------------------------------------*/
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;
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
/----------------------------------------*/
@ -413,6 +435,20 @@ namespace buzzuav_closures{
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() {
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_gimbal[0] = yaw;
rc_gimbal[1] = pitch;
rc_gimbal[1] = roll;
rc_gimbal[2] = pitch;
rc_gimbal[3] = t;
}

294
src/roscontroller.cpp
View File

@ -195,7 +195,9 @@ void roscontroller::RosControllerRun()
robot_id)) {
ROS_INFO("[%i] Bytecode file found and set", robot_id);
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
//////////////////////////////////////////////////////
@ -220,15 +222,8 @@ void roscontroller::RosControllerRun()
}
const uint8_t shrt_id= 0xFF;
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<<",";
else
log<<"0,";
@ -236,7 +231,7 @@ void roscontroller::RosControllerRun()
neighbours_pos_publisher();
send_MPpayload();
/*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<<(int)buzz_utility::get_inmsg_size()<<",";
/*Step buzz script */
@ -246,13 +241,13 @@ void roscontroller::RosControllerRun()
/*call flight controler service to set command long*/
flight_controller_service_call();
/*Set multi message available after update*/
if (get_update_status())
{
//if (get_update_status())
//{
/* set_read_update_status();
multi_msg = true;
log << cur_pos.latitude << "," << cur_pos.longitude << ","
<< cur_pos.altitude << ",";
collect_data(log);
collect_data(log);
map<int, buzz_utility::Pos_struct>::iterator it =
neighbours_pos_map.begin();
log << "," << neighbours_pos_map.size();
@ -262,40 +257,28 @@ void roscontroller::RosControllerRun()
<< "," << (double)it->second.z;
}
log << std::endl;*/
}
//}
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
// no_of_robots=get_number_of_robots();
get_number_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
SCRIPT IS NOT graphform.bzz*/
static buzzvm_t VM = buzz_utility::get_vm();
std::stringstream state_buff;
//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;
/*Retrive the state of the graph and uav and log*/
std::stringstream state_buff;
state_buff << buzzuav_closures::getuavstate();
log<<state_buff.str()<<std::endl;
// if(neighbours_pos_map.size() >0) no_of_robots
// =neighbours_pos_map.size()+1;
// buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates TODO only when not updating*/
// if(multi_msg)
updates_set_robots(no_of_robots);
// ROS_INFO("ROBOTS: %i , acutal :
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
get_xbee_status();
//updates_set_robots(no_of_robots);
//get_xbee_status(); // commented out because it may slow down the node too much, to be tested
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
ros::Rate loop_rate(BUZZRATE);
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)
buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
else
@ -633,7 +616,7 @@ void roscontroller::prepare_msg_and_publish()
delete[] out;
delete[] payload_out_ptr;
/*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)
{
uint8_t *buff_send = 0;
@ -675,7 +658,7 @@ void roscontroller::prepare_msg_and_publish()
payload_pub.publish(update_packets);
multi_msg = false;
delete[] payload_64;
}
}*/
}
/*---------------------------------------------------------------------------------
/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()
{
/* 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 == 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) {
double *goto_pos;
float *gimbal;
switch (buzzuav_closures::bzz_cmd()) {
SetMode("LOITER", 0);
armstate = 1;
Arm();
ros::Duration(0.5).sleep();
// 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)
case buzzuav_closures::COMMAND_TAKEOFF:
goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
if (!armstate) {
SetMode("LOITER", 0);
armstate = 1;
Arm();
ros::Duration(0.5).sleep();
// 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)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else
ROS_ERROR("Failed to call service from flight controller");
break;
}
} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*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 == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/
if (!armstate) {
SetMode("LOITER", 0);
armstate = 1;
Arm();
}
} else if (tmp == buzzuav_closures::COMMAND_DISARM) {
if (armstate) {
armstate = 0;
SetMode("LOITER", 0);
Arm();
}
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
} else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
ROS_INFO("TAKING A PICTURE HERE!! --------------");
cmd_srv.request.param1 = 90;
cmd_srv.request.param2 = 0;
cmd_srv.request.param3 = 0;
cmd_srv.request.param4 = 0;
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");
mavros_msgs::CommandBool capture_command;
capture_srv.call(capture_command);
case buzzuav_closures::COMMAND_GOTO: // NOT FULLY IMPLEMENTED/TESTED !!!
goto_pos = buzzuav_closures::getgoto();
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");
break;
case buzzuav_closures::COMMAND_ARM:
if (!armstate) {
SetMode("LOITER", 0);
armstate = 1;
Arm();
}
break;
case buzzuav_closures::COMMAND_DISARM:
if (armstate) {
armstate = 0;
SetMode("LOITER", 0);
Arm();
}
break;
case buzzuav_closures::COMMAND_MOVETO:
goto_pos = buzzuav_closures::getgoto();
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
break;
case buzzuav_closures::COMMAND_GIMBAL:
gimbal = buzzuav_closures::getgimbal();
cmd_srv.request.param1 = gimbal[0];
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
/---------------------------------------------*/
@ -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
/ 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) {
x = fmod(x,2*M_PI);
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.seq = setpoint_counter++;
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
* (then converted to NED)*/
// target[0]+=y; target[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;
//ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", local_pos_new[0], local_pos_new[1], x, y);
moveMsg.pose.position.x = local_pos_new[0] + y;
moveMsg.pose.position.y = local_pos_new[1] + x;
moveMsg.pose.position.z = z;
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);
/*Extract robot id of the neighbour*/
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*/
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],
cvt_neighbours_pos_payload[1],
@ -1175,7 +1141,7 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
break;
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
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;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;