From 86da07bca81ca278d4c9b9fc5d2b65ca1b9ed1bd Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 13 Apr 2019 16:10:04 +0300 Subject: [PATCH] simple_offboard: param for performing land only in offboard mode --- clever/src/simple_offboard.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/clever/src/simple_offboard.cpp b/clever/src/simple_offboard.cpp index 3af21297..a47b8ba9 100644 --- a/clever/src/simple_offboard.cpp +++ b/clever/src/simple_offboard.cpp @@ -70,6 +70,7 @@ ros::Duration global_position_timeout; ros::Duration battery_timeout; float default_speed; bool auto_release; +bool land_only_in_offboard; std::map reference_frames; // Publishers @@ -644,6 +645,12 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) checkState(); + if (land_only_in_offboard) { + if (state.mode != "OFFBOARD") { + throw std::runtime_error("Copter is not in OFFBOARD mode"); + } + } + static mavros_msgs::SetMode sm; sm.request.custom_mode = "AUTO.LAND"; @@ -688,6 +695,7 @@ int main(int argc, char **argv) nh.param("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link"); nh_priv.param("target_frame", target.child_frame_id, string("navigate_target")); nh_priv.param("auto_release", auto_release, true); + nh_priv.param("land_only_in_offboard", land_only_in_offboard, true); nh_priv.param("default_speed", default_speed, 0.5f); nh_priv.getParam("reference_frames", reference_frames);