diff --git a/OpenRTM_aist/GlobalFactory.py b/OpenRTM_aist/GlobalFactory.py index e6843010..092a1913 100644 --- a/OpenRTM_aist/GlobalFactory.py +++ b/OpenRTM_aist/GlobalFactory.py @@ -58,7 +58,7 @@ def addFactory(self, id, creator, prop=None): if id in self._creators: return self.ALREADY_EXISTS - self._creators[id] = creator + self._creators[id] = FactoryEntry(id, creator, prop) return self.FACTORY_OK # ReturnCode removeFactory(const Identifier& id) @@ -76,9 +76,34 @@ def createObject(self, id): if id not in self._creators: print("Factory.createObject return None id: ", id) return None - obj_ = self._creators[id]() + obj_ = self._creators[id].creator() return obj_ + def getProperties(self, id): + if not id in self._creators: + print("Factory.getProperties return None id: ", id) + return None + return self._creators[id].prop + + +class FactoryEntry: + def __init__(self, id, creator, prop=None): + self._id = id + self._creator = creator + self._prop = prop + + @property + def id(self): + return self._id + + @property + def creator(self): + return self._creator + + @property + def prop(self): + return self._prop + gfactory = None diff --git a/OpenRTM_aist/InPortBase.py b/OpenRTM_aist/InPortBase.py index c02534ac..d602d9a0 100644 --- a/OpenRTM_aist/InPortBase.py +++ b/OpenRTM_aist/InPortBase.py @@ -1175,10 +1175,25 @@ def initProviders(self): # InPortProvider supports "push" dataflow type if provider_types: + prop_options = OpenRTM_aist.Properties() self._rtcout.RTC_DEBUG("dataflow_type push is supported") self.appendProperty("dataport.dataflow_type", "push") for provider_type in provider_types: self.appendProperty("dataport.interface_type", provider_type) + prop_if = factory.getProperties(provider_type) + if prop_if is None: + prop_if = OpenRTM_aist.Properties() + + prop_node = prop_options.getNode(provider_type) + prop_node.mergeProperties(prop_if) + + prop = OpenRTM_aist.Properties() + OpenRTM_aist.NVUtil.copyToProperties( + prop, self._profile.properties) + prop_dataport = prop.getNode("dataport.interface_option") + prop_dataport.mergeProperties(prop_options) + OpenRTM_aist.NVUtil.copyFromProperties( + self._profile.properties, prop) self._providerTypes = provider_types return @@ -1219,10 +1234,25 @@ def initConsumers(self): # OutPortConsumer supports "pull" dataflow type if consumer_types: + prop_options = OpenRTM_aist.Properties() self._rtcout.RTC_PARANOID("dataflow_type pull is supported") self.appendProperty("dataport.dataflow_type", "pull") for consumer_type in consumer_types: self.appendProperty("dataport.interface_type", consumer_type) + prop_if = factory.getProperties(consumer_type) + if prop_if is None: + prop_if = OpenRTM_aist.Properties() + + prop_node = prop_options.getNode(consumer_type) + prop_node.mergeProperties(prop_if) + + prop = OpenRTM_aist.Properties() + OpenRTM_aist.NVUtil.copyToProperties( + prop, self._profile.properties) + prop_dataport = prop.getNode("dataport.interface_option") + prop_dataport.mergeProperties(prop_options) + OpenRTM_aist.NVUtil.copyFromProperties( + self._profile.properties, prop) self._consumerTypes = consumer_types return diff --git a/OpenRTM_aist/OutPortBase.py b/OpenRTM_aist/OutPortBase.py index d980d5c6..1983b32a 100644 --- a/OpenRTM_aist/OutPortBase.py +++ b/OpenRTM_aist/OutPortBase.py @@ -1185,11 +1185,27 @@ def initProviders(self): provider_types = provider_types + list(set_ptypes) # OutPortProvider supports "pull" dataflow type + if provider_types: + prop_options = OpenRTM_aist.Properties() self._rtcout.RTC_DEBUG("dataflow_type pull is supported") self.appendProperty("dataport.dataflow_type", "pull") for provider_type in provider_types: self.appendProperty("dataport.interface_type", provider_type) + prop_if = factory.getProperties(provider_type) + if prop_if is None: + prop_if = OpenRTM_aist.Properties() + + prop_node = prop_options.getNode(provider_type) + prop_node.mergeProperties(prop_if) + + prop = OpenRTM_aist.Properties() + OpenRTM_aist.NVUtil.copyToProperties( + prop, self._profile.properties) + prop_dataport = prop.getNode("dataport.interface_option") + prop_dataport.mergeProperties(prop_options) + OpenRTM_aist.NVUtil.copyFromProperties( + self._profile.properties, prop) self._providerTypes = provider_types @@ -1229,10 +1245,25 @@ def initConsumers(self): # InPortConsumer supports "push" dataflow type if consumer_types: + prop_options = OpenRTM_aist.Properties() self._rtcout.RTC_PARANOID("dataflow_type push is supported") self.appendProperty("dataport.dataflow_type", "push") for consumer_type in consumer_types: self.appendProperty("dataport.interface_type", consumer_type) + prop_if = factory.getProperties(consumer_type) + if prop_if is None: + prop_if = OpenRTM_aist.Properties() + + prop_node = prop_options.getNode(consumer_type) + prop_node.mergeProperties(prop_if) + + prop = OpenRTM_aist.Properties() + OpenRTM_aist.NVUtil.copyToProperties( + prop, self._profile.properties) + prop_dataport = prop.getNode("dataport.interface_option") + prop_dataport.mergeProperties(prop_options) + OpenRTM_aist.NVUtil.copyFromProperties( + self._profile.properties, prop) self._consumerTypes = consumer_types diff --git a/OpenRTM_aist/ext/transport/ROS2Transport/ROS2InPort.py b/OpenRTM_aist/ext/transport/ROS2Transport/ROS2InPort.py index c8b02bd8..f07eacbd 100644 --- a/OpenRTM_aist/ext/transport/ROS2Transport/ROS2InPort.py +++ b/OpenRTM_aist/ext/transport/ROS2Transport/ROS2InPort.py @@ -39,6 +39,7 @@ class ROS2InPort(OpenRTM_aist.InPortProvider): """ """ + ddstype = "fast-rtps" ## # @if jp @@ -67,7 +68,7 @@ def __init__(self): OpenRTM_aist.InPortProvider.__init__(self) # PortProfile setting - self.setInterfaceType("ros2") + self.setInterfaceType(self.ddstype) self._profile = None self._listeners = None @@ -144,12 +145,14 @@ def init(self, prop): self._properties = prop - args = [] - self._topicmgr = ROS2TopicManager.instance(args) + self._topicmgr = ROS2TopicManager.instance() self._messageType = prop.getProperty( "marshaling_type", "ros2:std_msgs/Float32") - self._topic = prop.getProperty("ros2.topic", "chatter") + + ddsprop = prop.getNode(self.ddstype) + + self._topic = ddsprop.getProperty("topic", "chatter") self._rtcout.RTC_VERBOSE("message type: %s", self._messageType) self._rtcout.RTC_VERBOSE("topic name: %s", self._topic) @@ -159,22 +162,28 @@ def init(self, prop): info_type = info.datatype() - qos = ROS2TopicManager.get_qosprofile(prop.getNode("ros2.subscriber.qos")) + qos = ROS2TopicManager.get_qosprofile(ddsprop.getNode("reader_qos")) self._rtcout.RTC_VERBOSE("history policy: %s", qos.history) self._rtcout.RTC_VERBOSE("depth: %d", qos.depth) self._rtcout.RTC_VERBOSE("reliability policy: %s", qos.reliability) self._rtcout.RTC_VERBOSE("durability policy: %s", qos.durability) - self._rtcout.RTC_VERBOSE("lifespan: %d [nsec]", qos.lifespan.nanoseconds) - self._rtcout.RTC_VERBOSE("deadline: %d [nsec]", qos.deadline.nanoseconds) + self._rtcout.RTC_VERBOSE( + "lifespan: %d [nsec]", qos.lifespan.nanoseconds) + self._rtcout.RTC_VERBOSE( + "deadline: %d [nsec]", qos.deadline.nanoseconds) self._rtcout.RTC_VERBOSE("liveliness policy: %s", qos.liveliness) - self._rtcout.RTC_VERBOSE("liveliness lease duration: %d [nsec]", qos.liveliness_lease_duration.nanoseconds) - self._rtcout.RTC_VERBOSE("avoid ros namespace conventions: %s", qos.avoid_ros_namespace_conventions) - + self._rtcout.RTC_VERBOSE( + "liveliness lease duration: %d [nsec]", qos.liveliness_lease_duration.nanoseconds) + self._rtcout.RTC_VERBOSE( + "avoid ros namespace conventions: %s", qos.avoid_ros_namespace_conventions) self._subscriber = self._topicmgr.createSubscriber( info_type, self._topic, self.ros2_callback, qos) + if self._subscriber is None: + raise MemoryError("Subscriber creation failed") + # virtual void setBuffer(BufferBase* buffer); def setBuffer(self, buffer): @@ -353,6 +362,66 @@ def onReceiverError(self, data): return data +## +# @if jp +# @class ROS2InPortB +# @brief ROS2Transportのインターフェース型ros2とDDS実装(fast-rtps or rti-connext-dds etc.) +# の2つで登録するために、ROS2InPortのインターフェース型名をros2に設定したオブジェクト +# +# @else +# @class ROS2InPortB +# @brief +# +# +# @endif +class ROS2InPortB(ROS2InPort): + def __init__(self): + self.ddstype = "ros2" + ROS2InPort.__init__(self) + + +ros2_sub_option = [ + "topic.__value__", "chatter", + "topic.__widget__", "text", + "topic.__constraint__", "none", + "reader_qos.durability.kind.__value__", "TRANSIENT_DURABILITY_QOS", + "reader_qos.durability.kind.__widget__", "radio", + "reader_qos.durability.kind.__constraint__", "(VOLATILE_DURABILITY_QOS, TRANSIENT_LOCAL_DURABILITY_QOS, SYSTEM_DEFAULT_QOS)", + "reader_qos.deadline.period.sec.__value__", "0", + "reader_qos.deadline.period.sec.__widget__", "spin", + "reader_qos.deadline.period.sec.__constraint__", "0 <= x <= 2147483647", + "reader_qos.deadline.period.nanosec.__value__", "0", + "reader_qos.deadline.period.nanosec.__widget__", "text", + "reader_qos.deadline.period.nanosec.__constraint__", "0 <= x <= 2147483647", + "reader_qos.liveliness.kind.__value__", "AUTOMATIC_LIVELINESS_QOS", + "reader_qos.liveliness.kind.__widget__", "radio", + "reader_qos.liveliness.kind.__constraint__", "(AUTOMATIC_LIVELINESS_QOS, MANUAL_BY_TOPIC_LIVELINESS_QOS, SYSTEM_DEFAULT_LIVELINESS_QOS)", + "reader_qos.liveliness.lease_duration.sec.__value__", "0", + "reader_qos.liveliness.lease_duration.sec.__widget__", "spin", + "reader_qos.liveliness.lease_duration.sec.__constraint__", "0 <= x <= 2147483647", + "reader_qos.liveliness.lease_duration.nanosec.__value__", "0", + "reader_qos.liveliness.lease_duration.nanosec.__widget__", "spin", + "reader_qos.liveliness.lease_duration.nanosec.__constraint__", "0 <= x <= 2147483647", + "reader_qos.reliability.kind.__value__", "RELIABLE_RELIABILITY_QOS", + "reader_qos.reliability.kind.__widget__", "radio", + "reader_qos.reliability.kind.__constraint__", "(BEST_EFFORT_RELIABILITY_QOS, RELIABLE_RELIABILITY_QOS, SYSTEM_DEFAULT_RELIABILITY_QOS)", + "reader_qos.history.kind.__value__", "KEEP_LAST_HISTORY_QOS", + "reader_qos.history.kind.__widget__", "radio", + "reader_qos.history.kind.__constraint__", "(KEEP_LAST_HISTORY_QOS, KEEP_ALL_HISTORY_QOS, SYSTEM_DEFAULT_HISTORY_QOS)", + "reader_qos.history.depth.__value__", "1", + "reader_qos.history.depth.__widget__", "spin", + "reader_qos.history.depth.__constraint__", "0 <= x <= 2147483647", + "reader_qos.lifespan.duration.sec.__value__", "0", + "reader_qos.lifespan.duration.sec.__widget__", "spin", + "reader_qos.lifespan.duration.sec.__constraint__", "0 <= x <= 2147483647", + "reader_qos.lifespan.duration.nanosec.__value__", "0", + "reader_qos.lifespan.duration.nanosec.__widget__", "spin", + "reader_qos.lifespan.duration.nanosec.__constraint__", "0 <= x <= 2147483647", + "reader_qos.avoid_ros_namespace_conventions.__value__", "YES", + "reader_qos.avoid_ros_namespace_conventions.__widget__", "radio", + "reader_qos.avoid_ros_namespace_conventions.__constraint__", "(YES, NO)", + ""] + ## # @if jp # @brief モジュール登録関数 @@ -364,7 +433,15 @@ def onReceiverError(self, data): # # @endif # -def ROS2InPortInit(): + + +def ROS2InPortInit(ddstype="fast-rtps"): + prop = OpenRTM_aist.Properties(defaults_str=ros2_sub_option) factory = OpenRTM_aist.InPortProviderFactory.instance() + factory.addFactory(ddstype, + ROS2InPort, + prop) factory.addFactory("ros2", - ROS2InPort) + ROS2InPortB, + prop) + ROS2InPort.ddstype = ddstype diff --git a/OpenRTM_aist/ext/transport/ROS2Transport/ROS2OutPort.py b/OpenRTM_aist/ext/transport/ROS2Transport/ROS2OutPort.py index 96f0e3e4..d65fcdf2 100644 --- a/OpenRTM_aist/ext/transport/ROS2Transport/ROS2OutPort.py +++ b/OpenRTM_aist/ext/transport/ROS2Transport/ROS2OutPort.py @@ -38,6 +38,7 @@ class ROS2OutPort(OpenRTM_aist.InPortConsumer): """ """ + ddstype = "fast-rtps" ## # @if jp @@ -114,12 +115,14 @@ def init(self, prop): self._properties = prop - args = [] - self._topicmgr = ROS2TopicManager.instance(args) + self._topicmgr = ROS2TopicManager.instance() self._messageType = prop.getProperty( "marshaling_type", "ros2:std_msgs/Float32") - self._topic = prop.getProperty("ros2.topic", "chatter") + + ddsprop = prop.getNode(self.ddstype) + + self._topic = ddsprop.getProperty("topic", "chatter") self._rtcout.RTC_VERBOSE("message type: %s", self._messageType) self._rtcout.RTC_VERBOSE("topic name: %s", self._topic) @@ -129,22 +132,29 @@ def init(self, prop): info_type = info.datatype() - qos = ROS2TopicManager.get_qosprofile(prop.getNode("ros2.publisher.qos")) + qos = ROS2TopicManager.get_qosprofile( + ddsprop.getNode("writer_qos")) self._rtcout.RTC_VERBOSE("history policy: %s", qos.history) self._rtcout.RTC_VERBOSE("depth: %d", qos.depth) self._rtcout.RTC_VERBOSE("reliability policy: %s", qos.reliability) self._rtcout.RTC_VERBOSE("durability policy: %s", qos.durability) - self._rtcout.RTC_VERBOSE("lifespan: %d [nsec]", qos.lifespan.nanoseconds) - self._rtcout.RTC_VERBOSE("deadline: %d [nsec]", qos.deadline.nanoseconds) + self._rtcout.RTC_VERBOSE( + "lifespan: %d [nsec]", qos.lifespan.nanoseconds) + self._rtcout.RTC_VERBOSE( + "deadline: %d [nsec]", qos.deadline.nanoseconds) self._rtcout.RTC_VERBOSE("liveliness policy: %s", qos.liveliness) - self._rtcout.RTC_VERBOSE("liveliness lease duration: %d [nsec]", qos.liveliness_lease_duration.nanoseconds) - self._rtcout.RTC_VERBOSE("avoid ros namespace conventions: %s", qos.avoid_ros_namespace_conventions) + self._rtcout.RTC_VERBOSE( + "liveliness lease duration: %d [nsec]", qos.liveliness_lease_duration.nanoseconds) + self._rtcout.RTC_VERBOSE( + "avoid ros namespace conventions: %s", qos.avoid_ros_namespace_conventions) - self._publisher = self._topicmgr.createPublisher( info_type, self._topic, qos) + if self._publisher is None: + raise MemoryError("Publisher creation failed") + ## # @if jp # @brief 接続先へのデータ送信 @@ -268,6 +278,66 @@ def unsubscribeInterface(self, properties): pass +## +# @if jp +# @class ROS2OutPortB +# @brief ROS2Transportのインターフェース型ros2とDDS実装(fast-rtps or rti-connext-dds etc.) +# の2つで登録するために、ROS2OutPortのインターフェース型名をros2に設定したオブジェクト +# +# @else +# @class ROS2OutPortB +# @brief +# +# +# @endif +class ROS2OutPortB(ROS2OutPort): + def __init__(self): + self.ddstype = "ros2" + ROS2OutPort.__init__(self) + + +ros2_pub_option = [ + "topic.__value__", "chatter", + "topic.__widget__", "text", + "topic.__constraint__", "none", + "writer_qos.durability.kind.__value__", "TRANSIENT_DURABILITY_QOS", + "writer_qos.durability.kind.__widget__", "radio", + "writer_qos.durability.kind.__constraint__", "(VOLATILE_DURABILITY_QOS, TRANSIENT_LOCAL_DURABILITY_QOS, SYSTEM_DEFAULT_QOS)", + "writer_qos.deadline.period.sec.__value__", "0", + "writer_qos.deadline.period.sec.__widget__", "spin", + "writer_qos.deadline.period.sec.__constraint__", "0 <= x <= 2147483647", + "writer_qos.deadline.period.nanosec.__value__", "0", + "writer_qos.deadline.period.nanosec.__widget__", "text", + "writer_qos.deadline.period.nanosec.__constraint__", "0 <= x <= 2147483647", + "writer_qos.liveliness.kind.__value__", "AUTOMATIC_LIVELINESS_QOS", + "writer_qos.liveliness.kind.__widget__", "radio", + "writer_qos.liveliness.kind.__constraint__", "(AUTOMATIC_LIVELINESS_QOS, MANUAL_BY_TOPIC_LIVELINESS_QOS, SYSTEM_DEFAULT_LIVELINESS_QOS)", + "writer_qos.liveliness.lease_duration.sec.__value__", "0", + "writer_qos.liveliness.lease_duration.sec.__widget__", "spin", + "writer_qos.liveliness.lease_duration.sec.__constraint__", "0 <= x <= 2147483647", + "writer_qos.liveliness.lease_duration.nanosec.__value__", "0", + "writer_qos.liveliness.lease_duration.nanosec.__widget__", "spin", + "writer_qos.liveliness.lease_duration.nanosec.__constraint__", "0 <= x <= 2147483647", + "writer_qos.reliability.kind.__value__", "RELIABLE_RELIABILITY_QOS", + "writer_qos.reliability.kind.__widget__", "radio", + "writer_qos.reliability.kind.__constraint__", "(BEST_EFFORT_RELIABILITY_QOS, RELIABLE_RELIABILITY_QOS, SYSTEM_DEFAULT_RELIABILITY_QOS)", + "writer_qos.history.kind.__value__", "KEEP_LAST_HISTORY_QOS", + "writer_qos.history.kind.__widget__", "radio", + "writer_qos.history.kind.__constraint__", "(KEEP_LAST_HISTORY_QOS, KEEP_ALL_HISTORY_QOS, SYSTEM_DEFAULT_HISTORY_QOS)", + "writer_qos.history.depth.__value__", "1", + "writer_qos.history.depth.__widget__", "spin", + "writer_qos.history.depth.__constraint__", "0 <= x <= 2147483647", + "writer_qos.lifespan.duration.sec.__value__", "0", + "writer_qos.lifespan.duration.sec.__widget__", "spin", + "writer_qos.lifespan.duration.sec.__constraint__", "0 <= x <= 2147483647", + "writer_qos.lifespan.duration.nanosec.__value__", "0", + "writer_qos.lifespan.duration.nanosec.__widget__", "spin", + "writer_qos.lifespan.duration.nanosec.__constraint__", "0 <= x <= 2147483647", + "writer_qos.avoid_ros_namespace_conventions.__value__", "YES", + "writer_qos.avoid_ros_namespace_conventions.__widget__", "radio", + "writer_qos.avoid_ros_namespace_conventions.__constraint__", "(YES, NO)", + ""] + ## # @if jp # @brief モジュール登録関数 @@ -279,7 +349,15 @@ def unsubscribeInterface(self, properties): # # @endif # -def ROS2OutPortInit(): + + +def ROS2OutPortInit(ddstype="fast-rtps"): + prop = OpenRTM_aist.Properties(defaults_str=ros2_pub_option) factory = OpenRTM_aist.InPortConsumerFactory.instance() + factory.addFactory(ddstype, + ROS2OutPort, + prop) factory.addFactory("ros2", - ROS2OutPort) + ROS2OutPortB, + prop) + ROS2OutPort.ddstype = ddstype diff --git a/OpenRTM_aist/ext/transport/ROS2Transport/ROS2TopicManager.py b/OpenRTM_aist/ext/transport/ROS2Transport/ROS2TopicManager.py index 9eaff5fa..7ed87650 100644 --- a/OpenRTM_aist/ext/transport/ROS2Transport/ROS2TopicManager.py +++ b/OpenRTM_aist/ext/transport/ROS2Transport/ROS2TopicManager.py @@ -20,6 +20,7 @@ import OpenRTM_aist import ROS2MessageInfo import rclpy +import rclpy.qos from rclpy.node import Node from rclpy.qos import QoSProfile @@ -101,9 +102,21 @@ def __del__(self): # # @endif - def start(self, args=[]): + def start(self, prop): + + tmp_args = prop.getProperty("args").split("\"") + args = [] + for i, tmp_arg in enumerate(tmp_args): + if i % 2 == 0: + args.extend(tmp_arg.strip().split(" ")) + else: + args.append(tmp_arg) + + args.insert(0, "manager") + rclpy.init(args=args) - self._node = Node("openrtm") + + self._node = Node(prop.getProperty("node.name", "openrtm")) def spin(): while self._loop: @@ -151,7 +164,7 @@ def shutdown(self): # @param self # @param msgtype # @param topic - # @param qos + # @param qos # @return # # @endif @@ -184,7 +197,7 @@ def createPublisher(self, msgtype, topic, qos=None): # @param msgtype # @param topic # @param listener - # @param qos + # @param qos # @return # # @endif @@ -203,6 +216,29 @@ def deletePublisher(self, pub): def deleteSubscriber(self, sub): pass + ## + # @if jp + # @brief 初期化 + # + # + # @else + # + # @brief + # + # + # @endif + + def init(prop): + global manager + global mutex + + guard = OpenRTM_aist.ScopedLock(mutex) + if manager is None: + manager = ROS2TopicManager() + manager.start(prop) + + init = staticmethod(init) + ## # @if jp # @brief インスタンス取得 @@ -217,14 +253,14 @@ def deleteSubscriber(self, sub): # # @endif - def instance(args=[]): + def instance(): global manager global mutex guard = OpenRTM_aist.ScopedLock(mutex) if manager is None: manager = ROS2TopicManager() - manager.start(args) + manager.start() return manager @@ -254,7 +290,6 @@ def shutdown_global(): shutdown_global = staticmethod(shutdown_global) - ## # @if jp # @brief プロパティからQoSProfileを設定する @@ -268,6 +303,7 @@ def shutdown_global(): # @return QoSProfile # # @endif + def get_qosprofile(prop): if hasattr(rclpy.qos, "HistoryPolicy"): @@ -295,90 +331,118 @@ def get_qosprofile(prop): else: LivelinessPolicy = rclpy.qos.QoSLivelinessPolicy - history = HistoryPolicy.KEEP_LAST - history_type = prop.getProperty("history", "KEEP_LAST") - - if history_type == "SYSTEM_DEFAULT": - history = HistoryPolicy.SYSTEM_DEFAULT - elif history_type == "KEEP_ALL": - history = HistoryPolicy.KEEP_ALL - else: - history = HistoryPolicy.KEEP_LAST - - - depth = 10 - depth_value_str = prop.getProperty("depth", "10") + durability_kind = DurabilityPolicy.SYSTEM_DEFAULT + durability_kind_str = prop.getProperty( + "durability.kind") + if durability_kind_str == "VOLATILE_DURABILITY_QOS": + durability_kind = DurabilityPolicy.VOLATILE + elif durability_kind_str == "TRANSIENT_LOCAL_DURABILITY_QOS": + durability_kind = DurabilityPolicy.TRANSIENT_LOCAL + elif durability_kind_str == "SYSTEM_DEFAULT_QOS": + durability_kind = DurabilityPolicy.SYSTEM_DEFAULT + + deadline_period = ROS2TopicManager.getDuration( + prop.getNode("deadline.period"), Duration) + + if deadline_period is None: + deadline_period = Duration( + seconds=0, nanoseconds=0) + + liveliness_kind = LivelinessPolicy.SYSTEM_DEFAULT + liveliness_kind_str = prop.getProperty( + "liveliness.kind") + if liveliness_kind_str == "AUTOMATIC_LIVELINESS_QOS": + liveliness_kind = LivelinessPolicy.AUTOMATIC + elif liveliness_kind_str == "MANUAL_BY_TOPIC_LIVELINESS_QOS": + liveliness_kind = LivelinessPolicy.MANUAL_BY_TOPIC + elif liveliness_kind_str == "SYSTEM_DEFAULT_LIVELINESS_QOS": + liveliness_kind = LivelinessPolicy.SYSTEM_DEFAULT + + liveliness_lease_duration_time = ROS2TopicManager.getDuration( + prop.getNode("liveliness.lease_duration"), Duration) + + if liveliness_lease_duration_time is None: + liveliness_lease_duration_time = Duration( + seconds=0, nanoseconds=0) + + reliability_kind = ReliabilityPolicy.SYSTEM_DEFAULT + reliability_kind_str = prop.getProperty( + "reliability.kind") + if reliability_kind_str == "BEST_EFFORT_RELIABILITY_QOS": + reliability_kind = ReliabilityPolicy.BEST_EFFORT + elif reliability_kind_str == "RELIABLE_RELIABILITY_QOS": + reliability_kind = ReliabilityPolicy.RELIABLE + elif reliability_kind_str == "SYSTEM_DEFAULT_RELIABILITY_QOS": + reliability_kind = ReliabilityPolicy.SYSTEM_DEFAULT + + history_qos_policy_kind = HistoryPolicy.SYSTEM_DEFAULT + history_qos_policy_kind_str = prop.getProperty( + "history.kind") + if history_qos_policy_kind_str == "KEEP_ALL_HISTORY_QOS": + history_qos_policy_kind = HistoryPolicy.KEEP_ALL + elif history_qos_policy_kind_str == "KEEP_LAST_HISTORY_QOS": + history_qos_policy_kind = HistoryPolicy.KEEP_LAST + elif history_qos_policy_kind_str == "SYSTEM_DEFAULT_HISTORY_QOS": + history_qos_policy_kind = HistoryPolicy.SYSTEM_DEFAULT + + history_depth = 1 try: - depth = int(depth_value_str) - except ValueError: + history_depth = int(prop.getProperty( + "history.depth")) + except ValueError as error: pass + # self._rtcout.RTC_ERROR(error) - reliability = ReliabilityPolicy.RELIABLE - reliability_type = prop.getProperty("reliability", "RELIABLE") - if reliability_type == "SYSTEM_DEFAULT": - reliability = ReliabilityPolicy.SYSTEM_DEFAULT - elif reliability_type == "BEST_EFFORT": - reliability = ReliabilityPolicy.BEST_EFFORT - else: - reliability = ReliabilityPolicy.RELIABLE - - durability = DurabilityPolicy.VOLATILE - durability_type = prop.getProperty("durability", "VOLATILE") - if durability_type == "SYSTEM_DEFAULT": - durability = DurabilityPolicy.SYSTEM_DEFAULT - elif durability_type == "TRANSIENT_LOCAL": - durability = DurabilityPolicy.TRANSIENT_LOCAL - else: - durability = DurabilityPolicy.VOLATILE - - lifespan = Duration(seconds=0, nanoseconds=0) - lifespan_value_str = prop.getProperty("lifespan", "0") - try: - lifespan_value = int(lifespan_value_str) - lifespan = Duration(nanoseconds=lifespan_value) - except ValueError: - pass - - deadline = Duration(seconds=0, nanoseconds=0) - deadline_value_str = prop.getProperty("deadline", "0") - try: - deadline_value = int(deadline_value_str) - deadline = Duration(nanoseconds=deadline_value) - except ValueError: - pass - - liveliness = LivelinessPolicy.SYSTEM_DEFAULT - liveliness_type = prop.getProperty("liveliness", "SYSTEM_DEFAULT") - if liveliness_type == "AUTOMATIC": - liveliness = LivelinessPolicy.AUTOMATIC - elif liveliness_type == "MANUAL_BY_NODE": - liveliness = LivelinessPolicy.MANUAL_BY_NODE - elif liveliness_type == "MANUAL_BY_TOPIC": - liveliness = LivelinessPolicy.MANUAL_BY_TOPIC - else: - liveliness = LivelinessPolicy.SYSTEM_DEFAULT + lifespan_duration = ROS2TopicManager.getDuration( + prop.getNode("lifespan.duration"), Duration) - liveliness_lease_duration = Duration(seconds=0, nanoseconds=0) - liveliness_lease_duration_value_str = prop.getProperty("liveliness_lease_duration", "0") - try: - liveliness_lease_duration_value = int(liveliness_lease_duration_value_str) - liveliness_lease_duration = Duration(nanoseconds=liveliness_lease_duration_value) - except ValueError: - pass + if lifespan_duration is None: + lifespan_duration = Duration( + seconds=10000, nanoseconds=2147483647) - avoid_ros_namespace_conventions = False - if OpenRTM_aist.toBool(prop.getProperty( - "avoid_ros_namespace_conventions"), "YES", "NO", False): - avoid_ros_namespace_conventions = True - else: - avoid_ros_namespace_conventions = False - + avoid_ros_namespace_conventions = OpenRTM_aist.toBool(prop.getProperty( + "avoid_ros_namespace_conventions"), "YES", "NO", False) - qos = QoSProfile(history=history, depth=depth, reliability=reliability, durability=durability, - lifespan=lifespan, deadline=deadline, liveliness=liveliness, - liveliness_lease_duration=liveliness_lease_duration, - avoid_ros_namespace_conventions=avoid_ros_namespace_conventions) + qos = QoSProfile(history=history_qos_policy_kind, depth=history_depth, + reliability=reliability_kind, + durability=durability_kind, + lifespan=lifespan_duration, + deadline=deadline_period, + liveliness=liveliness_kind, + liveliness_lease_duration=liveliness_lease_duration_time, + avoid_ros_namespace_conventions=avoid_ros_namespace_conventions) return qos get_qosprofile = staticmethod(get_qosprofile) + + ## + # @if jp + # @brief プロパティからDDS::Durationを設定して取得する + # + # @param self + # @param prop プロパティ(sec、nanosecの要素に値を格納する) + # @return DDS::Duration + # + # @else + # + # @brief + # + # @param self + # @param prop + # @return + # + # @endif + def getDuration(prop, DDSDuration): + sec_str = prop.getProperty("sec") + nanosec_str = prop.getProperty("nanosec") + try: + sec = int(sec_str) + nanosec = int(nanosec_str) + return DDSDuration(seconds=sec, nanoseconds=nanosec) + except ValueError as error: + return None + # self._rtcout.RTC_ERROR(error) + return None + + getDuration = staticmethod(getDuration) diff --git a/OpenRTM_aist/ext/transport/ROS2Transport/ROS2Transport.py b/OpenRTM_aist/ext/transport/ROS2Transport/ROS2Transport.py index d172d8d0..3e1601b3 100644 --- a/OpenRTM_aist/ext/transport/ROS2Transport/ROS2Transport.py +++ b/OpenRTM_aist/ext/transport/ROS2Transport/ROS2Transport.py @@ -17,6 +17,7 @@ # $Id$ # +import os import OpenRTM_aist import ROS2InPort import ROS2OutPort @@ -89,8 +90,26 @@ def postReinit(self): # @endif # def ROS2TransportInit(mgr): - ROS2InPort.ROS2InPortInit() - ROS2OutPort.ROS2OutPortInit() + ddstype_env = os.getenv("RMW_IMPLEMENTATION") + ddstype = "fast-rtps" + if ddstype_env == "rmw_fastrtps_cpp" or ddstype_env == "rmw_fastrtps_dynamic_cpp": + ddstype = "fast-rtps" + elif ddstype_env == "rmw_connext_cpp" or ddstype_env == "rti_connext_cpp" or ddstype_env == "rmw_connextdds": + ddstype = "rti-connext-dds" + elif ddstype_env == "rmw_opensplice_cpp": + ddstype = "opensplice" + elif ddstype_env == "rmw_iceoryx_cpp": + ddstype = "iceoryx" + elif ddstype_env == "rmw_connextddsmicro": + ddstype = "rti-connext-dds-micro" + elif ddstype_env == "rmw_cyclonedds_cpp": + ddstype = "cyclone-dds" + + ROS2OutPort.ROS2OutPortInit(ddstype) + ROS2InPort.ROS2InPortInit(ddstype) + ROS2Serializer.ROS2SerializerInit() + ROS2TopicManager.init(mgr.getConfig().getNode("ros2")) + mgr.addManagerActionListener(ManagerActionListener()) diff --git a/OpenRTM_aist/ext/transport/ROS2Transport/rtc.conf b/OpenRTM_aist/ext/transport/ROS2Transport/rtc.conf index 8d31c18c..f055ca1a 100644 --- a/OpenRTM_aist/ext/transport/ROS2Transport/rtc.conf +++ b/OpenRTM_aist/ext/transport/ROS2Transport/rtc.conf @@ -6,5 +6,5 @@ manager.modules.load_path: . #manager.components.precreate: ConsoleOut #manager.components.preconnect: ConsoleOut0.in?port=ConsoleIn0.out manager.modules.preload: ROS2Transport.py -manager.components.preconnect: ConsoleOut0.in?interface_type=ros2&marshaling_type=ROS2Float32, ConsoleIn0.out?interface_type=ros2&marshaling_type=ROS2Float32 manager.components.preactivation: ConsoleOut0, ConsoleIn0 +manager.components.preconnect: ConsoleOut0.in?interface_type=fast-rtps&marshaling_type=ros2:std_msgs/Float32, ConsoleIn0.out?interface_type=fast-rtps&marshaling_type=ros2:std_msgs/Float32