Merge pull request #154 from fzi-forschungszentrum-informatik/type-sy…

…stem-web-update

Implement stricter type system for ROS types

ros_bt_py/package.xml

@@ -15,6 +15,9 @@
   <depend>ros_bt_py_interfaces</depend>
   <depend>std_msgs</depend>
   <depend>diagnostic_msgs</depend>
+
+  <exec_depend>example_interfaces</exec_depend>
+
   <test_depend>std_srvs</test_depend>
   <test_depend>geometry_msgs</test_depend>
   <test_depend>sensor_msgs</test_depend>

ros_bt_py/ros_bt_py/custom_types.py

@@ -0,0 +1,98 @@
+# Copyright 2024 FZI Forschungszentrum Informatik
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the FZI Forschungszentrum Informatik nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from abc import ABC, abstractmethod
+import rosidl_runtime_py
+import rosidl_runtime_py.utilities
+
+# TODO Math types for operation and operand remain in `helpers.py`
+# to not cause a breaking change. If those are ever updated,
+# they should be moved here.
+from .helpers import MathBinaryOperator, MathUnaryOperator
+from .helpers import MathOperandType, MathUnaryOperandType
+
+
+class FilePath(object):
+    def __init__(self, path=""):
+        self.path = path
+
+
+class RosType(ABC):
+    # NOTE Subclasses should supply a working default
+    @abstractmethod
+    def __init__(self, type_str):
+        self.type_str = type_str
+
+    # NOTE We can't do the conversion in `__init__`,
+    # because jsonpickle bypasses the init function.
+    @abstractmethod
+    def get_type_obj(self):
+        pass
+
+
+class RosTopicType(RosType):
+    def __init__(self, type_str="std_msgs/msg/Empty"):
+        super().__init__(type_str)
+
+    def get_type_obj(self):
+        return rosidl_runtime_py.utilities.get_message(self.type_str)
+
+
+class RosServiceType(RosType):
+    def __init__(self, type_str="std_srvs/srv/Trigger"):
+        super().__init__(type_str)
+
+    def get_type_obj(self):
+        return rosidl_runtime_py.utilities.get_service(self.type_str)
+
+
+class RosActionType(RosType):
+    def __init__(self, type_str="example_interfaces/action/Fibonacci"):
+        super().__init__(type_str)
+
+    def get_type_obj(self):
+        return rosidl_runtime_py.utilities.get_action(self.type_str)
+
+
+# NOTE These should only be used if the service name is specified via options.
+# If the name is specified via inputs just use string instead
+class RosName(ABC):
+    def __init__(self, name=""):
+        self.name = name
+
+
+class RosTopicName(RosName):
+    pass
+
+
+class RosServiceName(RosName):
+    pass
+
+
+class RosActionName(RosName):
+    pass

ros_bt_py/ros_bt_py/helpers.py

@@ -55,6 +55,23 @@ def set_node_state_to_shutdown(tree):
     return tree
 
 
+class MathUnaryOperator(object):
+    def __init__(self, operator='sqrt'):
+        self.operator = operator
+
+class MathBinaryOperator(object):
+    def __init__(self, operator='+'):
+        self.operator = operator
+
+class MathOperandType(object):
+    def __init__(self, operand_type='float'):
+        self.operand_type = operand_type
+
+class MathUnaryOperandType(object):
+    def __init__(self, operand_type='float'):
+        self.operand_type = operand_type
+
+
 # handling nested objects,
 # see https://stackoverflow.com/questions/31174295/getattr-and-setattr-on-nested-objects
 def rgetattr(obj, attr, *args):
@@ -106,25 +123,6 @@ def json_decode(data):
     return jsonpickle.decode(data)
 
 
-class MathUnaryOperator(object):
-    def __init__(self, operator):
-        self.operator = operator
-
-
-class MathBinaryOperator(object):
-    def __init__(self, operator):
-        self.operator = operator
-
-
-class MathOperandType(object):
-    def __init__(self, operand_type):
-        self.operand_type = operand_type
-
-
-class MathUnaryOperandType(object):
-    def __init__(self, operand_type):
-        self.operand_type = operand_type
-
 
 class HashableCapabilityInterface:
     """Wrapper class to allow for the hashing of capability interfaces."""

ros_bt_py/ros_bt_py/node_data.py

@@ -28,7 +28,17 @@
 import rclpy
 import rclpy.logging
 
+from ros_bt_py.custom_types import (
+    FilePath,
+    RosActionName,
+    RosActionType,
+    RosServiceName,
+    RosServiceType,
+    RosTopicName,
+    RosTopicType,
+)
 from ros_bt_py.helpers import json_encode
+from ros_bt_py.ros_helpers import get_interface_name
 
 
 def from_string(data_type, string_value, static=False):
@@ -104,8 +114,24 @@ def set(self, new_value):
         if self._static and self.updated:
             raise Exception("Trying to overwrite data in static NodeData object")
         if not isinstance(new_value, self.data_type) and new_value is not None:
