From 37f3543a59a783517da74b281f2d93d21e28d11a Mon Sep 17 00:00:00 2001 From: Felix Divo Date: Sun, 12 May 2024 20:48:06 +0000 Subject: [PATCH 1/9] Switch from black for ruff formatter in VSCode --- .devcontainer/devcontainer.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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" ] } }, From 1d2a744561bb4e65e0f8a8bdf01e384a79db7eaf Mon Sep 17 00:00:00 2001 From: Felix Divo Date: Sun, 12 May 2024 21:21:05 +0000 Subject: [PATCH 2/9] Add action server example and make tests run with a multithreaded executor if run separately --- .../minimal_action_server_with_context.py | 93 +++++++++++++++++++ .../tests/example_nodes/run_node.py | 4 +- 2 files changed, 96 insertions(+), 1 deletion(-) create mode 100644 ros2_easy_test/tests/example_nodes/minimal_action_server_with_context.py diff --git a/ros2_easy_test/tests/example_nodes/minimal_action_server_with_context.py b/ros2_easy_test/tests/example_nodes/minimal_action_server_with_context.py new file mode 100644 index 0000000..5583f8a --- /dev/null +++ b/ros2_easy_test/tests/example_nodes/minimal_action_server_with_context.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 MinimalActionServerWithContext(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..1b139e1 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: From d691efade3720b67e2884a9b02c8d6b2998011a8 Mon Sep 17 00:00:00 2001 From: Felix Divo Date: Sun, 12 May 2024 21:27:07 +0000 Subject: [PATCH 3/9] Add launch config for fibonacci action server --- .../tests/example_launch_files/fibonacci_action.yaml | 5 +++++ ...ction_server_with_context.py => minimal_action_server.py} | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) create mode 100644 ros2_easy_test/tests/example_launch_files/fibonacci_action.yaml rename ros2_easy_test/tests/example_nodes/{minimal_action_server_with_context.py => minimal_action_server.py} (98%) 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_with_context.py b/ros2_easy_test/tests/example_nodes/minimal_action_server.py similarity index 98% rename from ros2_easy_test/tests/example_nodes/minimal_action_server_with_context.py rename to ros2_easy_test/tests/example_nodes/minimal_action_server.py index 5583f8a..4d9defd 100644 --- a/ros2_easy_test/tests/example_nodes/minimal_action_server_with_context.py +++ b/ros2_easy_test/tests/example_nodes/minimal_action_server.py @@ -24,7 +24,7 @@ from rclpy.node import Node -class MinimalActionServerWithContext(Node): +class MinimalActionServer(Node): def __init__(self, *args, **kwargs): super().__init__("minimal_action_server", *args, **kwargs) From 717d1dfc45907a843240f2aaf1de11d49a21b692 Mon Sep 17 00:00:00 2001 From: Felix Divo Date: Sun, 12 May 2024 21:27:25 +0000 Subject: [PATCH 4/9] Add action handling to ROS2TestEnvironment --- ros2_easy_test/ros2_easy_test/env.py | 144 ++++++++++++++++++++++++++- 1 file changed, 142 insertions(+), 2 deletions(-) diff --git a/ros2_easy_test/ros2_easy_test/env.py b/ros2_easy_test/ros2_easy_test/env.py index f85c12b..07497a3 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,132 @@ 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_msg: 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. + + 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_msg: 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. + + 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() From c9a6ac2c81033338b89e621e7f58c60f4eacb694 Mon Sep 17 00:00:00 2001 From: Felix Divo Date: Sun, 12 May 2024 21:27:32 +0000 Subject: [PATCH 5/9] Add tests for action testing --- ros2_easy_test/tests/test_actions.py | 47 ++++++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 ros2_easy_test/tests/test_actions.py 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]) From 073a640072bb1ba0ee71364c0db0297c08cd5fc2 Mon Sep 17 00:00:00 2001 From: Felix Divo Date: Sun, 12 May 2024 21:31:11 +0000 Subject: [PATCH 6/9] Make type checker run again --- pyproject.toml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 09930c5..7926c7e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -28,7 +28,8 @@ warn_unused_configs = true module = [ "rclpy.*", # For the CI pipeline to work "launch.*", - "example_interfaces.*", "std_msgs.*", + "action_msgs.*", + "example_interfaces.*", ] ignore_missing_imports = true From b2f4f2862d3c589aca89b65563ff36f7d8cfe2c2 Mon Sep 17 00:00:00 2001 From: Felix Divo Date: Sun, 12 May 2024 21:34:46 +0000 Subject: [PATCH 7/9] Better type checking --- pyproject.toml | 1 + ros2_easy_test/tests/example_nodes/run_node.py | 1 + 2 files changed, 2 insertions(+) diff --git a/pyproject.toml b/pyproject.toml index 7926c7e..e06ea24 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -28,6 +28,7 @@ warn_unused_configs = true module = [ "rclpy.*", # For the CI pipeline to work "launch.*", + "launch_ros.*", "std_msgs.*", "action_msgs.*", "example_interfaces.*", diff --git a/ros2_easy_test/tests/example_nodes/run_node.py b/ros2_easy_test/tests/example_nodes/run_node.py index 1b139e1..ffe603e 100644 --- a/ros2_easy_test/tests/example_nodes/run_node.py +++ b/ros2_easy_test/tests/example_nodes/run_node.py @@ -39,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) From 4a478b0434b8b12d336d3ab68eeeeef853b6f486 Mon Sep 17 00:00:00 2001 From: Felix Divo Date: Sun, 12 May 2024 22:18:00 +0000 Subject: [PATCH 8/9] Sneak in some Dockerfile change that is useful when testing on rolling --- Dockerfile | 3 +++ 1 file changed, 3 insertions(+) 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 From 798cd5a985f9e6b8b1a8c536c56de92b8a4ceb9c Mon Sep 17 00:00:00 2001 From: Felix Divo Date: Mon, 13 May 2024 15:25:24 +0000 Subject: [PATCH 9/9] Cleanup after review --- ros2_easy_test/ros2_easy_test/env.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ros2_easy_test/ros2_easy_test/env.py b/ros2_easy_test/ros2_easy_test/env.py index 07497a3..de28d42 100644 --- a/ros2_easy_test/ros2_easy_test/env.py +++ b/ros2_easy_test/ros2_easy_test/env.py @@ -406,11 +406,11 @@ def send_action_goal( Args: name: The name of the action - goal_msg: Goal message to send to the action server. + 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_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. @@ -457,7 +457,7 @@ def send_action_goal_and_wait_for_result( Args: name: The name of the action - goal_msg: Goal message to send to the action server. + 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. @@ -467,6 +467,7 @@ def send_action_goal_and_wait_for_result( 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.