fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add topic namespace support for micrortps agent generation

Browse Source 
Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen 2020-11-24 10:09:36 +02:00 committed by Nuno Marques
parent ce8f4dece2
commit fdb4ede6c2
7 changed files with 46 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
msg/templates/urtps/Publisher.cpp.em
View File

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

2
msg/templates/urtps/Publisher.h.em
View File

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

6
msg/templates/urtps/RtpsTopics.cpp.em
View File

@ -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);

2
msg/templates/urtps/RtpsTopics.h.em
View File

@ -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);

19
msg/templates/urtps/Subscriber.cpp.em
View File

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

2
msg/templates/urtps/Subscriber.h.em
View File

@ -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();

21
msg/templates/urtps/microRTPS_agent.cpp.em
View File

@ -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"
" -f <sw flow control> Activates UART link SW flow control\n"
" -h <hw flow control> Activates UART link HW flow control\n"
" -v <debug verbosity> Add more verbosity\n",
" -t <transport> [UART|UDP] Default UART\n"
" -v <debug verbosity> Add more verbosity\n"
" -w <sleep_time_us> 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;