dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

uxrce_dds_client: immediately create data writers on startup 

There is some race condition where in rare cases the topic publication
right after creating the writer did not get received on the ROS side.
This happens even with reliable QoS & reliable transport.
This commit is contained in:
Beat Küng 2024-01-18 13:12:28 +01:00
parent 39d7c5b60c
commit b32d1c295b
No known key found for this signature in database
GPG Key ID: 866DB5F0E24821BB
2 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/uxrce_dds_client/dds_topics.h.em
View File

@ -66,17 +66,24 @@ struct SendTopicsSubs {
uint32_t num_payload_sent{};
void init();
bool init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace);
void update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace);
void reset();
};
void SendTopicsSubs::init() {
bool SendTopicsSubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace) {
bool ret = true;
for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) {
fds[idx].fd = orb_subscribe(send_subscriptions[idx].orb_meta);
fds[idx].events = POLLIN;
orb_set_interval(fds[idx].fd, UXRCE_DEFAULT_POLL_RATE);
if (!create_data_writer(session, reliable_out_stream_id, participant_id, static_cast<ORB_ID>(send_subscriptions[idx].orb_meta->o_id), client_namespace, send_subscriptions[idx].orb_meta->o_name,
send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer, send_subscriptions[idx].reliable_qos)) {
ret = false;
}
}
return ret;
}
void SendTopicsSubs::reset() {
@ -96,11 +103,6 @@ void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream
if (fds[idx].revents & POLLIN) {
// Topic updated, copy data and send
orb_copy(send_subscriptions[idx].orb_meta, fds[idx].fd, &topic_data);
if (send_subscriptions[idx].data_writer.id == UXR_INVALID_ID) {
// data writer not created yet
create_data_writer(session, reliable_out_stream_id, participant_id, static_cast<ORB_ID>(send_subscriptions[idx].orb_meta->o_id), client_namespace, send_subscriptions[idx].orb_meta->o_name,
send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer, send_subscriptions[idx].reliable_qos);
}
if (send_subscriptions[idx].data_writer.id != UXR_INVALID_ID) {

7
src/modules/uxrce_dds_client/uxrce_dds_client.cpp
View File

@ -418,6 +418,11 @@ void UxrceddsClient::run()
return;
}
if (!_subs->init(&session, reliable_out, reliable_in, best_effort_in, participant_id, _client_namespace)) {
PX4_ERR("subs init failed");
return;
}
// create VehicleCommand replier
if (num_of_repliers < MAX_NUM_REPLIERS) {
if (add_replier(new VehicleCommandSrv(&session, reliable_out, reliable_in, participant_id, _client_namespace,
@ -463,8 +468,6 @@ void UxrceddsClient::run()
uint32_t last_num_payload_received{};
int poll_error_counter = 0;
_subs->init();
while (!should_exit() && _connected) {
/* Wait for topic updates for max 1000 ms (1sec) */