AP_DDS: Accept body-frame velocity inputs

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -11,21 +11,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 {
+            float(cmd_vel.linear.x),
+            float(cmd_vel.linear.y),
+            float(-cmd_vel.linear.z) };
+        const float yaw_rate = -cmd_vel.angular.z;
+
+        auto &ahrs = AP::ahrs();
+        linear_velocity = ahrs.body_to_earth(linear_velocity);
+        return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate);
     }
+
+    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;
 }