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

fix modes usage for spiris mavros compatibility

This commit is contained in:
dave 2018-09-23 22:02:05 -04:00
parent 4fa1e7a4ec
commit 4eb044a4a9
3 changed files with 29 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
buzz_scripts/include/act/states.bzz
View File

@ -48,7 +48,7 @@ function launch() {
BVMSTATE = "LAUNCH"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
barrier_ready(22)
} else {
@ -77,7 +77,7 @@ function stop() {
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
neighbors.broadcast("cmd", 21)
uav_land()
if(flight.status != 2 and flight.status != 3) {
if(pose.position.altitude <= 0.1) {
BVMSTATE = "TURNEDOFF"
#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
#barrier_ready(21)

1
include/roscontroller.h
View File

@ -139,6 +139,7 @@ private:
uint8_t no_cnt = 0;
uint8_t old_val = 0;
bool debug = false;
bool setmode = false;
std::string bzzfile_name;
std::string fcclient_name;
std::string armclient;

26
src/roscontroller.cpp
View File

@ -30,6 +30,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
buzz_update::set_bzz_file(bzzfile_name.c_str(),debug);
buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/");
// Initialize variables
if(setmode)
SetMode("LOITER", 0);
armstate = 0;
multi_msg = true;
@ -251,6 +252,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
ROS_ERROR("Provide a debug mode in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain setmode mode from launch file parameter
if (n_c.getParam("setmode", setmode))
;
else
{
ROS_ERROR("Provide a setmode mode in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain rc service option from parameter server
if (n_c.getParam("rcclient", rcclient))
{
@ -403,9 +412,10 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
// Service Clients
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
if(setmode)
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
if (rcclient == true)
if (rcclient)
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
@ -719,6 +729,7 @@ script
cmd_srv.request.command = buzzuav_closures::getcmd();
if (!armstate)
{
if(setmode)
SetMode("LOITER", 0);
armstate = 1;
Arm();
@ -726,7 +737,7 @@ script
// Registering HOME POINT.
home = cur_pos;
}
if (current_mode != "GUIDED")
if (current_mode != "GUIDED" && setmode)
SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo
if (mav_client.call(cmd_srv))
{
@ -738,12 +749,17 @@ script
case NAV_LAND:
cmd_srv.request.command = buzzuav_closures::getcmd();
if (current_mode != "LAND")
if (current_mode != "LAND" && setmode)
{
SetMode("LAND", 0);
armstate = 0;
Arm();
}
else if(cur_pos.altitude < 0.1) //disarm only when close to ground
{
armstate = 0;
Arm();
}
if (mav_client.call(cmd_srv))
{
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
@ -771,6 +787,7 @@ script
case COMPONENT_ARM_DISARM:
if (!armstate)
{
if(setmode)
SetMode("LOITER", 0);
armstate = 1;
Arm();
@ -780,8 +797,9 @@ script
case COMPONENT_ARM_DISARM+1:
if (armstate)
{
armstate = 0;
if(setmode)
SetMode("LOITER", 0);
armstate = 0;
Arm();
}
break;