AP_DDS: Add multi-topic support with NavSatFix

* Implement NavSatFix message
* Support covariance
* Set frame ID to WGS-84
* Closes #23284

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

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -2,6 +2,7 @@
 
 #if AP_DDS_ENABLED
 
+#include <AP_GPS/AP_GPS.h>
 #include <AP_RTC/AP_RTC.h>
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <GCS_MAVLink/GCS.h>
@@ -9,6 +10,11 @@
 #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 constexpr uint8_t GPS_INSTANCE_NO = 0; // Use primary GPS
+static char WGS_84_FRAME_ID[] = "WGS-84";
+
 AP_HAL::UARTDriver *dds_port;
 
 
@@ -31,6 +37,83 @@ void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg)
 
 }
 
+void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg)
+{
+    // Add a lambda that takes in navsatfix msg and populates the cov
+    // Make it constexpr if possible
+    // https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/
+    // constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; };
+
+
+    update_topic(msg.header.stamp);
+    strcpy(msg.header.frame_id, WGS_84_FRAME_ID);
+    msg.status.service = 0; // SERVICE_GPS
+    msg.status.status = -1; // STATUS_NO_FIX
+
+    if (!AP::gps().is_healthy()) {
+        msg.status.status = -1; // STATUS_NO_FIX
+        msg.status.service = 0; // No services supported
+        msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
+        return;
+    }
+
+
+    //! @todo What about glonass, compass, galileo?
+    //! This will be properly designed and implemented to spec in #23277
+    msg.status.service = 1; // SERVICE_GPS
+
+    const auto status = AP::gps().status(GPS_INSTANCE_NO);
+    switch (status) {
+    case AP_GPS::NO_GPS:
+    case AP_GPS::NO_FIX:
+        msg.status.status = -1; // STATUS_NO_FIX
+        msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
+        return;
+    case AP_GPS::GPS_OK_FIX_2D:
+    case AP_GPS::GPS_OK_FIX_3D:
+        msg.status.status = 0; // STATUS_FIX
+        msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED
+        break;
+    case AP_GPS::GPS_OK_FIX_3D_DGPS:
+        msg.status.status = 1; // STATUS_SBAS_FIX
+        msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED
+        break;
+    case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT:
+    case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED:
+        msg.status.status = 2; // STATUS_SBAS_FIX
+        msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED
+        // RTK provides "rtk_accuracy" member, should it be used for the covariance?
+        break;
+    default:
+        //! @todo Can we not just use an enum class and not worry about this condition?
+        break;
+    }
+    const auto loc = AP::gps().location(GPS_INSTANCE_NO);
+    msg.latitude = loc.lat * 1E-7;
+    msg.longitude = loc.lng * 1E-7;
+
+    int32_t alt_cm;
+    if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) {
+        // With absolute frame, this condition is unlikely
+        msg.status.status = -1; // STATUS_NO_FIX
+        msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
+        return;
+    }
+    msg.altitude = alt_cm / 100.0;
+
+    // Calculate covariance: https://answers.ros.org/question/10310/calculate-navsatfix-covariance/
+    // https://github.com/ros-drivers/nmea_navsat_driver/blob/indigo-devel/src/libnmea_navsat_driver/driver.py#L110-L114
+    //! @todo This calculation will be moved to AP::gps and fixed in #23259
+    //! It is a placeholder for now matching the ROS1 nmea_navsat_driver behavior
+    const auto hdop = AP::gps().get_hdop(GPS_INSTANCE_NO);
+    const auto hdopSq = hdop * hdop;
+    const auto vdop = AP::gps().get_vdop(GPS_INSTANCE_NO);
+    const auto vdopSq = vdop * vdop;
+    msg.position_covariance[0] = hdopSq;
+    msg.position_covariance[4] = hdopSq;
+    msg.position_covariance[8] = vdopSq;
+}
+
 
 /*
   class constructor
@@ -107,64 +190,108 @@ bool AP_DDS_Client::create()
     const char* participant_ref = "participant_profile";
     const auto participant_req_id = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id,0,participant_ref,UXR_REPLACE);
 
-    // Topic
+    //Participant requests
-    const uxrObjectId topic_id = {
+    constexpr uint8_t nRequestsParticipant = 1;
-        .id = topics[0].topic_id,
+    const uint16_t requestsParticipant[nRequestsParticipant] = {participant_req_id};
-        .type = UXR_TOPIC_ID
-    };
-    const char* topic_ref = topics[0].topic_profile_label;
-    const auto topic_req_id = uxr_buffer_create_topic_ref(&session,reliable_out,topic_id,participant_id,topic_ref,UXR_REPLACE);
-
-    // Publisher
-    const uxrObjectId pub_id = {
-        .id = topics[0].pub_id,
-        .type = UXR_PUBLISHER_ID
-    };
-    const char* pub_xml = "";
-    const auto pub_req_id = uxr_buffer_create_publisher_xml(&session,reliable_out,pub_id,participant_id,pub_xml,UXR_REPLACE);
-
-    // Data Writer
-    const char* data_writer_ref = topics[0].dw_profile_label;
-    const auto dwriter_req_id = uxr_buffer_create_datawriter_ref(&session,reliable_out,dwriter_id,pub_id,data_writer_ref,UXR_REPLACE);
-
-    //Status requests
-    constexpr uint8_t nRequests = 4;
-    const uint16_t requests[nRequests] = {participant_req_id, topic_req_id, pub_req_id, dwriter_req_id};
 
     constexpr int maxTimeMsPerRequestMs = 250;
-    constexpr int requestTimeoutMs = nRequests * maxTimeMsPerRequestMs;
+    constexpr int requestTimeoutParticipantMs = nRequestsParticipant * maxTimeMsPerRequestMs;
-    uint8_t status[nRequests];
+    uint8_t statusPartipant[nRequestsParticipant];
-    if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
+    if (!uxr_run_session_until_all_status(&session, requestTimeoutParticipantMs, requestsParticipant, statusPartipant, nRequestsParticipant)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Partipant session request failure");
         // TODO add a failure log message sharing the status results
         return false;
     }
 
+    for (size_t i = 0 ; i < ARRAY_SIZE(topics); i++) {
+        // Topic
+        const uxrObjectId topic_id = {
+            .id = topics[i].topic_id,
+            .type = UXR_TOPIC_ID
+        };
+        const char* topic_ref = topics[i].topic_profile_label;
+        const auto topic_req_id = uxr_buffer_create_topic_ref(&session,reliable_out,topic_id,participant_id,topic_ref,UXR_REPLACE);
+
+        // Publisher
+        const uxrObjectId pub_id = {
+            .id = topics[i].pub_id,
+            .type = UXR_PUBLISHER_ID
+        };
+        const char* pub_xml = "";
+        const auto pub_req_id = uxr_buffer_create_publisher_xml(&session,reliable_out,pub_id,participant_id,pub_xml,UXR_REPLACE);
+
+        // Data Writer
+        const char* data_writer_ref = topics[i].dw_profile_label;
+        const auto dwriter_req_id = uxr_buffer_create_datawriter_ref(&session,reliable_out,topics[i].dw_id,pub_id,data_writer_ref,UXR_REPLACE);
+
+        // Status requests
+        constexpr uint8_t nRequests = 3;
+        const uint16_t requests[nRequests] = {topic_req_id, pub_req_id, dwriter_req_id};
+        constexpr int requestTimeoutMs = nRequests * maxTimeMsPerRequestMs;
+        uint8_t status[nRequests];
+        if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Topic/Pub/Writer session request failure for index 'TODO'");
+            for (int s = 0 ; s < nRequests; s++) {
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Status '%d' result '%u'", s, status[s]);
+            }
+            // TODO add a failure log message sharing the status results
+            return false;
+        } else {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Topic/Pub/Writer session pass for index 'TOOO'");
+        }
+    }
+
     return true;
 }
 
-void AP_DDS_Client::write()
+void AP_DDS_Client::write_time_topic()
 {
     WITH_SEMAPHORE(csem);
     if (connected) {
         ucdrBuffer ub;
-        uint32_t topic_size = builtin_interfaces_msg_Time_size_of_topic(&time_topic, 0);
+        const uint32_t topic_size = builtin_interfaces_msg_Time_size_of_topic(&time_topic, 0);
-        uxr_prepare_output_stream(&session,reliable_out,dwriter_id,&ub,topic_size);
+        uxr_prepare_output_stream(&session,reliable_out,topics[0].dw_id,&ub,topic_size);
         const bool success = builtin_interfaces_msg_Time_serialize_topic(&ub, &time_topic);
+        if (!success) {
+            // TODO sometimes serialization fails on bootup. Determine why.
+            // AP_HAL::panic("FATAL: XRCE_Client failed to serialize\n");
+        }
+    }
+}
+
+void AP_DDS_Client::write_nav_sat_fix_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub;
+        const uint32_t topic_size = sensor_msgs_msg_NavSatFix_size_of_topic(&nav_sat_fix_topic, 0);
+        uxr_prepare_output_stream(&session,reliable_out,topics[1].dw_id,&ub,topic_size);
+        const bool success = sensor_msgs_msg_NavSatFix_serialize_topic(&ub, &nav_sat_fix_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);
 
-    update_topic(time_topic);
+    const auto cur_time_ms = AP_HAL::millis64();
-    write();
+
+    if (cur_time_ms - last_time_time_ms > DELAY_TIME_TOPIC_MS) {
+        update_topic(time_topic);
+        last_time_time_ms = cur_time_ms;
+        write_time_topic();
+    }
+
+    if (cur_time_ms - last_nav_sat_fix_time_ms > DELAY_NAV_SAT_FIX_TOPIC_MS) {
+        update_topic(nav_sat_fix_topic);
+        last_nav_sat_fix_time_ms = cur_time_ms;
+        write_nav_sat_fix_topic();
+    }
+
     connected = uxr_run_session_time(&session, 1);
 }
 

libraries/AP_DDS/AP_DDS_Client.h

@@ -6,6 +6,7 @@
 #include "ucdr/microcdr.h"
 #include "generated/Time.h"
 #include "AP_DDS_Generic_Fn_T.h"
+#include "generated/NavSatFix.h"
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/Scheduler.h>
@@ -39,12 +40,7 @@ private:
 
     // Topic
     builtin_interfaces_msg_Time time_topic;
-
+    sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
-    // Data Writer
-    const uxrObjectId dwriter_id = {
-        .id = 0x01,
-        .type = UXR_DATAWRITER_ID
-    };
 
     HAL_Semaphore csem;
 
@@ -52,6 +48,13 @@ private:
     bool connected = true;
 
     static void update_topic(builtin_interfaces_msg_Time& msg);
+    static void update_topic(sensor_msgs_msg_NavSatFix& msg);
+
+    // The last ms timestamp AP_DDS wrote a Time message
+    uint64_t last_time_time_ms;
+    // The last ms timestamp AP_DDS wrote a NavSatFix message
+    uint64_t last_nav_sat_fix_time_ms;
+
 
 public:
     // Constructor
@@ -68,8 +71,10 @@ public:
     //! @return True on successful creation, false on failure
     bool create() WARN_IF_UNUSED;
 
-    //! @brief Serialize the current data state and publish to to the IO stream(s)
+    //! @brief Serialize the current time state and publish to to the IO stream(s)
-    void write();
+    void write_time_topic();
+    //! @brief Serialize the current nav_sat_fix state and publish to to the IO stream(s)
+    void write_nav_sat_fix_topic();
     //! @brief Update the internally stored DDS messages with latest data
     void update();
 
@@ -80,6 +85,7 @@ public:
     struct Topic_table {
         const uint8_t topic_id;
         const uint8_t pub_id;
+        const uxrObjectId dw_id;
         const char* topic_profile_label;
         const char* dw_profile_label;
         Generic_serialize_topic_fn_t serialize;

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -1,6 +1,9 @@
-// #include "Generated/Time.h" // might change to brackets for include path
+#include "generated/Time.h"
+#include "generated/NavSatFix.h"
+
 
 #include "AP_DDS_Generic_Fn_T.h"
+#include "uxr/client/client.h"
 
 // Code generated table based on the enabled topics.
 // Mavgen is using python, loops are not readable.
@@ -11,10 +14,21 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
     {
         .topic_id = 0x01,
         .pub_id = 0x01,
+        .dw_id = uxrObjectId{.id=0x01, .type=UXR_DATAWRITER_ID},
         .topic_profile_label = "time__t",
         .dw_profile_label = "time__dw",
         .serialize = Generic_serialize_topic_fn_t(&builtin_interfaces_msg_Time_serialize_topic),
         .deserialize = Generic_deserialize_topic_fn_t(&builtin_interfaces_msg_Time_deserialize_topic),
         .size_of = Generic_size_of_topic_fn_t(&builtin_interfaces_msg_Time_size_of_topic),
     },
+    {
+        .topic_id = 0x02,
+        .pub_id = 0x02,
+        .dw_id = uxrObjectId{.id=0x02, .type=UXR_DATAWRITER_ID},
+        .topic_profile_label = "navsatfix0__t",
+        .dw_profile_label = "navsatfix0__dw",
+        .serialize = Generic_serialize_topic_fn_t(&sensor_msgs_msg_NavSatFix_serialize_topic),
+        .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),
+    },
 };

libraries/AP_DDS/dds_xrce_profile.xml

@@ -30,4 +30,29 @@
       </historyQos>
     </topic>
   </data_writer>
+  <topic profile_name="navsatfix0__t">
+    <name>rt/ROS2_NavSatFix0</name>
+    <dataType>sensor_msgs::msg::dds_::NavSatFix_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>20</depth>
+    </historyQos>
+  </topic>
+  <data_writer profile_name="navsatfix0__dw">
+    <historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
+    <qos>
+      <reliability>
+        <kind>RELIABLE</kind>
+      </reliability>
+    </qos>
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ROS2_NavSatFix0</name>
+      <dataType>sensor_msgs::msg::dds_::NavSatFix_</dataType>
+      <historyQos>
+        <kind>KEEP_LAST</kind>
+        <depth>20</depth>
+      </historyQos>
+    </topic>
+  </data_writer>
 </profiles>