AP_DDS: publish gps global origin

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

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -37,6 +37,7 @@ 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 constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = 1000;
 static constexpr uint16_t DELAY_PING_MS = 500;
 
 // Define the subscriber data members, which are static class scope.
@@ -459,6 +460,24 @@ void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
     update_topic(msg.clock);
 }
 
+void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg)
+{
+    update_topic(msg.header.stamp);
+    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+
+    auto &ahrs = AP::ahrs();
+    WITH_SEMAPHORE(ahrs.get_semaphore());
+
+    Location ekf_origin;
+    // LLA is WGS-84 geodetic coordinate.
+    // Altitude converted from cm to m.
+    if (ahrs.get_origin(ekf_origin)) {
+        msg.position.latitude = ekf_origin.lat * 1E-7;
+        msg.position.longitude = ekf_origin.lng * 1E-7;
+        msg.position.altitude = ekf_origin.alt * 0.01;
+    }
+}
+
 /*
   start the DDS thread
  */
@@ -1040,6 +1059,20 @@ void AP_DDS_Client::write_clock_topic()
     }
 }
 
+void AP_DDS_Client::write_gps_global_origin_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub {};
+        const uint32_t topic_size = geographic_msgs_msg_GeoPointStamped_size_of_topic(&gps_global_origin_topic, 0);
+        uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB)].dw_id, &ub, topic_size);
+        const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &gps_global_origin_topic);
+        if (!success) {
+            // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
+        }
+    }
+}
+
 void AP_DDS_Client::update()
 {
     WITH_SEMAPHORE(csem);
@@ -1093,6 +1126,12 @@ void AP_DDS_Client::update()
         write_clock_topic();
     }
 
+    if (cur_time_ms - last_gps_global_origin_time_ms > DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS) {
+        update_topic(gps_global_origin_topic);
+        last_gps_global_origin_time_ms = cur_time_ms;
+        write_gps_global_origin_topic();
+    }
+
     status_ok = uxr_run_session_time(&session, 1);
 }
 

libraries/AP_DDS/AP_DDS_Client.h

@@ -15,6 +15,7 @@
 #include "sensor_msgs/msg/Joy.h"
 #include "geometry_msgs/msg/PoseStamped.h"
 #include "geometry_msgs/msg/TwistStamped.h"
+#include "geographic_msgs/msg/GeoPointStamped.h"
 #include "geographic_msgs/msg/GeoPoseStamped.h"
 #include "rosgraph_msgs/msg/Clock.h"
 
@@ -58,6 +59,7 @@ private:
 
     // Outgoing Sensor and AHRS data
     builtin_interfaces_msg_Time time_topic;
+    geographic_msgs_msg_GeoPointStamped gps_global_origin_topic;
     geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
     geometry_msgs_msg_PoseStamped local_pose_topic;
     geometry_msgs_msg_TwistStamped tx_local_velocity_topic;
@@ -91,6 +93,7 @@ private:
     static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg);
     static void update_topic(sensor_msgs_msg_Imu& msg);
     static void update_topic(rosgraph_msgs_msg_Clock& msg);
+    static void update_topic(geographic_msgs_msg_GeoPointStamped& msg);
 
     // subscription callback function
     static void on_topic_trampoline(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length, void* args);
@@ -124,6 +127,8 @@ private:
     uint64_t last_geo_pose_time_ms;
     // The last ms timestamp AP_DDS wrote a Clock message
     uint64_t last_clock_time_ms;
+    // The last ms timestamp AP_DDS wrote a gps global origin message
+    uint64_t last_gps_global_origin_time_ms;
 
     // functions for serial transport
     bool ddsSerialInit();
@@ -196,6 +201,8 @@ public:
     void write_imu_topic();
     //! @brief Serialize the current clock and publish to the IO stream(s)
     void write_clock_topic();
+    //! @brief Serialize the current gps global origin and publish to the IO stream(s)
+    void write_gps_global_origin_topic();
     //! @brief Update the internally stored DDS messages with latest data
     void update();
 

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -21,6 +21,7 @@ enum class TopicIndex: uint8_t {
     LOCAL_VELOCITY_PUB,
     GEOPOSE_PUB,
     CLOCK_PUB,
+    GPS_GLOBAL_ORIGIN_PUB,
     JOY_SUB,
     DYNAMIC_TRANSFORMS_SUB,
     VELOCITY_CONTROL_SUB,
@@ -125,6 +126,16 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .dw_profile_label = "clock__dw",
         .dr_profile_label = "",
     },
+    {
+        .topic_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB),
+        .pub_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB),
+        .sub_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB), .type=UXR_DATAREADER_ID},
+        .topic_profile_label = "gps_global_origin__t",
+        .dw_profile_label = "gps_global_origin__dw",
+        .dr_profile_label = "",
+    },
     {
         .topic_id = to_underlying(TopicIndex::JOY_SUB),
         .pub_id = to_underlying(TopicIndex::JOY_SUB),

libraries/AP_DDS/dds_xrce_profile.xml

@@ -246,6 +246,34 @@
       </historyQos>
     </topic>
   </data_writer>
+  <topic profile_name="gps_global_origin__t">
+    <name>rt/ap/gps_global_origin/filtered</name>
+    <dataType>geographic_msgs::msg::dds_::GeoPointStamped_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>5</depth>
+    </historyQos>
+  </topic>
+  <data_writer profile_name="gps_global_origin__dw">
+    <historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
+    <qos>
+      <reliability>
+        <kind>BEST_EFFORT</kind>
+      </reliability>
+      <durability>
+        <kind>VOLATILE</kind>
+      </durability>
+    </qos>
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ap/gps_global_origin/filtered</name>
+      <dataType>geographic_msgs::msg::dds_::GeoPointStamped_</dataType>
+      <historyQos>
+        <kind>KEEP_LAST</kind>
+        <depth>5</depth>
+      </historyQos>
+    </topic>
+  </data_writer>
   <topic profile_name="joy__t">
     <name>rt/ap/joy</name>
     <dataType>sensor_msgs::msg::dds_::Joy_</dataType>