AP_DDS: Add dynamic TF subscriber support

* This is the first step for GSOC Cartographer external odometry input
* Moved subscriber data members to class member variables

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

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -22,6 +22,12 @@ 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,
+// the AP_DDS_Client::on_topic frame size is exceeded.
+sensor_msgs_msg_Joy AP_DDS_Client::rx_joy_topic {};
+tf2_msgs_msg_TFMessage AP_DDS_Client::rx_dynamic_transforms_topic {};
+
 const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
 
     // @Param: _ENABLE
@@ -412,9 +418,8 @@ void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, ui
     */
 
     switch (object_id.id) {
-    case topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id:
-        sensor_msgs_msg_Joy topic;
-        const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, &topic);
+    case topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id: {
+        const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, &rx_joy_topic);
 
         if (success == false) {
             break;
@@ -422,14 +427,29 @@ void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, ui
 
         uint32_t* count_ptr = (uint32_t*) args;
         (*count_ptr)++;
-        if (topic.axes_size >= 4) {
+        if (rx_joy_topic.axes_size >= 4) {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: %f, %f, %f, %f",
-                          topic.axes[0], topic.axes[1], topic.axes[2], topic.axes[3]);
+                          rx_joy_topic.axes[0], rx_joy_topic.axes[1], rx_joy_topic.axes[2], rx_joy_topic.axes[3]);
         } else {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: Insufficient axes size ");
         }
         break;
     }
+    case topics[to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB)].dr_id.id: {
+        const bool success = tf2_msgs_msg_TFMessage_deserialize_topic(ub, &rx_dynamic_transforms_topic);
+        if (success == false) {
+            break;
+        }
+        uint32_t* count_ptr = (uint32_t*) args;
+        (*count_ptr)++;
+        if (rx_dynamic_transforms_topic.transforms_size > 0) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage of length: %u",
+                          static_cast<unsigned>(rx_dynamic_transforms_topic.transforms_size));
+        } else {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage: Insufficient size ");
+        }
+    }
+    }
 
 }
 
@@ -444,7 +464,7 @@ void AP_DDS_Client::main_loop(void)
     }
     GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Initialization passed");
 
-    populate_static_transforms(static_transforms_topic);
+    populate_static_transforms(tx_static_transforms_topic);
     write_static_transforms();
 
     while (true) {
@@ -636,9 +656,9 @@ 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);
+        const uint32_t topic_size = tf2_msgs_msg_TFMessage_size_of_topic(&tx_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);
+        const bool success = tf2_msgs_msg_TFMessage_serialize_topic(&ub, &tx_static_transforms_topic);
         if (!success) {
             // TODO sometimes serialization fails on bootup. Determine why.
             // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");

libraries/AP_DDS/AP_DDS_Client.h

@@ -54,16 +54,21 @@ private:
     uxrStreamId reliable_in;
     uxrStreamId reliable_out;
 
-    // Topic
+    // Outgoing Sensor and AHRS data
     builtin_interfaces_msg_Time time_topic;
-    sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
-    tf2_msgs_msg_TFMessage static_transforms_topic;
-    sensor_msgs_msg_BatteryState battery_state_topic;
-    sensor_msgs_msg_Joy joy_topic;
+    geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
     geometry_msgs_msg_PoseStamped local_pose_topic;
     geometry_msgs_msg_TwistStamped local_velocity_topic;
-    geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
+    sensor_msgs_msg_BatteryState battery_state_topic;
+    sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
     rosgraph_msgs_msg_Clock clock_topic;
+    // incoming joystick data
+    static sensor_msgs_msg_Joy rx_joy_topic;
+    // outgoing transforms
+    tf2_msgs_msg_TFMessage tx_static_transforms_topic;
+    tf2_msgs_msg_TFMessage tx_dynamic_transforms_topic;
+    // incoming transforms
+    static tf2_msgs_msg_TFMessage rx_dynamic_transforms_topic;
 
     HAL_Semaphore csem;
 

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -20,6 +20,7 @@ enum class TopicIndex: uint8_t {
     GEOPOSE_PUB,
     CLOCK_PUB,
     JOY_SUB,
+    DYNAMIC_TRANSFORMS_SUB,
 };
 
 static inline constexpr uint8_t to_underlying(const TopicIndex index)
@@ -120,4 +121,14 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .dw_profile_label = "",
         .dr_profile_label = "joy__dr",
     },
+    {
+        .topic_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
+        .pub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
+        .sub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), .type=UXR_DATAREADER_ID},
+        .topic_profile_label = "dynamictf__t",
+        .dw_profile_label = "",
+        .dr_profile_label = "dynamictf__dr",
+    },
 };

libraries/AP_DDS/README.md

@@ -213,6 +213,7 @@ Next, follow the associated section for your chosen transport, and finally you c
 
   Subscribed topics:
   * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber
+  * /ap/tf [tf2_msgs/msg/TFMessage] 1 subscriber
 
   $ ros2 topic hz /ap/time
   average rate: 50.115

libraries/AP_DDS/dds_xrce_profile.xml

@@ -238,4 +238,19 @@
       <dataType>sensor_msgs::msg::dds_::Joy_</dataType>
     </topic>
   </data_reader>
+  <topic profile_name="dynamictf__t">
+    <name>rt/ap/tf</name>
+    <dataType>tf2_msgs::msg::dds_::TFMessage_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>5</depth>
+    </historyQos>
+  </topic>
+  <data_reader profile_name="dynamictf__dr">
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ap/tf</name>
+      <dataType>tf2_msgs::msg::dds_::TFMessage_</dataType>
+    </topic>
+  </data_reader>
 </profiles>