AP_DDS: Rally Interface to get-set rally points.

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

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
 )
 

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

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

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

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -19,13 +19,18 @@
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_Common/AP_FWVersion.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
+#include <AP_Rally/AP_Rally.h>
 
 #if AP_DDS_ARM_SERVER_ENABLED
 #include "ardupilot_msgs/srv/ArmMotors.h"
 #endif // AP_DDS_ARM_SERVER_ENABLED
 #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"
@@ -874,6 +879,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<uint8_t>(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<int32_t>(rally_set_request.rally.point.latitude * 1e7);
+            rally_location.lng = static_cast<int32_t>(rally_set_request.rally.point.longitude * 1e7);
+            rally_location.alt = static_cast<int16_t>(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<uint8_t>(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);