AP_DDS: Added copter takeoff support

ArduCopter/Copter.cpp

@@ -272,7 +272,8 @@ void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
 
 constexpr int8_t Copter::_failsafe_priorities[7];
 
-#if AP_SCRIPTING_ENABLED
+
+#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 #if MODE_GUIDED_ENABLED == ENABLED
 // start takeoff to given altitude (for use by scripting)
 bool Copter::start_takeoff(float alt)
@@ -289,6 +290,7 @@ bool Copter::start_takeoff(float alt)
     return false;
 }
 
+
 // set target location (for use by scripting)
 bool Copter::set_target_location(const Location& target_loc)
 {

ArduCopter/Copter.h

@@ -666,7 +666,7 @@ class Copter : public AP_Vehicle {
     void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
                              uint8_t &task_count,
                              uint32_t &log_bit) override;
-#if AP_SCRIPTING_ENABLED
+#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 #if MODE_GUIDED_ENABLED == ENABLED
     bool start_takeoff(float alt) override;
     bool set_target_location(const Location& target_loc) override;

ArduCopter/mode.cpp

@@ -248,6 +248,7 @@ bool Copter::gcs_mode_enabled(const Mode::Number mode_num)
 // optional force parameter used to force the flight mode change (used only first time mode is set)
 // returns true if mode was successfully set
 // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
+
 bool Copter::set_mode(Mode::Number mode, ModeReason reason)
 {
     // update last reason

Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py

@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+# Copyright 2023 ArduPilot.org.
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program. If not, see <https://www.gnu.org/licenses/>.
+
+"""
+Run takeoff test on Copter.
+
+Warning - This is NOT production code; it's a simple demo of capability.
+"""
+
+import math
+import rclpy
+import time
+import errno
+
+from rclpy.node import Node
+from builtin_interfaces.msg import Time
+from ardupilot_msgs.msg import GlobalPosition
+from geographic_msgs.msg import GeoPoseStamped
+from geopy import distance
+from geopy import point
+from ardupilot_msgs.srv import ArmMotors
+from ardupilot_msgs.srv import ModeSwitch
+from ardupilot_msgs.srv import Takeoff
+
+
+
+COPTER_MODE_GUIDED = 4
+
+TAKEOFF_ALT = 10
+
+class CopterTakeoff(Node):
+    """Copter takeoff using guided control."""
+
+    def __init__(self):
+        """Initialise the node."""
+        super().__init__("copter_takeoff")
+
+        self.declare_parameter("arm_topic", "/ap/arm_motors")
+        self._arm_topic = self.get_parameter("arm_topic").get_parameter_value().string_value
+        self._client_arm = self.create_client(ArmMotors, self._arm_topic)
+        while not self._client_arm.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('arm service not available, waiting again...')
+
+        self.declare_parameter("mode_topic", "/ap/mode_switch")
+        self._mode_topic = self.get_parameter("mode_topic").get_parameter_value().string_value
+        self._client_mode_switch = self.create_client(ModeSwitch, self._mode_topic)
+        while not self._client_mode_switch.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('mode switch service not available, waiting again...')
+
+        self.declare_parameter("takeoff_service", "/ap/takeoff")
+        self._takeoff_topic = self.get_parameter("takeoff_service").get_parameter_value().string_value
+        self._client_takeoff = self.create_client(Takeoff, self._takeoff_topic)
+        while not self._client_takeoff.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('takeoff service not available, waiting again...')
+
+        self.declare_parameter("geopose_topic", "/ap/geopose/filtered")
+        self._geopose_topic = self.get_parameter("geopose_topic").get_parameter_value().string_value
+        qos = rclpy.qos.QoSProfile(
+            reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, durability=rclpy.qos.DurabilityPolicy.VOLATILE, depth=1
+        )
+
+        self._subscription_geopose = self.create_subscription(GeoPoseStamped, self._geopose_topic, self.geopose_cb, qos)
+        self._cur_geopose = GeoPoseStamped()
+
+    def geopose_cb(self, msg: GeoPoseStamped):
+        """Process a GeoPose message."""
+        stamp = msg.header.stamp
+        if stamp.sec:
+            self.get_logger().info("From AP : Geopose [sec:{}, nsec: {}]".format(stamp.sec, stamp.nanosec))
+
+            # Store current state
+            self._cur_geopose = msg
+
+
+    def arm(self):
+        req = ArmMotors.Request()
+        req.arm = True
+        future = self._client_arm.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def arm_with_timeout(self, timeout: rclpy.duration.Duration):
+        """Try to arm. Returns true on success, or false if arming fails or times out."""
+        armed = False
+        start = self.get_clock().now()
+        while not armed and self.get_clock().now() - start < timeout:
+            armed = self.arm().result
+            time.sleep(1)
+        return armed
+
+    def switch_mode(self, mode):
+        req = ModeSwitch.Request()
+        assert mode in [COPTER_MODE_GUIDED]
+        req.mode = mode
+        future = self._client_mode_switch.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Duration):
+        """Try to switch mode. Returns true on success, or false if mode switch fails or times out."""
+        is_in_desired_mode = False
+        start = self.get_clock().now()
+        while not is_in_desired_mode:
+            result = self.switch_mode(desired_mode)
+            # Handle successful switch or the case that the vehicle is already in expected mode
+            is_in_desired_mode = result.status or result.curr_mode == desired_mode
+            time.sleep(1)
+
+        return is_in_desired_mode
+
+    def takeoff(self, alt):
+        req = Takeoff.Request()
+        req.alt = alt
+        future = self._client_takeoff.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def takeoff_with_timeout(self, alt: int, timeout: rclpy.duration.Duration):
+        """Try to takeoff. Returns true on success, or false if takeoff fails or times out."""
+        takeoff_success = False
+        start = self.get_clock().now()
+        while not takeoff_success:
+            result = self.takeoff(alt)
+            takeoff_success = result.status
+            time.sleep(1)
+
+        return takeoff_success
+
+    def get_cur_geopose(self):
+        """Return latest geopose."""
+        return self._cur_geopose
+
+def main(args=None):
+    """Node entry point."""
+    rclpy.init(args=args)
+    node = CopterTakeoff()
+    try:
+        if not node.switch_mode_with_timeout(COPTER_MODE_GUIDED, rclpy.duration.Duration(seconds=20)):
+            raise RuntimeError("Unable to switch to guided mode")
+        # Block till armed, which will wait for EKF3 to initialize
+        if not node.arm_with_timeout(rclpy.duration.Duration(seconds=30)):
+            raise RuntimeError("Unable to arm")
+
+        # Block till in takeoff
+        if not node.takeoff_with_timeout(TAKEOFF_ALT, rclpy.duration.Duration(seconds=20)):
+            raise RuntimeError("Unable to takeoff")
+
+        is_ascending_to_takeoff_alt = True
+        while is_ascending_to_takeoff_alt:
+            rclpy.spin_once(node)
+            time.sleep(1.0)
+
+            is_ascending_to_takeoff_alt = node.get_cur_geopose().pose.position.altitude < TAKEOFF_ALT
+
+        if is_ascending_to_takeoff_alt:
+            raise RuntimeError("Failed to reach takeoff altitude")
+
+    except KeyboardInterrupt:
+        pass
+
+    # Destroy the node explicitly.
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

Tools/ros2/ardupilot_dds_tests/setup.py

@@ -26,6 +26,7 @@
         'console_scripts': [
             "time_listener = ardupilot_dds_tests.time_listener:main",
             "plane_waypoint_follower = ardupilot_dds_tests.plane_waypoint_follower:main",
+            "copter_takeoff = ardupilot_dds_tests.copter_takeoff:main",
         ],
     },
 )

