From bd067f9615bead09be2a20e69ace821e7bd75400 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Sun, 20 Oct 2024 18:42:39 -0600 Subject: [PATCH] AP_DDS: Set GPS instance ID in the GPS frame ID Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- libraries/AP_DDS/AP_DDS_Client.cpp | 3 ++- libraries/AP_DDS/AP_DDS_Topic_Table.h | 2 +- libraries/AP_DDS/README.md | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index fca7d2f614..36d8d1bf71 100644 --- 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 index 8e3823bc23..f578d2e8a4 100644 --- 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 index 0284856597..61aed38a6d 100644 --- 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)