AP_DDS: Service to check if vehicle is armable

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 <https://www.gnu.org/licenses/>.
+
+"""
+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()

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",
         ],
     },
 )

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 <https://www.gnu.org/licenses/>.
+
+"""
+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

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"
@@ -37,6 +40,8 @@
 #include "AP_DDS_Service_Table.h"
 #include "AP_DDS_External_Odom.h"
 
+#define STRCPY(D,S) strncpy(D, S, ARRAY_SIZE(D))
+
 // Enable DDS at runtime by default
 static constexpr uint8_t ENABLED_BY_DEFAULT = 1;
 #if AP_DDS_TIME_PUB_ENABLED
@@ -289,8 +294,8 @@ void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
         char gps_frame_id[16];
         //! @todo should GPS frame ID's be 0 or 1 indexed in ROS?
         hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i);
-        strcpy(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID);
-        strcpy(msg.transforms[i].child_frame_id, gps_frame_id);
+        STRCPY(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID);
+        STRCPY(msg.transforms[i].child_frame_id, gps_frame_id);
         // The body-frame offsets
         // X - Forward
         // Y - Right
@@ -392,7 +397,7 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_
 void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -443,7 +448,7 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
 void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -486,7 +491,7 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
 bool AP_DDS_Client::update_topic(geometry_msgs_msg_Vector3Stamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
     // In ROS REP 103, axis orientation uses the following convention:
@@ -515,7 +520,7 @@ bool AP_DDS_Client::update_topic(geometry_msgs_msg_Vector3Stamped& msg)
 void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -556,7 +561,7 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
 void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_NED_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_NED_FRAME_ID);
 
     auto &imu = AP::ins();
     auto &ahrs = AP::ahrs();
@@ -603,7 +608,7 @@ void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
 void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -874,6 +879,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);