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

microRTPS: simplify the attribution of the RTPS IDs by makiing it automatic

Browse Source 
1. The RTPS IDs are now automatically assigned to the topics
2. Only the topics that get defined to be sent or received in the urtps_bridge_topics.yaml (renamed, since now it doesn't contain IDs) receive the IDs
3. Any addition or removal on the urtps_bridge_topics.yaml file might update the topic IDs - this will require that the agent and the client ID list has to be in sync. This will further require a robustification of the way we check the IDs and the message definitions when starting the bridge.
This commit is contained in:
TSC21
2021-07-17 17:25:01 +02:00
committed by Nuno Marques
parent f557fbc99f
commit c478e2985a
19 changed files with 226 additions and 802 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/urtps/Publisher.cpp.em
+1 -3
View File
@@ -11,12 +11,10 @@
@# - ids (List) list of all RTPS msg ids
@###############################################
@{
from packaging import version
import genmsg.msgs
from packaging import version
import re
from px_generate_uorb_topic_helper import * # this is in Tools/
topic = alias if alias else spec.short_name
try:
msg/templates/urtps/Publisher.h.em
+2 -3
View File
@@ -11,10 +11,9 @@
@# - ids (List) list of all RTPS msg ids
@###############################################
@{
from packaging import version
import genmsg.msgs
from px_generate_uorb_topic_helper import * # this is in Tools/
from packaging import version
import re
topic = alias if alias else spec.short_name
try:
msg/templates/urtps/RtpsTopics.cpp.em
+27 -21
View File
@@ -10,11 +10,8 @@
@# - ids (List) list of all RTPS msg ids
@###############################################
@{
import os
import genmsg.msgs
from px_generate_uorb_topic_helper import * # this is in Tools/
import os
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
@@ -64,7 +61,7 @@ bool RtpsTopics::init(std::condition_variable *t_send_queue_cv, std::mutex *t_se
std::cout << "\033[0;36m--- Subscribers ---\033[0m" << std::endl;
@[for topic in recv_topics]@
if (_@(topic)_sub.init(@(rtps_message_id(ids, topic)), t_send_queue_cv, t_send_queue_mutex, t_send_queue, ns)) {
if (_@(topic)_sub.init(@(ids[0].index(topic) + 1), t_send_queue_cv, t_send_queue_mutex, t_send_queue, ns)) {
std::cout << "- @(topic) subscriber started" << std::endl;
} else {
@@ -76,6 +73,7 @@ bool RtpsTopics::init(std::condition_variable *t_send_queue_cv, std::mutex *t_se
std::cout << "\033[0;36m-----------------------\033[0m" << std::endl << std::endl;
@[end if]@
@[if send_topics]@
// Initialise publishers
std::cout << "\033[0;36m---- Publishers ----\033[0m" << std::endl;
@[for topic in send_topics]@
@@ -107,28 +105,32 @@ bool RtpsTopics::init(std::condition_variable *t_send_queue_cv, std::mutex *t_se
}
@[if send_topics]@
template <typename T>
void RtpsTopics::sync_timestamp_of_incoming_data(T &msg) {
uint64_t timestamp = getMsgTimestamp(&msg);
uint64_t timestamp_sample = getMsgTimestampSample(&msg);
_timesync->subtractOffset(timestamp);
setMsgTimestamp(&msg, timestamp);
_timesync->subtractOffset(timestamp_sample);
setMsgTimestampSample(&msg, timestamp_sample);
}
void RtpsTopics::publish(const uint8_t topic_ID, char data_buffer[], size_t len)
{
switch (topic_ID) {
@[for topic in send_topics]@
case @(rtps_message_id(ids, topic)): { // @(topic)
case @(ids[0].index(topic) + 1): { // @(topic) publisher
@(topic)_msg_t st;
eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len);
eprosima::fastcdr::Cdr cdr_des(cdrbuffer);
st.deserialize(cdr_des);
@[ if topic == 'Timesync' or topic == 'timesync']@
_timesync->processTimesyncMsg(&st, &_@(topic)_pub);
@[ end if]@
// apply timestamp offset
uint64_t timestamp = getMsgTimestamp(&st);
uint64_t timestamp_sample = getMsgTimestampSample(&st);
_timesync->subtractOffset(timestamp);
setMsgTimestamp(&st, timestamp);
_timesync->subtractOffset(timestamp_sample);
setMsgTimestampSample(&st, timestamp_sample);
sync_timestamp_of_incoming_data(st);
_@(topic)_pub.publish(&st);
}
@@ -143,6 +145,15 @@ void RtpsTopics::publish(const uint8_t topic_ID, char data_buffer[], size_t len)
}
@[end if]@
@[if recv_topics]@
template <typename T>
void RtpsTopics::sync_timestamp_of_outgoing_data(T &msg) {
uint64_t timestamp = getMsgTimestamp(&msg);
uint64_t timestamp_sample = getMsgTimestampSample(&msg);
_timesync->addOffset(timestamp);
setMsgTimestamp(&msg, timestamp);
_timesync->addOffset(timestamp_sample);
setMsgTimestampSample(&msg, timestamp_sample);
}
bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
{
@@ -151,17 +162,12 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
switch (topic_ID) {
@[for topic in recv_topics]@
case @(rtps_message_id(ids, topic)): // @(topic)
case @(ids[0].index(topic) + 1): // @(topic) subscriber
if (_@(topic)_sub.hasMsg()) {
@(topic)_msg_t msg = _@(topic)_sub.getMsg();
// apply timestamps offset
uint64_t timestamp = getMsgTimestamp(&msg);
uint64_t timestamp_sample = getMsgTimestampSample(&msg);
_timesync->addOffset(timestamp);
setMsgTimestamp(&msg, timestamp);
_timesync->addOffset(timestamp_sample);
setMsgTimestampSample(&msg, timestamp_sample);
// apply timestamp offset
sync_timestamp_of_outgoing_data(msg);
msg.serialize(scdr);
ret = true;
msg/templates/urtps/RtpsTopics.h.em
+5 -4
View File
@@ -10,12 +10,9 @@
@# - ids (List) list of all RTPS msg ids
@###############################################
@{
import genmsg.msgs
import os
from packaging import version
import genmsg.msgs
from px_generate_uorb_topic_helper import * # this is in Tools/
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
@@ -98,9 +95,13 @@ public:
const std::string &ns);
void set_timesync(const std::shared_ptr<TimeSync> &timesync) { _timesync = timesync; };
@[if send_topics]@
template <typename T>
void sync_timestamp_of_incoming_data(T &msg);
void publish(const uint8_t topic_ID, char data_buffer[], size_t len);
@[end if]@
@[if recv_topics]@
template <typename T>
void sync_timestamp_of_outgoing_data(T &msg);
bool getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr);
@[end if]@
msg/templates/urtps/Subscriber.cpp.em
+1 -3
View File
@@ -11,12 +11,10 @@
@# - ids (List) list of all RTPS msg ids
@###############################################
@{
from packaging import version
import genmsg.msgs
from packaging import version
import re
from px_generate_uorb_topic_helper import * # this is in Tools/
topic = alias if alias else spec.short_name
try:
ros2_distro = ros2_distro.decode("utf-8")
msg/templates/urtps/Subscriber.h.em
+2 -3
View File
@@ -11,10 +11,9 @@
@# - ids (List) list of all RTPS msg ids
@###############################################
@{
from packaging import version
import genmsg.msgs
from px_generate_uorb_topic_helper import * # this is in Tools/
from packaging import version
import re
topic = alias if alias else spec.short_name
try:
msg/templates/urtps/microRTPS_agent.cpp.em
-2
View File
@@ -12,8 +12,6 @@
@###############################################
@{
import genmsg.msgs
from px_generate_uorb_topic_helper import * # this is in Tools/
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
try:
msg/templates/urtps/microRTPS_timesync.cpp.em
-2
View File
@@ -12,8 +12,6 @@
@###############################################
@{
import genmsg.msgs
from px_generate_uorb_topic_helper import * # this is in Tools/
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
package = package[0]
msg/templates/urtps/microRTPS_timesync.h.em
+1 -3
View File
@@ -11,10 +11,8 @@
@# - ids (List) list of all RTPS msg ids
@###############################################
@{
from packaging import version
import genmsg.msgs
from px_generate_uorb_topic_helper import * # this is in Tools/
from packaging import version
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
package = package[0]
msg/templates/urtps/msg.idl.em
+1 -2
View File
@@ -37,9 +37,8 @@
@#
@################################################################################
@{
from packaging import version
import genmsg.msgs
from packaging import version
from px_generate_uorb_topic_helper import * # this is in Tools/
builtin_types = set()