From aabf9bca13c3a5bcdf8d57c3f5d602997f32382d Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 23 May 2024 17:33:12 +0100 Subject: [PATCH] AP_DDS: add QoS to topic and service tables - Add QoS struct to topic and service tables. - Replace profile labels with enums. Signed-off-by: Rhys Mainwaring --- libraries/AP_DDS/AP_DDS_Client.cpp | 75 ++----------- libraries/AP_DDS/AP_DDS_Client.h | 27 +++-- libraries/AP_DDS/AP_DDS_Service_Table.h | 12 +- libraries/AP_DDS/AP_DDS_Topic_Table.h | 140 +++++++++++++++++------- 4 files changed, 134 insertions(+), 120 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 261e9185c25e94..5e2b5fc89e4067 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -822,10 +822,8 @@ bool AP_DDS_Client::create() .id = topics[i].topic_id, .type = UXR_TOPIC_ID }; - const char* topic_name = topics[i].topic_name; - const char* type_name = topics[i].type_name; const auto topic_req_id = uxr_buffer_create_topic_bin(&session, reliable_out, topic_id, - participant_id, topic_name, type_name , UXR_REPLACE); + participant_id, topics[i].topic_name, topics[i].type_name , UXR_REPLACE); // Status requests constexpr uint8_t nRequests = 3; @@ -833,7 +831,7 @@ bool AP_DDS_Client::create() constexpr uint16_t requestTimeoutMs = nRequests * maxTimeMsPerRequestMs; uint8_t status[nRequests]; - if (strlen(topics[i].dw_profile_label) > 0) { + if (topics[i].topic_rw == Topic_rw::DataWriter) { // Publisher const uxrObjectId pub_id = { .id = topics[i].pub_id, @@ -843,40 +841,8 @@ bool AP_DDS_Client::create() participant_id, UXR_REPLACE); // Data Writer - - // /ap/static_tf - // const uxrQoS_t qos = { - // .durability = UXR_DURABILITY_TRANSIENT_LOCAL, - // .reliability = UXR_RELIABILITY_RELIABLE, - // .history = UXR_HISTORY_KEEP_LAST, - // .depth = 1, - // } - - // /ap/clock, /ap/time - // const uxrQoS_t qos = { - // .durability = UXR_DURABILITY_VOLATILE, - // .reliability = UXR_RELIABILITY_RELIABLE, - // .history = UXR_HISTORY_KEEP_LAST, - // .depth = 1, - // } - - // default - // const uxrQoS_t qos = { - // .durability = UXR_DURABILITY_TRANSIENT_LOCAL, - // .reliability = UXR_RELIABILITY_RELIABLE, - // .history = UXR_HISTORY_KEEP_LAST, - // .depth = 5, - // }; - - // everything else - const uxrQoS_t qos = { - .durability = UXR_DURABILITY_VOLATILE, - .reliability = UXR_RELIABILITY_BEST_EFFORT, - .history = UXR_HISTORY_KEEP_LAST, - .depth = 5, - }; const auto dwriter_req_id = uxr_buffer_create_datawriter_bin(&session, reliable_out, topics[i].dw_id, - pub_id, topic_id, qos, UXR_REPLACE); + pub_id, topic_id, topics[i].qos, UXR_REPLACE); // save the request statuses requests[0] = topic_req_id; @@ -893,7 +859,7 @@ bool AP_DDS_Client::create() } else { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Topic/Pub/Writer session pass for index '%u'", msg_prefix, i); } - } else if (strlen(topics[i].dr_profile_label) > 0) { + } else if (topics[i].topic_rw == Topic_rw::DataReader) { // Subscriber const uxrObjectId sub_id = { .id = topics[i].sub_id, @@ -903,17 +869,8 @@ bool AP_DDS_Client::create() participant_id, UXR_REPLACE); // Data Reader - - // default - const uxrQoS_t qos = { - .durability = UXR_DURABILITY_TRANSIENT_LOCAL, - .reliability = UXR_RELIABILITY_RELIABLE, - .history = UXR_HISTORY_KEEP_LAST, - .depth = 5, - }; - const auto dreader_req_id = uxr_buffer_create_datareader_bin(&session, reliable_out, topics[i].dr_id, - sub_id, topic_id, qos, UXR_REPLACE); + sub_id, topic_id, topics[i].qos, UXR_REPLACE); // save the request statuses requests[0] = topic_req_id; @@ -940,28 +897,14 @@ bool AP_DDS_Client::create() constexpr uint16_t requestTimeoutMs = maxTimeMsPerRequestMs; - if (strlen(services[i].rep_profile_label) > 0) { + if (services[i].service_rr == Service_rr::Replier) { const uxrObjectId rep_id = { .id = services[i].rep_id, .type = UXR_REPLIER_ID }; - const char* service_name = services[i].service_name; - const char* request_type = services[i].request_type; - const char* reply_type = services[i].reply_type; - const char* request_topic_name = services[i].request_topic_name; - const char* reply_topic_name = services[i].reply_topic_name; - - // default - const uxrQoS_t qos = { - .durability = UXR_DURABILITY_TRANSIENT_LOCAL, - .reliability = UXR_RELIABILITY_RELIABLE, - .history = UXR_HISTORY_KEEP_LAST, - .depth = 5, - }; - const auto replier_req_id = uxr_buffer_create_replier_bin(&session, reliable_out, rep_id, - participant_id, service_name, request_type, reply_type, - request_topic_name, reply_topic_name, qos, UXR_REPLACE); + participant_id, services[i].service_name, services[i].request_type, services[i].reply_type, + services[i].request_topic_name, services[i].reply_topic_name, services[i].qos, UXR_REPLACE); uint16_t request = replier_req_id; uint8_t status; @@ -976,7 +919,7 @@ bool AP_DDS_Client::create() uxr_buffer_request_data(&session, reliable_out, rep_id, reliable_in, &delivery_control); } - } else if (strlen(services[i].req_profile_label) > 0) { + } else if (services[i].service_rr == Service_rr::Requester) { // TODO : Add Similar Code for Requester Profile } } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 861719851a180e..e796b78edb3fd4 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -213,6 +213,12 @@ class AP_DDS_Client //! @brief Parameter storage static const struct AP_Param::GroupInfo var_info[]; + //! @brief Enum used to mark a topic as a data reader or writer + enum class Topic_rw : uint8_t { + DataReader = 0, + DataWriter = 1, + }; + //! @brief Convenience grouping for a single "channel" of data struct Topic_table { const uint8_t topic_id; @@ -220,14 +226,19 @@ class AP_DDS_Client const uint8_t sub_id; // added sub_id fields to avoid confusion const uxrObjectId dw_id; const uxrObjectId dr_id; // added dr_id fields to avoid confusion - const char* topic_profile_label; - const char* dw_profile_label; - const char* dr_profile_label; + const Topic_rw topic_rw; const char* topic_name; const char* type_name; + const uxrQoS_t qos; }; static const struct Topic_table topics[]; + //! @brief Enum used to mark a service as a requester or replier + enum class Service_rr : uint8_t { + Requester = 0, + Replier = 1, + }; + //! @brief Convenience grouping for a single "channel" of services struct Service_table { //! @brief Request ID for the service @@ -236,11 +247,8 @@ class AP_DDS_Client //! @brief Reply ID for the service const uint8_t rep_id; - //! @brief Profile Label for the service requester - const char* req_profile_label; - - //! @brief Profile Label for the service replier - const char* rep_profile_label; + //! @brief Service is requester or replier + const Service_rr service_rr; //! @brief Service name as it appears in ROS const char* service_name; @@ -256,6 +264,9 @@ class AP_DDS_Client //! @brief Service replier topic name const char* reply_topic_name; + + //! @brief QoS for the service + const uxrQoS_t qos; }; static const struct Service_table services[]; }; diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index cb33896b007e03..22f6716beffd0a 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -15,19 +15,23 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { { .req_id = to_underlying(ServiceIndex::ARMING_MOTORS), .rep_id = to_underlying(ServiceIndex::ARMING_MOTORS), - .req_profile_label = "", - .rep_profile_label = "arm_motors__replier", + .service_rr = Service_rr::Replier, .service_name = "rs/ap/arm_motorsService", .request_type = "ardupilot_msgs::srv::dds_::ArmMotors_Request_", .reply_type = "ardupilot_msgs::srv::dds_::ArmMotors_Response_", .request_topic_name = "rq/ap/arm_motorsRequest", .reply_topic_name = "rr/ap/arm_motorsReply", + .qos = { + .durability = UXR_DURABILITY_TRANSIENT_LOCAL, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .req_id = to_underlying(ServiceIndex::MODE_SWITCH), .rep_id = to_underlying(ServiceIndex::MODE_SWITCH), - .req_profile_label = "", - .rep_profile_label = "mode_switch__replier", + .service_rr = Service_rr::Replier, .service_name = "rs/ap/mode_switchService", .request_type = "ardupilot_msgs::srv::dds_::ModeSwitch_Request_", .reply_type = "ardupilot_msgs::srv::dds_::ModeSwitch_Response_", diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 5dbc763ee62c4a..85b2952312362c 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -42,11 +42,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::TIME_PUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::TIME_PUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::TIME_PUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "time__t", - .dw_profile_label = "time__dw", - .dr_profile_label = "", + .topic_rw = Topic_rw::DataWriter, .topic_name = "rt/ap/time", .type_name = "builtin_interfaces::msg::dds_::Time_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 20, + }, }, { .topic_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB), @@ -54,11 +58,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .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_profile_label = "navsatfix0__t", - .dw_profile_label = "navsatfix0__dw", - .dr_profile_label = "", + .topic_rw = Topic_rw::DataWriter, .topic_name = "rt/ap/navsat/navsat0", .type_name = "tf2_msgs::msg::dds_::TFMessage_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .topic_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), @@ -66,11 +74,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "statictransforms__t", - .dw_profile_label = "statictransforms__dw", - .dr_profile_label = "", + .topic_rw = Topic_rw::DataWriter, .topic_name = "rt/ap/tf_static", .type_name = "tf2_msgs::msg::dds_::TFMessage_", + .qos = { + .durability = UXR_DURABILITY_TRANSIENT_LOCAL, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 1, + }, }, { .topic_id = to_underlying(TopicIndex::BATTERY_STATE_PUB), @@ -78,11 +90,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::BATTERY_STATE_PUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::BATTERY_STATE_PUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::BATTERY_STATE_PUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "batterystate0__t", - .dw_profile_label = "batterystate0__dw", - .dr_profile_label = "", + .topic_rw = Topic_rw::DataWriter, .topic_name = "rt/ap/battery/battery0", .type_name = "sensor_msgs::msg::dds_::BatteryState_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .topic_id = to_underlying(TopicIndex::IMU_PUB), @@ -90,11 +106,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::IMU_PUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "imu__t", - .dw_profile_label = "imu__dw", - .dr_profile_label = "", + .topic_rw = Topic_rw::DataWriter, .topic_name = "rt/ap/imu/experimental/data", .type_name = "sensor_msgs::msg::dds_::Imu_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .topic_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), @@ -102,11 +122,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_POSE_PUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_POSE_PUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "localpose__t", - .dw_profile_label = "localpose__dw", - .dr_profile_label = "", + .topic_rw = Topic_rw::DataWriter, .topic_name = "rt/ap/pose/filtered", .type_name = "geometry_msgs::msg::dds_::PoseStamped_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .topic_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), @@ -114,11 +138,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "localvelocity__t", - .dw_profile_label = "localvelocity__dw", - .dr_profile_label = "", + .topic_rw = Topic_rw::DataWriter, .topic_name = "rt/ap/twist/filtered", .type_name = "geometry_msgs::msg::dds_::TwistStamped_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .topic_id = to_underlying(TopicIndex::GEOPOSE_PUB), @@ -126,11 +154,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::GEOPOSE_PUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GEOPOSE_PUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GEOPOSE_PUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "geopose__t", - .dw_profile_label = "geopose__dw", - .dr_profile_label = "", + .topic_rw = Topic_rw::DataWriter, .topic_name = "rt/ap/geopose/filtered", .type_name = "geographic_msgs::msg::dds_::GeoPoseStamped_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .topic_id = to_underlying(TopicIndex::CLOCK_PUB), @@ -138,11 +170,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::CLOCK_PUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::CLOCK_PUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::CLOCK_PUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "clock__t", - .dw_profile_label = "clock__dw", - .dr_profile_label = "", + .topic_rw = Topic_rw::DataWriter, .topic_name = "rt/ap/clock", .type_name = "rosgraph_msgs::msg::dds_::Clock_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 20, + }, }, { .topic_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB), @@ -150,11 +186,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .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_rw = Topic_rw::DataWriter, .topic_name = "rt/ap/gps_global_origin/filtered", .type_name = "geographic_msgs::msg::dds_::GeoPointStamped_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_BEST_EFFORT, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .topic_id = to_underlying(TopicIndex::JOY_SUB), @@ -162,11 +202,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::JOY_SUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::JOY_SUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::JOY_SUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "joy__t", - .dw_profile_label = "", - .dr_profile_label = "joy__dr", + .topic_rw = Topic_rw::DataReader, .topic_name = "rt/ap/joy", .type_name = "sensor_msgs::msg::dds_::Joy_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .topic_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), @@ -174,11 +218,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .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", + .topic_rw = Topic_rw::DataReader, .topic_name = "rt/ap/tf", .type_name = "tf2_msgs::msg::dds_::TFMessage_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .topic_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), @@ -186,11 +234,15 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "velocitycontrol__t", - .dw_profile_label = "", - .dr_profile_label = "velocitycontrol__dr", + .topic_rw = Topic_rw::DataReader, .topic_name = "rt/ap/cmd_vel", .type_name = "geometry_msgs::msg::dds_::TwistStamped_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, { .topic_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB), @@ -198,10 +250,14 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .sub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAWRITER_ID}, .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAREADER_ID}, - .topic_profile_label = "globalposcontrol__t", - .dw_profile_label = "", - .dr_profile_label = "globalposcontrol__dr", + .topic_rw = Topic_rw::DataReader, .topic_name = "rt/ap/cmd_gps_pose", .type_name = "ardupilot_msgs::msg::dds_::GlobalPosition_", + .qos = { + .durability = UXR_DURABILITY_VOLATILE, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 5, + }, }, };