AP_DDS: use AP_ExternalControl for velocity control

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
Co-authored-by: Andrew Tridgell <tridge60@gmail.com>

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -10,6 +10,11 @@
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Arming/AP_Arming.h>
+#include <AP_ExternalControl/AP_ExternalControl_config.h>
+#if AP_EXTERNAL_CONTROL_ENABLED
+#include "AP_DDS_ExternalControl.h"
+#endif
+#include "AP_DDS_Frames.h"
 
 #include "AP_DDS_Client.h"
 #include "AP_DDS_Topic_Table.h"
@@ -24,10 +29,6 @@ static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33;
 static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33;
 static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33;
 static constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10;
-static char WGS_84_FRAME_ID[] = "WGS-84";
-
-// https://www.ros.org/reps/rep-0105.html#base-link
-static char BASE_LINK_FRAME_ID[] = "base_link";
 
 // Define the subscriber data members, which are static class scope.
 // If these are created on the stack in the subscriber,
@@ -475,8 +476,11 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
         }
 
         subscribe_sample_count++;
-        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received geometry_msgs/TwistStamped");
-        // TODO implement the velocity control to AP
+#if AP_EXTERNAL_CONTROL_ENABLED
+        if (!AP_DDS_External_Control::handle_velocity_control(rx_velocity_control_topic)) {
+            // TODO #23430 handle velocity control failure through rosout, throttled.
+        }
+#endif // AP_EXTERNAL_CONTROL_ENABLED
         break;
     }
     }

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -0,0 +1,32 @@
+#if AP_DDS_ENABLED
+
+#include "AP_DDS_ExternalControl.h"
+#include "AP_DDS_Frames.h"
+
+#include <AP_ExternalControl/AP_ExternalControl.h>
+
+bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel)
+{
+    auto *external_control = AP::externalcontrol();
+    if (external_control == nullptr) {
+        return false;
+    }
+    if (strcmp(cmd_vel.header.frame_id, MAP_FRAME) != 0) {
+        // Although REP-147 says cmd_vel should be in body frame, all the AP math is done in earth frame.
+        // This is because accounting for the gravity vector.
+        // Although the ROS 2 interface could support body-frame velocity control in the future,
+        // it is currently not supported.
+        return false;
+    }
+
+    // Convert commands from ENU to NED frame
+    Vector3f linear_velocity {
+        float(cmd_vel.twist.linear.y),
+        float(cmd_vel.twist.linear.x),
+        float(-cmd_vel.twist.linear.z) };
+    const float yaw_rate = -cmd_vel.twist.angular.z;
+    return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);
+}
+
+
+#endif // AP_DDS_ENABLED

libraries/AP_DDS/AP_DDS_ExternalControl.h

@@ -0,0 +1,11 @@
+#pragma once
+
+#if AP_DDS_ENABLED
+#include "geometry_msgs/msg/TwistStamped.h"
+
+class AP_DDS_External_Control
+{
+public:
+    static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel);
+};
+#endif // AP_DDS_ENABLED

libraries/AP_DDS/AP_DDS_Frames.h

@@ -0,0 +1,7 @@
+#pragma once
+
+static constexpr char WGS_84_FRAME_ID[] = "WGS-84";
+// https://www.ros.org/reps/rep-0105.html#base-link
+static constexpr char BASE_LINK_FRAME_ID[] = "base_link";
+// https://www.ros.org/reps/rep-0105.html#map
+static constexpr char MAP_FRAME[] = "map";