minor updates in controller
This commit is contained in:
parent
2e2193fcf9
commit
4b0e18b762
|
@ -148,9 +148,14 @@ private:
|
||||||
|
|
||||||
void Arm();
|
void Arm();
|
||||||
|
|
||||||
void SetMode();
|
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);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,4 +1,5 @@
|
||||||
#include "roscontroller.h"
|
#include "roscontroller.h"
|
||||||
|
#include <thread>
|
||||||
|
|
||||||
namespace rosbzz_node{
|
namespace rosbzz_node{
|
||||||
|
|
||||||
|
@ -236,10 +237,15 @@ namespace rosbzz_node{
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void roscontroller::SetMode(){
|
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;
|
mavros_msgs::SetMode set_mode_message;
|
||||||
set_mode_message.request.base_mode = 0;
|
set_mode_message.request.base_mode = 0;
|
||||||
set_mode_message.request.custom_mode = "GUIDED"; // shit!
|
set_mode_message.request.custom_mode = mode;
|
||||||
if(mode_client.call(set_mode_message)) {
|
if(mode_client.call(set_mode_message)) {
|
||||||
ROS_INFO("Service called!");
|
ROS_INFO("Service called!");
|
||||||
} else {
|
} else {
|
||||||
|
@ -247,6 +253,12 @@ namespace rosbzz_node{
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//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*/
|
/*Prepare messages for each step and publish*/
|
||||||
/*******************************************************************************************************/
|
/*******************************************************************************************************/
|
||||||
|
@ -695,7 +707,9 @@ 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 */
|
||||||
SetMode();
|
SetMode("GUIDED", 0);
|
||||||
|
cout<< "this..."<<endl;
|
||||||
|
SetModeAsync("LAND", 5000);
|
||||||
Arm();
|
Arm();
|
||||||
buzzuav_closures::rc_call(rc_cmd);
|
buzzuav_closures::rc_call(rc_cmd);
|
||||||
res.success = true;
|
res.success = true;
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
|
|
||||||
# We need this for 2D vectors
|
# We need this for 2D vectors
|
||||||
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
|
||||||
include "/home/ubuntu/buzz/src/include/vec2.bzz"
|
include "/home/ivan/dev/buzz/src/include/vec2.bzz"
|
||||||
####################################################################################################
|
####################################################################################################
|
||||||
# Updater related
|
# Updater related
|
||||||
# This should be here for the updater to work, changing position of code will crash the updater
|
# This should be here for the updater to work, changing position of code will crash the updater
|
||||||
|
|
Loading…
Reference in New Issue