Repository: felixdivo/ros2-easy-test (language: python)

Issue:
Using this package for action tests.
Hi,
I am trying to use the ROS2TestEnvironment to help write unit tests for an action server.  I think I have a basic test working.  I believe this can be easily generalized and added to the ROS2TestEnvironment class.  The code looks like:

```python
class FibonacciActionClient:
    env: ROS2TestEnvironment
    action_client: ActionClient
    send_goal_future: Optional[Future]
    get_result_future: Optional[Future]
    goal_response = Optional[ClientGoalHandle]
    feedbacks = List[Fibonacci.Feedback]
    result = Optional[Fibonacci.Result]
    # A MutuallyExclusive CallbackGroup is required to force all the callbacks:
    # goal_response, feedback and goal_done to run sequentially.
    # Without this we get a race condition between goal_response and feedback.
    action_client_cb_group = CallbackGroup

    def __init__(self, env):
        self.env = env
        self.action_client_cb_group = MutuallyExclusiveCallbackGroup()
        self.action_client = ActionClient(
            env, Fibonacci, "fibonacci", callback_group=self.action_client_cb_group
        )
        self.goal_response = None
        self.send_goal_future = None
        self.get_result_future = None
        self.feedbacks = []
        self.result = None

    def feedback_callback(self, feedback_msg: Fibonacci.Feedback):
        feedback = feedback_msg.feedback  # pyright: ignore[reportAttributeAccessIssue]
        self.feedbacks.append(feedback)  # pyright: ignore[reportCallIssue]

    def goal_response_callback(self, future: Future):
        goal_handle = future.result()
        assert goal_handle is not None
        if not goal_handle.accepted:
            self.env.get_logger().info("Goal rejected :(")
            return

        self.env.get_logger().info("Goal accepted :)")

        self.get_result_future = goal_handle.get_result_async()

    def send_goal_and_wait_for_send_future(self, goal_msg: Fibonacci.Goal):
        timeout_availability = 2.0
        is_ready = self.action_client.wait_for_server()
        if not is_ready:
            raise TimeoutError(
                f"Action server did not become ready within {timeout_availability} seconds"
            )
        self.send_goal_future = self.action_client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_callback
        )
        self.send_goal_future.add_done_callback(self.goal_response_callback)

        assert self.env.executor is not None
        self.env.executor.spin_until_future_complete(
            self.send_goal_future, timeout_sec=2.0
        )
        if self.send_goal_future.done():
            self.goal_response = self.send_goal_future.result()
        else:
            raise TimeoutError(f"Future did not complete within {2.0} seconds")

    def spin_until_goal_done(self):
        assert self.env.executor is not None
        assert self.get_result_future is not None

        GET_RESULT_TIMEOUT_S = 2.0
        self.env.executor.spin_until_future_complete(
            self.get_result_future, timeout_sec=GET_RESULT_TIMEOUT_S
        )
        if self.get_result_future.done():
            self.result = self.get_result_future.result()
        else:
            raise TimeoutError(
                f"Future did not complete within {GET_RESULT_TIMEOUT_S} seconds"
            )

        self.env.executor.spin_once()


@with_single_node(FibonacciActionServer)
def test_fibonacci_action(env: ROS2TestEnvironment) -> None:
    """Test action."""
    client = FibonacciActionClient(env)
    client.send_goal_and_wait_for_send_future(Fibonacci.Goal(order=25))

    assert client.env.executor is not None

    # Spin so we have a goal_response_callback and get_result_future is set.
    while client.get_result_future is None:
        client.env.executor.spin_once()

    client.spin_until_goal_done()

    assert client.feedbacks is not None
    assert client.result is not None
    print(f"{client.feedbacks=}")
    print(f"{client.result=}")
 ```
 
I imagine as part of ros2-test-easy this could be simplified to a single call like:
`response, feedback, done_result = env.call_action("fibonacci", action_goal)`

WDYT
 
Add capability for testing actions.
Per discussion in #25 this PR adds the capability to test actions with a single call like:
```python
    goal_handle, feedbacks, result_response = env.send_action_goal_and_wait_for_response(
        name="fibonacci",
        action_type=Fibonacci,
        goal_msg=Fibonacci.Goal(order=4),
    )
```
I couldn't figure out how to improve typing hints, so have a bunch of `Any` types.
Also I couldn't figure out how to auto-infer action type similar to _get_service_client, could use help with that.



