diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 65b16b1e0c..60deb22e8e 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -17,6 +17,7 @@ static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000; 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 char WGS_84_FRAME_ID[] = "WGS-84"; // https://www.ros.org/reps/rep-0105.html#base-link static char BASE_LINK_FRAME_ID[] = "base_link"; @@ -374,6 +375,11 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) } } +void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg) +{ + update_topic(msg.clock); +} + /* start the DDS thread */ @@ -627,6 +633,21 @@ void AP_DDS_Client::write_geo_pose_topic() } } +void AP_DDS_Client::write_clock_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub; + const uint32_t topic_size = rosgraph_msgs_msg_Clock_size_of_topic(&clock_topic, 0); + uxr_prepare_output_stream(&session,reliable_out,topics[7].dw_id,&ub,topic_size); + const bool success = rosgraph_msgs_msg_Clock_serialize_topic(&ub, &clock_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); @@ -668,6 +689,12 @@ void AP_DDS_Client::update() write_geo_pose_topic(); } + if (cur_time_ms - last_clock_time_ms > DELAY_CLOCK_TOPIC_MS) { + update_topic(clock_topic); + last_clock_time_ms = cur_time_ms; + write_clock_topic(); + } + connected = 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 96eb0ef92c..00127a1b10 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -13,6 +13,7 @@ #include "geometry_msgs/msg/PoseStamped.h" #include "geometry_msgs/msg/TwistStamped.h" #include "geographic_msgs/msg/GeoPoseStamped.h" +#include "rosgraph_msgs/msg/Clock.h" #include #include @@ -60,6 +61,7 @@ private: geometry_msgs_msg_PoseStamped local_pose_topic; geometry_msgs_msg_TwistStamped local_velocity_topic; geographic_msgs_msg_GeoPoseStamped geo_pose_topic; + rosgraph_msgs_msg_Clock clock_topic; HAL_Semaphore csem; @@ -73,6 +75,7 @@ private: static void update_topic(geometry_msgs_msg_PoseStamped& msg); static void update_topic(geometry_msgs_msg_TwistStamped& msg); static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg); + static void update_topic(rosgraph_msgs_msg_Clock& msg); // The last ms timestamp AP_DDS wrote a Time message uint64_t last_time_time_ms; @@ -86,6 +89,8 @@ private: uint64_t last_local_velocity_time_ms; // The last ms timestamp AP_DDS wrote a GeoPose message uint64_t last_geo_pose_time_ms; + // The last ms timestamp AP_DDS wrote a Clock message + uint64_t last_clock_time_ms; // functions for serial transport bool ddsSerialInit(); @@ -146,6 +151,8 @@ public: void write_local_velocity_topic(); //! @brief Serialize the current geo_pose and publish to the IO stream(s) void write_geo_pose_topic(); + //! @brief Serialize the current clock and publish to the IO stream(s) + void write_clock_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 084363b494..62ff01ce8b 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -83,4 +83,14 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .deserialize = Generic_deserialize_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_deserialize_topic), .size_of = Generic_size_of_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_size_of_topic), }, + { + .topic_id = 0x08, + .pub_id = 0x08, + .dw_id = uxrObjectId{.id=0x08, .type=UXR_DATAWRITER_ID}, + .topic_profile_label = "clock__t", + .dw_profile_label = "clock__dw", + .serialize = Generic_serialize_topic_fn_t(&rosgraph_msgs_msg_Clock_serialize_topic), + .deserialize = Generic_deserialize_topic_fn_t(&rosgraph_msgs_msg_Clock_deserialize_topic), + .size_of = Generic_size_of_topic_fn_t(&rosgraph_msgs_msg_Clock_size_of_topic), + }, }; diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index 75455fd549..8d3f2d9e1b 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -198,4 +198,29 @@ + + rt/ap/clock + rosgraph_msgs::msg::dds_::Clock_ + + KEEP_LAST + 20 + + + + PREALLOCATED_WITH_REALLOC + + + RELIABLE + + + + NO_KEY + rt/ap/clock + rosgraph_msgs::msg::dds_::Clock_ + + KEEP_LAST + 20 + + +