local takeoff and land topics

mavros/src/plugins/command.cpp

@@ -30,6 +30,7 @@
 #include "mavros_msgs/srv/command_bool.hpp"
 #include "mavros_msgs/srv/command_home.hpp"
 #include "mavros_msgs/srv/command_tol.hpp"
+#include "mavros_msgs/srv/command_tol_local.hpp"
 #include "mavros_msgs/srv/command_trigger_control.hpp"
 #include "mavros_msgs/srv/command_trigger_interval.hpp"
 #include "mavros_msgs/srv/command_vtol_transition.hpp"
@@ -115,11 +116,22 @@ class CommandPlugin : public plugin::Plugin
       std::bind(
         &CommandPlugin::takeoff_cb, this, _1, _2,
         _3), rmw_qos_profile_services_default, srv_cg);
+    takeoff_local_srv =
+      node->create_service<mavros_msgs::srv::CommandTOLLocal>(
+      "~/takeoff_local",
+      std::bind(
+        &CommandPlugin::takeoff_local_cb, this, _1, _2,
+        _3), rmw_qos_profile_services_default, srv_cg);
     land_srv =
       node->create_service<mavros_msgs::srv::CommandTOL>(
       "~/land",
       std::bind(&CommandPlugin::land_cb, this, _1, _2, _3), rmw_qos_profile_services_default,
       srv_cg);
+    land_local_srv = 
+      node->create_service<mavros_msgs::srv::CommandTOLLocal>(
+      "~/land_local",
+      std::bind(&CommandPlugin::land_local_cb, this, _1, _2, _3), rmw_qos_profile_services_default,
+      srv_cg);
     trigger_control_srv = node->create_service<mavros_msgs::srv::CommandTriggerControl>(
       "~/trigger_control", std::bind(
         &CommandPlugin::trigger_control_cb, this, _1, _2,
@@ -152,7 +164,9 @@ class CommandPlugin : public plugin::Plugin
   rclcpp::Service<mavros_msgs::srv::CommandBool>::SharedPtr arming_srv;
   rclcpp::Service<mavros_msgs::srv::CommandHome>::SharedPtr set_home_srv;
   rclcpp::Service<mavros_msgs::srv::CommandTOL>::SharedPtr takeoff_srv;
+  rclcpp::Service<mavros_msgs::srv::CommandTOLLocal>::SharedPtr takeoff_local_srv;
   rclcpp::Service<mavros_msgs::srv::CommandTOL>::SharedPtr land_srv;
+  rclcpp::Service<mavros_msgs::srv::CommandTOLLocal>::SharedPtr land_local_srv;
   rclcpp::Service<mavros_msgs::srv::CommandTriggerControl>::SharedPtr trigger_control_srv;
   rclcpp::Service<mavros_msgs::srv::CommandTriggerInterval>::SharedPtr trigger_interval_srv;
   rclcpp::Service<mavros_msgs::srv::CommandVtolTransition>::SharedPtr vtol_transition_srv;
@@ -434,6 +448,23 @@ class CommandPlugin : public plugin::Plugin
       res->success, res->result);
   }
 
+  void takeoff_local_cb(
+    const std::shared_ptr<rmw_request_id_t> req_header [[maybe_unused]],
+    const mavros_msgs::srv::CommandTOLLocal::Request::SharedPtr req,
+    mavros_msgs::srv::CommandTOLLocal::Response::SharedPtr res)
+  {
+    using mavlink::common::MAV_CMD;
+    send_command_long_and_wait(
+      false,
+      enum_value(MAV_CMD::NAV_TAKEOFF_LOCAL), 1,
+      req->min_pitch,
+      0,
+      req->rate,
+      req->yaw,
+      req->position.y, req->position.x, req->position.z,
+      res->success, res->result);
+  }
+
   void land_cb(
     const std::shared_ptr<rmw_request_id_t> req_header [[maybe_unused]],
     const mavros_msgs::srv::CommandTOL::Request::SharedPtr req,
@@ -449,6 +480,23 @@ class CommandPlugin : public plugin::Plugin
       res->success, res->result);
   }
 
+  void land_local_cb(
+    const std::shared_ptr<rmw_request_id_t> req_header [[maybe_unused]],
+    const mavros_msgs::srv::CommandTOLLocal::Request::SharedPtr req,
+    mavros_msgs::srv::CommandTOLLocal::Response::SharedPtr res)
+  {
+    using mavlink::common::MAV_CMD;
+    send_command_long_and_wait(
+      false,
+      enum_value(MAV_CMD::NAV_LAND_LOCAL), 1,
+      0, //landing target number (what does it means?)
+      req->offset,
+      req->rate,
+      req->yaw,
+      req->position.y, req->position.x, req->position.z,
+      res->success, res->result);
+  }
+
   void trigger_control_cb(
     const std::shared_ptr<rmw_request_id_t> req_header [[maybe_unused]],
     const mavros_msgs::srv::CommandTriggerControl::Request::SharedPtr req,

mavros_msgs/CMakeLists.txt

@@ -108,6 +108,7 @@ set(srv_files
   srv/CommandHome.srv
   srv/CommandInt.srv
   srv/CommandLong.srv
+  srv/CommandTOLLocal.srv
   srv/CommandTOL.srv
   srv/CommandTriggerControl.srv
   srv/CommandTriggerInterval.srv

mavros_msgs/srv/CommandTOLLocal.srv

@@ -0,0 +1,10 @@
+#Common type for LOCAL Take Off and Landing
+
+float32 min_pitch		# used by takeoff
+float32 offset    		# used by land (landing position accuracy)
+float32 rate			# speed of takeoff/land in m/s
+float32 yaw			# in radians
+geometry_msgs/Vector3 position 	#(x,y,z) in meters
+---
+bool success
+uint8 result