AP_DDS: add REP-147 Global Position Control

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -44,6 +44,7 @@ static constexpr uint16_t DELAY_PING_MS = 500;
 sensor_msgs_msg_Joy AP_DDS_Client::rx_joy_topic {};
 tf2_msgs_msg_TFMessage AP_DDS_Client::rx_dynamic_transforms_topic {};
 geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {};
+ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {};
 
 
 const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
@@ -391,6 +392,8 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
     if (ahrs.get_location(loc)) {
         msg.pose.position.latitude = loc.lat * 1E-7;
         msg.pose.position.longitude = loc.lng * 1E-7;
+        // TODO this is assumed to be absolute frame in WGS-84 as per the GeoPose message definition in ROS.
+        // Use loc.get_alt_frame() to convert if necessary.
         msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m
     }
 
@@ -502,6 +505,19 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
         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;
+    }
+    case topics[to_underlying(TopicIndex::GLOBAL_POSITION_SUB)].dr_id.id: {
+        const bool success = ardupilot_msgs_msg_GlobalPosition_deserialize_topic(ub, &rx_global_position_control_topic);
+        if (success == false) {
+            break;
+        }
+
+#if AP_EXTERNAL_CONTROL_ENABLED
+        if (!AP_DDS_External_Control::handle_global_position_control(rx_global_position_control_topic)) {
+            // TODO #23430 handle global position control failure through rosout, throttled.
+        }
 #endif // AP_EXTERNAL_CONTROL_ENABLED
         break;
     }

libraries/AP_DDS/AP_DDS_Client.h

@@ -4,6 +4,8 @@
 
 #include "uxr/client/client.h"
 #include "ucdr/microcdr.h"
+
+#include "ardupilot_msgs/msg/GlobalPosition.h"
 #include "builtin_interfaces/msg/Time.h"
 
 #include "sensor_msgs/msg/NavSatFix.h"
@@ -65,6 +67,8 @@ private:
     static sensor_msgs_msg_Joy rx_joy_topic;
     // incoming REP147 velocity control
     static geometry_msgs_msg_TwistStamped rx_velocity_control_topic;
+    // incoming REP147 goal interface global position
+    static ardupilot_msgs_msg_GlobalPosition rx_global_position_control_topic;
     // outgoing transforms
     tf2_msgs_msg_TFMessage tx_static_transforms_topic;
     tf2_msgs_msg_TFMessage tx_dynamic_transforms_topic;

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -6,6 +6,49 @@
 
 #include <AP_ExternalControl/AP_ExternalControl.h>
 
+// These are the Goal Interface constants. Because microxrceddsgen does not expose
+// them in the generated code, they are manually maintained
+// Position ignore flags
+static constexpr uint16_t TYPE_MASK_IGNORE_LATITUDE = 1;
+static constexpr uint16_t TYPE_MASK_IGNORE_LONGITUDE = 2;
+static constexpr uint16_t TYPE_MASK_IGNORE_ALTITUDE = 4;
+
+bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos)
+{
+    auto *external_control = AP::externalcontrol();
+    if (external_control == nullptr) {
+        return false;
+    }
+
+    if (strcmp(cmd_pos.header.frame_id, MAP_FRAME) == 0) {
+        // Narrow the altitude
+        const int32_t alt_cm  = static_cast<int32_t>(cmd_pos.altitude * 100);
+
+        Location::AltFrame alt_frame;
+        if (!convert_alt_frame(cmd_pos.coordinate_frame, alt_frame)) {
+            return false;
+        }
+
+        constexpr uint32_t MASK_POS_IGNORE =
+            TYPE_MASK_IGNORE_LATITUDE |
+            TYPE_MASK_IGNORE_LONGITUDE |
+            TYPE_MASK_IGNORE_ALTITUDE;
+
+        if (!(cmd_pos.type_mask & MASK_POS_IGNORE)) {
+            Location loc(cmd_pos.latitude * 1E7, cmd_pos.longitude * 1E7, alt_cm, alt_frame);
+            if (!external_control->set_global_position(loc)) {
+                return false; // Don't try sending other commands if this fails
+            }
+        }
+
+        // TODO add velocity and accel handling
+
+        return true;
+    }
+
+    return false;
+}
+
 bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel)
 {
     auto *external_control = AP::externalcontrol();
@@ -40,5 +83,25 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
     return false;
 }
 
+bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out)
+{
+
+    // Specified in ROS REP-147; only some are supported.
+    switch (frame_in) {
+    case 5: // FRAME_GLOBAL_INT
+        frame_out = Location::AltFrame::ABSOLUTE;
+        break;
+    case 6: // FRAME_GLOBAL_REL_ALT
+        frame_out = Location::AltFrame::ABOVE_HOME;
+        break;
+    case 11: // FRAME_GLOBAL_TERRAIN_ALT
+        frame_out = Location::AltFrame::ABOVE_TERRAIN;
+        break;
+    default:
+        return false;
+    }
+    return true;
+}
+
 
 #endif // AP_DDS_ENABLED

libraries/AP_DDS/AP_DDS_ExternalControl.h

@@ -1,11 +1,19 @@
 #pragma once
 
 #if AP_DDS_ENABLED
+#include "ardupilot_msgs/msg/GlobalPosition.h"
 #include "geometry_msgs/msg/TwistStamped.h"
 
