diff --git a/msg/templates/urtps/Publisher.cpp.em b/msg/templates/urtps/Publisher.cpp.em index f0175400c8..8454b52221 100644 --- a/msg/templates/urtps/Publisher.cpp.em +++ b/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(&m_listener)); if(mp_publisher == nullptr) diff --git a/msg/templates/urtps/Publisher.h.em b/msg/templates/urtps/Publisher.h.em index 48cccb335b..76236b832b 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(); + bool init(const std::string& ns); 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 18a7d33128..b514103301 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ b/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* t_send_queue) +bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue* 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); diff --git a/msg/templates/urtps/RtpsTopics.h.em b/msg/templates/urtps/RtpsTopics.h.em index adc79b4977..6d8d2975f4 100644 --- a/msg/templates/urtps/RtpsTopics.h.em +++ b/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* t_send_queue); + bool init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue* t_send_queue, const std::string& ns); void set_timesync(const std::shared_ptr& timesync) { _timesync = timesync; }; @[if send_topics]@ void publish(uint8_t topic_ID, char data_buffer[], size_t len); diff --git a/msg/templates/urtps/Subscriber.cpp.em b/msg/templates/urtps/Subscriber.cpp.em index 217d835799..4cb048d74e 100644 --- a/msg/templates/urtps/Subscriber.cpp.em +++ b/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* 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* 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(&m_listener)); if(mp_subscriber == nullptr) diff --git a/msg/templates/urtps/Subscriber.h.em b/msg/templates/urtps/Subscriber.h.em index 96f7d7acbf..c24e32dcca 100644 --- a/msg/templates/urtps/Subscriber.h.em +++ b/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* t_send_queue); + 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); void run(); bool hasMsg(); @(topic)_msg_t getMsg(); diff --git a/msg/templates/urtps/microRTPS_agent.cpp.em b/msg/templates/urtps/microRTPS_agent.cpp.em index 13a499cb20..728eb2a9d2 100644 --- a/msg/templates/urtps/microRTPS_agent.cpp.em +++ b/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 [UART|UDP] Default UART\n" - " -d UART device. Default /dev/ttyACM0\n" - " -w Time in us for which each iteration sleep. Default 1ms\n" " -b UART device baudrate. Default 460800\n" + " -d UART device. Default /dev/ttyACM0\n" + " -f Activates UART link SW flow control\n" + " -h Activates UART link HW flow control\n" + " -i Target IP for UDP. Default 127.0.0.1\n" + " -n ROS 2 topics namespace. Identifies the vehicle in a multi-agent network\n" " -p Time in ms to poll over UART. Default 1ms\n" " -r UDP port for receiving. Default 2019\n" " -s UDP port for sending. Default 2020\n" - " -i Target IP for UDP. Default 127.0.0.1\n" - " -f Activates UART link SW flow control\n" - " -h Activates UART link HW flow control\n" - " -v Add more verbosity\n", + " -t [UART|UDP] Default UART\n" + " -v Add more verbosity\n" + " -w Time in us for which each iteration sleep. Default 1ms\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;