AP_DDS: /ap/cmd_vel accepts body-frame messages

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -2,6 +2,7 @@
 
 #include "AP_DDS_ExternalControl.h"
 #include "AP_DDS_Frames.h"
+#include <AP_AHRS/AP_AHRS.h>
 
 #include <AP_ExternalControl/AP_ExternalControl.h>
 
@@ -11,21 +12,32 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
     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;
+
+    if (strcmp(cmd_vel.header.frame_id, BASE_LINK_FRAME_ID) == 0) {
+        // Convert commands from body frame (x-forward, y-left, z-up) to NED.
+        Vector3f linear_velocity;
+        Vector3f linear_velocity_base_link {
+            float(cmd_vel.twist.linear.x),
+            float(cmd_vel.twist.linear.y),
+            float(-cmd_vel.twist.linear.z) };
+        const float yaw_rate = -cmd_vel.twist.angular.z;
+
+        auto &ahrs = AP::ahrs();
+        linear_velocity = ahrs.body_to_earth(linear_velocity_base_link);
+        return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);
+    }
+
+    else if (strcmp(cmd_vel.header.frame_id, MAP_FRAME) == 0) {
+        // 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);
     }
 
-    // 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);
+    return false;
 }