diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index a16286a6a4a936..e0cda18e3daac5 100644
--- a/ArduCopter/Copter.cpp
+++ b/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)
{
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 3285f267976e5c..98ce438ae2f0ff 100644
--- a/ArduCopter/Copter.h
+++ b/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;
diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
index fe900d307db9ae..51097c93074fd9 100644
--- a/ArduCopter/mode.cpp
+++ b/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
diff --git a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py
new file mode 100755
index 00000000000000..10f3c43426496c
--- /dev/null
+++ b/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 .
+
+"""
+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()
diff --git a/Tools/ros2/ardupilot_dds_tests/setup.py b/Tools/ros2/ardupilot_dds_tests/setup.py
index bbe11e6a5c8123..a8f2e57fe25424 100644
--- a/Tools/ros2/ardupilot_dds_tests/setup.py
+++ b/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",
],
},
)
diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt
index d066a2d32b2e43..96fa09107575b3 100644
--- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt
+++ b/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
)
diff --git a/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv b/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv
new file mode 100644
index 00000000000000..879cc906fc7d73
--- /dev/null
+++ b/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
diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
index 373d93bc62ecac..4d1abe4e8b79cc 100644
--- a/libraries/AP_DDS/AP_DDS_Client.cpp
+++ b/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;
+ }
}
}
diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h
index 50611276a5539e..353067ab16a1b8 100644
--- a/libraries/AP_DDS/AP_DDS_Service_Table.h
+++ b/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",
+ },
};
diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl
new file mode 100644
index 00000000000000..0d56fd474a649e
--- /dev/null
+++ b/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;
+ };
+ };
+};
diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md
index f08aaf12490d82..be91c64b727eff 100644
--- a/libraries/AP_DDS/README.md
+++ b/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
diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml
index 3dcece3f9d30cd..e7fb9d6b7291a6 100644
--- a/libraries/AP_DDS/dds_xrce_profile.xml
+++ b/libraries/AP_DDS/dds_xrce_profile.xml
@@ -327,6 +327,10 @@
rq/ap/mode_switchRequest
rr/ap/mode_switchReply
+
+ rq/ap/takeoffRequest
+ rr/ap/takeoffReply
+
rt/ap/cmd_gps_pose
ardupilot_msgs::msg::dds_::GlobalPosition_