microRTPS: agent: split FMU input from output topics in Pubs/Subs

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<PublisherListener *>(&m_listener));
 
 	if (mp_publisher == nullptr) {

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:

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)_pub.init(ns)) {
-		std::cout << "- @(topic) publisher started" << std::endl;
 @[    if topic == 'Timesync' or topic == 'timesync']@
-		_timesync->start(&_@(topic)_pub);
+	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;
 @[    end if]@
 
 	} else {

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]@
 

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<uint8_t> *t_send_queue, const std::string &ns)
+			       std::mutex *t_send_queue_mutex, std::queue<uint8_t> *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<SubscriberListener *>(&m_listener));
 
 	if (mp_subscriber == nullptr) {

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<uint8_t> *t_send_queue, const std::string &ns);
+		  std::queue<uint8_t> *t_send_queue, const std::string &ns, std::string topic_name = "");
 	void run();
 	bool hasMsg();
 	@(topic)_msg_t getMsg();