AP_DDS: add publisher for rosgraph_msgs/msg/Clock

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -17,6 +17,7 @@ static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000;
 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";
@@ -374,6 +375,11 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
     }
 }
 
+void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
+{
+    update_topic(msg.clock);
+}
+
 /*
   start the DDS thread
  */
@@ -627,6 +633,21 @@ void AP_DDS_Client::write_geo_pose_topic()
     }
 }
 
+void AP_DDS_Client::write_clock_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub;
+        const uint32_t topic_size = rosgraph_msgs_msg_Clock_size_of_topic(&clock_topic, 0);
+        uxr_prepare_output_stream(&session,reliable_out,topics[7].dw_id,&ub,topic_size);
+        const bool success = rosgraph_msgs_msg_Clock_serialize_topic(&ub, &clock_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);
@@ -668,6 +689,12 @@ void AP_DDS_Client::update()
         write_geo_pose_topic();
     }
 
+    if (cur_time_ms - last_clock_time_ms > DELAY_CLOCK_TOPIC_MS) {
+        update_topic(clock_topic);
+        last_clock_time_ms = cur_time_ms;
+        write_clock_topic();
+    }
+
     connected = uxr_run_session_time(&session, 1);
 }
 

libraries/AP_DDS/AP_DDS_Client.h

@@ -13,6 +13,7 @@
 #include "geometry_msgs/msg/PoseStamped.h"
 #include "geometry_msgs/msg/TwistStamped.h"
 #include "geographic_msgs/msg/GeoPoseStamped.h"
+#include "rosgraph_msgs/msg/Clock.h"
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/Scheduler.h>
@@ -60,6 +61,7 @@ private:
     geometry_msgs_msg_PoseStamped local_pose_topic;
     geometry_msgs_msg_TwistStamped local_velocity_topic;
     geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
+    rosgraph_msgs_msg_Clock clock_topic;
 
     HAL_Semaphore csem;
 
@@ -73,6 +75,7 @@ private:
     static void update_topic(geometry_msgs_msg_PoseStamped& msg);
     static void update_topic(geometry_msgs_msg_TwistStamped& msg);
     static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg);
+    static void update_topic(rosgraph_msgs_msg_Clock& msg);
 
     // The last ms timestamp AP_DDS wrote a Time message
     uint64_t last_time_time_ms;
@@ -86,6 +89,8 @@ private:
     uint64_t last_local_velocity_time_ms;
     // The last ms timestamp AP_DDS wrote a GeoPose message
     uint64_t last_geo_pose_time_ms;
+    // The last ms timestamp AP_DDS wrote a Clock message
+    uint64_t last_clock_time_ms;
 
     // functions for serial transport
     bool ddsSerialInit();
@@ -146,6 +151,8 @@ public:
     void write_local_velocity_topic();
     //! @brief Serialize the current geo_pose and publish to the IO stream(s)
     void write_geo_pose_topic();
+    //! @brief Serialize the current clock and publish to the IO stream(s)
+    void write_clock_topic();
     //! @brief Update the internally stored DDS messages with latest data
     void update();
 

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -83,4 +83,14 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .deserialize = Generic_deserialize_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_deserialize_topic),
         .size_of = Generic_size_of_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_size_of_topic),
     },
+    {
+        .topic_id = 0x08,
+        .pub_id = 0x08,
+        .dw_id = uxrObjectId{.id=0x08, .type=UXR_DATAWRITER_ID},
+        .topic_profile_label = "clock__t",
+        .dw_profile_label = "clock__dw",
+        .serialize = Generic_serialize_topic_fn_t(&rosgraph_msgs_msg_Clock_serialize_topic),
+        .deserialize = Generic_deserialize_topic_fn_t(&rosgraph_msgs_msg_Clock_deserialize_topic),
+        .size_of = Generic_size_of_topic_fn_t(&rosgraph_msgs_msg_Clock_size_of_topic),
+    },
 };

libraries/AP_DDS/dds_xrce_profile.xml

@@ -198,4 +198,29 @@
       </historyQos>
     </topic>
   </data_writer>
+  <topic profile_name="clock__t">
+    <name>rt/ap/clock</name>
+    <dataType>rosgraph_msgs::msg::dds_::Clock_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>20</depth>
+    </historyQos>
+  </topic>
+  <data_writer profile_name="clock__dw">
+    <historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
+    <qos>
+      <reliability>
+        <kind>RELIABLE</kind>
+      </reliability>
+    </qos>
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ap/clock</name>
+      <dataType>rosgraph_msgs::msg::dds_::Clock_</dataType>
+      <historyQos>
+        <kind>KEEP_LAST</kind>
+        <depth>20</depth>
+      </historyQos>
+    </topic>
+  </data_writer>
 </profiles>