Tools/ros2/ardupilot_msgs/CMakeLists.txt

@@ -16,6 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/GlobalPosition.msg"
   "srv/ArmMotors.srv"
   "srv/ModeSwitch.srv"
+  "srv/Takeoff.srv"
   DEPENDENCIES geometry_msgs std_msgs
   ADD_LINTER_TESTS
 )

Tools/ros2/ardupilot_msgs/srv/Takeoff.srv

@@ -0,0 +1,10 @@
+
+# This service requests the vehicle to takeoff
+
+# alt : Set the takeoff altitude above home or above terrain(in case of rangefinder)
+
+uint8 alt
+---
+# status    : True if the request for mode switch was successful, False otherwise
+
+bool status

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -17,6 +17,7 @@
 
 #include "ardupilot_msgs/srv/ArmMotors.h"
 #include "ardupilot_msgs/srv/ModeSwitch.h"
+#include "ardupilot_msgs/srv/Takeoff.h"
 
 #if AP_EXTERNAL_CONTROL_ENABLED
 #include "AP_DDS_ExternalControl.h"
@@ -652,6 +653,33 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Mode Switch : %s", msg_prefix, mode_switch_response.status ? "SUCCESS" : "FAIL");
         break;
     }
+    case services[to_underlying(ServiceIndex::TAKEOFF)].rep_id: {
+        ardupilot_msgs_srv_Takeoff_Request takeoff_request;
+        ardupilot_msgs_srv_Takeoff_Response takeoff_response;
+        const bool deserialize_success = ardupilot_msgs_srv_Takeoff_Request_deserialize_topic(ub, &takeoff_request);
+        if (deserialize_success == false) {
+            break;
+        }
+        takeoff_response.status = AP::vehicle()->start_takeoff(takeoff_request.alt);
+
+        const uxrObjectId replier_id = {
+            .id = services[to_underlying(ServiceIndex::TAKEOFF)].rep_id,
+            .type = UXR_REPLIER_ID
+        };
+
+        uint8_t reply_buffer[8] {};
+        ucdrBuffer reply_ub;
+
+        ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
+        const bool serialize_success = ardupilot_msgs_srv_Takeoff_Response_serialize_topic(&reply_ub, &takeoff_response);
+        if (serialize_success == false) {
+            break;
+        }
+
+        uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Takeoff : %s", msg_prefix, takeoff_response.status ? "SUCCESS" : "FAIL");
+        break;
+    }
     }
 }
 

