Add topic namespace support for micrortps agent generation

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>

msg/templates/urtps/Publisher.cpp.em

@@ -82,7 +82,7 @@ except AttributeError:
     Domain::removeParticipant(mp_participant);
 }
 
-bool @(topic)_Publisher::init()
+bool @(topic)_Publisher::init(const std::string& ns)
 {
     // Create RTPSParticipant
     ParticipantAttributes PParam;
@@ -96,7 +96,9 @@ bool @(topic)_Publisher::init()
 @[else]@
     PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite;
 @[end if]@
-    PParam.rtps.setName("@(topic)_publisher");  //You can put here the name you want
+    std::string nodeName = ns;
+    nodeName.append("@(topic)_publisher");
+    PParam.rtps.setName(nodeName.c_str());
     mp_participant = Domain::createParticipant(PParam);
     if(mp_participant == nullptr)
         return false;
@@ -111,12 +113,19 @@ bool @(topic)_Publisher::init()
 @[if ros2_distro]@
 @[    if ros2_distro == "ardent"]@
     Wparam.qos.m_partition.push_back("rt");
-    Wparam.topic.topicName = "@(topic)_PubSubTopic";
+    std::string topicName = ns;
+    topicName.append("@(topic)_PubSubTopic");
+    Wparam.topic.topicName = topicName;
 @[    else]@
-    Wparam.topic.topicName = "rt/@(topic)_PubSubTopic";
+    std::string topicName = "rt/";
+    topicName.append(ns);
+    topicName.append("@(topic)_PubSubTopic");
+    Wparam.topic.topicName = topicName;
 @[    end if]@
 @[else]@
-    Wparam.topic.topicName = "@(topic)PubSubTopic";
+    std::string topicName = ns;
+    topicName.append("@(topic)PubSubTopic");
+    Wparam.topic.topicName = topicName;
 @[end if]@
     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();
+    bool init(const std::string& ns);
     void run();
     void publish(@(topic)_msg_t* st);
 private:

msg/templates/urtps/RtpsTopics.cpp.em

@@ -56,13 +56,13 @@ package = package[0]
 
 #include "RtpsTopics.h"
 
-bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue<uint8_t>* t_send_queue)
+bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue<uint8_t>* t_send_queue, const std::string& ns)
 {
 @[if recv_topics]@
     // Initialise subscribers
     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)) {
+    if (_@(topic)_sub.init(@(rtps_message_id(ids, topic)), t_send_queue_cv, t_send_queue_mutex, t_send_queue, ns)) {
         std::cout << "- @(topic) subscriber started" << std::endl;
     } else {
         std::cerr << "Failed starting @(topic) subscriber" << std::endl;
@@ -75,7 +75,7 @@ bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_se
     // Initialise publishers
     std::cout << "\033[0;36m----   Publishers  ----\033[0m" << std::endl;
 @[for topic in send_topics]@
-    if (_@(topic)_pub.init()) {
+    if (_@(topic)_pub.init(ns)) {
         std::cout << "- @(topic) publisher started" << std::endl;
 @[    if topic == 'Timesync' or topic == 'timesync']@
         _timesync->start(&_@(topic)_pub);

msg/templates/urtps/RtpsTopics.h.em

@@ -92,7 +92,7 @@ using @(topic)_msg_t = @(topic);
 
 class RtpsTopics {
 public:
-    bool init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue<uint8_t>* t_send_queue);
+    bool init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue<uint8_t>* t_send_queue, const std::string& ns);
     void set_timesync(const std::shared_ptr<TimeSync>& timesync) { _timesync = timesync; };
 @[if send_topics]@
     void publish(uint8_t topic_ID, char data_buffer[], size_t len);

msg/templates/urtps/Subscriber.cpp.em

@@ -81,7 +81,7 @@ except AttributeError:
     Domain::removeParticipant(mp_participant);
 }
 
-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)
+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)
 {
     m_listener.topic_ID = topic_ID;
     m_listener.t_send_queue_cv = t_send_queue_cv;
@@ -100,7 +100,9 @@ bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable* t_send
 @[else]@
     PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite;
 @[end if]@
-    PParam.rtps.setName("@(topic)_subscriber");
+    std::string nodeName = ns;
+    nodeName.append("@(topic)_subscriber");
+    PParam.rtps.setName(nodeName.c_str());
     mp_participant = Domain::createParticipant(PParam);
     if(mp_participant == nullptr)
             return false;
@@ -115,12 +117,19 @@ bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable* t_send
 @[if ros2_distro]@
 @[    if ros2_distro == "ardent"]@
     Rparam.qos.m_partition.push_back("rt");
-    Rparam.topic.topicName = "@(topic)_PubSubTopic";
+    std::string topicName = ns;
+    topicName.append("@(topic)_PubSubTopic");
+    Rparam.topic.topicName = topicName;
 @[    else]@
-    Rparam.topic.topicName = "rt/@(topic)_PubSubTopic";
+    std::string topicName = "rt/";
+    topicName.append(ns);
+    topicName.append("@(topic)_PubSubTopic");
+    Rparam.topic.topicName = topicName;
 @[    end if]@
 @[else]@
-    Rparam.topic.topicName = "@(topic)PubSubTopic";
+    std::string topicName = ns;
+    topicName.append("@(topic)PubSubTopic");
+    Rparam.topic.topicName = topicName;
 @[end if]@
     mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, static_cast<SubscriberListener*>(&m_listener));
     if(mp_subscriber == nullptr)