+            # Convert str based params to the FilePath or Ros...Name format.
+            if self.data_type in [
+                RosServiceName,
+                RosTopicName,
+                RosActionName,
+                FilePath,
+            ] and isinstance(new_value, str):
+                new_value = self.data_type(new_value)
+            # Convert Ros...Type from type variables!
+            elif self.data_type in [
+                RosServiceType,
+                RosActionType,
+                RosTopicType,
+            ] and isinstance(new_value, type):
+                new_value_type = get_interface_name(new_value)
+                new_value = self.data_type(new_value_type)
             # Silently convert ints to float
-            if self.data_type == float and isinstance(new_value, int):
+            elif self.data_type == float and isinstance(new_value, int):
                 new_value = float(new_value)
             else:
                 if type(new_value) is dict and "py/type" in new_value:

ros_bt_py/ros_bt_py/nodes/getters.py

@@ -376,6 +376,7 @@ def _do_tick(self):
 
         if self.inputs.is_updated("object"):
             try:
+                #TODO Maybe it would be nice to allow for calling 0-argument functions this way?
                 self.outputs["attr"] = rgetattr(
                     self.inputs["object"], self.options["attr_name"]
                 )

ros_bt_py/ros_bt_py/package_manager.py

@@ -39,7 +39,7 @@
 import rosidl_runtime_py
 import rosidl_runtime_py.utilities
 
-from ros_bt_py_interfaces.msg import Message, Messages, Package, Packages, Tree
+from ros_bt_py_interfaces.msg import MessageTypes, Package, Packages, Tree
 from ros_bt_py_interfaces.srv import (
     GetMessageFields,
     SaveTree,
@@ -184,34 +184,32 @@ def publish_message_list(self):
             )
             return
 
-        messages = []
+        message_types = MessageTypes()
 
-        packages = list(ament_index_python.get_packages_with_prefixes().keys())
+        packages = list(rosidl_runtime_py.get_interface_packages().keys())
         for package, package_messages in rosidl_runtime_py.get_message_interfaces(
             packages
         ).items():
             for message in package_messages:
-                messages.append(
-                    Message(msg=package + "/" + message, service=False, action=False)
+                message_types.topics.append(
+                    package + "/" + message
                 )
         for package, package_services in rosidl_runtime_py.get_service_interfaces(
             packages
         ).items():
             for service in package_services:
-                messages.append(
-                    Message(msg=package + "/" + service, service=True, action=False)
+                message_types.services.append(
+                    package + "/" + service
                 )
         for package, package_actions in rosidl_runtime_py.get_action_interfaces(
             packages
         ).items():
             for action in package_actions:
-                messages.append(
-                    Message(msg=package + "/" + action, service=False, action=True)
+                message_types.actions.append(
+                    package + "/" + action
                 )
 
-        msg = Messages()
-        msg.messages = messages
-        self.message_list_pub.publish(msg)
+        self.message_list_pub.publish(message_types)
 
     def get_message_fields(
         self, request: GetMessageFields.Request, response: GetMessageFields.Response

ros_bt_py/ros_bt_py/ros_helpers.py

@@ -28,8 +28,11 @@
 import inspect
 
 import rclpy.logging
+from rclpy import action
+from rclpy.node import Node, Publisher
 
 from ros_bt_py.exceptions import BehaviorTreeException
+from ros_bt_py_interfaces.msg import MessageChannel, MessageChannels
 
 
 class LoggerLevel(object):
@@ -48,6 +51,21 @@ def __init__(self, enum_value=""):
         self.enum_value = enum_value
 
 
+def get_interface_name(msg_metaclass: type) -> str:
+    """
+    Extract the interface name from a ROS2 message metaclass.
+
+    :param msg_metaclass: The ROS2 message metaclass.
+
+    :returns: The interface name in the format 'package_name/message_type/message_name'.
+    """
+    module_parts = msg_metaclass.__module__.split(".")
+    package_name = module_parts[0]
+    message_type = module_parts[-2]  # Extract 'msg', 'srv', or 'action'
+    message_name = msg_metaclass.__name__
+    return f"{package_name}/{message_type}/{message_name}"
+
+
 def get_message_constant_fields(message_class):
     """Return all constant fields of a message as a list."""
     if inspect.isclass(message_class):
@@ -62,3 +80,16 @@ def get_message_constant_fields(message_class):
         return members
     else:
         raise BehaviorTreeException(f"{message_class} is not a ROS Message")
+
+
+def publish_message_channels(node: Node, publisher: Publisher):
+    """Return all known topic-, service-, and action-names."""
+    msg = MessageChannels()
+    # Types are returned as 1?-element lists, so we need to unpack them
+    for name, [interface, *_] in node.get_topic_names_and_types():
+        msg.topics.append(MessageChannel(name=name, type=interface))
+    for name, [interface, *_] in node.get_service_names_and_types():
+        msg.services.append(MessageChannel(name=name, type=interface))
+    for name, [interface, *_] in action.get_action_names_and_types(node):
+        msg.actions.append(MessageChannel(name=name, type=interface))
+    publisher.publish(msg)