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

Ready for final test :feelsgood: -- SOLO

Browse Source
This commit is contained in:
isvogor 2017-02-23 21:10:49 -05:00
parent 12ca11b275
commit 5f4aaeb1e0
4 changed files with 605 additions and 622 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
include/roscontroller.h
View File

@ -11,9 +11,9 @@
#include "mavros_msgs/State.h" #include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h" #include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/Mavlink.h" #include "mavros_msgs/Mavlink.h"
#include "sensor_msgs/NavSatStatus.h"
#include "mavros_msgs/WaypointPush.h" #include "mavros_msgs/WaypointPush.h"
#include "mavros_msgs/Waypoint.h" #include "mavros_msgs/Waypoint.h"
#include "sensor_msgs/NavSatStatus.h"
#include <sensor_msgs/LaserScan.h> #include <sensor_msgs/LaserScan.h>
#include <rosbuzz/neigh_pos.h> #include <rosbuzz/neigh_pos.h>
#include <sstream> #include <sstream>
@ -153,16 +153,15 @@ private:
void Arm(); void Arm();
void SetMode();
void SetMode(std::string mode, int delay_miliseconds); void SetMode(std::string mode, int delay_miliseconds);
void SetModeAsync(std::string mode, int delay_miliseconds);
//void SetModeAsync(std::string mode, int delay);
void Subscribe(ros::NodeHandle n_c); void Subscribe(ros::NodeHandle n_c);
void WaypointMissionSetup(float lat, float lng, float alg); void WaypointMissionSetup(float lat, float lng, float alt);
void fc_command_setup();
}; };
} }

3
launch/rosbuzz-testing.launch
View File

@ -26,12 +26,11 @@
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" /> <param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" /> <param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="No_of_dev" type="int" value="3" /> <param name="No_of_dev" type="int" value="3" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<!-- run rosbuzz --> <!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" > <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/> <rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testalone.bzz" /> <param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testflockfev.bzz" />
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />
<param name="rcservice_name" value="/buzzcmd" /> <param name="rcservice_name" value="/buzzcmd" />
<param name="in_payload" value="/inMavlink"/> <param name="in_payload" value="/inMavlink"/>

87
src/buzz_utility.cpp
View File

@ -29,7 +29,10 @@ void neighbour_pos_callback(std::map<int, Pos_struct> neighbours_pos_map) {
/* Get robot id and update neighbor information */ /* Get robot id and update neighbor information */
map< int, Pos_struct >::iterator it; map< int, Pos_struct >::iterator it;
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){ for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
buzzneighbors_add(VM, it->first, (it->second).x, (it->second).y, buzzneighbors_add(VM,
it->first,
(it->second).x,
(it->second).y,
(it->second).z); (it->second).z);
} }
} }
@ -155,81 +158,17 @@ uint64_t* out_msg_process() {
/* Send messages from FIFO */ /* Send messages from FIFO */
do { do {
/* Are there more messages? */ /* Are there more messages? */
if (buzzoutmsg_queue_isempty(VM)) if(buzzoutmsg_queue_isempty(VM)) break;
break;
/* Get first message */ /* Get first message */
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */ /* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */
if (tot + buzzmsg_payload_size(m) + sizeof(uint16_t) > MSG_SIZE) { if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
>
MSG_SIZE) {
buzzmsg_payload_destroy(&m); buzzmsg_payload_destroy(&m);
break; break;
} }
<<<<<<< HEAD
/****************************************/
/*Buzz hooks that can be used inside .bzz file*/
/****************************************/
static int buzz_register_hooks() {
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
buzzvm_gstore(VM);
return BUZZVM_STATE_READY;
}
/**************************************************/
/*Register dummy Buzz hooks for test during update*/
/**************************************************/
static int testing_buzz_register_hooks() {
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
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_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
return BUZZVM_STATE_READY;
}
=======
/* Add message length to data buffer */ /* Add message length to data buffer */
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m); *(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
@ -237,7 +176,6 @@ uint64_t* out_msg_process() {
/* Add payload to data buffer */ /* Add payload to data buffer */
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m)); memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
tot += buzzmsg_payload_size(m); tot += buzzmsg_payload_size(m);
>>>>>>> 4ac9d89f7c5e82fe99a48f616c587efd01d50ddd
/* Get rid of message */ /* Get rid of message */
buzzoutmsg_queue_next(VM); buzzoutmsg_queue_next(VM);
@ -301,6 +239,9 @@ buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
buzzvm_gstore(VM); buzzvm_gstore(VM);
@ -330,6 +271,9 @@ buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 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_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 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);
@ -342,6 +286,7 @@ buzzvm_gstore(VM);
return BUZZVM_STATE_READY; return BUZZVM_STATE_READY;
} }
/****************************************/ /****************************************/
/*Sets the .bzz and .bdbg file*/ /*Sets the .bzz and .bdbg file*/
/****************************************/ /****************************************/
@ -602,6 +547,7 @@ if(VM) {
fprintf(stdout, "Script execution stopped.\n"); fprintf(stdout, "Script execution stopped.\n");
} }
/****************************************/ /****************************************/
/****************************************/ /****************************************/
@ -646,3 +592,4 @@ return VM;
} }

