From 7d1bc64df01ea38712fc68dc48b6100c50316063 Mon Sep 17 00:00:00 2001 From: fiorenzani Date: Mon, 14 Oct 2024 16:39:39 +0000 Subject: [PATCH] AP_DDS: Service to check if vehicle is armable --- .../pre_arm_check_service.py | 77 ++++++++ Tools/ros2/ardupilot_dds_tests/setup.py | 1 + .../test_prearm_service.py | 166 ++++++++++++++++++ libraries/AP_DDS/AP_DDS_Client.cpp | 28 +++ libraries/AP_DDS/AP_DDS_Service_Table.h | 13 +- libraries/AP_DDS/Idl/std_srvs/srv/Trigger.idl | 21 +++ libraries/AP_DDS/README.md | 16 ++ 7 files changed, 321 insertions(+), 1 deletion(-) create mode 100644 Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/pre_arm_check_service.py create mode 100644 Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_prearm_service.py create mode 100644 libraries/AP_DDS/Idl/std_srvs/srv/Trigger.idl 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..9bf88081bbc67 --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/pre_arm_check_service.py @@ -0,0 +1,77 @@ +#!/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 9ba608c20290d..f4aceb04eecdf 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -18,6 +18,7 @@ #include "ardupilot_msgs/srv/ArmMotors.h" #include "ardupilot_msgs/srv/ModeSwitch.h" +#include "std_srvs/srv/Trigger.h" #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" @@ -789,6 +790,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::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; + } } } diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 22f6716beffd0..75a37610bcc4d 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, + PREARM_CHECK }; static inline constexpr uint8_t to_underlying(const ServiceIndex index) @@ -38,4 +39,14 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .request_topic_name = "rq/ap/mode_switchRequest", .reply_topic_name = "rr/ap/mode_switchReply", }, + { + .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", + }, }; 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 02848565979d2..d89c71a50ff35 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.