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 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])