Merge pull request #37 from felixdivo/support-calling-actions

Support testing actions

.devcontainer/devcontainer.json

@@ -12,7 +12,7 @@
         "bungcip.better-toml",
         "trond-snekvik.simple-rst",
         "GitHub.vscode-github-actions",
-        "ms-python.black-formatter"
+        "charliermarsh.ruff"
       ]
     }
   },

Dockerfile

@@ -11,6 +11,9 @@ FROM ros:humble-ros-core
 RUN apt-get update -q && apt-get install -qy python3-pip python-is-python3 git ros-dev-tools
 RUN rosdep init && rosdep update
 
+# Make sure we can install packages with pip on Jazzy+
+ENV PIP_BREAK_SYSTEM_PACKAGES=1
+
 # Need to have setuptools version 64+ for editable installs
 RUN pip install --upgrade pip setuptools
 

pyproject.toml

@@ -28,7 +28,9 @@ warn_unused_configs = true
 module = [
     "rclpy.*",  # For the CI pipeline to work
     "launch.*",
-    "example_interfaces.*",
+    "launch_ros.*",
     "std_msgs.*",
+    "action_msgs.*",
+    "example_interfaces.*",
 ]
 ignore_missing_imports = true

ros2_easy_test/ros2_easy_test/env.py

@@ -8,9 +8,13 @@
 from time import monotonic, sleep
 
 # Typing
-from typing import Any, Dict, List, Mapping, Optional, Type, cast
+from typing import Any, Dict, List, Mapping, Optional, Tuple, Type, cast
 
 # ROS
+from action_msgs.msg import GoalStatus
+from rclpy.action import ActionClient
+from rclpy.action.client import ClientGoalHandle
+from rclpy.callback_groups import CallbackGroup, MutuallyExclusiveCallbackGroup
 from rclpy.client import Client, SrvTypeRequest, SrvTypeResponse
 from rclpy.node import Node
 from rclpy.publisher import Publisher
@@ -94,10 +98,17 @@ def callback(message: RosMessage, arriving_topic=topic) -> None:
         self._registered_publishers_lock = RLock()
         self._registered_publishers: Dict[str, Publisher] = {}
 
-        # Similarly for services
+        # Similar for services
         self._registered_services_lock = RLock()
         self._registered_services: Dict[str, Client] = {}
 
