mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-20 20:03:54 +08:00
microRTPS: templates: fix support for ROS2 Dashing
This commit is contained in:
@@ -16,6 +16,7 @@ import gencpp
|
||||
from px_generate_uorb_topic_helper import * # this is in Tools/
|
||||
|
||||
topic = alias if alias else spec.short_name
|
||||
ros2_distro = ros2_distro.decode("utf-8")
|
||||
}@
|
||||
/****************************************************************************
|
||||
*
|
||||
@@ -80,7 +81,7 @@ bool @(topic)_Publisher::init()
|
||||
// Create RTPSParticipant
|
||||
ParticipantAttributes PParam;
|
||||
PParam.rtps.builtin.domainId = 0;
|
||||
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
|
||||
@[if 1.5 <= fastrtpsgen_version <= 1.7 or ros2_distro == "ardent" or ros2_distro == "bouncy" or ros2_distro == "crystal" or ros2_distro == "dashing"]@
|
||||
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
|
||||
@[else]@
|
||||
PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite;
|
||||
@@ -98,15 +99,26 @@ bool @(topic)_Publisher::init()
|
||||
Wparam.topic.topicKind = NO_KEY;
|
||||
Wparam.topic.topicDataType = myType.getName(); //This type MUST be registered
|
||||
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
|
||||
Wparam.topic.topicName = "@(topic)_PubSubTopic";
|
||||
@[else]@
|
||||
Wparam.topic.topicName = "@(topic)PubSubTopic";
|
||||
@[end if]@
|
||||
@[if ros2_distro]@
|
||||
@[ if ros2_distro == "ardent"]@
|
||||
@[ if ros2_distro]@
|
||||
@[ if ros2_distro == "ardent"]@
|
||||
Wparam.qos.m_partition.push_back("rt");
|
||||
Wparam.topic.topicName = "@(topic)_PubSubTopic";
|
||||
@[ else]@
|
||||
Wparam.topic.topicName = "rt/@(topic)_PubSubTopic";
|
||||
@[ end if]@
|
||||
@[ else]@
|
||||
Wparam.topic.topicName = "rt/" + Wparam.topic.topicName;
|
||||
Wparam.topic.topicName = "@(topic)_PubSubTopic";
|
||||
@[ end if]@
|
||||
@[else]@
|
||||
@[ if ros2_distro]@
|
||||
@[ if ros2_distro == "ardent"]@
|
||||
Wparam.qos.m_partition.push_back("rt");
|
||||
Wparam.topic.topicName = "@(topic)PubSubTopic";
|
||||
@[ else]@
|
||||
Wparam.topic.topicName = "rt/@(topic)PubSubTopic";
|
||||
@[ end if]@
|
||||
@[ else]@
|
||||
Wparam.topic.topicName = "@(topic)PubSubTopic";
|
||||
@[ end if]@
|
||||
@[end if]@
|
||||
mp_publisher = Domain::createPublisher(mp_participant, Wparam, static_cast<PublisherListener*>(&m_listener));
|
||||
|
||||
@@ -16,6 +16,7 @@ import gencpp
|
||||
from px_generate_uorb_topic_helper import * # this is in Tools/
|
||||
|
||||
topic = alias if alias else spec.short_name
|
||||
ros2_distro = ros2_distro.decode("utf-8")
|
||||
}@
|
||||
/****************************************************************************
|
||||
*
|
||||
|
||||
@@ -19,6 +19,9 @@ from px_generate_uorb_topic_files import MsgScope # this is in Tools/
|
||||
|
||||
send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
|
||||
recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
|
||||
package = package[0]
|
||||
fastrtpsgen_version = fastrtpsgen_version[0]
|
||||
ros2_distro = ros2_distro[0].decode("utf-8")
|
||||
}@
|
||||
/****************************************************************************
|
||||
*
|
||||
@@ -92,15 +95,15 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
|
||||
@[for topic in send_topics]@
|
||||
case @(rtps_message_id(ids, topic)): // @(topic)
|
||||
{
|
||||
@[ if 1.5 <= fastrtpsgen_version[0] <= 1.7]@
|
||||
@[ if ros2_distro[0]]@
|
||||
@(package[0])::msg::dds_::@(topic)_ st;
|
||||
@[ if 1.5 <= fastrtpsgen_version <= 1.7]@
|
||||
@[ if ros2_distro]@
|
||||
@(package)::msg::dds_::@(topic)_ st;
|
||||
@[ else]@
|
||||
@(topic)_ st;
|
||||
@[ end if]@
|
||||
@[ else]@
|
||||
@[ if ros2_distro[0]]@
|
||||
@(package[0])::msg::@(topic) st;
|
||||
@[ if ros2_distro]@
|
||||
@(package)::msg::@(topic) st;
|
||||
@[ else]@
|
||||
@(topic) st;
|
||||
@[ end if]@
|
||||
@@ -157,15 +160,15 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
|
||||
case @(rtps_message_id(ids, topic)): // @(topic)
|
||||
if (_@(topic)_sub.hasMsg())
|
||||
{
|
||||
@[ if 1.5 <= fastrtpsgen_version[0] <= 1.7]@
|
||||
@[ if ros2_distro[0]]@
|
||||
@(package[0])::msg::dds_::@(topic)_ msg = _@(topic)_sub.getMsg();
|
||||
@[ if 1.5 <= fastrtpsgen_version <= 1.7]@
|
||||
@[ if ros2_distro]@
|
||||
@(package)::msg::dds_::@(topic)_ msg = _@(topic)_sub.getMsg();
|
||||
@[ else]@
|
||||
@(topic)_ msg = _@(topic)_sub.getMsg();
|
||||
@[ end if]@
|
||||
@[ else]@
|
||||
@[ if ros2_distro[0]]@
|
||||
@(package[0])::msg::@(topic) msg = _@(topic)_sub.getMsg();
|
||||
@[ if ros2_distro]@
|
||||
@(package)::msg::@(topic) msg = _@(topic)_sub.getMsg();
|
||||
@[ else]@
|
||||
@(topic) msg = _@(topic)_sub.getMsg();
|
||||
@[ end if]@
|
||||
|
||||
@@ -16,6 +16,7 @@ import gencpp
|
||||
from px_generate_uorb_topic_helper import * # this is in Tools/
|
||||
|
||||
topic = alias if alias else spec.short_name
|
||||
ros2_distro = ros2_distro.decode("utf-8")
|
||||
}@
|
||||
/****************************************************************************
|
||||
*
|
||||
@@ -75,7 +76,7 @@ bool @(topic)_Subscriber::init()
|
||||
// Create RTPSParticipant
|
||||
ParticipantAttributes PParam;
|
||||
PParam.rtps.builtin.domainId = 0; // MUST BE THE SAME AS IN THE PUBLISHER
|
||||
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
|
||||
@[if 1.5 <= fastrtpsgen_version <= 1.7 or ros2_distro == "ardent" or ros2_distro == "bouncy" or ros2_distro == "crystal" or ros2_distro == "dashing"]@
|
||||
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
|
||||
@[else]@
|
||||
PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite;
|
||||
@@ -93,15 +94,26 @@ bool @(topic)_Subscriber::init()
|
||||
Rparam.topic.topicKind = NO_KEY;
|
||||
Rparam.topic.topicDataType = myType.getName(); //Must be registered before the creation of the subscriber
|
||||
@[if 1.5 <= fastrtpsgen_version <= 1.7]@
|
||||
Rparam.topic.topicName = "@(topic)_PubSubTopic";
|
||||
@[else]@
|
||||
Rparam.topic.topicName = "@(topic)PubSubTopic";
|
||||
@[end if]@
|
||||
@[if ros2_distro]@
|
||||
@[ if ros2_distro == "ardent"]@
|
||||
@[ if ros2_distro]@
|
||||
@[ if ros2_distro == "ardent"]@
|
||||
Rparam.qos.m_partition.push_back("rt");
|
||||
Rparam.topic.topicName = "@(topic)_PubSubTopic";
|
||||
@[ else]@
|
||||
Rparam.topic.topicName = "rt/@(topic)_PubSubTopic";
|
||||
@[ end if]@
|
||||
@[ else]@
|
||||
Rparam.topic.topicName = "rt/" + Rparam.topic.topicName;
|
||||
Rparam.topic.topicName = "@(topic)_PubSubTopic";
|
||||
@[ end if]@
|
||||
@[else]@
|
||||
@[ if ros2_distro]@
|
||||
@[ if ros2_distro == "ardent"]@
|
||||
Rparam.qos.m_partition.push_back("rt");
|
||||
Rparam.topic.topicName = "@(topic)PubSubTopic";
|
||||
@[ else]@
|
||||
Rparam.topic.topicName = "rt/@(topic)PubSubTopic";
|
||||
@[ end if]@
|
||||
@[ else]@
|
||||
Rparam.topic.topicName = "@(topic)PubSubTopic";
|
||||
@[ end if]@
|
||||
@[end if]@
|
||||
mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, static_cast<SubscriberListener*>(&m_listener));
|
||||
|
||||
@@ -16,6 +16,7 @@ import gencpp
|
||||
from px_generate_uorb_topic_helper import * # this is in Tools/
|
||||
|
||||
topic = alias if alias else spec.short_name
|
||||
ros2_distro = ros2_distro.decode("utf-8")
|
||||
}@
|
||||
/****************************************************************************
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user