AP_DDS: Restrict support to map-relative frame

* Against REP-147 for now till AP_ExternalControl has an interface for body-frame control Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
ArduPilot · Aug 16, 2023 · f5f491b · f5f491b
1 parent 3cf9f7f
commit f5f491b
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 4 deletions.
diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
@@ -14,6 +14,7 @@
 #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"
@@ -28,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,

diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp
@@ -1,6 +1,7 @@
 #if AP_DDS_ENABLED
 
 #include "AP_DDS_ExternalControl.h"
+#include "AP_DDS_Frames.h"
 
 #include <AP_ExternalControl/AP_ExternalControl.h>
 
@@ -10,6 +11,13 @@ 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;
+    }
 
     // Convert commands from ENU to NED frame
     Vector3f linear_velocity {

diff --git a/libraries/AP_DDS/AP_DDS_Frames.h b/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";