diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_joy_msg_received.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_joy_msg_received.py new file mode 100644 index 0000000000..9df6c37145 --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_joy_msg_received.py @@ -0,0 +1,229 @@ +# 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 Joy message is being published. + +Arms the copter and commands a throttle up. Check that the vehicle climbs. + +colcon test --packages-select ardupilot_dds_tests \ +--event-handlers=console_cohesion+ --pytest-args -k test_joy_msg_received + +""" + +import rclpy +import time +import launch_pytest +from rclpy.node import Node +from builtin_interfaces.msg import Time +from geographic_msgs.msg import GeoPoseStamped +from geometry_msgs.msg import TwistStamped +from geopy import point +from ardupilot_msgs.srv import ArmMotors +from ardupilot_msgs.srv import ModeSwitch +from sensor_msgs.msg import Joy +from scipy.spatial.transform import Rotation as R +import pytest +from launch_pytest.tools import process as process_tools +from launch import LaunchDescription +import threading + +GUIDED = 4 +LOITER = 5 + +FRAME_GLOBAL_INT = 5 + +# Hard code some known locations +# Note - Altitude in geopy is in km! +GRAYHOUND_TRACK = point.Point(latitude=-35.345996, longitude=149.159017, altitude=0.575) +CMAC = point.Point(latitude=-35.3627010, longitude=149.1651513, altitude=0.585) + + +class PlaneFbwbJoyControl(Node): + """Plane follow waypoints using guided control.""" + + def __init__(self): + """Initialise the node.""" + super().__init__("copter_joy_listener") + + self.arming_event_object = threading.Event() + self.mode_event_object = threading.Event() + self.climbing_event_object = threading.Event() + + self._client_arm = self.create_client(ArmMotors, "/ap/arm_motors") + + self._client_mode_switch = self.create_client(ModeSwitch, "/ap/mode_switch") + + self._joy_pub = self.create_publisher(Joy, "/ap/joy", 1) + + # Create subscriptions after services are up + qos = rclpy.qos.QoSProfile( + reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, durability=rclpy.qos.DurabilityPolicy.VOLATILE, depth=1 + ) + self._subscription_geopose = self.create_subscription(GeoPoseStamped, "/ap/geopose/filtered", self.geopose_cb, qos) + self._cur_geopose = GeoPoseStamped() + + self._subscription_twist = self.create_subscription(TwistStamped, "/ap/twist/filtered", self.twist_cb, qos) + self._climb_rate = 0.0 + + # Add a spin thread. + self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def geopose_cb(self, msg: GeoPoseStamped): + """Process a GeoPose message.""" + stamp = msg.header.stamp + if stamp.sec: + # Store current state + self._cur_geopose = msg + + def twist_cb(self, msg: TwistStamped): + """Process a Twist message.""" + linear = msg.twist.linear + self._climb_rate = linear.z + + def arm(self): + req = ArmMotors.Request() + req.arm = True + future = self._client_arm.call_async(req) + time.sleep(0.2) + try: + return future.result().result + except: + return False + + def arm_with_timeout(self, timeout: rclpy.duration.Duration): + """Try to arm. Returns true on success, or false if arming fails or times out.""" + armed = False + start = self.get_clock().now() + while not armed and self.get_clock().now() - start < timeout: + armed = self.arm() + time.sleep(1) + return armed + + def switch_mode(self, mode): + req = ModeSwitch.Request() + req.mode = mode + future = self._client_mode_switch.call_async(req) + time.sleep(1) + try: + return future.result().status and future.result().curr_mode == mode + except: + return False + + def switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Duration): + """Try to switch mode. Returns true on success, or false if mode switch fails or times out.""" + is_in_desired_mode = False + start = self.get_clock().now() + while not is_in_desired_mode: + is_in_desired_mode = self.switch_mode(desired_mode) + time.sleep(1) + + return is_in_desired_mode + + def get_cur_geopose(self): + """Return latest geopose.""" + return self._cur_geopose + + def send_joy_value(self, joy_val): + self._joy_pub.publish(joy_val) + + #-------------- PROCESSES ----------------- + def process_arm(self): + if self.arm_with_timeout(rclpy.duration.Duration(seconds=30)): + self.arming_event_object.set() + + def start_arm(self): + self.arm_thread = threading.Thread(target=self.process_arm) + self.arm_thread.start() + + def process_climb(self): + joy_msg = Joy() + joy_msg.axes.append(0.0) + joy_msg.axes.append(0.0) + joy_msg.axes.append(0.8) #- straight up + joy_msg.axes.append(0.0) + + while True: + self.send_joy_value(joy_msg) + self.arm() #- Keep the system armed (sometimes it disarms and the test fails) + time.sleep(0.1) + self.get_logger().info("From AP : Climb rate {}".format(self._climb_rate)) + if self._climb_rate > 0.5: + self.climbing_event_object.set() + return + + def climb(self): + self.climb_thread = threading.Thread(target=self.process_climb) + self.climb_thread.start() + + def arm_and_takeoff_process(self): + self.process_arm() + self.climb() + + def arm_and_takeoff(self): + self.climb_thread = threading.Thread(target=self.arm_and_takeoff_process) + self.climb_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_joy_msg_recv(launch_context, launch_sitl_copter_dds_udp): + """Test joy 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 = PlaneFbwbJoyControl() + node.arm_and_takeoff() + climb_flag = node.climbing_event_object.wait(10) + assert climb_flag, "Could not climb" + finally: + rclpy.shutdown() + yield diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index a17d8b5231..d224a22551 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -573,11 +574,23 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin } if (rx_joy_topic.axes_size >= 4) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Received sensor_msgs/Joy: %f, %f, %f, %f", - msg_prefix, rx_joy_topic.axes[0], rx_joy_topic.axes[1], rx_joy_topic.axes[2], rx_joy_topic.axes[3]); - // TODO implement joystick RC control to AP - } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Received sensor_msgs/Joy. Axes size must be >= 4", msg_prefix); + const uint32_t t_now = AP_HAL::millis(); + + for (uint8_t i = 0; i < MIN(8U, rx_joy_topic.axes_size); i++) { + // Ignore channel override if NaN. + if (std::isnan(rx_joy_topic.axes[i])) { + // Setting the RC override to 0U releases the channel back to the RC. + RC_Channels::set_override(i, 0U, t_now); + } else { + const uint16_t mapped_data = static_cast( + linear_interpolate(rc().channel(i)->get_radio_min(), + rc().channel(i)->get_radio_max(), + rx_joy_topic.axes[i], + -1.0, 1.0)); + RC_Channels::set_override(i, mapped_data, t_now); + } + } + } break; } diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 6d0c39e154..3b163a3948 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -76,6 +76,7 @@ Run the simulator with the following command. If using UDP, the only parameter y | DDS_ENABLE | Set to 1 to enable DDS, or 0 to disable | 1 | | SERIAL1_BAUD | The serial baud rate for DDS | 57 | | SERIAL1_PROTOCOL | Set this to 45 to use DDS on the serial port | 0 | + ```console # Wipe params till you see "AP: ArduPilot Ready" # Select your favorite vehicle type @@ -220,7 +221,7 @@ In order to consume the transforms, it's highly recommended to [create and run a ## Using ROS 2 services -The `AP_DDS` library exposes services which are automatically mapped to ROS 2 +The `AP_DDS` library exposes services which are automatically mapped to ROS 2 services using appropriate naming conventions for topics and message and service types. An earlier version of `AP_DDS` required the use of the eProsima [Integration Service](https://github.com/eProsima/Integration-Service) to map @@ -253,7 +254,32 @@ requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=4) response: ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4) ``` - + +## Commanding using ROS 2 Topics + +The following topic can be used to control the vehicle. + +- `/ap/joy` (type `sensor_msgs/msg/Joy`): overrides a maximum of 8 RC channels, +at least 4 axes must be sent. Values are clamped between -1.0 and 1.0. +Use `NaN` to disable the override of a single channel. +A channel defaults back to RC after 1 second of not receiving commands. + +```bash +ros2 topic pub /ap/joy sensor_msgs/msg/Joy "{axes: [0.0, 0.0, 0.0, 0.0]}" + +publisher: beginning loop +publishing #1: sensor_msgs.msg.Joy(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), axes=[0.0, 0.0, 0.0, 0.0], buttons=[]) +``` +- `/ap/cmd_gps_pose` (type `ardupilot_msgs/msg/GlobalPosition`): sends +a waypoint to head to when the selected mode is GUIDED. + +```bash +ros2 topic pub /ap/cmd_gps_pose ardupilot_msgs/msg/GlobalPosition "{latitude: 34, longitude: 118, altitude: 1000}" + +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 @@ -296,7 +322,7 @@ topic and service tables. ROS 2 message and interface definitions are mangled by the `rosidl_adapter` when mapping from ROS 2 to DDS to avoid naming conflicts in the C/C++ libraries. The ROS 2 object `namespace::Struct` is mangled to `namespace::dds_::Struct_` -for DDS. The table below provides some example mappings: +for DDS. The table below provides some example mappings: | ROS 2 | DDS | | --- | --- | @@ -322,7 +348,7 @@ The request / response pair for services require an additional suffix. | parameter | rp/ | | | action | ra/ | | -The table below provides example mappings for topics and services +The table below provides example mappings for topics and services | ROS 2 | DDS | | --- | --- |