From 25dbffe1aac8d8c672d63f98e0c552452fed2fe1 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Mon, 12 Jul 2021 19:27:30 +0200 Subject: [PATCH] microRTPS: agent: split FMU input from output topics in Pubs/Subs --- msg/templates/urtps/Publisher.cpp.em | 23 ++++++++++++++++------- msg/templates/urtps/Publisher.h.em | 2 +- msg/templates/urtps/RtpsTopics.cpp.em | 15 +++++++++++---- msg/templates/urtps/RtpsTopics.h.em | 5 +++++ msg/templates/urtps/Subscriber.cpp.em | 23 ++++++++++++++++------- msg/templates/urtps/Subscriber.h.em | 2 +- 6 files changed, 50 insertions(+), 20 deletions(-) diff --git a/msg/templates/urtps/Publisher.cpp.em b/msg/templates/urtps/Publisher.cpp.em index 5805286d61..67c9b29d45 100644 --- a/msg/templates/urtps/Publisher.cpp.em +++ b/msg/templates/urtps/Publisher.cpp.em @@ -13,14 +13,27 @@ @{ from packaging import version import genmsg.msgs +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") except AttributeError: pass + +topic_name = topic + +# For ROS, use the topic pattern convention defined in +# http://wiki.ros.org/ROS/Patterns/Conventions +if ros2_distro: + topic_name_split = re.sub( r"([A-Z])", r" \1", topic).split() + topic_name = topic_name_split[0] + for w in topic_name_split[1:]: + topic_name += "_" + w + topic_name = topic_name.lower() }@ /**************************************************************************** * @@ -87,7 +100,7 @@ using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransport Domain::removeParticipant(mp_participant); } -bool @(topic)_Publisher::init(const std::string &ns) +bool @(topic)_Publisher::init(const std::string &ns, std::string topic_name) { // Create RTPSParticipant ParticipantAttributes PParam; @@ -156,21 +169,17 @@ bool @(topic)_Publisher::init(const std::string &ns) @[ if ros2_distro == "ardent"]@ Wparam.qos.m_partition.push_back("rt"); std::string topicName = ns; - topicName.append("@(topic)_PubSubTopic"); - Wparam.topic.topicName = topicName; @[ else]@ std::string topicName = "rt/"; topicName.append(ns); - topicName.append("@(topic)_PubSubTopic"); - Wparam.topic.topicName = topicName; @[ end if]@ // ROS2 default publish mode QoS policy Wparam.qos.m_publishMode.kind = ASYNCHRONOUS_PUBLISH_MODE; @[else]@ std::string topicName = ns; - topicName.append("@(topic)PubSubTopic"); - Wparam.topic.topicName = topicName; @[end if]@ + topic_name.empty() ? topicName.append("fmu/@(topic_name)/out") : topicName.append(topic_name); + Wparam.topic.topicName = topicName; mp_publisher = Domain::createPublisher(mp_participant, Wparam, static_cast(&m_listener)); if (mp_publisher == nullptr) { diff --git a/msg/templates/urtps/Publisher.h.em b/msg/templates/urtps/Publisher.h.em index 9b701b669c..27c06a7389 100644 --- a/msg/templates/urtps/Publisher.h.em +++ b/msg/templates/urtps/Publisher.h.em @@ -101,7 +101,7 @@ class @(topic)_Publisher public: @(topic)_Publisher(); virtual ~@(topic)_Publisher(); - bool init(const std::string &ns); + bool init(const std::string &ns, std::string topic_name = ""); void run(); void publish(@(topic)_msg_t *st); private: diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em index f1a38ac5d2..1e0d5b0329 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ b/msg/templates/urtps/RtpsTopics.cpp.em @@ -80,12 +80,19 @@ bool RtpsTopics::init(std::condition_variable *t_send_queue_cv, std::mutex *t_se std::cout << "\033[0;36m---- Publishers ----\033[0m" << std::endl; @[for topic in send_topics]@ +@[ if topic == 'Timesync' or topic == 'timesync']@ + if (_@(topic)_pub.init(ns)) { + if (_@(topic)_fmu_in_pub.init(ns, std::string("fmu/timesync/in"))) { + _timesync->start(&_@(topic)_fmu_in_pub); + std::cout << "- @(topic) publishers started" << std::endl; + } +@[ elif topic == 'TimesyncStatus' or topic == 'timesync_status']@ + if (_@(topic)_pub.init(ns, std::string("timesync_status"))) { + _timesync->init_status_pub(&_@(topic)_pub); + std::cout << "- @(topic) publisher started" << std::endl; +@[ else]@ if (_@(topic)_pub.init(ns)) { std::cout << "- @(topic) publisher started" << std::endl; -@[ if topic == 'Timesync' or topic == 'timesync']@ - _timesync->start(&_@(topic)_pub); -@[ elif topic == 'TimesyncStatus' or topic == 'timesync_status']@ - _timesync->init_status_pub(&_@(topic)_pub); @[ end if]@ } else { diff --git a/msg/templates/urtps/RtpsTopics.h.em b/msg/templates/urtps/RtpsTopics.h.em index e23f0d4626..276d09f279 100644 --- a/msg/templates/urtps/RtpsTopics.h.em +++ b/msg/templates/urtps/RtpsTopics.h.em @@ -108,7 +108,12 @@ private: @[if send_topics]@ /** Publishers **/ @[for topic in send_topics]@ +@[ if topic == 'Timesync' or topic == 'timesync']@ @(topic)_Publisher _@(topic)_pub; + @(topic)_Publisher _@(topic)_fmu_in_pub; +@[ else]@ + @(topic)_Publisher _@(topic)_pub; +@[ end if]@ @[end for]@ @[end if]@ diff --git a/msg/templates/urtps/Subscriber.cpp.em b/msg/templates/urtps/Subscriber.cpp.em index 69ee1d8138..e0cd906810 100644 --- a/msg/templates/urtps/Subscriber.cpp.em +++ b/msg/templates/urtps/Subscriber.cpp.em @@ -13,6 +13,7 @@ @{ from packaging import version import genmsg.msgs +import re from px_generate_uorb_topic_helper import * # this is in Tools/ @@ -21,6 +22,17 @@ try: ros2_distro = ros2_distro.decode("utf-8") except AttributeError: pass + +topic_name = topic + +# For ROS, use the topic pattern convention defined in +# http://wiki.ros.org/ROS/Patterns/Conventions +if ros2_distro: + topic_name_split = re.sub( r"([A-Z])", r" \1", topic).split() + topic_name = topic_name_split[0] + for w in topic_name_split[1:]: + topic_name += "_" + w + topic_name = topic_name.lower() }@ /**************************************************************************** * @@ -88,7 +100,8 @@ using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransport } bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv, - std::mutex *t_send_queue_mutex, std::queue *t_send_queue, const std::string &ns) + std::mutex *t_send_queue_mutex, std::queue *t_send_queue, const std::string &ns, + std::string topic_name) { m_listener.topic_ID = topic_ID; m_listener.t_send_queue_cv = t_send_queue_cv; @@ -162,19 +175,15 @@ bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable *t_send @[ if ros2_distro == "ardent"]@ Rparam.qos.m_partition.push_back("rt"); std::string topicName = ns; - topicName.append("@(topic)_PubSubTopic"); - Rparam.topic.topicName = topicName; @[ else]@ std::string topicName = "rt/"; topicName.append(ns); - topicName.append("@(topic)_PubSubTopic"); - Rparam.topic.topicName = topicName; @[ end if]@ @[else]@ std::string topicName = ns; - topicName.append("@(topic)PubSubTopic"); - Rparam.topic.topicName = topicName; @[end if]@ + topic_name.empty() ? topicName.append("fmu/@(topic_name)/in") : topicName.append(topic_name); + Rparam.topic.topicName = topicName; mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, static_cast(&m_listener)); if (mp_subscriber == nullptr) { diff --git a/msg/templates/urtps/Subscriber.h.em b/msg/templates/urtps/Subscriber.h.em index fc4a7e558a..8a42ee3024 100644 --- a/msg/templates/urtps/Subscriber.h.em +++ b/msg/templates/urtps/Subscriber.h.em @@ -106,7 +106,7 @@ public: @(topic)_Subscriber(); virtual ~@(topic)_Subscriber(); bool init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, - std::queue *t_send_queue, const std::string &ns); + std::queue *t_send_queue, const std::string &ns, std::string topic_name = ""); void run(); bool hasMsg(); @(topic)_msg_t getMsg();