AP_DDS: Set GPS instance ID in the GPS frame ID

Signed-off-by: Ryan Friedman <[email protected]>
bnsgeyer · Nov 1, 2024 · bd067f9 · bd067f9
1 parent c270c39
commit bd067f9
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 4 deletions.
diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
@@ -197,7 +197,8 @@ bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i
 
 
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, WGS_84_FRAME_ID);
+    static_assert(GPS_MAX_RECEIVERS <= 9, "GPS_MAX_RECEIVERS is greater than 9");
+    hal.util->snprintf(msg.header.frame_id, 2, "%u", instance);
     msg.status.service = 0; // SERVICE_GPS
     msg.status.status = -1; // STATUS_NO_FIX
 

diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h
@@ -96,7 +96,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .type=UXR_DATAWRITER_ID},
         .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .type=UXR_DATAREADER_ID},
         .topic_rw = Topic_rw::DataWriter,
-        .topic_name = "rt/ap/navsat/navsat0",
+        .topic_name = "rt/ap/navsat",
         .type_name = "sensor_msgs::msg::dds_::NavSatFix_",
         .qos = {
             .durability = UXR_DURABILITY_VOLATILE,

diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md
@@ -178,7 +178,7 @@ Published topics:
  * /ap/geopose/filtered [geographic_msgs/msg/GeoPoseStamped] 1 publisher
  * /ap/gps_global_origin/filtered [geographic_msgs/msg/GeoPointStamped] 1 publisher
  * /ap/imu/experimental/data [sensor_msgs/msg/Imu] 1 publisher
- * /ap/navsat/navsat0 [sensor_msgs/msg/NavSatFix] 1 publisher
+ * /ap/navsat [sensor_msgs/msg/NavSatFix] 1 publisher
  * /ap/pose/filtered [geometry_msgs/msg/PoseStamped] 1 publisher
  * /ap/tf_static [tf2_msgs/msg/TFMessage] 1 publisher
  * /ap/time [builtin_interfaces/msg/Time] 1 publisher
@@ -354,7 +354,7 @@ The table below provides example mappings for topics and services
 | ROS 2 | DDS |
 | --- | --- |
 | ap/clock | rt/ap/clock |
-| ap/navsat/navsat0 | rt/ap/navsat/navsat0 |
+| ap/navsat | rt/ap/navsat |
 | ap/arm_motors | rq/ap/arm_motorsRequest, rr/ap/arm_motorsReply |
 
 Refer to existing mappings in [`AP_DDS_Topic_Table`](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_DDS/AP_DDS_Topic_Table.h)