diff --git a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/pre_arm_check_service.py b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/pre_arm_check_service.py
new file mode 100644
index 0000000000000..e835209015fa4
--- /dev/null
+++ b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/pre_arm_check_service.py
@@ -0,0 +1,78 @@
+#!/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 pre_arm check 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 std_srvs.srv import Trigger
+
+
+class CopterPreArm(Node):
+ """Check Prearm Service."""
+
+ def __init__(self):
+ """Initialise the node."""
+ super().__init__("copter_prearm")
+
+ self.declare_parameter("pre_arm_service", "/ap/prearm_check")
+ self._prearm_service = self.get_parameter("pre_arm_service").get_parameter_value().string_value
+ self._client_prearm = self.create_client(Trigger, self._prearm_service)
+ while not self._client_prearm.wait_for_service(timeout_sec=1.0):
+ self.get_logger().info('prearm service not available, waiting again...')
+
+ def prearm(self):
+ req = Trigger.Request()
+ future = self._client_prearm.call_async(req)
+ rclpy.spin_until_future_complete(self, future)
+ return future.result()
+
+ def prearm_with_timeout(self, timeout: rclpy.duration.Duration):
+ """Check if armable. Returns true on success, or false if arming will fail or times out."""
+ armable = False
+ start = self.get_clock().now()
+ while not armable and self.get_clock().now() - start < timeout:
+ armable = self.prearm().success
+ time.sleep(1)
+ return armable
+
+def main(args=None):
+ """Node entry point."""
+ rclpy.init(args=args)
+ node = CopterPreArm()
+ try:
+ # Block till armed, which will wait for EKF3 to initialize
+ if not node.prearm_with_timeout(rclpy.duration.Duration(seconds=30)):
+ raise RuntimeError("Vehicle not armable")
+ 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 bbe11e6a5c812..c1a7fc1d4557f 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",
+ "pre_arm_check = ardupilot_dds_tests.pre_arm_check_service:main",
],
},
)
diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_prearm_service.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_prearm_service.py
new file mode 100644
index 0000000000000..f081d5cc1cfe6
--- /dev/null
+++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_prearm_service.py
@@ -0,0 +1,166 @@
+# 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 .
+
+"""
+Bring up ArduPilot SITL and check whether the vehicle can be armed.
+
+colcon test --executor sequential --parallel-workers 0 --base-paths src/ardupilot \
+ --event-handlers=console_cohesion+ --packages-select ardupilot_dds_tests \
+ --pytest-args -k test_prearm_service
+
+"""
+
+import launch_pytest
+import math
+import time
+import pytest
+import rclpy
+import rclpy.node
+import threading
+
+from launch import LaunchDescription
+
+from launch_pytest.tools import process as process_tools
+from std_srvs.srv import Trigger
+
+
+SERVICE = "/ap/prearm_check"
+
+class PreamService(rclpy.node.Node):
+ def __init__(self):
+ """Initialise the node."""
+ super().__init__("prearm_client")
+ self.service_available_object = threading.Event()
+ self.is_armable_object = threading.Event()
+ self._client_prearm = self.create_client(Trigger, "/ap/prearm_check")
+
+ def start_node(self):
+ # Add a spin thread.
+ self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,))
+ self.ros_spin_thread.start()
+
+ def prearm_check(self):
+ req = Trigger.Request()
+ future = self._client_prearm.call_async(req)
+ time.sleep(0.2)
+ try:
+ return future.result().success
+ except Exception as e:
+ print(e)
+ return False
+
+ def prearm_with_timeout(self, timeout: rclpy.duration.Duration):
+ result = False
+ start = self.get_clock().now()
+ while not result and self.get_clock().now() - start < timeout:
+ result = self.prearm_check()
+ time.sleep(1)
+ return result
+
+ def process_prearm(self):
+ if self.prearm_with_timeout(rclpy.duration.Duration(seconds=30)):
+ self.is_armable_object.set()
+
+ def start_prearm(self):
+ try:
+ self.prearm_thread.stop()
+ except:
+ print("start_prearm not started yet")
+ self.prearm_thread = threading.Thread(target=self.process_prearm)
+ self.prearm_thread.start()
+
+
+
+
+@launch_pytest.fixture
+def launch_sitl_copter_dds_serial(sitl_copter_dds_serial):
+ """Fixture to create the launch description."""
+ sitl_ld, sitl_actions = sitl_copter_dds_serial
+
+ ld = LaunchDescription(
+ [
+ sitl_ld,
+ launch_pytest.actions.ReadyToTest(),
+ ]
+ )
+ actions = sitl_actions
+ yield ld, actions
+
+
+@launch_pytest.fixture
+def launch_sitl_copter_dds_udp(sitl_copter_dds_udp):
+ """Fixture to create the launch description."""
+ sitl_ld, sitl_actions = sitl_copter_dds_udp
+
+ ld = LaunchDescription(
+ [
+ sitl_ld,
+ launch_pytest.actions.ReadyToTest(),
+ ]
+ )
+ actions = sitl_actions
+ yield ld, actions
+
+
+@pytest.mark.launch(fixture=launch_sitl_copter_dds_serial)
+def test_dds_serial_prearm_service_call(launch_context, launch_sitl_copter_dds_serial):
+ """Test prearm service AP_DDS."""
+ _, actions = launch_sitl_copter_dds_serial
+ virtual_ports = actions["virtual_ports"].action
+ micro_ros_agent = actions["micro_ros_agent"].action
+ mavproxy = actions["mavproxy"].action
+ sitl = actions["sitl"].action
+
+ # Wait for process to start.
+ process_tools.wait_for_start_sync(launch_context, virtual_ports, timeout=2)
+ process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2)
+ process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2)
+ process_tools.wait_for_start_sync(launch_context, sitl, timeout=2)
+
+ rclpy.init()
+ try:
+ node = PreamService()
+ node.start_node()
+ node.start_prearm()
+ is_armable_flag = node.is_armable_object.wait(timeout=25.0)
+ assert is_armable_flag, f"Vehicle not armable."
+ finally:
+ rclpy.shutdown()
+ yield
+
+
+@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp)
+def test_dds_udp_prearm_service_call(launch_context, launch_sitl_copter_dds_udp):
+ """Test prearm service AP_DDS."""
+ _, actions = launch_sitl_copter_dds_udp
+ micro_ros_agent = actions["micro_ros_agent"].action
+ mavproxy = actions["mavproxy"].action
+ sitl = actions["sitl"].action
+
+ # Wait for process to start.
+ process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2)
+ process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2)
+ process_tools.wait_for_start_sync(launch_context, sitl, timeout=2)
+
+ rclpy.init()
+ try:
+ node = PreamService()
+ node.start_node()
+ node.start_prearm()
+ is_armable_flag = node.is_armable_object.wait(timeout=25.0)
+ assert is_armable_flag, f"Vehicle not armable."
+ finally:
+ rclpy.shutdown()
+ yield
diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
index a39240143fb55..e1a1ccc83c397 100644
--- a/libraries/AP_DDS/AP_DDS_Client.cpp
+++ b/libraries/AP_DDS/AP_DDS_Client.cpp
@@ -26,6 +26,9 @@
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
#include "ardupilot_msgs/srv/ModeSwitch.h"
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+#include "std_srvs/srv/Trigger.h"
+#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
#if AP_EXTERNAL_CONTROL_ENABLED
#include "AP_DDS_ExternalControl.h"
@@ -868,6 +871,35 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
break;
}
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+ case services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id: {
+ std_srvs_srv_Trigger_Request prearm_check_request;
+ std_srvs_srv_Trigger_Response prearm_check_response;
+ const bool deserialize_success = std_srvs_srv_Trigger_Request_deserialize_topic(ub, &prearm_check_request);
+ if (deserialize_success == false) {
+ break;
+ }
+ prearm_check_response.success = AP::arming().pre_arm_checks(false);
+ strcpy(prearm_check_response.message, prearm_check_response.success ? "Vehicle is Armable" : "Vehicle is Not Armable");
+
+ const uxrObjectId replier_id = {
+ .id = services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id,
+ .type = UXR_REPLIER_ID
+ };
+
+ uint8_t reply_buffer[sizeof(prearm_check_response.message) + 1] {};
+ ucdrBuffer reply_ub;
+
+ ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
+ const bool serialize_success = std_srvs_srv_Trigger_Response_serialize_topic(&reply_ub, &prearm_check_response);
+ if (serialize_success == false) {
+ break;
+ }
+
+ uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
+ break;
+ }
+#endif //AP_DDS_ARM_CHECK_SERVER_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: {
const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request);
diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h
index 667149d6aa756..ba3adc6f07e18 100644
--- a/libraries/AP_DDS/AP_DDS_Service_Table.h
+++ b/libraries/AP_DDS/AP_DDS_Service_Table.h
@@ -8,6 +8,9 @@ enum class ServiceIndex: uint8_t {
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
MODE_SWITCH,
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+ PREARM_CHECK,
+#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
SET_PARAMETERS,
GET_PARAMETERS
@@ -51,6 +54,18 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
.reply_topic_name = "rr/ap/mode_switchReply",
},
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+ {
+ .req_id = to_underlying(ServiceIndex::PREARM_CHECK),
+ .rep_id = to_underlying(ServiceIndex::PREARM_CHECK),
+ .service_rr = Service_rr::Replier,
+ .service_name = "rs/ap/prearm_checkService",
+ .request_type = "std_srvs::srv::dds_::Trigger_Request_",
+ .reply_type = "std_srvs::srv::dds_::Trigger_Response_",
+ .request_topic_name = "rq/ap/prearm_checkRequest",
+ .reply_topic_name = "rr/ap/prearm_checkReply",
+ },
+#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
#if AP_DDS_PARAMETER_SERVER_ENABLED
{
.req_id = to_underlying(ServiceIndex::SET_PARAMETERS),
diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h
index 5785ca23b1212..bfdb247a343b9 100644
--- a/libraries/AP_DDS/AP_DDS_config.h
+++ b/libraries/AP_DDS/AP_DDS_config.h
@@ -138,6 +138,10 @@
#define AP_DDS_PARAMETER_SERVER_ENABLED 1
#endif
+#ifndef AP_DDS_ARM_CHECK_SERVER_ENABLED
+#define AP_DDS_ARM_CHECK_SERVER_ENABLED 1
+#endif
+
// Whether to include Twist support
#define AP_DDS_NEEDS_TWIST AP_DDS_VEL_CTRL_ENABLED || AP_DDS_LOCAL_VEL_PUB_ENABLED
diff --git a/libraries/AP_DDS/Idl/std_srvs/srv/Trigger.idl b/libraries/AP_DDS/Idl/std_srvs/srv/Trigger.idl
new file mode 100644
index 0000000000000..6c763b5e7aab0
--- /dev/null
+++ b/libraries/AP_DDS/Idl/std_srvs/srv/Trigger.idl
@@ -0,0 +1,21 @@
+// generated from rosidl_adapter/resource/srv.idl.em
+// with input from std_srvs/srv/Trigger.srv
+// generated code does not contain a copyright notice
+
+
+module std_srvs {
+ module srv {
+ struct Trigger_Request {
+ uint8 structure_needs_at_least_one_member;
+ };
+ struct Trigger_Response {
+ @verbatim (language="comment", text=
+ "indicate successful run of triggered service")
+ boolean success;
+
+ @verbatim (language="comment", text=
+ "informational, e.g. for error messages")
+ string message;
+ };
+ };
+};
diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md
index 03a1aa92938bf..3a60b881900b2 100644
--- a/libraries/AP_DDS/README.md
+++ b/libraries/AP_DDS/README.md
@@ -209,6 +209,7 @@ nanosec: 729410000
$ ros2 service list
/ap/arm_motors
/ap/mode_switch
+/ap/prearm_check
---
```
@@ -234,6 +235,7 @@ List the available services:
$ ros2 service list -t
/ap/arm_motors [ardupilot_msgs/srv/ArmMotors]
/ap/mode_switch [ardupilot_msgs/srv/ModeSwitch]
+/ap/prearm_check [std_srvs/srv/Trigger]
```
Call the arm motors service:
@@ -256,6 +258,20 @@ response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4)
```
+Call the prearm check service:
+
+```bash
+$ ros2 service call /ap/prearm_check std_srvs/srv/Trigger
+requester: making request: std_srvs.srv.Trigger_Request()
+
+response:
+std_srvs.srv.Trigger_Response(success=False, message='Vehicle is Not Armable')
+
+or
+
+std_srvs.srv.Trigger_Response(success=True, message='Vehicle is Armable')
+```
+
## Commanding using ROS 2 Topics
The following topic can be used to control the vehicle.