Discussion / hints:
Yes, this sounds useful! We don't use actions right now over at my group, so I haven't yet bothered implementing it. But sure, contributions are welcome. 😃 

As a side note: This should already be possible, by simply following [this tutorial](https://docs.ros.org/en/foxy/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html#writing-an-action-client) and using the fact that the `env: ROS2TestEnvironment` is a `Node` instance. See also [here](https://github.com/felixdivo/ros2-easy-test?tab=readme-ov-file#how-you-can-interact-with-the-nodes).

We should think about this. Would it be a convenience function for calling the action "synchronously", i.e., in a way that only returns if the action is completed fully? I think that sounds reasonable.

Additionally, we might consider adding an example of how to create a service and an action and call it normally by using the `env`. This might be a bit too hidden?
> Additionally, we might consider adding an example of how to create a service and an action and call it normally by using the `env`. This might be a bit too hidden?

You could have a look at #26, if you want to. @vik748 
@felixdivo PTAL.
Hey @vik748, I know I owe you a few comments, also regarding mock testing. Sorry for taking so long. I'll probably respond by the end of the week. I really value your efforts, and at first glance, it looked great. I'll have to find a few more minutes to go through it more slowly, though, hopefully this weekend. :)
Regarding the automatic detection of action type. We always get a goal, e.g. `Fibonacci.Goal(order=4)`. Can't we then parse and import the "main" Type (in this case `Fibonacci`) ver similar to how it is done for services in `_get_service_client`?

This might works if you pass in `goal_class = Fibonacci.Goal`:

```python
# Get the type of the service
# 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_class: Type = getattr(module, base_type_name)
```
Great work so far!
@vik748 I am sorry for my slow responses. I will make sure to answer at least on a weekly basis from May one!
For the record, this is how I do this kind of stuff now. It's an implementation without sleeps, so perhaps can serve as inspiration:

```python
from action_msgs.srv import CancelGoal
from my_package.action import MyAction

def test():
    client = ActionClient(node, MyAction, "my_action")
    assert client.wait_for_server(10.0), "No actionclient for my_action found"

    event = threading.Event()

    def unblock(future):
        nonlocal event
        event.set()

    send_goal_future = self.send_goal_async(goal, **kwargs)
    send_goal_future.add_done_callback(unblock)

    event.wait()
    goal_handle = send_goal_future.result()
    assert goal_handle.accepted

    print("Canceling action")
    response: CancelGoal.Response = goal_handle.cancel_goal()
    assert response.return_code == CancelGoal.Response.ERROR_NONE, "Canceling action failed"


> ```python
> event.wait()
> ```

You could even pass a timeout to it.
I'll react to PRs etc. more regularly from now on.

Fix patch:
diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index 9a122ef..2703c44 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -12,7 +12,7 @@
         "bungcip.better-toml",
         "trond-snekvik.simple-rst",
         "GitHub.vscode-github-actions",
-        "ms-python.black-formatter"
+        "charliermarsh.ruff"
       ]
     }
   },
diff --git a/Dockerfile b/Dockerfile
index a3ca1de..c4c080b 100644
--- a/Dockerfile
+++ b/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
 
diff --git a/pyproject.toml b/pyproject.toml
index 09930c5..e06ea24 100644
--- a/pyproject.toml
+++ b/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


Test patch:
diff --git a/ros2_easy_test/ros2_easy_test/env.py b/ros2_easy_test/ros2_easy_test/env.py
index f85c12b..de28d42 100644
--- a/ros2_easy_test/ros2_easy_test/env.py
+++ b/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()
diff --git a/ros2_easy_test/tests/example_launch_files/fibonacci_action.yaml b/ros2_easy_test/tests/example_launch_files/fibonacci_action.yaml
new file mode 100644
index 0000000..4ffe5bc
--- /dev/null
+++ b/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
diff --git a/ros2_easy_test/tests/example_nodes/minimal_action_server.py b/ros2_easy_test/tests/example_nodes/minimal_action_server.py
new file mode 100644
index 0000000..4d9defd
--- /dev/null
+++ b/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
diff --git a/ros2_easy_test/tests/example_nodes/run_node.py b/ros2_easy_test/tests/example_nodes/run_node.py
index 4163501..ffe603e 100644
--- a/ros2_easy_test/tests/example_nodes/run_node.py
+++ b/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)
 
diff --git a/ros2_easy_test/tests/test_actions.py b/ros2_easy_test/tests/test_actions.py
new file mode 100644
index 0000000..74f6927
--- /dev/null
+++ b/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])