msg/templates/urtps/Subscriber.h.em

@@ -105,7 +105,7 @@ class @(topic)_Subscriber
 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);
+    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);
     void run();
     bool hasMsg();
     @(topic)_msg_t getMsg();

msg/templates/urtps/microRTPS_agent.cpp.em

@@ -107,22 +107,24 @@ struct options {
     bool sw_flow_control = false;
     bool hw_flow_control = false;
     bool verbose_debug = false;
+    std::string ns = "";
 } _options;
 
 static void usage(const char *name)
 {
     printf("usage: %s [options]\n\n"
-             "  -t <transport>          [UART|UDP] Default UART\n"
-             "  -d <device>             UART device. Default /dev/ttyACM0\n"
-             "  -w <sleep_time_us>      Time in us for which each iteration sleep. Default 1ms\n"
              "  -b <baudrate>           UART device baudrate. Default 460800\n"
+             "  -d <device>             UART device. Default /dev/ttyACM0\n"
+             "  -f <sw flow control>    Activates UART link SW flow control\n"
+             "  -h <hw flow control>    Activates UART link HW flow control\n"
+             "  -i <ip_address>         Target IP for UDP. Default 127.0.0.1\n"
+             "  -n <namespace>          ROS 2 topics namespace. Identifies the vehicle in a multi-agent network\n"
              "  -p <poll_ms>            Time in ms to poll over UART. Default 1ms\n"
              "  -r <reception port>     UDP port for receiving. Default 2019\n"
              "  -s <sending port>       UDP port for sending. Default 2020\n"
-             "  -i <ip_address>         Target IP for UDP. Default 127.0.0.1\n"
+             "  -t <transport>          [UART|UDP] Default UART\n"
-             "  -f <sw flow control>    Activates UART link SW flow control\n"
+             "  -v <debug verbosity>    Add more verbosity\n"
-             "  -h <hw flow control>    Activates UART link HW flow control\n"
+             "  -w <sleep_time_us>      Time in us for which each iteration sleep. Default 1ms\n",
-             "  -v <debug verbosity>    Add more verbosity\n",
              name);
 }
 
@@ -130,7 +132,7 @@ static int parse_options(int argc, char **argv)
 {
     int ch;
 
-    while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:i:fhv")) != EOF)
+    while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:i:fhvn:")) != EOF)
     {
         switch (ch)
         {
@@ -147,6 +149,7 @@ static int parse_options(int argc, char **argv)
             case 'f': _options.sw_flow_control = true;                          break;
             case 'h': _options.hw_flow_control = true;                          break;
             case 'v': _options.verbose_debug = true;                            break;
+            case 'n': if (nullptr != optarg) _options.ns = std::string(optarg) + "/"; break;
             default:
                 usage(argv[0]);
                 return -1;
@@ -273,7 +276,7 @@ int main(int argc, char** argv)
     topics.set_timesync(timeSync);
 
 @[if recv_topics]@
-    topics.init(&t_send_queue_cv, &t_send_queue_mutex, &t_send_queue);
+    topics.init(&t_send_queue_cv, &t_send_queue_mutex, &t_send_queue, _options.ns);
 @[end if]@
 
     running = true;