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 f8af788e8b81ec..6b8de2c9d0e59e 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,10 +15,13 @@ find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/GlobalPosition.msg"
+ "msg/Rally.msg"
"msg/Status.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 a39240143fb559..c6a0dcde7d4185 100644
--- a/libraries/AP_DDS/AP_DDS_Client.cpp
+++ b/libraries/AP_DDS/AP_DDS_Client.cpp
@@ -19,6 +19,7 @@
#include
#include
#include
+#include
#if AP_DDS_ARM_SERVER_ENABLED
#include "ardupilot_msgs/srv/ArmMotors.h"
@@ -26,6 +27,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"
@@ -868,6 +873,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 Req Rally %u valid: lat: %.4f lon: %.4f",
+ 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 Req Rally %u 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
#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 667149d6aa7566..fa301a1f489ba4 100644
--- a/libraries/AP_DDS/AP_DDS_Service_Table.h
+++ b/libraries/AP_DDS/AP_DDS_Service_Table.h
@@ -8,6 +8,10 @@ enum class ServiceIndex: uint8_t {
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
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
#if AP_DDS_PARAMETER_SERVER_ENABLED
SET_PARAMETERS,
GET_PARAMETERS
@@ -20,6 +24,8 @@ static inline constexpr uint8_t to_underlying(const ServiceIndex index)
return static_cast(index);
}
+constexpr auto ardupilot_msg_string_base = "ardupilot_msgs::srv::dds_::";
+
constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
#if AP_DDS_ARM_SERVER_ENABLED
{
@@ -51,6 +57,28 @@ 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
#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 5785ca23b12127..be510b4b622c86 100644
--- a/libraries/AP_DDS/AP_DDS_config.h
+++ b/libraries/AP_DDS/AP_DDS_config.h
@@ -134,6 +134,10 @@
#define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1
#endif
+#ifndef AP_DDS_RALLY_SERVER_ENABLED
+#define AP_DDS_RALLY_SERVER_ENABLED 1
+#endif
+
#ifndef AP_DDS_PARAMETER_SERVER_ENABLED
#define AP_DDS_PARAMETER_SERVER_ENABLED 1
#endif
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 03a1aa92938bf5..e918cee2d7f0b5 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.