AP_DDS: Add support for static transforms

* This encodes the position of the GPS receivers relative to the aircraft origin

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

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -3,6 +3,7 @@
 #if AP_DDS_ENABLED
 
 #include <AP_GPS/AP_GPS.h>
+#include <AP_HAL/AP_HAL.h>
 #include <AP_RTC/AP_RTC.h>
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <GCS_MAVLink/GCS.h>
@@ -10,9 +11,12 @@
 #include "AP_DDS_Client.h"
 #include "generated/Time.h"
 
+
 static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10;
 static constexpr uint16_t DELAY_NAV_SAT_FIX_TOPIC_MS = 1000;
 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";
 
 AP_HAL::UARTDriver *dds_port;
 
@@ -120,6 +124,52 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i
     msg.position_covariance[8] = vdopSq;
 }
 
+#include "generated/TransformStamped.h"
+
+void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
+{
+    msg.transforms_size = 0;
+
+    auto &gps = AP::gps();
+    for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+        const auto gps_type = gps.get_type(i);
+        if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) {
+            continue;
+        }
+        update_topic(msg.transforms[i].header.stamp);
+        char gps_frame_id[16];
+        //! @todo should GPS frame ID's be 0 or 1 indexed in ROS?
+        hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i);
+        strcpy(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID);
+        strcpy(msg.transforms[i].child_frame_id, gps_frame_id);
+        // The body-frame offsets
+        // X - Forward
+        // Y - Right
+        // Z - Down
+        // https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation
+
+        const auto offset = gps.get_antenna_offset(i);
+
+        // In ROS REP 103, it follows this convention
+        // X - Forward
+        // Y - Left
+        // Z - Up
+        // https://www.ros.org/reps/rep-0103.html#axis-orientation
+
+        msg.transforms[i].transform.translation.x = offset[0];
+        msg.transforms[i].transform.translation.y = -1 * offset[1];
+        msg.transforms[i].transform.translation.z = -1 * offset[2];
+
+        msg.transforms_size++;
+    }
+
+    // msg.transforms[0] = transform;
+
+
+
+    // const auto offset = AP::GPS::
+}
+
 
 /*
   class constructor
@@ -143,6 +193,10 @@ void AP_DDS_Client::main_loop(void)
         return;
     }
     GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Initialization passed");
+
+    populate_static_transforms(static_transforms_topic);
+    write_static_transforms();
+
     while (true) {
         hal.scheduler->delay(1);
         update();
@@ -280,6 +334,21 @@ void AP_DDS_Client::write_nav_sat_fix_topic()
     }
 }
 
+void AP_DDS_Client::write_static_transforms()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub;
+        const uint32_t topic_size = tf2_msgs_msg_TFMessage_size_of_topic(&static_transforms_topic, 0);
+        uxr_prepare_output_stream(&session,reliable_out,topics[2].dw_id,&ub,topic_size);
+        const bool success = tf2_msgs_msg_TFMessage_serialize_topic(&ub, &static_transforms_topic);
+        if (!success) {
+            // TODO sometimes serialization fails on bootup. Determine why.
+            // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
+        }
+    }
+}
+
 void AP_DDS_Client::update()
 {
     WITH_SEMAPHORE(csem);

libraries/AP_DDS/AP_DDS_Client.h

@@ -7,6 +7,8 @@
 #include "generated/Time.h"
 #include "AP_DDS_Generic_Fn_T.h"
 #include "generated/NavSatFix.h"
+#include "generated/TFMessage.h"
+
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/Scheduler.h>
@@ -41,6 +43,7 @@ private:
     // Topic
     builtin_interfaces_msg_Time time_topic;
     sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
+    tf2_msgs_msg_TFMessage static_transforms_topic;
 
     HAL_Semaphore csem;
 
@@ -49,6 +52,7 @@ private:
 
     static void update_topic(builtin_interfaces_msg_Time& msg);
     static void update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance);
+    static void populate_static_transforms(tf2_msgs_msg_TFMessage& msg);
 
     // The last ms timestamp AP_DDS wrote a Time message
     uint64_t last_time_time_ms;
@@ -71,10 +75,12 @@ public:
     //! @return True on successful creation, false on failure
     bool create() WARN_IF_UNUSED;
 
-    //! @brief Serialize the current time state and publish to to the IO stream(s)
+    //! @brief Serialize the current time state and publish to the IO stream(s)
     void write_time_topic();
-    //! @brief Serialize the current nav_sat_fix state and publish to to the IO stream(s)
+    //! @brief Serialize the current nav_sat_fix state and publish to the IO stream(s)
     void write_nav_sat_fix_topic();
+    //! @brief Serialize the static transforms and publish to the IO stream(s)
+    void write_static_transforms();
     //! @brief Update the internally stored DDS messages with latest data
     void update();
 

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -1,5 +1,6 @@
 #include "generated/Time.h"
 #include "generated/NavSatFix.h"
+#include "generated/TransformStamped.h"
 
 
 #include "AP_DDS_Generic_Fn_T.h"
@@ -31,4 +32,14 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_NavSatFix_deserialize_topic),
         .size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_NavSatFix_size_of_topic),
     },
+    {
+        .topic_id = 0x03,
+        .pub_id = 0x03,
+        .dw_id = uxrObjectId{.id=0x03, .type=UXR_DATAWRITER_ID},
+        .topic_profile_label = "statictransforms__t",
+        .dw_profile_label = "statictransforms__dw",
+        .serialize = Generic_serialize_topic_fn_t(&tf2_msgs_msg_TFMessage_serialize_topic),
+        .deserialize = Generic_deserialize_topic_fn_t(&tf2_msgs_msg_TFMessage_deserialize_topic),
+        .size_of = Generic_size_of_topic_fn_t(&tf2_msgs_msg_TFMessage_size_of_topic),
+    },
 };

libraries/AP_DDS/Idl/TFMessage.idl

@@ -0,0 +1,13 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from tf2_msgs/msg/TFMessage.msg
+// generated code does not contain a copyright notice
+
+#include "TransformStamped.idl"
+
+module tf2_msgs {
+  module msg {
+    struct TFMessage {
+      sequence<geometry_msgs::msg::TransformStamped> transforms;
+    };
+  };
+};

libraries/AP_DDS/Idl/Transform.idl

@@ -0,0 +1,18 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from geometry_msgs/msg/Transform.msg
+// generated code does not contain a copyright notice
+
+#include "Quaternion.idl"
+#include "Vector3.idl"
+
+module geometry_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "This represents the transform between two coordinate frames in free space.")
+    struct Transform {
+      geometry_msgs::msg::Vector3 translation;
+
+      geometry_msgs::msg::Quaternion rotation;
+    };
+  };
+};

libraries/AP_DDS/Idl/TransformStamped.idl

@@ -0,0 +1,35 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from geometry_msgs/msg/TransformStamped.msg
+// generated code does not contain a copyright notice
+
+#include "Transform.idl"
+#include "Header.idl"
+
+module geometry_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "This expresses a transform from coordinate frame header.frame_id" "\n"
+      "to the coordinate frame child_frame_id at the time of header.stamp" "\n"
+      "" "\n"
+      "This message is mostly used by the" "\n"
+      "<a href=\"https://index.ros.org/p/tf2/\">tf2</a> package." "\n"
+      "See its documentation for more information." "\n"
+      "" "\n"
+      "The child_frame_id is necessary in addition to the frame_id" "\n"
+      "in the Header to communicate the full reference for the transform" "\n"
+      "in a self contained message.")
+    struct TransformStamped {
+      @verbatim (language="comment", text=
+        "The frame id in the header is used as the reference frame of this transform.")
+      std_msgs::msg::Header header;
+
+      @verbatim (language="comment", text=
+        "The frame id of the child frame to which this transform points.")
+      string child_frame_id;
+
+      @verbatim (language="comment", text=
+        "Translation and rotation in 3-dimensions of child_frame_id from header.frame_id.")
+      geometry_msgs::msg::Transform transform;
+    };
+  };
+};

libraries/AP_DDS/README.md

@@ -146,9 +146,11 @@ After your setups are complete, do the following:
 
   $ ros2 topic list  -v
   Published topics:
-  * /ROS2_Time [builtin_interfaces/msg/Time] 1 publisher
-  * /parameter_events [rcl_interfaces/msg/ParameterEvent] 1 publisher
-  * /rosout [rcl_interfaces/msg/Log] 1 publisher
+   * /ROS2_NavSatFix0 [sensor_msgs/msg/NavSatFix] 1 publisher
+   * /ROS2_Time [builtin_interfaces/msg/Time] 1 publisher
+   * /parameter_events [rcl_interfaces/msg/ParameterEvent] 1 publisher
+   * /rosout [rcl_interfaces/msg/Log] 1 publisher
+   * /tf [tf2_msgs/msg/TFMessage] 1 publisher
 
   Subscribed topics:
 
@@ -162,6 +164,12 @@ After your setups are complete, do the following:
   ---
   ```
 
+  The static transforms for enabled sensors are also published, and can be recieved like so:
+  ```console
+  ros2 topic echo /tf --qos-depth 1 --qos-history keep_last --qos-reliability reliable --qos-durability transient_local --once
+  ```
+  In order to consume the transforms, it's highly recommended to [create and run a transform broadcaster in ROS 2](https://docs.ros.org/en/humble/Concepts/About-Tf2.html#tutorials). 
+
 ## Adding DDS messages to Ardupilot
 
 Unlike the use of ROS 2 `.msg` files, since Ardupilot supports native DDS, the message files follow [OMG IDL DDS v4.2](https://www.omg.org/spec/IDL/4.2/PDF).

libraries/AP_DDS/dds_xrce_profile.xml

@@ -58,4 +58,32 @@
       </historyQos>
     </topic>
   </data_writer>
+  <topic profile_name="statictransforms__t">
+    <name>rt/tf</name>
+    <dataType>tf2_msgs::msg::dds_::TFMessage_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>1</depth>
+    </historyQos>
+  </topic>
+  <data_writer profile_name="statictransforms__dw">
+    <historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
+    <qos>
+      <reliability>
+        <kind>RELIABLE</kind>
+      </reliability>
+      <durability>
+        <kind>TRANSIENT_LOCAL</kind>
+      </durability>
+    </qos>
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/tf</name>
+      <dataType>tf2_msgs::msg::dds_::TFMessage_</dataType>
+      <historyQos>
+        <kind>KEEP_LAST</kind>
+        <depth>1</depth>
+      </historyQos>
+    </topic>
+  </data_writer>
 </profiles>

libraries/AP_DDS/gen_config_h.py

@@ -22,7 +22,7 @@ config = {
     'UCLIENT_MIN_HEARTBEAT_TIME_INTERVAL': 200,
     'UCLIENT_UDP_TRANSPORT_MTU': 300,
     'UCLIENT_TCP_TRANSPORT_MTU': 350,
-    'UCLIENT_SERIAL_TRANSPORT_MTU': 250,
+    'UCLIENT_SERIAL_TRANSPORT_MTU': 1024,
     'UCLIENT_CUSTOM_TRANSPORT_MTU': 400,
     'CONFIG_MACHINE_ENDIANNESS': 1,  # little endian
     'UCLIENT_SHARED_MEMORY_MAX_ENTITIES': 0,

libraries/AP_DDS/wscript

@@ -83,6 +83,9 @@ def build(bld):
         'PoseStamped.idl',
         'Quaternion.idl',
         'Time.idl',
+        'Transform.idl',
+        'TransformStamped.idl',
+        "TFMessage.idl",
         'Twist.idl',
         'TwistStamped.idl',
         'Vector3.idl',