diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rc_msg_received.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rc_msg_received.py new file mode 100644 index 0000000000..9ee31c9c8b --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_rc_msg_received.py @@ -0,0 +1,156 @@ +# 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 the RC message is being published. + +Checks whether a message is received and that only frame_id = '0' is received, +as SITL has only one rc available. + +colcon test --packages-select ardupilot_dds_tests \ +--event-handlers=console_cohesion+ --pytest-args -k test_rc_msg_received + +""" + +import launch_pytest +import pytest +import rclpy +import rclpy.node +import threading + +from launch import LaunchDescription + +from launch_pytest.tools import process as process_tools + +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from rclpy.qos import QoSHistoryPolicy + +from ardupilot_msgs.msg import Rc + +TOPIC = "ap/rc" + + +class RcListener(rclpy.node.Node): + """Subscribe to Rc messages.""" + + def __init__(self): + """Initialise the node.""" + super().__init__("rc_listener") + self.msg_event_object = threading.Event() + + # Declare and acquire `topic` parameter + self.declare_parameter("topic", TOPIC) + self.topic = self.get_parameter("topic").get_parameter_value().string_value + + def start_subscriber(self): + """Start the subscriber.""" + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.subscription = self.create_subscription(Rc, self.topic, self.subscriber_callback, qos_profile) + + # Add a spin thread. + self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def subscriber_callback(self, msg): + """Process a Rc message.""" + self.msg_event_object.set() + + +@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_rc_msg_recv(launch_context, launch_sitl_copter_dds_serial): + """Test rc messages are published by 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 = RcListener() + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=10.0) + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." + finally: + rclpy.shutdown() + yield + + +@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp) +def test_dds_udp_rc_msg_recv(launch_context, launch_sitl_copter_dds_udp): + """Test rc messages are published by 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 = RcListener() + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=10.0) + assert msgs_received_flag, f"Did not receive '{TOPIC}' msgs." + + finally: + rclpy.shutdown() + yield + \ No newline at end of file diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index 767b89a89b..71a901c482 100644 --- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt +++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/GlobalPosition.msg" + "msg/Rc.msg" "msg/Status.msg" "srv/ArmMotors.srv" "srv/ModeSwitch.srv" diff --git a/Tools/ros2/ardupilot_msgs/msg/Rc.msg b/Tools/ros2/ardupilot_msgs/msg/Rc.msg new file mode 100644 index 0000000000..191d58944d --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/msg/Rc.msg @@ -0,0 +1,13 @@ +std_msgs/Header header + +# returns true if radio is connected. +bool is_connected + +# returns [0, 100] for receiver RSSI. +uint8 receiver_rssi + +# channels values. +int16[<=32] channels + +# sets true if a channel is overridden. +bool[<=32] active_overrides diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index e5404898d9..c377856cae 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -32,6 +32,9 @@ #if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED #include "ardupilot_msgs/srv/Takeoff.h" #endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED +#if AP_DDS_RC_PUB_ENABLED +#include "AP_RSSI/AP_RSSI.h" +#endif // AP_DDS_RC_PUB_ENABLED #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" @@ -65,6 +68,9 @@ static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = AP_DDS_DELAY_LOCAL_VEL #if AP_DDS_AIRSPEED_PUB_ENABLED static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = AP_DDS_DELAY_AIRSPEED_TOPIC_MS; #endif // AP_DDS_AIRSPEED_PUB_ENABLED +#if AP_DDS_RC_PUB_ENABLED +static constexpr uint16_t DELAY_RC_TOPIC_MS = AP_DDS_DELAY_RC_TOPIC_MS; +#endif // AP_DDS_RC_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = AP_DDS_DELAY_GEO_POSE_TOPIC_MS; #endif // AP_DDS_GEOPOSE_PUB_ENABLED @@ -522,6 +528,38 @@ bool AP_DDS_Client::update_topic(geometry_msgs_msg_Vector3Stamped& msg) } #endif // AP_DDS_AIRSPEED_PUB_ENABLED +#if AP_DDS_RC_PUB_ENABLED +bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Rc& msg) +{ + update_topic(msg.header.stamp); + AP_RSSI *ap_rssi = AP_RSSI::get_singleton(); + auto rc = RC_Channels::get_singleton(); + static int16_t counter = 0; + + // Is connected if not in failsafe. + // This is only valid if the RC has been connected at least once + msg.is_connected = !rc->in_rc_failsafe(); + // Receiver RSSI is reported between 0.0 and 1.0. + msg.receiver_rssi = static_cast(ap_rssi->read_receiver_rssi()*100.f); + + // Limit the max number of available channels to 8 + msg.channels_size = MIN(static_cast(rc->get_valid_channel_count()), 32U); + msg.active_overrides_size = msg.channels_size; + if (msg.channels_size) { + for (uint8_t i = 0; i < static_cast(msg.channels_size); i++) { + msg.channels[i] = rc->rc_channel(i)->get_radio_in(); + msg.active_overrides[i] = rc->rc_channel(i)->has_override(); + } + } else { + // If no channels are available, the RC is disconnected. + msg.is_connected = false; + } + + // Return true if Radio is connected, or once every 10 steps to reduce useless traffic. + return msg.is_connected ? true : (counter++ % 10 == 0); +} +#endif // AP_DDS_RC_PUB_ENABLED + #if AP_DDS_GEOPOSE_PUB_ENABLED void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) { @@ -1557,6 +1595,22 @@ void AP_DDS_Client::write_tx_local_airspeed_topic() } } #endif // AP_DDS_AIRSPEED_PUB_ENABLED +#if AP_DDS_RC_PUB_ENABLED +void AP_DDS_Client::write_tx_local_rc_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub {}; + const uint32_t topic_size = ardupilot_msgs_msg_Rc_size_of_topic(&tx_local_rc_topic, 0); + uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::LOCAL_RC_PUB)].dw_id, &ub, topic_size); + const bool success = ardupilot_msgs_msg_Rc_serialize_topic(&ub, &tx_local_rc_topic); + if (!success) { + // TODO sometimes serialization fails on bootup. Determine why. + // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + } + } +} +#endif // AP_DDS_RC_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED void AP_DDS_Client::write_imu_topic() { @@ -1708,6 +1762,14 @@ void AP_DDS_Client::update() } } #endif // AP_DDS_AIRSPEED_PUB_ENABLED +#if AP_DDS_RC_PUB_ENABLED + if (cur_time_ms - last_rc_time_ms > DELAY_RC_TOPIC_MS) { + last_rc_time_ms = cur_time_ms; + if (update_topic(tx_local_rc_topic)) { + write_tx_local_rc_topic(); + } + } +#endif // AP_DDS_RC_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) { update_topic(imu_topic); diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index d5581c4ed5..65c0091e37 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -43,6 +43,9 @@ #if AP_DDS_AIRSPEED_PUB_ENABLED #include "geometry_msgs/msg/Vector3Stamped.h" #endif // AP_DDS_AIRSPEED_PUB_ENABLED +#if AP_DDS_RC_PUB_ENABLED +#include "ardupilot_msgs/msg/Rc.h" +#endif // AP_DDS_RC_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED #include "geographic_msgs/msg/GeoPoseStamped.h" #endif // AP_DDS_GEOPOSE_PUB_ENABLED @@ -160,6 +163,15 @@ private: static bool update_topic(geometry_msgs_msg_Vector3Stamped& msg); #endif //AP_DDS_AIRSPEED_PUB_ENABLED +#if AP_DDS_RC_PUB_ENABLED + ardupilot_msgs_msg_Rc tx_local_rc_topic; + // The last ms timestamp AP_DDS wrote a rc message + uint64_t last_rc_time_ms; + //! @brief Serialize the current local rc and publish to the IO stream(s) + void write_tx_local_rc_topic(); + static bool update_topic(ardupilot_msgs_msg_Rc& msg); +#endif //AP_DDS_RC_PUB_ENABLED + #if AP_DDS_BATTERY_STATE_PUB_ENABLED sensor_msgs_msg_BatteryState battery_state_topic; // The last ms timestamp AP_DDS wrote a BatteryState message diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 5212a42c90..7c4175da6a 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -39,6 +39,9 @@ enum class TopicIndex: uint8_t { #if AP_DDS_AIRSPEED_PUB_ENABLED LOCAL_AIRSPEED_PUB, #endif // AP_DDS_AIRSPEED_PUB_ENABLED +#if AP_DDS_RC_PUB_ENABLED + LOCAL_RC_PUB, +#endif // AP_DDS_RC_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED GEOPOSE_PUB, #endif // AP_DDS_GEOPOSE_PUB_ENABLED @@ -220,6 +223,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { }, }, #endif // AP_DDS_AIRSPEED_PUB_ENABLED +#if AP_DDS_RC_PUB_ENABLED + { + .topic_id = to_underlying(TopicIndex::LOCAL_RC_PUB), + .pub_id = to_underlying(TopicIndex::LOCAL_RC_PUB), + .sub_id = to_underlying(TopicIndex::LOCAL_RC_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_RC_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_RC_PUB), .type=UXR_DATAREADER_ID}, + .topic_rw = Topic_rw::DataWriter, + .topic_name = "rt/ap/rc", + .type_name = "ardupilot_msgs::msg::dds_::Rc_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 1, + }, + }, +#endif // AP_DDS_RC_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::GEOPOSE_PUB), diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 3a68a349d5..1ac6e6e286 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -86,6 +86,14 @@ #define AP_DDS_DELAY_AIRSPEED_TOPIC_MS 33 #endif +#ifndef AP_DDS_RC_PUB_ENABLED +#define AP_DDS_RC_PUB_ENABLED 1 +#endif + +#ifndef AP_DDS_DELAY_RC_TOPIC_MS +#define AP_DDS_DELAY_RC_TOPIC_MS 100 +#endif + #ifndef AP_DDS_BATTERY_STATE_PUB_ENABLED #define AP_DDS_BATTERY_STATE_PUB_ENABLED 1 #endif diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Rc.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Rc.idl new file mode 100644 index 0000000000..1e4e470ee4 --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Rc.idl @@ -0,0 +1,29 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from ardupilot_msgs/msg/Rc.msg +// generated code does not contain a copyright notice + +#include "std_msgs/msg/Header.idl" + +module ardupilot_msgs { + module msg { + struct Rc { + std_msgs::msg::Header header; + + @verbatim (language="comment", text= + "returns true if radio is connected.") + boolean is_connected; + + @verbatim (language="comment", text= + "returns [0, 100] for receiver RSSI.") + uint8 receiver_rssi; + + @verbatim (language="comment", text= + "channels values.") + sequence channels; + + @verbatim (language="comment", text= + "sets true if a channel is overridden.") + sequence active_overrides; + }; + }; +}; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index ad323ba173..417aed1ab9 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -180,6 +180,8 @@ Published topics: * /ap/imu/experimental/data [sensor_msgs/msg/Imu] 1 publisher * /ap/navsat [sensor_msgs/msg/NavSatFix] 1 publisher * /ap/pose/filtered [geometry_msgs/msg/PoseStamped] 1 publisher + * /ap/rc [ardupilot_msgs/msg/Rc] 1 publisher + * /ap/status [ardupilot_msgs/msg/Status] 1 publisher * /ap/tf_static [tf2_msgs/msg/TFMessage] 1 publisher * /ap/time [builtin_interfaces/msg/Time] 1 publisher * /ap/twist/filtered [geometry_msgs/msg/TwistStamped] 1 publisher