diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index a931a782bb..9aca8a5af0 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -22,6 +22,12 @@ 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"; +// Define the subscriber data members, which are static class scope. +// If these are created on the stack in the subscriber, +// the AP_DDS_Client::on_topic frame size is exceeded. +sensor_msgs_msg_Joy AP_DDS_Client::rx_joy_topic {}; +tf2_msgs_msg_TFMessage AP_DDS_Client::rx_dynamic_transforms_topic {}; + const AP_Param::GroupInfo AP_DDS_Client::var_info[] { // @Param: _ENABLE @@ -412,9 +418,8 @@ void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, ui */ switch (object_id.id) { - case topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id: - sensor_msgs_msg_Joy topic; - const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, &topic); + case topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id: { + const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, &rx_joy_topic); if (success == false) { break; @@ -422,14 +427,29 @@ void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, ui uint32_t* count_ptr = (uint32_t*) args; (*count_ptr)++; - if (topic.axes_size >= 4) { + if (rx_joy_topic.axes_size >= 4) { GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: %f, %f, %f, %f", - topic.axes[0], topic.axes[1], topic.axes[2], topic.axes[3]); + rx_joy_topic.axes[0], rx_joy_topic.axes[1], rx_joy_topic.axes[2], rx_joy_topic.axes[3]); } else { GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: Insufficient axes size "); } break; } + case topics[to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB)].dr_id.id: { + const bool success = tf2_msgs_msg_TFMessage_deserialize_topic(ub, &rx_dynamic_transforms_topic); + if (success == false) { + break; + } + uint32_t* count_ptr = (uint32_t*) args; + (*count_ptr)++; + if (rx_dynamic_transforms_topic.transforms_size > 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage of length: %u", + static_cast(rx_dynamic_transforms_topic.transforms_size)); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage: Insufficient size "); + } + } + } } @@ -444,7 +464,7 @@ void AP_DDS_Client::main_loop(void) } GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Initialization passed"); - populate_static_transforms(static_transforms_topic); + populate_static_transforms(tx_static_transforms_topic); write_static_transforms(); while (true) { @@ -636,9 +656,9 @@ void AP_DDS_Client::write_static_transforms() WITH_SEMAPHORE(csem); if (connected) { ucdrBuffer ub {}; - const uint32_t topic_size = tf2_msgs_msg_TFMessage_size_of_topic(&static_transforms_topic, 0); + const uint32_t topic_size = tf2_msgs_msg_TFMessage_size_of_topic(&tx_static_transforms_topic, 0); uxr_prepare_output_stream(&session,reliable_out,topics[2].dw_id,&ub,topic_size); - const bool success = tf2_msgs_msg_TFMessage_serialize_topic(&ub, &static_transforms_topic); + const bool success = tf2_msgs_msg_TFMessage_serialize_topic(&ub, &tx_static_transforms_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 084546c270..edd5739a30 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -54,16 +54,21 @@ private: uxrStreamId reliable_in; uxrStreamId reliable_out; - // Topic + // Outgoing Sensor and AHRS data builtin_interfaces_msg_Time time_topic; - sensor_msgs_msg_NavSatFix nav_sat_fix_topic; - tf2_msgs_msg_TFMessage static_transforms_topic; - sensor_msgs_msg_BatteryState battery_state_topic; - sensor_msgs_msg_Joy joy_topic; + geographic_msgs_msg_GeoPoseStamped geo_pose_topic; geometry_msgs_msg_PoseStamped local_pose_topic; geometry_msgs_msg_TwistStamped local_velocity_topic; - geographic_msgs_msg_GeoPoseStamped geo_pose_topic; + sensor_msgs_msg_BatteryState battery_state_topic; + sensor_msgs_msg_NavSatFix nav_sat_fix_topic; rosgraph_msgs_msg_Clock clock_topic; + // incoming joystick data + static sensor_msgs_msg_Joy rx_joy_topic; + // outgoing transforms + tf2_msgs_msg_TFMessage tx_static_transforms_topic; + tf2_msgs_msg_TFMessage tx_dynamic_transforms_topic; + // incoming transforms + static tf2_msgs_msg_TFMessage rx_dynamic_transforms_topic; HAL_Semaphore csem; diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 376bf9dfc1..e86f110fc7 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -20,6 +20,7 @@ enum class TopicIndex: uint8_t { GEOPOSE_PUB, CLOCK_PUB, JOY_SUB, + DYNAMIC_TRANSFORMS_SUB, }; static inline constexpr uint8_t to_underlying(const TopicIndex index) @@ -120,4 +121,14 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .dw_profile_label = "", .dr_profile_label = "joy__dr", }, + { + .topic_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), + .pub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), + .sub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), .type=UXR_DATAREADER_ID}, + .topic_profile_label = "dynamictf__t", + .dw_profile_label = "", + .dr_profile_label = "dynamictf__dr", + }, }; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index f785c70f70..c1b59b81da 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -213,6 +213,7 @@ Next, follow the associated section for your chosen transport, and finally you c Subscribed topics: * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber + * /ap/tf [tf2_msgs/msg/TFMessage] 1 subscriber $ ros2 topic hz /ap/time average rate: 50.115 diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index 5cf78c37c6..8178b624c4 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -238,4 +238,19 @@ sensor_msgs::msg::dds_::Joy_ + + rt/ap/tf + tf2_msgs::msg::dds_::TFMessage_ + + KEEP_LAST + 5 + + + + + NO_KEY + rt/ap/tf + tf2_msgs::msg::dds_::TFMessage_ + +