From 92ce630cda8f3262bc65058075a631bff8e899e4 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 | 200 ++++++++++++++++++ Tools/ros2/ardupilot_msgs/CMakeLists.txt | 6 +- Tools/ros2/ardupilot_msgs/msg/Rally.msg | 11 + Tools/ros2/ardupilot_msgs/srv/RallyGet.srv | 11 + Tools/ros2/ardupilot_msgs/srv/RallySet.srv | 12 ++ libraries/AP_DDS/AP_DDS_Client.cpp | 109 ++++++++++ libraries/AP_DDS/AP_DDS_Service_Table.h | 26 +++ libraries/AP_DDS/AP_DDS_config.h | 4 + .../AP_DDS/Idl/ardupilot_msgs/msg/Rally.idl | 23 ++ .../Idl/ardupilot_msgs/srv/RallyGet.idl | 30 +++ .../Idl/ardupilot_msgs/srv/RallySet.idl | 29 +++ libraries/AP_DDS/README.md | 45 +++- 12 files changed, 504 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 0000000000..97ca74419c --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rally_point.py @@ -0,0 +1,200 @@ +# 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: + 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 767b89a89b..4808393ec3 100644 --- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt +++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt @@ -5,6 +5,7 @@ project(ardupilot_msgs) # Find dependencies. find_package(ament_cmake REQUIRED) +find_package(geographic_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) @@ -14,11 +15,14 @@ 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" "srv/Takeoff.srv" - DEPENDENCIES geometry_msgs std_msgs + "srv/RallyGet.srv" + "srv/RallySet.srv" + DEPENDENCIES geographic_msgs geometry_msgs std_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 0000000000..ce0b03d671 --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/msg/Rally.msg @@ -0,0 +1,11 @@ +# Rally message provides a geographic point and +# its altitude reference frame. + +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 0000000000..86fc8a19a4 --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/srv/RallyGet.srv @@ -0,0 +1,11 @@ +# This service requests a Rally point from its ID. + +# Rally Point ID. +uint8 index +--- +# True is the retrieved point is valid. +bool success +# Size of Rally Points list. +uint8 size +# 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 0000000000..116ab35aac --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/srv/RallySet.srv @@ -0,0 +1,12 @@ +# 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 0a7a81689b..3614c38500 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -32,6 +32,11 @@ #if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED #include "ardupilot_msgs/srv/Takeoff.h" #endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED +#if AP_DDS_RALLY_SERVER_ENABLED +#include +#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" @@ -976,6 +981,110 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u break; } #endif //AP_DDS_ARM_CHECK_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.size = rally->get_rally_total(); + 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); + } + } else { + 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; + } 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_HOME) + || rally_set_request.rally.altitude_frame == static_cast(Location::AltFrame::ABOVE_ORIGIN) + || rally_set_request.rally.altitude_frame == static_cast(Location::AltFrame::ABOVE_TERRAIN) + || rally_set_request.rally.altitude_frame == static_cast(Location::AltFrame::ABSOLUTE))) { + if (rally->append(rally_location)) { + rally_set_response.success = true; + } else { + rally_set_response.success = false; + } + } else { + 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 7f04ab3248..85a2958c24 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -14,6 +14,10 @@ enum class ServiceIndex: uint8_t { #if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED TAKEOFF, #endif // AP_DDS_VTOL_TAKEOFF_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 @@ -81,6 +85,28 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .reply_topic_name = "rr/ap/experimental/takeoffReply", }, #endif // AP_DDS_VTOL_TAKEOFF_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 3a68a349d5..736f055111 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -145,6 +145,10 @@ #define AP_DDS_VTOL_TAKEOFF_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 0000000000..8c8236391b --- /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 0000000000..62e75e646c --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/RallyGet.idl @@ -0,0 +1,30 @@ +// 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 { + @verbatim (language="comment", text= + "This service requests a Rally point from its ID.") + struct RallyGet_Request { + @verbatim (language="comment", text= + "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= + "Size of Rally Points list.") + uint8 size; + + @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 0000000000..c649e28bb3 --- /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 ad323ba173..8b2aae7df6 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -211,6 +211,8 @@ $ ros2 service list /ap/mode_switch /ap/prearm_check /ap/experimental/takeoff +/ap/rally_get +/ap/rally_set --- ``` @@ -238,6 +240,8 @@ $ ros2 service list -t /ap/mode_switch [ardupilot_msgs/srv/ModeSwitch] /ap/prearm_check [std_srvs/srv/Trigger] /ap/experimental/takeoff [ardupilot_msgs/srv/Takeoff] +/ap/rally_get [ardupilot_msgs/srv/RallyGet] +/ap/rally_set [ardupilot_msgs/srv/RallySet] ``` Call the arm motors service: @@ -284,6 +288,45 @@ response: ardupilot_msgs.srv.Takeoff_Response(status=True) ``` +Clear the Rally Points list (the provided Point will be ignored): + +```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), altitude_frame=0), clear=True) + +response: +ardupilot_msgs.srv.RallySet_Response(success=True, size=0) +``` + +Append a Rally Point: + +Rally points units of measure are: +* degrees for the latitude and longitude (up to 7 decimal digits resolution); +* meters for the altitude (with no decimals). + +The `altitude_frame` specifies the datum for the altitude, among Home, Absolute, Terrain or EKF Origin +(use the enumerator specified in the message definition). + +```bash +ros2 service call /ap/rally_set ardupilot_msgs/srv/RallySet "{rally: {altitude_frame: 1, point: {latitude: -35.12345, longitude: 151.12345, altitude: 400}}}" + +requester: making request: ardupilot_msgs.srv.RallySet_Request(rally=ardupilot_msgs.msg.Rally(point=geographic_msgs.msg.GeoPoint(latitude=-35.12345, longitude=151.12345, altitude=400.0), altitude_frame=1), clear=False) + +response: +ardupilot_msgs.srv.RallySet_Response(success=True, size=1) +``` + +Get a specific Rally Point (the same service can be used to request the size of the Rally list): + +```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, size=1, rally=ardupilot_msgs.msg.Rally(point=geographic_msgs.msg.GeoPoint(latitude=35.12345504760742, longitude=151.12345176604336, altitude=400.0), altitude_frame=1)) +``` + ## Commanding using ROS 2 Topics The following topic can be used to control the vehicle. @@ -308,7 +351,7 @@ ros2 topic pub /ap/cmd_gps_pose ardupilot_msgs/msg/GlobalPosition "{latitude: 34 publisher: beginning loop publishing #1: ardupilot_msgs.msg.GlobalPosition(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), coordinate_frame=0, type_mask=0, latitude=34.0, longitude=118.0, altitude=1000.0, velocity=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), acceleration_or_force=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), yaw=0.0) ``` - + ## Contributing to `AP_DDS` library ### Adding DDS messages to Ardupilot