Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_DDS: RC channels message #28907

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
@@ -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 <https://www.gnu.org/licenses/>.

"""
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

1 change: 1 addition & 0 deletions Tools/ros2/ardupilot_msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
13 changes: 13 additions & 0 deletions Tools/ros2/ardupilot_msgs/msg/Rc.msg
Original file line number Diff line number Diff line change
@@ -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
62 changes: 62 additions & 0 deletions libraries/AP_DDS/AP_DDS_Client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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<int16_t>(ap_rssi->read_receiver_rssi()*100.f);

// Limit the max number of available channels to 8
msg.channels_size = MIN(static_cast<uint32_t>(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<uint8_t>(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)
{
Expand Down Expand Up @@ -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()
{
Expand Down Expand Up @@ -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);
Expand Down
12 changes: 12 additions & 0 deletions libraries/AP_DDS/AP_DDS_Client.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -160,6 +163,15 @@ class AP_DDS_Client
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
Expand Down
21 changes: 21 additions & 0 deletions libraries/AP_DDS/AP_DDS_Topic_Table.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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),
Expand Down
8 changes: 8 additions & 0 deletions libraries/AP_DDS/AP_DDS_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
29 changes: 29 additions & 0 deletions libraries/AP_DDS/Idl/ardupilot_msgs/msg/Rc.idl
Original file line number Diff line number Diff line change
@@ -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<int16, 32> channels;

@verbatim (language="comment", text=
"sets true if a channel is overridden.")
sequence<boolean, 32> active_overrides;
};
};
};
2 changes: 2 additions & 0 deletions libraries/AP_DDS/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading