Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

microRTPS: agent: make sure that is able to subscribe a timesync stream from itself

Browse Source
This commit is contained in:
TSC21
2020-03-05 15:38:18 +00:00
committed by Nuno Marques
parent cfd8e368df
commit 6dea2dd97d
3 changed files with 16 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/urtps/Publisher.cpp.em
+1 -2
View File
@@ -139,8 +139,7 @@ void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher* pub, Match
// If the matching happens for the same entity, do not make a match // If the matching happens for the same entity, do not make a match
if (is_different_endpoint) { if (is_different_endpoint) {
if (info.status == MATCHED_MATCHING) if (info.status == MATCHED_MATCHING) {
{
n_matched++; n_matched++;
std::cout << " - @(topic) publisher matched" << std::endl; std::cout << " - @(topic) publisher matched" << std::endl;
} else { } else {
msg/templates/urtps/RtpsTopics.cpp.em
+2 -2
View File
@@ -82,7 +82,7 @@ bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_se
@[for topic in send_topics]@ @[for topic in send_topics]@
if (_@(topic)_pub.init()) { if (_@(topic)_pub.init()) {
std::cout << "- @(topic) publisher started" << std::endl; std::cout << "- @(topic) publisher started" << std::endl;
@[ if topic == 'Timesync']@ @[ if topic == 'Timesync' or topic == 'timesync']@
_timesync->start(&_@(topic)_pub); _timesync->start(&_@(topic)_pub);
@[ end if]@ @[ end if]@
} else { } else {
@@ -119,7 +119,7 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len); eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len);
eprosima::fastcdr::Cdr cdr_des(cdrbuffer); eprosima::fastcdr::Cdr cdr_des(cdrbuffer);
st.deserialize(cdr_des); st.deserialize(cdr_des);
@[ if topic == 'Timesync']@ @[ if topic == 'Timesync' or topic == 'timesync']@
_timesync->processTimesyncMsg(&st); _timesync->processTimesyncMsg(&st);
@[ end if]@ @[ end if]@
// apply timestamp offset // apply timestamp offset
msg/templates/urtps/Subscriber.cpp.em
+13 -2
View File
@@ -127,6 +127,9 @@ bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable* t_send
void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub, MatchingInfo& info) void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub, MatchingInfo& info)
{ {
@# Since the time sync runs on the bridge itself, it is required that there is a
@# match between two topics of the same entity
@[if topic != 'Timesync' and topic != 'timesync']@
// The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain // The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain
// are the same for all its subcomponents (publishers, subscribers) // are the same for all its subcomponents (publishers, subscribers)
bool is_different_endpoint = false; bool is_different_endpoint = false;
@@ -139,8 +142,7 @@ void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub, Ma
// If the matching happens for the same entity, do not make a match // If the matching happens for the same entity, do not make a match
if (is_different_endpoint) { if (is_different_endpoint) {
if (info.status == MATCHED_MATCHING) if (info.status == MATCHED_MATCHING) {
{
n_matched++; n_matched++;
std::cout << " - @(topic) subscriber matched" << std::endl; std::cout << " - @(topic) subscriber matched" << std::endl;
} else { } else {
@@ -148,6 +150,15 @@ void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub, Ma
std::cout << " - @(topic) subscriber unmatched" << std::endl; std::cout << " - @(topic) subscriber unmatched" << std::endl;
} }
} }
@[else]@
(void)sub;
if (info.status == MATCHED_MATCHING) {
n_matched++;
} else {
n_matched--;
}
@[end if]@
} }
void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber* sub) void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber* sub)