+        # Similar for actions
+        self._registered_actions_lock = RLock()
+        self._registered_actions: Dict[str, ActionClient] = {}
+        # A MutuallyExclusive CallbackGroup is required to force all the callbacks: goal_response, feedback
+        # and goal_done to run sequentially. This relieves us from some synchronization burden.
+        self._action_cb_group: CallbackGroup = MutuallyExclusiveCallbackGroup()
+
     def _get_mailbox_for(self, topic: str) -> "SimpleQueue[RosMessage]":
         """Checks that a topic is watched for and returns the appropriate mailbox. Handles synchronization.
 
@@ -363,3 +374,133 @@ def call_service(
 
         future = client.call_async(request)
         return cast(SrvTypeResponse, self.await_future(future, timeout=timeout_call))
+
+    def _get_action_client(self, name: str, goal_class: Type) -> ActionClient:
+        """Returns the action client for the given action class."""
+
+        with self._registered_actions_lock:
+            try:
+                return self._registered_actions[name]
+            except KeyError:
+                # This is a bit tricky but relieves the user from passing it explicitly
+                module = import_module(goal_class.__module__)
+                # We cut away the trailing "_Goal" from the type name, which has length 5
+                base_type_name = goal_class.__name__[:-5]
+                base_type: Type = getattr(module, base_type_name)
+
+                client = ActionClient(self, base_type, name, callback_group=self._action_cb_group)
+                self._registered_actions[name] = client
+                return client
+
+    def send_action_goal(
+        self,
+        name: str,
+        goal: Any,
+        collect_feedback: bool = True,
+        timeout_availability: Optional[float] = 1,
+        timeout_receive_goal: Optional[float] = 1,
+    ) -> Tuple[ClientGoalHandle, List[Any]]:
+        """Sends the goal to the given action but does not handle the result.
+
+        This does not check if the goal was accepted and does not attempt to retrieve the result.
+
+        Args:
+            name: The name of the action
+            goal: Goal message to send to the action server.
+            collect_feedback: Whether to collect feedbacks asynchronously. Defaults to True.
+                This is provided for performance reasons.
+            timeout_availability: The timeout to wait for the action to be available. Defaults to 1.
+            timeout_receive_goal: The timeout to wait for the action server to accept the goal. Defaults to 1.
+
+        Raises:
+            TimeoutError: If the action server does not respond within the specified timeouts.
+
+        Returns:
+            Return the goal handle and feedback list (that might still grow asynchronously).
+
+        See Also:
+            :meth:`send_action_goal_and_wait_for_result`
+        """
+
+        # Create an action client
+        client = self._get_action_client(name, type(goal))
+
+        # Wait for the action to be available
+        if not client.wait_for_server(timeout_availability):
+            raise TimeoutError(f"Action {name} did not become ready within {timeout_availability} seconds")
+
+        # Prepare collecting feedbacks
+        feedbacks: List[RosMessage] = []
+
+        def feedback_callback(feedback_msg: RosMessage) -> None:
+            feedbacks.append(feedback_msg.feedback)
+
+        # Send the goal and wait for the acceptence
+        send_goal_future = client.send_goal_async(
+            goal, feedback_callback=feedback_callback if collect_feedback else None
+        )
+        goal_handle: ClientGoalHandle = self.await_future(send_goal_future, timeout=timeout_receive_goal)
+
+        # Return the results to the test case
+        return goal_handle, feedbacks
+
+    def send_action_goal_and_wait_for_result(
+        self,
+        name: str,
+        goal: Any,
+        collect_feedback: bool = True,
+        timeout_availability: Optional[float] = 1,
+        timeout_accept_goal: Optional[float] = 1,
+        timeout_get_result: Optional[float] = 10,
+    ) -> Tuple[List[Any], Any]:
+        """Sends the goal to the given action and returns the response handle, feedbacks and result.
+
+        Args:
+            name: The name of the action
+            goal: Goal message to send to the action server.
+            collect_feedback: Whether to collect feedbacks asynchronously. Defaults to True.
+                This is provided for performance reasons.
+            timeout_availability: The timeout to wait for the action to be available. Defaults to 1.
+            timeout_accept_goal: The timeout to wait for the action server to accept the goal. Defaults to 1.
+            timeout_get_result: The timeout to wait for the action server to return the result.
+                Defaults to 10.
+
+        Raises:
+            TimeoutError: If the action server does not respond within the specified timeouts.
+            AssertionError: If the goal was not accepted or the goal did not succeed.
+
+        Returns:
+            Return the goal handle, all the feedback responses and the final result.
+
+        See Also:
+            :meth:`send_action_goal`
+        """
+
+        # Send the goal and wait for the handle to be available
+        goal_handle, feedbacks = self.send_action_goal(
+            name=name,
+            goal=goal,
+            collect_feedback=collect_feedback,
+            timeout_availability=timeout_availability,
+            timeout_receive_goal=timeout_accept_goal,
+        )
+
+        # Make sure the goal was accepted
+        assert goal_handle.accepted, f"Goal was not accepted: {goal_handle.status=}"
+
+        # Wait for the result
+        result = self.await_future(goal_handle.get_result_async(), timeout=timeout_get_result)
+
+        # Make sure the goal was reached successfully
+        assert goal_handle.status == GoalStatus.STATUS_SUCCEEDED
+
+        # Return the results to the test case
+        return feedbacks, result
+
+    def destroy_node(self):
+        # Actions don't get destroyed automatically
+        with self._registered_actions_lock:
+            for client in self._registered_actions.values():
+                client.destroy()
+
+        return super().destroy_node()

ros2_easy_test/tests/example_launch_files/fibonacci_action.yaml

@@ -0,0 +1,5 @@
+launch:
+- executable:
+    name: Talker
+    cmd: python ros2_easy_test/tests/example_nodes/run_node.py ros2_easy_test/tests/example_nodes/minimal_action_server.py MinimalActionServer
+    output: screen

ros2_easy_test/tests/example_nodes/minimal_action_server.py

@@ -0,0 +1,93 @@
+# Modified from original example in examples_rclpy_minimal_action_server.server as follows:
+#       1. pass *args, **kwargs to __init__ and super().__init__.
+#       2. Reduce sleep times.
+#
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import time
+
+from example_interfaces.action import Fibonacci
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+
+
+class MinimalActionServer(Node):
+    def __init__(self, *args, **kwargs):
+        super().__init__("minimal_action_server", *args, **kwargs)
+
+        self._action_server = ActionServer(
+            self,
+            Fibonacci,
+            "fibonacci",
+            execute_callback=self.execute_callback,
+            callback_group=ReentrantCallbackGroup(),
+            goal_callback=self.goal_callback,
+            cancel_callback=self.cancel_callback,
+        )
+
+    def destroy(self):
+        self._action_server.destroy()
+        super().destroy_node()
+
+    def goal_callback(self, goal_request):
+        """Accept or reject a client request to begin an action."""
+        # This server allows multiple goals in parallel
+        self.get_logger().info("Received goal request")
+        return GoalResponse.ACCEPT
+
+    def cancel_callback(self, goal_handle):
+        """Accept or reject a client request to cancel an action."""
+        self.get_logger().info("Received cancel request")
+        return CancelResponse.ACCEPT
+
+    async def execute_callback(self, goal_handle):
+        """Execute a goal."""
+        self.get_logger().info("Executing goal...")
+
+        # Append the seeds for the Fibonacci sequence
+        feedback_msg = Fibonacci.Feedback()
+        feedback_msg.sequence = [0, 1]
+
+        # Helps avoid race condition in testing.
+        time.sleep(0.1)
+
+        # Start executing the action
+        for i in range(1, goal_handle.request.order):
+            if goal_handle.is_cancel_requested:
+                goal_handle.canceled()
+                self.get_logger().info("Goal canceled")
+                return Fibonacci.Result()
+
+            # Update Fibonacci sequence
+            feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i - 1])
+
+            self.get_logger().info(f"Publishing feedback: {feedback_msg.sequence}")
+
+            # Publish the feedback
+            goal_handle.publish_feedback(feedback_msg)
+
+            # Sleep for demonstration purposes
+            time.sleep(0.01)
+
+        goal_handle.succeed()
+
+        # Populate result message
+        result = Fibonacci.Result()
+        result.sequence = feedback_msg.sequence
+
+        self.get_logger().info(f"Returning result: {result.sequence}")
+
+        return result

ros2_easy_test/tests/example_nodes/run_node.py

@@ -7,16 +7,18 @@
 from typing import Type
 
 import rclpy
+from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 
 
 def main(node_class: Type[Node]) -> None:
     rclpy.init()
+    executor = MultiThreadedExecutor(num_threads=2)
 
     node = node_class()
 
     try:
-        rclpy.spin(node)
+        rclpy.spin(node, executor=executor)
     except KeyboardInterrupt:
         pass  # Ignore Ctrl+C
     finally:
@@ -37,6 +39,7 @@ def main(node_class: Type[Node]) -> None:
     if spec is None:
         raise RuntimeError(f"{file_path} is not a valid Python file.")
     my_module = module_from_spec(spec)
+    assert spec.loader is not None, "spec.loader is None, this should not happen."
     spec.loader.exec_module(my_module)
     node_class = getattr(my_module, node_class_name)
 

ros2_easy_test/tests/test_actions.py

@@ -0,0 +1,47 @@
+"""Tests that actions can be checked correctly."""
+
+# ROS2 infrastructure
+from example_interfaces.action import Fibonacci
+
+# What we are testing
+from ros2_easy_test import ROS2TestEnvironment, with_launch_file, with_single_node
+
+# Module under test and interfaces
+from . import LAUNCH_FILES
+from .example_nodes.minimal_action_server import MinimalActionServer
+
+
+@with_single_node(MinimalActionServer)
+def test_fibonacci_action_direct(env: ROS2TestEnvironment) -> None:
+    """Test calling an action."""
+
+    feedbacks, result = env.send_action_goal_and_wait_for_result(
+        name="fibonacci", goal=Fibonacci.Goal(order=4)
+    )
+
+    assert len(feedbacks) == 3
+    assert feedbacks == [
+        Fibonacci.Feedback(sequence=[0, 1, 1]),
+        Fibonacci.Feedback(sequence=[0, 1, 1, 2]),
+        Fibonacci.Feedback(sequence=[0, 1, 1, 2, 3]),
+    ]
+
+    assert result.result == Fibonacci.Result(sequence=[0, 1, 1, 2, 3])
+
+
+@with_launch_file(LAUNCH_FILES / "fibonacci_action.yaml")
+def test_fibonacci_action_launch_file(env: ROS2TestEnvironment) -> None:
+    """Test calling an action."""
+
+    feedbacks, result = env.send_action_goal_and_wait_for_result(
+        name="fibonacci", goal=Fibonacci.Goal(order=4)
+    )
+
+    assert len(feedbacks) == 3
+    assert feedbacks == [
+        Fibonacci.Feedback(sequence=[0, 1, 1]),
+        Fibonacci.Feedback(sequence=[0, 1, 1, 2]),
+        Fibonacci.Feedback(sequence=[0, 1, 1, 2, 3]),
+    ]
+
+    assert result.result == Fibonacci.Result(sequence=[0, 1, 1, 2, 3])