dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
TSC21 2021-07-12 19:27:30 +02:00 committed by Nuno Marques
parent d31b7feb31
commit 25dbffe1aa
6 changed files with 50 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

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

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

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

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

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

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

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

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