ROS2Transportのパラメータの情報を設定する

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
 

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

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
 

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<cdrMemoryStream>* 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