diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
index 5cba92373a..ac9125e3ba 100644
--- a/libraries/AP_DDS/AP_DDS_Client.cpp
+++ b/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);
}
diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h
index 48d2acff38..186b40351f 100644
--- a/libraries/AP_DDS/AP_DDS_Client.h
+++ b/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();
diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h
index 96dfd75fe9..bee632d28c 100644
--- a/libraries/AP_DDS/AP_DDS_Topic_Table.h
+++ b/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),
diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml
index 7d4e4c2001..3dcece3f9d 100644
--- a/libraries/AP_DDS/dds_xrce_profile.xml
+++ b/libraries/AP_DDS/dds_xrce_profile.xml
@@ -246,6 +246,34 @@
+
+ rt/ap/gps_global_origin/filtered
+ geographic_msgs::msg::dds_::GeoPointStamped_
+
+ KEEP_LAST
+ 5
+
+
+
+ PREALLOCATED_WITH_REALLOC
+
+
+ BEST_EFFORT
+
+
+ VOLATILE
+
+
+
+ NO_KEY
+ rt/ap/gps_global_origin/filtered
+ geographic_msgs::msg::dds_::GeoPointStamped_
+
+ KEEP_LAST
+ 5
+
+
+
rt/ap/joy
sensor_msgs::msg::dds_::Joy_