Merge pull request #18 from automatika-robotics/feature/launch_tests

Adds launch tests to package

CMakeLists.txt

@@ -46,4 +46,32 @@ ament_export_dependencies(rosidl_default_runtime)
 # Install Python modules
 ament_python_install_package(ros_sugar)
 
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  # pytest scripts are to be found in the source tree.
+  ament_add_pytest_test(events_pytest "test/events_test.py"
+    PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}"
+    WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/test"
+  )
+
+  ament_add_pytest_test(actions_pytest "test/actions_test.py"
+    PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}"
+    WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/test"
+  )
+
+  ament_add_pytest_test(component_timed_pytest "test/component/timed_run_test.py"
+    PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}"
+    WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/test"
+  )
+
+  ament_add_pytest_test(component_server_pytest "test/component/server_run_test.py"
+    PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}"
+    WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/test"
+  )
+endif()
+
 ament_package()

package.xml

@@ -24,6 +24,13 @@
   <depend>python3-jinja2</depend>
   <depend>python3-msgpack</depend>
 
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>ament_index_python</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_testing</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>

ros_sugar/core/component.py

@@ -487,8 +487,6 @@ def create_all_timers(self):
         Creates all node timers
         """
         # If component is not used as a server start the main execution timer
-        if self.run_type != ComponentRunType.TIMED:
-            return
         self.get_logger().info("CREATING MAIN TIMER")
         self._execution_timer = self.create_timer(
             timer_period_sec=1 / self.config.loop_rate,
@@ -1187,7 +1185,6 @@ def main_action_callback(self, goal_handle):
         """
         if self.run_type == ComponentRunType.ACTION_SERVER:
             raise NotImplementedError
-        pass
 
     def _main_action_goal_callback(self, _):
         """
@@ -1686,36 +1683,38 @@ def _main(self):
         """
         Component execution step every loop_step
         """
+        if self.__enable_health_publishing and self.health_status_publisher:
+            self.health_status_publisher.publish(self.health_status())
+
+        # If it is not a timed component -> only publish status
+        if self.run_type != ComponentRunType.TIMED:
+            return
+
         # Additional execution loop if exists
         if hasattr(self, "_extra_execute_loop"):
             self._extra_execute_loop()
 
         # Execute main loop
         self._execution_step()
 
-        if self.__enable_health_publishing and self.health_status_publisher:
-            self.health_status_publisher.publish(self.health_status())
-
         # Execute once
         if not hasattr(self, "_exec_started"):
             self._execute_once()
             if hasattr(self, "_extra_execute_once"):
                 self._extra_execute_once()
             self._exec_started = True
 
-    # ABSTRACT METHODS
-    @abstractmethod
+    # Timed runtype execution step
     def _execution_step(self):
         """
         Main execution of the component, executed at each timer tick with rate 'loop_rate' from config
         """
-        raise NotImplementedError(
-            "Child components should implement a main execution step"
-        )
+        raise NotImplementedError
 
+    # Timed runtype execution step
     def _execute_once(self):
         """
-        Executed once when the component is started
+        Executed once when the component is started in TIMED runtype
         """
         pass
 

ros_sugar/core/event.py

@@ -8,6 +8,8 @@
 from typing import Any, Callable, Dict, List, Union
 from launch.event import Event as ROSLaunchEvent
 from launch.event_handler import EventHandler as ROSLaunchEventHandler
+import array
+import numpy as np
 
 from ..io.topic import Topic
 from .action import Action
@@ -92,6 +94,11 @@ def _check_attribute(cls, expected_type, attrs: tuple):
             if not hasattr(current_cls, attr):
                 return False
             current_cls = getattr(current_cls, str(attr))
+        # Handle the case of MultiArray data type
+        if isinstance(current_cls, array.array) and (
+            expected_type in [List, np.ndarray, list]
+        ):
+            return True
         return isinstance(current_cls, expected_type)
     except AttributeError:
         return False
@@ -172,6 +179,9 @@ def __init__(self, ros_message: Any, attributes: List[str]) -> None:
         self.value: Union[float, int, bool, str, List] = _access_attribute(
             ros_message, attributes
         )
+        # Handle array case for all std MultiArray messages
+        if isinstance(self.value, array.array):
+            self.value = self.value.tolist()
 
         self.type_error_msg: str = "Cannot compare values of different types"
 
@@ -225,7 +235,7 @@ def __eq__(self, __value: object) -> bool:
         :rtype: bool
         """
         self._check_similar_types(__value)
-        return self.value is __value
+        return self.value == __value
 
     def __ne__(self, __value: object) -> bool:
         """
@@ -353,13 +363,13 @@ def __init__(
 
         elif isinstance(event_source, Topic):
             self.event_topic = event_source
-            if trigger_value is not None:
-                # Trigger access attributes
+            if nested_attributes is not None:
                 self._attrs: List[str] = (
                     nested_attributes
                     if isinstance(nested_attributes, List)
                     else [nested_attributes]
                 )
+            if trigger_value is not None:
                 self.trigger_ref_value = trigger_value
 
         else:

ros_sugar/events.py

@@ -125,7 +125,14 @@ def __init__(
         :rtype: None
         """
         # passing trigger_value as zero as it will not be used in this event