+#include <AP_Common/Location.h>
+
 class AP_DDS_External_Control
 {
 public:
+    // REP-147 Goal Interface Global Position Control
+    // https://ros.org/reps/rep-0147.html#goal-interface
+    static bool handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos);
     static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel);
+private:
+    static bool convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out);
 };
 #endif // AP_DDS_ENABLED

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -22,6 +22,7 @@ enum class TopicIndex: uint8_t {
     JOY_SUB,
     DYNAMIC_TRANSFORMS_SUB,
     VELOCITY_CONTROL_SUB,
+    GLOBAL_POSITION_SUB,
 };
 
 static inline constexpr uint8_t to_underlying(const TopicIndex index)
@@ -142,4 +143,14 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .dw_profile_label = "",
         .dr_profile_label = "velocitycontrol__dr",
     },
+    {
+        .topic_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
+        .pub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
+        .sub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAREADER_ID},
+        .topic_profile_label = "globalposcontrol__t",
+        .dw_profile_label = "",
+        .dr_profile_label = "globalposcontrol__dr",
+    },
 };

libraries/AP_DDS/Idl/ardupilot_msgs/msg/GlobalPosition.idl

@@ -0,0 +1,60 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from ardupilot_msgs/msg/GlobalPosition.msg
+// generated code does not contain a copyright notice
+
+#include "geometry_msgs/msg/Twist.idl"
+#include "std_msgs/msg/Header.idl"
+
+module ardupilot_msgs {
+  module msg {
+    module GlobalPosition_Constants {
+      const uint8 FRAME_GLOBAL_INT = 5;
+      const uint8 FRAME_GLOBAL_REL_ALT = 6;
+      const uint8 FRAME_GLOBAL_TERRAIN_ALT = 11;
+      @verbatim (language="comment", text=
+        "Position ignore flags")
+      const uint16 IGNORE_LATITUDE = 1;
+      const uint16 IGNORE_LONGITUDE = 2;
+      const uint16 IGNORE_ALTITUDE = 4;
+      @verbatim (language="comment", text=
+        "Velocity vector ignore flags")
+      const uint16 IGNORE_VX = 8;
+      const uint16 IGNORE_VY = 16;
+      const uint16 IGNORE_VZ = 32;
+      @verbatim (language="comment", text=
+        "Acceleration/Force vector ignore flags")
+      const uint16 IGNORE_AFX = 64;
+      const uint16 IGNORE_AFY = 128;
+      const uint16 IGNORE_AFZ = 256;
+      @verbatim (language="comment", text=
+        "Force in af vector flag")
+      const uint16 FORCE = 512;
+      const uint16 IGNORE_YAW = 1024;
+      const uint16 IGNORE_YAW_RATE = 2048;
+    };
+    @verbatim (language="comment", text=
+      "Experimental REP-147 Goal Interface" "\n"
+      "https://ros.org/reps/rep-0147.html#goal-interface")
+    struct GlobalPosition {
+      std_msgs::msg::Header header;
+
+      uint8 coordinate_frame;
+
+      uint16 type_mask;
+
+      double latitude;
+
+      double longitude;
+
+      @verbatim (language="comment", text=
+        "in meters, AMSL or above terrain")
+      float altitude;
+
+      geometry_msgs::msg::Twist velocity;
+
+      geometry_msgs::msg::Twist acceleration_or_force;
+
+      float yaw;
+    };
+  };
+};

libraries/AP_DDS/README.md

@@ -216,6 +216,7 @@ Published topics:
  * /rosout [rcl_interfaces/msg/Log] 1 publisher
 
 Subscribed topics:
+ * /ap/cmd_gps_pose [ardupilot_msgs/msg/GlobalPosition] 1 subscriber
  * /ap/cmd_vel [geometry_msgs/msg/TwistStamped] 1 subscriber
  * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber
  * /ap/tf [tf2_msgs/msg/TFMessage] 1 subscriber
@@ -311,6 +312,10 @@ cp /opt/ros/humble/share/builtin_interfaces/msg/Time.idl libraries/AP_DDS/Idl/bu
 # Build the code again with the `--enable-dds` flag as described above
 ```
 
+If the message is custom for ardupilot, first create the ROS message in `Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg`.
+Then, build ardupilot_msgs with colcon.
+Finally, copy the IDL folder from the install directory into the source tree.
+
 ### Rules for adding topics and services to `dds_xrce_profile.xml`
 
 Topics and services available from `AP_DDS` are automatically mapped into ROS 2

libraries/AP_DDS/dds_xrce_profile.xml

@@ -276,4 +276,19 @@
     <request_topic_name>rq/ap/mode_switchRequest</request_topic_name>
     <reply_topic_name>rr/ap/mode_switchReply</reply_topic_name>
   </replier>
+  <topic profile_name="globalposcontrol__t">
+    <name>rt/ap/cmd_gps_pose</name>
+    <dataType>ardupilot_msgs::msg::dds_::GlobalPosition_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>5</depth>
+    </historyQos>
+  </topic>
+  <data_reader profile_name="globalposcontrol__dr">
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ap/cmd_gps_pose</name>
+      <dataType>ardupilot_msgs::msg::dds_::GlobalPosition_</dataType>
+    </topic>
+  </data_reader>
 </profiles>