libraries/AP_DDS/AP_DDS_Service_Table.h

@@ -2,7 +2,8 @@
 
 enum class ServiceIndex: uint8_t {
     ARMING_MOTORS,
-    MODE_SWITCH
+    MODE_SWITCH,
+    TAKEOFF
 };
 
 static inline constexpr uint8_t to_underlying(const ServiceIndex index)
@@ -24,4 +25,10 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
         .req_profile_label = "",
         .rep_profile_label = "mode_switch__replier",
     },
+    {
+        .req_id = to_underlying(ServiceIndex::TAKEOFF),
+        .rep_id = to_underlying(ServiceIndex::TAKEOFF),
+        .req_profile_label = "",
+        .rep_profile_label = "takeoff__replier",
+    },
 };

libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl

@@ -0,0 +1,20 @@
+// generated from rosidl_adapter/resource/srv.idl.em
+// with input from ardupilot_msgs/srv/Takeoff.srv
+// generated code does not contain a copyright notice
+
+
+module ardupilot_msgs {
+  module srv {
+    struct Takeoff_Request {
+      @verbatim (language="comment", text=
+        "This service requests the vehicle to takeoff" "\n"
+        "alt : Set the takeoff altitude")
+      uint8 alt;
+    };
+    @verbatim (language="comment", text=
+      "status    : True if the request for takeoff was successful, False otherwise")
+    struct Takeoff_Response {
+      boolean status;
+    };
+  };
+};

libraries/AP_DDS/README.md

@@ -240,6 +240,7 @@ nanosec: 729410000
 $ ros2 service list
 /ap/arm_motors
 /ap/mode_switch
+/ap/takeoff
 ---
 ```
 
@@ -265,6 +266,7 @@ List the available services:
 $ ros2 service list -t
 /ap/arm_motors [ardupilot_msgs/srv/ArmMotors]
 /ap/mode_switch [ardupilot_msgs/srv/ModeSwitch]
+/ap/takeoff [ardupilot_msgs/srv/Takeoff]
 ```
 
 Call the arm motors service:
@@ -286,6 +288,16 @@ requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=4)
 response:
 ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4)
 ```
+
+Call the takeoff service:
+
+```bash
+$ ros2 service call /ap/takeoff ardupilot_msgs/srv/takeoff "{alt: 10}"
+requester: making request: ardupilot_msgs.srv.Takeoff_Request(alt=10)
+
+response:
+ardupilot_msgs.srv.Takeoff_Response(status=True)
+```
 
 ## Contributing to `AP_DDS` library
 

libraries/AP_DDS/dds_xrce_profile.xml

@@ -327,6 +327,10 @@
     <request_topic_name>rq/ap/mode_switchRequest</request_topic_name>
     <reply_topic_name>rr/ap/mode_switchReply</reply_topic_name>
   </replier>
+  <replier profile_name="takeoff__replier" service_name="rs/ap/takeoffService" request_type="ardupilot_msgs::srv::dds_::Takeoff_Request_" reply_type="ardupilot_msgs::srv::dds_::Takeoff_Response_">
+    <request_topic_name>rq/ap/takeoffRequest</request_topic_name>
+    <reply_topic_name>rr/ap/takeoffReply</reply_topic_name>
+  </replier>
   <topic profile_name="globalposcontrol__t">
     <name>rt/ap/cmd_gps_pose</name>
     <dataType>ardupilot_msgs::msg::dds_::GlobalPosition_</dataType>