From 420c0d174db51d8e3c513b611fc3d223ed71fbd9 Mon Sep 17 00:00:00 2001 From: Tiziano Fiorenzani Date: Tue, 29 Oct 2024 15:02:48 +0000 Subject: [PATCH] AP_DDS: Rally Interface to get-set rally points. --- .../ardupilot_dds_tests/test_rally_point.py | 201 ++++++++++++++++++ Tools/ros2/ardupilot_msgs/CMakeLists.txt | 6 +- Tools/ros2/ardupilot_msgs/msg/Rally.msg | 8 + Tools/ros2/ardupilot_msgs/srv/RallyGet.srv | 10 + Tools/ros2/ardupilot_msgs/srv/RallySet.srv | 13 ++ libraries/AP_DDS/AP_DDS_Client.cpp | 115 ++++++++++ libraries/AP_DDS/AP_DDS_Service_Table.h | 28 ++- libraries/AP_DDS/AP_DDS_config.h | 4 + .../AP_DDS/Idl/ardupilot_msgs/msg/Rally.idl | 23 ++ .../Idl/ardupilot_msgs/srv/RallyGet.idl | 25 +++ .../Idl/ardupilot_msgs/srv/RallySet.idl | 29 +++ libraries/AP_DDS/README.md | 35 +++ 12 files changed, 495 insertions(+), 2 deletions(-) create mode 100644 Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rally_point.py create mode 100644 Tools/ros2/ardupilot_msgs/msg/Rally.msg create mode 100644 Tools/ros2/ardupilot_msgs/srv/RallyGet.srv create mode 100644 Tools/ros2/ardupilot_msgs/srv/RallySet.srv create mode 100644 libraries/AP_DDS/Idl/ardupilot_msgs/msg/Rally.idl create mode 100644 libraries/AP_DDS/Idl/ardupilot_msgs/srv/RallyGet.idl create mode 100644 libraries/AP_DDS/Idl/ardupilot_msgs/srv/RallySet.idl diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rally_point.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rally_point.py new file mode 100644 index 00000000000000..4ba5bfd072af89 --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rally_point.py @@ -0,0 +1,201 @@ +# 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 . + +""" +Tests the Rally Point interface. + +Clear the list, adds points and verifies that they are correctly updated. + +colcon test --packages-select ardupilot_dds_tests \ +--event-handlers=console_cohesion+ --pytest-args -k test_rally_point + +""" + +import rclpy +import time +import launch_pytest +from rclpy.node import Node +from builtin_interfaces.msg import Time +from ardupilot_msgs.msg import Rally +from ardupilot_msgs.srv import RallyGet +from ardupilot_msgs.srv import RallySet +import pytest +from launch_pytest.tools import process as process_tools +from launch import LaunchDescription +import threading + +RALLY_0 = Rally() +RALLY_0.point.latitude =-35.0 +RALLY_0.point.longitude = 149.0 +RALLY_0.point.altitude = 400.0 +RALLY_0.altitude_frame = 2 + + +class RallyControl(Node): + """Push/Pull Rally points.""" + + def __init__(self): + """Initialise the node.""" + super().__init__("rally_service") + self.clear_rally_event = threading.Event() + self.add_rally_event = threading.Event() + self.get_rally_event = threading.Event() + + self._client_rally_get = self.create_client(RallyGet, "/ap/rally_get") + self._client_rally_set = self.create_client(RallySet, "/ap/rally_set") + # Add a spin thread. + self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def _clear_list(self): + req = RallySet.Request() + req.clear = True + self.future = self._client_rally_set.call_async(req) + time.sleep(1.0) + if self.future.result() is None: + return False + else: + print(self.future) + print(self.future.result()) + return self.future.result().size == 0 + + def _send_rally(self, rally): + req = RallySet.Request() + req.clear = False + req.rally = rally + self.future = self._client_rally_set.call_async(req) + time.sleep(1.0) + return self.future.result() + + def _get_rally(self): + req = RallyGet.Request() + req.index = 0 + self.future = self._client_rally_get.call_async(req) + time.sleep(1.0) + return self.future.result() + + #-------------- PROCESSES ----------------- + def process_clear(self): + print("---> Process start") + while True: + if self._clear_list(): + print("---> List Cleared") + self.clear_rally_event.set() + return + time.sleep(1) + + def clear_list(self): + try: + self.clear_thread.stop() + except: + print("---> Starting thread") + self.clear_thread = threading.Thread(target=self.process_clear) + self.clear_thread.start() + + def process_send(self, rally): + while True: + response = self._send_rally(rally) + if response is not None: + if response.success: + self.add_rally_event.set() + return + time.sleep(1) + + def send_rally(self, rally): + try: + self.send_thread.stop() + except: + self.send_thread = threading.Thread(target=self.process_send, args=[rally]) + self.send_thread.start() + + def process_compare(self, rally): + while True: + response = self._get_rally() + if response is not None: + if response.success: + print (f"{response.rally.point.latitude} - {rally.point.latitude}") + if (abs(response.rally.point.latitude - rally.point.latitude) < 1e-7 + and abs(response.rally.point.longitude - rally.point.longitude) < 1e-7 + and response.rally.altitude_frame == rally.altitude_frame): + self.get_rally_event.set() + return + time.sleep(1) + + def get_and_compare_rally(self, rally): + try: + self.get_thread.stop() + except: + self.get_thread = threading.Thread(target=self.process_compare, args=[rally]) + self.get_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_udp) +def test_dds_udp_rally(launch_context, launch_sitl_copter_dds_udp): + """Test Rally Points with 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() + time.sleep(5) + try: + node = RallyControl() + node.clear_list() + clear_flag = node.clear_rally_event.wait(20) + assert clear_flag, "Could not clear" + node.send_rally(RALLY_0) + + send_flag = node.add_rally_event.wait(10) + assert send_flag, "Could not send Rally" + + node.get_and_compare_rally(RALLY_0) + send_flag = node.get_rally_event.wait(10) + assert send_flag, "Wrong Rally point back" + finally: + rclpy.shutdown() + yield diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index d066a2d32b2e43..a48b8f97185150 100644 --- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt +++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt @@ -6,6 +6,7 @@ project(ardupilot_msgs) find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(geographic_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -14,9 +15,12 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/GlobalPosition.msg" + "msg/Rally.msg" "srv/ArmMotors.srv" "srv/ModeSwitch.srv" - DEPENDENCIES geometry_msgs std_msgs + "srv/RallyGet.srv" + "srv/RallySet.srv" + DEPENDENCIES geometry_msgs std_msgs geographic_msgs ADD_LINTER_TESTS ) diff --git a/Tools/ros2/ardupilot_msgs/msg/Rally.msg b/Tools/ros2/ardupilot_msgs/msg/Rally.msg new file mode 100644 index 00000000000000..c2dfb9034ddf19 --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/msg/Rally.msg @@ -0,0 +1,8 @@ +geographic_msgs/GeoPoint point + +# Altitude reference frame +uint8 ALTITUDE_FRAME_ABSOLUTE = 0 +uint8 ALTITUDE_FRAME_HOME = 1 +uint8 ALTITUDE_FRAME_ORIGIN = 2 +uint8 ALTITUDE_FRAME_TERRAIN = 3 +uint8 altitude_frame diff --git a/Tools/ros2/ardupilot_msgs/srv/RallyGet.srv b/Tools/ros2/ardupilot_msgs/srv/RallyGet.srv new file mode 100644 index 00000000000000..8002e97b3f4969 --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/srv/RallyGet.srv @@ -0,0 +1,10 @@ + +# This service requests a Rally point from its ID. + +# Rally Point ID. +uint8 index +--- +# True is the retrieved point is valid. +bool success +# Rally point. +ardupilot_msgs/Rally rally diff --git a/Tools/ros2/ardupilot_msgs/srv/RallySet.srv b/Tools/ros2/ardupilot_msgs/srv/RallySet.srv new file mode 100644 index 00000000000000..91d97b6f5603c3 --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/srv/RallySet.srv @@ -0,0 +1,13 @@ + +# This service appends a Rally Point to the list or clears the whole list. + +# Rally Point. +ardupilot_msgs/Rally rally + +# True to clear the whole list (rally entry is ignored). +bool clear +--- +# True if the action is successful. +bool success +# Size of Rally Points list. +uint8 size diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index fca7d2f6146871..34ea67d3592bed 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -18,6 +18,7 @@ # endif // AP_DDS_ARM_SERVER_ENABLED #include #include +#include #if AP_DDS_ARM_SERVER_ENABLED #include "ardupilot_msgs/srv/ArmMotors.h" @@ -25,6 +26,10 @@ #if AP_DDS_MODE_SWITCH_SERVER_ENABLED #include "ardupilot_msgs/srv/ModeSwitch.h" #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_RALLY_SERVER_ENABLED +#include "ardupilot_msgs/srv/RallyGet.h" +#include "ardupilot_msgs/srv/RallySet.h" +#endif // AP_DDS_RALLY_SERVER_ENABLED #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" @@ -800,6 +805,116 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u break; } #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_RALLY_SERVER_ENABLED + case services[to_underlying(ServiceIndex::GET_RALLY)].rep_id: { + ardupilot_msgs_srv_RallyGet_Request rally_get_request; + ardupilot_msgs_srv_RallyGet_Response rally_get_response; + const bool deserialize_success = ardupilot_msgs_srv_RallyGet_Request_deserialize_topic(ub, &rally_get_request); + if (deserialize_success == false) { + break; + } + + const uxrObjectId replier_id = { + .id = services[to_underlying(ServiceIndex::GET_RALLY)].rep_id, + .type = UXR_REPLIER_ID + }; + + RallyLocation rally_location {}; + const AP_Rally *rally = AP::rally(); + if (rally->get_rally_point_with_index(rally_get_request.index, rally_location)) { + rally_get_response.success = true; + rally_get_response.rally.point.latitude = rally_location.lat * 1e-7; + rally_get_response.rally.point.longitude = rally_location.lng * 1e-7; + rally_get_response.rally.point.altitude = rally_location.alt; + bool alt_frame_valid = rally_location.flags & (1 << 2); + if (alt_frame_valid) { + rally_get_response.rally.altitude_frame = (rally_location.flags >> 3) & ((1 << 2) - 1); + } else { + rally_get_response.rally.altitude_frame = static_cast(Location::AltFrame::ABOVE_HOME); + } + // rally_get_response.rally.flags = rally_location.flags; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request Rally index %u is valid: lat: %f lon: %f", + msg_prefix, rally_get_request.index, rally_get_response.rally.point.latitude, rally_get_response.rally.point.longitude); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s Request Rally index %u is invalid", msg_prefix, rally_get_request.index); + rally_get_response.success = false; + } + + uint8_t reply_buffer[ardupilot_msgs_srv_RallyGet_Response_size_of_topic(&rally_get_response, 0)] {}; + ucdrBuffer reply_ub; + + ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer)); + const bool serialize_success = ardupilot_msgs_srv_RallyGet_Response_serialize_topic(&reply_ub, &rally_get_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; + } + case services[to_underlying(ServiceIndex::SET_RALLY)].rep_id: { + + ardupilot_msgs_srv_RallySet_Request rally_set_request; + ardupilot_msgs_srv_RallySet_Response rally_set_response; + const bool deserialize_success = ardupilot_msgs_srv_RallySet_Request_deserialize_topic(ub, &rally_set_request); + if (deserialize_success == false) { + break; + } + + const uxrObjectId replier_id = { + .id = services[to_underlying(ServiceIndex::SET_RALLY)].rep_id, + .type = UXR_REPLIER_ID + }; + + RallyLocation rally_location{}; + AP_Rally *rally = AP::rally(); + + if (rally_set_request.clear) { + rally->truncate(0); + rally_set_response.success = rally->get_rally_total() == 0; + if (rally_set_response.success) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Rally Point List cleared", msg_prefix); + } + } else { + rally_location.lat = static_cast(rally_set_request.rally.point.latitude * 1e7); + rally_location.lng = static_cast(rally_set_request.rally.point.longitude * 1e7); + rally_location.alt = static_cast(rally_set_request.rally.point.altitude); + rally_location.flags = 0; + rally_location.flags |= (1 << 2); // Use the provided frame. + rally_location.flags |= (rally_set_request.rally.altitude_frame << 3); + + if (rally_location.lat != 0 && rally_location.lng != 0 + && rally_set_request.rally.altitude_frame <= static_cast(Location::AltFrame::ABOVE_TERRAIN)) { + if (rally->append(rally_location)) { + rally_set_response.success = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Rally Point appended", msg_prefix); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s Failed to append Rally Point", msg_prefix); + rally_set_response.success = false; + } + } else { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s Invalid Rally requested", msg_prefix); + rally_set_response.success = false; + } + } + + rally_set_response.size = rally->get_rally_total(); + + uint8_t reply_buffer[ardupilot_msgs_srv_RallySet_Response_size_of_topic(&rally_set_response, 0)] {}; + ucdrBuffer reply_ub; + + ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer)); + const bool serialize_success = ardupilot_msgs_srv_RallySet_Response_serialize_topic(&reply_ub, &rally_set_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_RALLY_SERVER_ENABLED } } diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 0e86496dd57779..1b966e2b758394 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -6,8 +6,12 @@ enum class ServiceIndex: uint8_t { ARMING_MOTORS, #endif // #if AP_DDS_ARM_SERVER_ENABLED #if AP_DDS_MODE_SWITCH_SERVER_ENABLED - MODE_SWITCH + MODE_SWITCH, #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_RALLY_SERVER_ENABLED + GET_RALLY, + SET_RALLY, +#endif // AP_DDS_RALLY_SERVER_ENABLED }; static inline constexpr uint8_t to_underlying(const ServiceIndex index) @@ -47,4 +51,26 @@ 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_RALLY_SERVER_ENABLED + { + .req_id = to_underlying(ServiceIndex::GET_RALLY), + .rep_id = to_underlying(ServiceIndex::GET_RALLY), + .service_rr = Service_rr::Replier, + .service_name = "rs/ap/rally_getService", + .request_type = "ardupilot_msgs::srv::dds_::RallyGet_Request_", + .reply_type = "ardupilot_msgs::srv::dds_::RallyGet_Response_", + .request_topic_name = "rq/ap/rally_getRequest", + .reply_topic_name = "rr/ap/rally_getReply", + }, + { + .req_id = to_underlying(ServiceIndex::SET_RALLY), + .rep_id = to_underlying(ServiceIndex::SET_RALLY), + .service_rr = Service_rr::Replier, + .service_name = "rs/ap/rally_setService", + .request_type = "ardupilot_msgs::srv::dds_::RallySet_Request_", + .reply_type = "ardupilot_msgs::srv::dds_::RallySet_Response_", + .request_topic_name = "rq/ap/rally_setRequest", + .reply_topic_name = "rr/ap/rally_setReply", + }, +#endif // AP_DDS_RALLY_SERVER_ENABLED }; diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index b8f31e368c8a40..d38c06d35fc016 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -126,6 +126,10 @@ #define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1 #endif +#ifndef AP_DDS_RALLY_SERVER_ENABLED +#define AP_DDS_RALLY_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/ardupilot_msgs/msg/Rally.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Rally.idl new file mode 100644 index 00000000000000..8c8236391bb103 --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Rally.idl @@ -0,0 +1,23 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from ardupilot_msgs/msg/Rally.msg +// generated code does not contain a copyright notice + +#include "geographic_msgs/msg/GeoPoint.idl" + +module ardupilot_msgs { + module msg { + module Rally_Constants { + @verbatim (language="comment", text= + "Altitude reference frame") + const uint8 ALTITUDE_FRAME_ABSOLUTE = 0; + const uint8 ALTITUDE_FRAME_HOME = 1; + const uint8 ALTITUDE_FRAME_ORIGIN = 2; + const uint8 ALTITUDE_FRAME_TERRAIN = 3; + }; + struct Rally { + geographic_msgs::msg::GeoPoint point; + + uint8 altitude_frame; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/srv/RallyGet.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/RallyGet.idl new file mode 100644 index 00000000000000..b903611eb61b0c --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/RallyGet.idl @@ -0,0 +1,25 @@ +// generated from rosidl_adapter/resource/srv.idl.em +// with input from ardupilot_msgs/srv/RallyGet.srv +// generated code does not contain a copyright notice + +#include "ardupilot_msgs/msg/Rally.idl" + +module ardupilot_msgs { + module srv { + struct RallyGet_Request { + @verbatim (language="comment", text= + "This service requests a Rally point from its ID." "\n" + "Rally Point ID.") + uint8 index; + }; + @verbatim (language="comment", text= + "True is the retrieved point is valid.") + struct RallyGet_Response { + boolean success; + + @verbatim (language="comment", text= + "Rally point.") + ardupilot_msgs::msg::Rally rally; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/srv/RallySet.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/RallySet.idl new file mode 100644 index 00000000000000..c649e28bb3e8c2 --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/RallySet.idl @@ -0,0 +1,29 @@ +// generated from rosidl_adapter/resource/srv.idl.em +// with input from ardupilot_msgs/srv/RallySet.srv +// generated code does not contain a copyright notice + +#include "ardupilot_msgs/msg/Rally.idl" + +module ardupilot_msgs { + module srv { + struct RallySet_Request { + @verbatim (language="comment", text= + "This service appends a Rally Point to the list or clears the whole list." "\n" + "Rally Point.") + ardupilot_msgs::msg::Rally rally; + + @verbatim (language="comment", text= + "True to clear the whole list (rally entry is ignored).") + boolean clear; + }; + @verbatim (language="comment", text= + "True if the action is successful.") + struct RallySet_Response { + boolean success; + + @verbatim (language="comment", text= + "Size of Rally Points list.") + uint8 size; + }; + }; +}; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 02848565979d20..f9f14def6a30cc 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -209,6 +209,8 @@ nanosec: 729410000 $ ros2 service list /ap/arm_motors /ap/mode_switch +/ap/rally_get +/ap/rally_set --- ``` @@ -234,6 +236,8 @@ List the available services: $ ros2 service list -t /ap/arm_motors [ardupilot_msgs/srv/ArmMotors] /ap/mode_switch [ardupilot_msgs/srv/ModeSwitch] +/ap/rally_get [ardupilot_msgs/srv/RallyGet] +/ap/rally_set [ardupilot_msgs/srv/RallySet] ``` Call the arm motors service: @@ -256,6 +260,37 @@ response: ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4) ``` +Clear the Rally Points list: + +```bash +ros2 service call /ap/rally_set ardupilot_msgs/srv/RallySet "{clear: true}" +requester: making request: ardupilot_msgs.srv.RallySet_Request(rally=ardupilot_msgs.msg.Rally(point=geographic_msgs.msg.GeoPoint(latitude=0.0, longitude=0.0, altitude=0.0), break_altitude=0.0, land_dir=0.0, flag_favorable_winds=False, flag_do_auto_land=False, altitude_frame=0), clear=True) + +response: +ardupilot_msgs.srv.RallySet_Response(success=True, size=0) +``` + +Append a Rally Point: + +```bash +ros2 service call /ap/rally_set ardupilot_msgs/srv/RallySet "{rally: {point: {latitude: -35, longitude: 151, altitude: 400}}}" +requester: making request: ardupilot_msgs.srv.RallySet_Request(rally=ardupilot_msgs.msg.Rally(point=geographic_msgs.msg.GeoPoint(latitude=-35.0, longitude=151.0, altitude=400.0), break_altitude=0.0, land_dir=0.0, flag_favorable_winds=False, flag_do_auto_land=False, altitude_frame=0), clear=False) + +response: +ardupilot_msgs.srv.RallySet_Response(success=True, size=1) +``` + +Get a specific Rally Point: + +```bash +ros2 service call /ap/rally_get ardupilot_msgs/srv/RallyGet "index: 0" +requester: making request: ardupilot_msgs.srv.RallyGet_Request(index=0) + +response: +ardupilot_msgs.srv.RallyGet_Response(success=True, rally=ardupilot_msgs.msg.Rally(point=geographic_msgs.msg.GeoPoint(latitude=-35.0, longitude=151.0, altitude=400.0), break_altitude=0.0, land_dir=0.0, flag_favorable_winds=False, flag_do_auto_land=False, altitude_frame=0)) +``` + + ## Commanding using ROS 2 Topics The following topic can be used to control the vehicle.