AP_DDS: Implement joystick support

Signed-off-by: Ryan Friedman <[email protected]>
Co-Authored-by: Tiziano Fiorenzani

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -5,6 +5,7 @@
 
 #include <AP_GPS/AP_GPS.h>
 #include <AP_HAL/AP_HAL.h>
+#include <RC_Channel/RC_Channel.h>
 #include <AP_RTC/AP_RTC.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
@@ -554,11 +555,31 @@ 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
+            GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "%s Received %d sensor_msgs/Joy: %f, %f, %f, %f",
+                          msg_prefix, rx_joy_topic.axes_size, rx_joy_topic.axes[0],
+                          rx_joy_topic.axes[1], rx_joy_topic.axes[2], rx_joy_topic.axes[3]);
+            if (rx_joy_topic.axes_size > 8U) {
+                GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "%s Only first 8 Joy Axes will be processed.", 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 channels is NaN.
+                // Setting the override to 0U releases the channel back to the RC.
+                if (std::isnan(rx_joy_topic.axes[i])) {
+                    RC_Channels::set_override(i, 0U, t_now);
+                } else {
+                    const uint16_t mapped_data = static_cast<uint16_t>(
+                                                     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);
+                }
+            }
+
         } else {
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Received sensor_msgs/Joy. Axes size must be >= 4", msg_prefix);
+            GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "%s Received sensor_msgs/Joy. Axes size must be >= 4", msg_prefix);
         }
         break;
     }

libraries/AP_DDS/README.md

@@ -253,6 +253,29 @@ 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. Use `NaN` to disable the override of a single channel.
+
+```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