162
src/roscontroller.cpp
View File

@ -1,6 +1,7 @@
#include "roscontroller.h" #include "roscontroller.h"
#include <thread> #include <thread>
namespace rosbzz_node{ namespace rosbzz_node{
/***Constructor***/ /***Constructor***/
@ -76,7 +77,6 @@ namespace rosbzz_node{
/*sleep for the mentioned loop rate*/ /*sleep for the mentioned loop rate*/
timer_step+=1; timer_step+=1;
maintain_pos(timer_step); maintain_pos(timer_step);
} }
/* Destroy updater and Cleanup */ /* Destroy updater and Cleanup */
//update_routine(bcfname.c_str(), dbgfname.c_str(),1); //update_routine(bcfname.c_str(), dbgfname.c_str(),1);
@ -162,11 +162,13 @@ namespace rosbzz_node{
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000); neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
/* Service Clients*/ /* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient); arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mission_client = n_c.serviceClient<mavros_msgs::WaypointPush>("/mavros/mission/push");
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient); mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name); mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
mission_client = n_c.serviceClient<mavros_msgs::WaypointPush>("/mavros/mission/push");
multi_msg=true; multi_msg=true;
armstate = 0;
} }
void roscontroller::Subscribe(ros::NodeHandle n_c){ void roscontroller::Subscribe(ros::NodeHandle n_c){
@ -226,6 +228,18 @@ namespace rosbzz_node{
} }
} }
void roscontroller::SetMode(){
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = "GUIDED"; // shit!
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
}
void roscontroller::WaypointMissionSetup(float lat, float lng, float alt){ void roscontroller::WaypointMissionSetup(float lat, float lng, float alt){
mavros_msgs::WaypointPush srv; mavros_msgs::WaypointPush srv;
mavros_msgs::Waypoint waypoint; mavros_msgs::Waypoint waypoint;
@ -241,56 +255,29 @@ namespace rosbzz_node{
srv.request.waypoints.push_back(waypoint); srv.request.waypoints.push_back(waypoint);
if(mission_client.call(srv)){ if(mission_client.call(srv)){
ROS_INFO("Mission service called!"); ROS_INFO("Waypoint setup service called!");
} else { } else {
ROS_INFO("Mission service failed!"); ROS_INFO("Waypoint setup service failed!");
} }
} }
void roscontroller::SetMode(std::string mode, int delay_miliseconds){ void roscontroller::fc_command_setup(){
// wait if necessary
if (delay_miliseconds > 0){
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
}
// set mode
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = mode;
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
}
//todo: this
void roscontroller::SetModeAsync(std::string mode, int delay_miliseconds){
std::thread([&](){SetMode(mode, delay_miliseconds);}).detach();
cout << "Async call called... " <<endl;
}
/*Prepare messages for each step and publish*/
/*******************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _________________________________________________________________________________________________ */
/*| | | | | | */
/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */
/*|_____|_____|_____|________________________________________________|______________________________| */
/*******************************************************************************************************/
void roscontroller::prepare_msg_and_publish(){
/* flight controller client call if requested from Buzz*/ /* flight controller client call if requested from Buzz*/
/*FC call for takeoff,land and gohome*/ /*FC call for takeoff,land and gohome*/
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){
cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.param7 = goto_pos[2];
//cmd_srv.request.z = goto_pos[2]; //cmd_srv.request.z = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } if (mav_client.call(cmd_srv)){
else ROS_ERROR("Failed to call service from flight controller"); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
}
else
ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == 2) { /*FC call for goto*/ } else if (tmp == 2) { /*FC call for goto*/
/*prepare the goto publish message */ /*prepare the goto publish message */
cmd_srv.request.param5 = goto_pos[0]; cmd_srv.request.param5 = goto_pos[0];
@ -305,6 +292,60 @@ namespace rosbzz_node{
} else if (tmp == 3) { /*FC call for arm*/ } else if (tmp == 3) { /*FC call for arm*/
Arm(); Arm();
} }
}
// TODO: split this
/*Prepare messages for each step and publish*/
/*******************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _________________________________________________________________________________________________ */
/*| | | | | | */
/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */
/*|_____|_____|_____|________________________________________________|______________________________| */
/*******************************************************************************************************/
void roscontroller::prepare_msg_and_publish(){
if(fcclient_name == "/mavros/cmd/command"){
int tmp = buzzuav_closures::bzz_cmd();
double* goto_pos = buzzuav_closures::getgoto();
switch(tmp){
// TAKEOFF -- LAND
case 1:
if(!armstate){
SetMode("LOITER", 0);
Arm();
armstate = 1;
SetMode("GUIDED", 2000);
}
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
// just to be safe -- while landing!
if(armstate == 1 && buzzuav_closures::getcmd() == 22){
SetMode("GUIDED", 0);
}
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;
// GOTO
case 2:
WaypointMissionSetup(goto_pos[0], goto_pos[1], goto_pos[2]);
break;
// ARM
case 3:
Arm();
break;
}
} else {
fc_command_setup();
}
/*obtain Pay load to be sent*/ /*obtain Pay load to be sent*/
//fprintf(stdout, "before getting msg from utility\n"); //fprintf(stdout, "before getting msg from utility\n");
uint64_t* payload_out_ptr= buzz_utility::out_msg_process(); uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
@ -601,7 +642,7 @@ namespace rosbzz_node{
buzzuav_closures::flight_status_update(1); buzzuav_closures::flight_status_update(1);
else if (msg->mode == "LAND") else if (msg->mode == "LAND")
buzzuav_closures::flight_status_update(4); buzzuav_closures::flight_status_update(4);
else else // ground standby = LOITER?
buzzuav_closures::flight_status_update(1);//? buzzuav_closures::flight_status_update(1);//?
} }
@ -708,6 +749,22 @@ namespace rosbzz_node{
} }
void roscontroller::SetMode(std::string mode, int delay_miliseconds){
// wait if necessary
if (delay_miliseconds > 0){
std::this_thread::sleep_for( std::chrono::milliseconds ( delay_miliseconds ) );
}
// set mode
mavros_msgs::SetMode set_mode_message;
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = mode;
if(mode_client.call(set_mode_message)) {
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Set Mode Service call failed!");
}
}
/* RC command service */ /* RC command service */
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){
@ -720,19 +777,14 @@ namespace rosbzz_node{
ROS_INFO("RC_call: TAKE OFF!!!!"); ROS_INFO("RC_call: TAKE OFF!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
/* arming */ /* arming */
<<<<<<< HEAD
SetMode(); SetMode();
cout << "ARM: " << armstate <<endl;
if(!armstate) { if(!armstate) {
armstate = 1; armstate = 1;
Arm();
}
=======
SetMode("LOITER", 0); SetMode("LOITER", 0);
//SetMode("GUIDED", 0);
cout << "this..." << endl;
SetModeAsync("GUIDED", 2000);
Arm(); Arm();
>>>>>>> 4ac9d89f7c5e82fe99a48f616c587efd01d50ddd if(fcclient_name == "/mavros/cmd/command") { SetMode("LOITER", 2000); }
}
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
break; break;
@ -759,25 +811,12 @@ namespace rosbzz_node{
res.success = true; res.success = true;
break; break;
case mavros_msgs::CommandCode::NAV_WAYPOINT: case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! ");
//WaypointMissionSetup();
double rc_goto[3]; double rc_goto[3];
rc_goto[0] = req.param5; rc_goto[0] = req.param5;
rc_goto[1] = req.param6; rc_goto[1] = req.param6;
rc_goto[2] = req.param7; rc_goto[2] = req.param7;
WaypointMissionSetup(req.param5, req.param6, req.param7); WaypointMissionSetup(req.param5, req.param6, req.param7);
/*
WaypointMissionSetup(45.505293f, -73.614722f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
WaypointMissionSetup(45.505324f, -73.614502f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
WaypointMissionSetup(45.505452f, -73.614655f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
WaypointMissionSetup(45.505463f, -73.614389f, 2.0f);
std::this_thread::sleep_for( std::chrono::milliseconds ( 100 ) );
*/
buzzuav_closures::rc_set_goto(rc_goto); buzzuav_closures::rc_set_goto(rc_goto);
rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
@ -797,4 +836,3 @@ namespace rosbzz_node{
} }