Skip to content

Commit

Permalink
AP_DDS: add QoS to topic and service tables
Browse files Browse the repository at this point in the history 
- Add QoS struct to topic and service tables.
- Replace profile labels with enums.

Signed-off-by: Rhys Mainwaring <[email protected]>
  • Loading branch information
@srmainwaring
srmainwaring committed May 23, 2024
1 parent 26631df commit aabf9bc
Show file tree
Hide file tree
Showing 4 changed files with 134 additions and 120 deletions.
75 changes: 9 additions & 66 deletions libraries/AP_DDS/AP_DDS_Client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -822,18 +822,16 @@ 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;
uint16_t requests[nRequests];
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,
Expand All @@ -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;
Expand All @@ -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,
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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
}
}
Expand Down
27 changes: 19 additions & 8 deletions libraries/AP_DDS/AP_DDS_Client.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -213,21 +213,32 @@ 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;
const uint8_t pub_id;
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
Expand All @@ -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;
Expand All @@ -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[];
};
Expand Down
12 changes: 8 additions & 4 deletions libraries/AP_DDS/AP_DDS_Service_Table.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_",
Expand Down
Loading

0 comments on commit aabf9bc

Please sign in to comment.