-        super().__init__(event_name, event_source, None, nested_attributes, **kwargs)
+        super().__init__(
+            event_name=event_name,
+            event_source=event_source,
+            trigger_value=None,
+            nested_attributes=nested_attributes,
+            **kwargs,
+        )
+        self._previous_event_value = None
 
     def callback(self, msg) -> None:
         """
@@ -142,7 +149,10 @@ def _update_trigger(self) -> None:
         """
         Set trigger  to True if event value is equal to reference value
         """
-        if self._event_value != self._previous_event_value:
+        if (
+            self._previous_event_value is not None
+            and self._event_value != self._previous_event_value
+        ):
             self.trigger = True
         else:
             self.trigger = False
@@ -187,6 +197,7 @@ def __init__(
         super().__init__(
             event_name, event_source, trigger_value, nested_attributes, **kwargs
         )
+        self._previous_event_value = None
 
     def callback(self, msg) -> None:
         """
@@ -203,14 +214,16 @@ def _update_trigger(self) -> None:
         """
         Set trigger  to True if event value is equal to reference value
         """
-        if hasattr(self, "_previous_event_value"):
+        if self._previous_event_value is not None:
             if (
                 self._event_value != self._previous_event_value
                 and self._event_value == self.trigger_ref_value
             ):
                 self.trigger = True
             else:
                 self.trigger = False
+        else:
+            self.trigger = False
 
 
 class OnEqual(Event):
@@ -283,7 +296,12 @@ def _update_trigger(self) -> None:
         """
         Set trigger  to True if event value contains all of the reference values
         """
-        self.trigger = self.trigger_ref_value in self._event_value
+        if isinstance(self.trigger_ref_value, List):
+            self.trigger = all(
+                val in self._event_value for val in self.trigger_ref_value
+            )
+        else:
+            self.trigger = self.trigger_ref_value in self._event_value
 
 
 class OnContainsAny(Event):

ros_sugar/io/callbacks.py

@@ -203,6 +203,45 @@ def _get_output(self, **_):
         return self.msg.data
 
 
+class StdMsgArrayCallback(GenericCallback):
+    """std_msgs callback"""
+
+    def __init__(self, input_topic, node_name: Optional[str] = None) -> None:
+        super().__init__(input_topic, node_name)
+
+    def _get_output(self, **_) -> np.ndarray:
+        """
+        Gets std_msg data
+        """
+        if not self.msg:
+            return None
+
+        # Shape of the array from the dimensions
+        dims = self.msg.layout.dim
+        shape = []
+        for dim in dims:
+            shape.append(dim.size)
+
+        # If no dimensions are specified, treat as 1D array
+        if not shape:
+            return np.array(self.msg.data)
+
+        # Convert flat data array to properly shaped numpy array
+        try:
+            # Ensure total size matches the product of dimensions
+            expected_size = np.prod(shape)
+            if len(self.msg.data) != expected_size:
+                raise ValueError(
+                    f"Data size {len(self.msg.data)} doesn't match layout dimensions product {expected_size}"
+                )
+
+            # Reshape the data according to the layout
+            return np.array(self.msg.data).reshape(shape)
+
+        except Exception as e:
+            raise ValueError(f"Failed to parse MultiArray message: {str(e)}") from e
+
+
 class ImageCallback(GenericCallback):
     """
     Image Callback class. Its get method saves an image as bytes

ros_sugar/io/supported_types.py

@@ -28,10 +28,17 @@
 from std_msgs.msg import ByteMultiArray
 from std_msgs.msg import String as ROSString
 from std_msgs.msg import Bool as ROSBool
-from std_msgs.msg import Float32 as ROSFloat32
-from std_msgs.msg import Float64 as ROSFloat64
+from std_msgs.msg import (
+    Float32 as ROSFloat32,
+    Float32MultiArray as ROSFloat32MultiArray,
+)
+from std_msgs.msg import (
+    Float64 as ROSFloat64,
+    Float64MultiArray as ROSFloat64MultiArray,
+)
 
 from . import callbacks
+from .utils import numpy_to_multiarray
 
 
 _additional_types = []
@@ -214,14 +221,29 @@ class Float32(SupportedType):
     @classmethod
     def convert(cls, output: float, **_) -> ROSFloat32:
         """
-        Takes a bool and returns a ROS message of type Bool
+        Takes a float and returns a ROS message of type Float32
         :return: Float32
         """
         msg = ROSFloat32()
         msg.data = output
         return msg
 
 
+class Float32MultiArray(SupportedType):
+    """Float32MultiArray."""
+
+    _ros_type = ROSFloat32MultiArray
+    callback = callbacks.StdMsgArrayCallback
+
+    @classmethod
+    def convert(cls, output: np.ndarray, **_) -> ROSFloat32MultiArray:
+        """
+        Takes a numpy array and returns a ROS message of type Float32MultiArray
+        :return: Float32
+        """
+        return numpy_to_multiarray(output, ROSFloat32MultiArray)
+
+
 class Float64(SupportedType):
     """Float64."""
 
@@ -231,14 +253,29 @@ class Float64(SupportedType):
     @classmethod
     def convert(cls, output: float, **_) -> ROSFloat64:
         """
-        Takes a bool and returns a ROS message of type Bool
+        Takes a float and returns a ROS message of type Float64
         :return: Float64
         """
         msg = ROSFloat64()
         msg.data = output
         return msg
 
 
+class Float64MultiArray(SupportedType):
+    """Float64MultiArray."""
+
+    _ros_type = ROSFloat64MultiArray
+    callback = callbacks.StdMsgArrayCallback
+
+    @classmethod
+    def convert(cls, output: np.ndarray, **_) -> ROSFloat64MultiArray:
+        """
+        Takes a numpy array and returns a ROS message of type Float64MultiArray
+        :return: Float32
+        """
+        return numpy_to_multiarray(output, ROSFloat64MultiArray)
+
+
 class Image(SupportedType):
     """Image."""