From 06daaf84e23604d8d8d3dac1733e2f3137126c0e Mon Sep 17 00:00:00 2001 From: maciejmajek Date: Fri, 19 Dec 2025 15:05:31 +0100 Subject: [PATCH 01/10] feat: allow passing ros2 types in send_message --- .../rai/communication/ros2/api/base.py | 19 +++++- .../rai/communication/ros2/api/topic.py | 14 ++++- .../rai/communication/ros2/connectors/base.py | 31 ++++++--- tests/communication/ros2/test_api.py | 63 ++++++++++++++++--- tests/communication/ros2/test_connectors.py | 58 ++++++++++++++--- 5 files changed, 154 insertions(+), 31 deletions(-) diff --git a/src/rai_core/rai/communication/ros2/api/base.py b/src/rai_core/rai/communication/ros2/api/base.py index 7692f5cbd..b138c7d10 100644 --- a/src/rai_core/rai/communication/ros2/api/base.py +++ b/src/rai_core/rai/communication/ros2/api/base.py @@ -36,7 +36,12 @@ from rclpy.topic_endpoint_info import TopicEndpointInfo from rosidl_parser.definition import NamespacedType from rosidl_runtime_py.import_message import import_message_from_namespaced_type -from rosidl_runtime_py.utilities import get_namespaced_type +from rosidl_runtime_py.utilities import ( + get_namespaced_type, + is_action, + is_message, + is_service, +) from rai.communication.ros2.api.conversion import import_message_from_str @@ -134,3 +139,15 @@ def get_topic_type(self, topic: str) -> str: raise ValueError(f"Topic {topic} has multiple types: {types}") return types[0] raise ValueError(f"Topic {topic} not found") + + @staticmethod + def is_ros2_message(msg: Any) -> bool: + return is_message(msg) + + @staticmethod + def is_ros2_service(msg: Any) -> bool: + return is_service(msg) + + @staticmethod + def is_ros2_action(msg: Any) -> bool: + return is_action(msg) diff --git a/src/rai_core/rai/communication/ros2/api/topic.py b/src/rai_core/rai/communication/ros2/api/topic.py index cb55002f6..88ab01a09 100644 --- a/src/rai_core/rai/communication/ros2/api/topic.py +++ b/src/rai_core/rai/communication/ros2/api/topic.py @@ -34,6 +34,7 @@ from rai.communication.ros2.api.base import ( BaseROS2API, + IROS2Message, ) from rai.communication.ros2.api.conversion import import_message_from_str @@ -140,8 +141,8 @@ def get_topic_names_and_types( def publish( self, topic: str, - msg_content: Dict[str, Any], - msg_type: str, + msg_content: IROS2Message | Dict[str, Any], + msg_type: str | None = None, *, auto_qos_matching: bool = True, qos_profile: Optional[QoSProfile] = None, @@ -162,7 +163,14 @@ def publish( topic, auto_qos_matching, qos_profile, for_publisher=True ) - msg = self.build_ros2_msg(msg_type, msg_content) + if self.is_ros2_message(msg_content): + msg = msg_content + elif isinstance(msg_content, dict) and msg_type is not None: + msg = self.build_ros2_msg(msg_type, msg_content) + elif isinstance(msg_content, dict) and msg_type is None: + raise ValueError("msg_type must be provided if msg_content is a dict") + else: + raise ValueError(f"Invalid message content type: {type(msg_content)}") publisher = self._get_or_create_publisher(topic, type(msg), qos_profile) publisher.publish(msg) diff --git a/src/rai_core/rai/communication/ros2/connectors/base.py b/src/rai_core/rai/communication/ros2/connectors/base.py index 4f0accec1..fa80cd393 100644 --- a/src/rai_core/rai/communication/ros2/connectors/base.py +++ b/src/rai_core/rai/communication/ros2/connectors/base.py @@ -16,7 +16,17 @@ import time import uuid from functools import partial -from typing import Any, Callable, Dict, Final, List, Literal, Optional, Tuple, TypeVar +from typing import ( + Any, + Callable, + Dict, + Final, + List, + Literal, + Optional, + Tuple, + TypeVar, +) import rclpy import rclpy.executors @@ -31,6 +41,7 @@ from rai.communication import BaseConnector from rai.communication.ros2.api import ( + IROS2Message, ROS2ActionAPI, ROS2ServiceAPI, ROS2TopicAPI, @@ -241,10 +252,10 @@ def get_actions_names_and_types(self) -> List[Tuple[str, List[str]]]: def send_message( self, - message: T, + message: T | IROS2Message, target: str, *, - msg_type: str, + msg_type: str | None = None, auto_qos_matching: bool = True, qos_profile: Optional[QoSProfile] = None, **kwargs: Any, @@ -253,12 +264,12 @@ def send_message( Parameters ---------- - message : T - The message to send. + message : T | IROS2Message + The message to send. Can be a subclass of ROS2Message (payload is a dict) or any ROS2 message. target : str The target topic name. - msg_type : str - The ROS2 message type. + msg_type : str | None, optional + The ROS2 message type. If None, the message type will be inferred from the message content. Must be provided if msg_content is a ROS2Message subclass. auto_qos_matching : bool, optional Whether to automatically match QoS profiles, by default True. qos_profile : Optional[QoSProfile], optional @@ -266,9 +277,13 @@ def send_message( **kwargs : Any Additional keyword arguments. """ + if isinstance(message, ROS2Message): # T class + msg_content = message.payload + else: # An actual ROS 2 message + msg_content = message self._topic_api.publish( topic=target, - msg_content=message.payload, + msg_content=msg_content, msg_type=msg_type, auto_qos_matching=auto_qos_matching, qos_profile=qos_profile, diff --git a/tests/communication/ros2/test_api.py b/tests/communication/ros2/test_api.py index 64974fb76..9765371ea 100644 --- a/tests/communication/ros2/test_api.py +++ b/tests/communication/ros2/test_api.py @@ -22,6 +22,13 @@ import pytest from action_msgs.msg import GoalStatus from action_msgs.srv import CancelGoal +from geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + PoseWithCovarianceStamped, + Quaternion, +) from nav2_msgs.action import NavigateToPose from rai.communication.ros2.api import ( ROS2ActionAPI, @@ -35,6 +42,7 @@ ) from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node +from std_msgs.msg import Header, String from std_srvs.srv import SetBool from .helpers import ( @@ -51,12 +59,39 @@ _ = ros_setup # Explicitly use the fixture to prevent pytest warnings +@pytest.mark.parametrize( + "message_content,msg_type,actual_type", + [ + ({"data": "Hello, ROS2!"}, "std_msgs/msg/String", String), + (String(data="Hello, ROS2!"), None, String), + (String(), None, String), + (Pose(), None, Pose), + (PoseWithCovarianceStamped(), None, PoseWithCovarianceStamped), + ( + PoseWithCovarianceStamped( + header=Header(), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=1.0, y=2.0, z=3.0), + orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.4), + ) + ), + ), + None, + PoseWithCovarianceStamped, + ), + ], +) def test_ros2_single_message_publish( - ros_setup: None, request: pytest.FixtureRequest + ros_setup: None, + request: pytest.FixtureRequest, + message_content: Any, + msg_type: str | None, + actual_type: type, ) -> None: topic_name = f"{request.node.originalname}_topic" # type: ignore node_name = f"{request.node.originalname}_node" # type: ignore - message_receiver = MessageSubscriber(topic_name) + message_receiver = MessageSubscriber(topic_name, actual_type) node = Node(node_name) executors, threads = multi_threaded_spinner([message_receiver, node]) @@ -64,12 +99,12 @@ def test_ros2_single_message_publish( topic_api = ROS2TopicAPI(node) topic_api.publish( topic_name, - {"data": "Hello, ROS2!"}, - msg_type="std_msgs/msg/String", + message_content, + msg_type=msg_type, ) - time.sleep(1) + time.sleep(0.1) assert len(message_receiver.received_messages) == 1 - assert message_receiver.received_messages[0].data == "Hello, ROS2!" + assert isinstance(message_receiver.received_messages[0], actual_type) finally: shutdown_executors_and_threads(executors, threads) @@ -116,8 +151,18 @@ def test_ros2_single_message_publish_wrong_msg_content( shutdown_executors_and_threads(executors, threads) +@pytest.mark.parametrize( + "message_content,msg_type", + [ + ({"data": "Hello, ROS2!"}, "std_msgs/msg/String"), + (String(data="Hello, ROS2!"), None), + ], +) def test_ros2_single_message_publish_wrong_qos_setup( - ros_setup: None, request: pytest.FixtureRequest + ros_setup: None, + request: pytest.FixtureRequest, + message_content: Any, + msg_type: str | None, ) -> None: topic_name = f"{request.node.originalname}_topic" # type: ignore node_name = f"{request.node.originalname}_node" # type: ignore @@ -130,8 +175,8 @@ def test_ros2_single_message_publish_wrong_qos_setup( with pytest.raises(ValueError): topic_api.publish( topic_name, - {"data": "Hello, ROS2!"}, - msg_type="std_msgs/msg/String", + message_content, + msg_type=msg_type, auto_qos_matching=False, qos_profile=None, ) diff --git a/tests/communication/ros2/test_connectors.py b/tests/communication/ros2/test_connectors.py index 9fbf0d4e1..526d4c46c 100644 --- a/tests/communication/ros2/test_connectors.py +++ b/tests/communication/ros2/test_connectors.py @@ -19,6 +19,14 @@ from unittest.mock import MagicMock import pytest +from builtin_interfaces.msg import Time +from geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + PoseWithCovarianceStamped, + Quaternion, +) from nav2_msgs.action import NavigateToPose from PIL import Image from pydub import AudioSegment @@ -33,7 +41,7 @@ MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup, ) -from std_msgs.msg import String +from std_msgs.msg import Header, String from std_srvs.srv import SetBool from .helpers import ( @@ -52,21 +60,51 @@ _ = ros_setup # Explicitly use the fixture to prevent pytest warnings -def test_ros2_connector_send_message(ros_setup: None, request: pytest.FixtureRequest): +@pytest.mark.parametrize( + "message_content,msg_type,actual_type", + [ + (ROS2Message(payload={"data": "Hello, ROS2!"}), "std_msgs/msg/String", String), + (String(data="Hello, ROS2!"), None, String), + (String(), None, String), + (Pose(), None, Pose), + (PoseWithCovarianceStamped(), None, PoseWithCovarianceStamped), + ( + PoseWithCovarianceStamped( + header=Header( + stamp=Time(sec=1, nanosec=100000000), + frame_id="test_frame", + ), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=1.0, y=2.0, z=3.0), + orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.4), + ), + covariance=[0.0] * 36, + ), + ), + None, + PoseWithCovarianceStamped, + ), + ], +) +def test_ros2_connector_send_message( + ros_setup: None, + request: pytest.FixtureRequest, + message_content: ROS2Message, + msg_type: str | None, + actual_type: type, +): topic_name = f"{request.node.originalname}_topic" # type: ignore - message_receiver = MessageSubscriber(topic_name) + message_receiver = MessageSubscriber(topic_name, actual_type) executors, threads = multi_threaded_spinner([message_receiver]) connector = ROS2Connector() try: - message = ROS2Message( - payload={"data": "Hello, world!"}, - metadata={"msg_type": "std_msgs/msg/String"}, - ) connector.send_message( - message=message, target=topic_name, msg_type="std_msgs/msg/String" + message=message_content, target=topic_name, msg_type=msg_type ) - time.sleep(1) # wait for the message to be received - assert message_receiver.received_messages == [String(data="Hello, world!")] + time.sleep(0.1) # wait for the message to be received + assert len(message_receiver.received_messages) == 1 + assert isinstance(message_receiver.received_messages[0], actual_type) finally: connector.shutdown() shutdown_executors_and_threads(executors, threads) From 409d9ba944d5b816dd4dc7130e97c2099e19dc85 Mon Sep 17 00:00:00 2001 From: maciejmajek Date: Fri, 19 Dec 2025 15:28:37 +0100 Subject: [PATCH 02/10] test: extend test suite --- tests/communication/ros2/test_api.py | 114 +++++++++++++++++++++++++++ 1 file changed, 114 insertions(+) diff --git a/tests/communication/ros2/test_api.py b/tests/communication/ros2/test_api.py index 9765371ea..8a9747e08 100644 --- a/tests/communication/ros2/test_api.py +++ b/tests/communication/ros2/test_api.py @@ -35,6 +35,7 @@ ROS2ServiceAPI, ROS2TopicAPI, ) +from rai.communication.ros2.api.base import BaseROS2API from rclpy.callback_groups import ( CallbackGroup, MutuallyExclusiveCallbackGroup, @@ -59,6 +60,63 @@ _ = ros_setup # Explicitly use the fixture to prevent pytest warnings +@pytest.mark.parametrize( + "entity,is_message,is_service,is_action", + [ + ({"data": "Hello, ROS2!"}, False, False, False), + ({}, False, False, False), + ("", False, False, False), + ("data: Hello, ROS2!", False, False, False), + (None, False, False, False), + (String(), True, False, False), + (Pose(), True, False, False), + (PoseWithCovarianceStamped(), True, False, False), + ( + PoseWithCovarianceStamped( + header=Header(), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=1.0, y=2.0, z=3.0), + orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.4), + ) + ), + ), + True, + False, + False, + ), + (SetBool.Request(data=True), True, False, False), + ( + SetBool.Response(success=True, message="Test service called"), + True, + False, + False, + ), + (SetBool, False, True, False), + ( + NavigateToPose.Goal( + pose=Pose( + position=Point(x=1.0, y=2.0, z=3.0), + orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.4), + ) + ), + True, + False, + False, + ), + (NavigateToPose.Result(success=True), True, False, False), + (NavigateToPose.Feedback(feedback="Test feedback"), True, False, False), + (NavigateToPose, False, False, True), + ], +) +def test_is_message_type( + ros_setup: None, entity: Any, is_message: bool, is_service: bool, is_action: bool +) -> None: + assert is_message == BaseROS2API.is_ros2_message(entity) + assert is_service == BaseROS2API.is_ros2_service(entity) + assert is_action == BaseROS2API.is_ros2_action(entity) + + @pytest.mark.parametrize( "message_content,msg_type,actual_type", [ @@ -184,6 +242,62 @@ def test_ros2_single_message_publish_wrong_qos_setup( shutdown_executors_and_threads(executors, threads) +def test_ros2_single_message_dict_no_type( + ros_setup: None, request: pytest.FixtureRequest +) -> None: + topic_name = f"{request.node.originalname}_topic" # type: ignore + node_name = f"{request.node.originalname}_node" # type: ignore + message_receiver = MessageSubscriber(topic_name) + node = Node(node_name) + executors, threads = multi_threaded_spinner([message_receiver, node]) + + try: + topic_api = ROS2TopicAPI(node) + with pytest.raises(ValueError): + topic_api.publish( + topic_name, + {"data": "Hello, ROS2!"}, + msg_type=None, + ) + finally: + shutdown_executors_and_threads(executors, threads) + + +@pytest.mark.parametrize( + "message_content,msg_type", + [ + ((), "std_msgs/msg/String"), + ((), None), + (None, "std_msgs/msg/String"), + (None, None), + ("data: Hello, ROS2!", "std_msgs/msg/String"), + ("data: Hello, ROS2!", None), + ], +) +def test_ros2_single_message_invalid_type( + ros_setup: None, + request: pytest.FixtureRequest, + message_content: Any, + msg_type: str | None, +) -> None: + topic_name = f"{request.node.originalname}_topic" # type: ignore + node_name = f"{request.node.originalname}_node" # type: ignore + message_receiver = MessageSubscriber(topic_name) + node = Node(node_name) + executors, threads = multi_threaded_spinner([message_receiver, node]) + + try: + topic_api = ROS2TopicAPI(node) + with pytest.raises(ValueError): + topic_api.publish( + topic_name, + message_content, + msg_type=msg_type, + ) + finally: + shutdown_executors_and_threads(executors, threads) + + def invoke_set_bool_service( service_name: str, service_api: ROS2ServiceAPI, reuse_client: bool = True ): From 72725a74c90561b59f16abe79bb99853673577eb Mon Sep 17 00:00:00 2001 From: maciejmajek Date: Fri, 19 Dec 2025 15:32:28 +0100 Subject: [PATCH 03/10] fix: tests on humble --- tests/communication/ros2/test_api.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tests/communication/ros2/test_api.py b/tests/communication/ros2/test_api.py index 8a9747e08..1d3d1b74b 100644 --- a/tests/communication/ros2/test_api.py +++ b/tests/communication/ros2/test_api.py @@ -25,6 +25,7 @@ from geometry_msgs.msg import ( Point, Pose, + PoseStamped, PoseWithCovariance, PoseWithCovarianceStamped, Quaternion, @@ -95,7 +96,7 @@ (SetBool, False, True, False), ( NavigateToPose.Goal( - pose=Pose( + pose=PoseStamped( position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.4), ) From 9cba727cabdb5293f347e87e6807f1218974d4b1 Mon Sep 17 00:00:00 2001 From: maciejmajek Date: Fri, 19 Dec 2025 15:43:40 +0100 Subject: [PATCH 04/10] fix: tests on humble --- tests/communication/ros2/test_api.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/tests/communication/ros2/test_api.py b/tests/communication/ros2/test_api.py index 1d3d1b74b..35322c909 100644 --- a/tests/communication/ros2/test_api.py +++ b/tests/communication/ros2/test_api.py @@ -97,8 +97,11 @@ ( NavigateToPose.Goal( pose=PoseStamped( - position=Point(x=1.0, y=2.0, z=3.0), - orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.4), + header=Header(), + pose=Pose( + position=Point(x=1.0, y=2.0, z=3.0), + orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.4), + ), ) ), True, From ff20a5defe062e8bc2226f6e1cfb25a307c05761 Mon Sep 17 00:00:00 2001 From: maciejmajek Date: Fri, 19 Dec 2025 15:46:38 +0100 Subject: [PATCH 05/10] fix: tests on humble --- tests/communication/ros2/test_api.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/communication/ros2/test_api.py b/tests/communication/ros2/test_api.py index 35322c909..b2b7d7d21 100644 --- a/tests/communication/ros2/test_api.py +++ b/tests/communication/ros2/test_api.py @@ -108,8 +108,8 @@ False, False, ), - (NavigateToPose.Result(success=True), True, False, False), - (NavigateToPose.Feedback(feedback="Test feedback"), True, False, False), + (NavigateToPose.Result(), True, False, False), + (NavigateToPose.Feedback(), True, False, False), (NavigateToPose, False, False, True), ], ) From 73452bcf03540ab408ca3e72f096e81ea8c237f3 Mon Sep 17 00:00:00 2001 From: maciejmajek Date: Fri, 19 Dec 2025 15:47:05 +0100 Subject: [PATCH 06/10] docs: update --- docs/API_documentation/connectors/ROS_2_Connectors.md | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/docs/API_documentation/connectors/ROS_2_Connectors.md b/docs/API_documentation/connectors/ROS_2_Connectors.md index afd7ac796..cfb8d65bd 100644 --- a/docs/API_documentation/connectors/ROS_2_Connectors.md +++ b/docs/API_documentation/connectors/ROS_2_Connectors.md @@ -34,12 +34,19 @@ The `ROS2Connector` is the main interface for publishing, subscribing, and calli ```python from rai.communication.ros2.connectors import ROS2Connector +from std_msgs.msg import String connector = ROS2Connector() -# Send a message to a topic +# Send a raw ROS 2 message (msg_type is inferred) connector.send_message( - message=my_msg, # ROS2Message + message=String(data="Hello"), + target="/my_topic" +) + +# Send a message using a dictionary (msg_type is required) +connector.send_message( + message={"data": "Hello"}, target="/my_topic", msg_type="std_msgs/msg/String" ) From e58c8b4b8341b766e84bb1936d804f7c0f590687 Mon Sep 17 00:00:00 2001 From: maciejmajek Date: Fri, 19 Dec 2025 15:48:30 +0100 Subject: [PATCH 07/10] docs: update --- docs/API_documentation/connectors/ROS_2_Connectors.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/API_documentation/connectors/ROS_2_Connectors.md b/docs/API_documentation/connectors/ROS_2_Connectors.md index cfb8d65bd..77eaf656d 100644 --- a/docs/API_documentation/connectors/ROS_2_Connectors.md +++ b/docs/API_documentation/connectors/ROS_2_Connectors.md @@ -33,7 +33,7 @@ The `ROS2Connector` is the main interface for publishing, subscribing, and calli ### Example Usage ```python -from rai.communication.ros2.connectors import ROS2Connector +from rai.communication.ros2.connectors import ROS2Connector, ROS2Message from std_msgs.msg import String connector = ROS2Connector() @@ -46,7 +46,7 @@ connector.send_message( # Send a message using a dictionary (msg_type is required) connector.send_message( - message={"data": "Hello"}, + message=ROS2Message(payload={"data": "Hello"}), target="/my_topic", msg_type="std_msgs/msg/String" ) From 55ac7eaf66a285fb264346bc9f762e2786551aa3 Mon Sep 17 00:00:00 2001 From: Maciej Majek Date: Thu, 10 Sep 2026 12:18:43 +0200 Subject: [PATCH 08/10] feat: allow passing ros2 types in service_call and start_action --- .../connectors/ROS_2_Connectors.md | 30 ++- .../rai/communication/ros2/api/action.py | 16 +- .../rai/communication/ros2/api/base.py | 63 ++++++ .../rai/communication/ros2/api/service.py | 21 +- .../rai/communication/ros2/api/topic.py | 23 +- .../ros2/connectors/action_mixin.py | 15 +- .../rai/communication/ros2/connectors/base.py | 15 +- .../ros2/connectors/service_mixin.py | 11 +- tests/communication/ros2/test_api.py | 199 +++++++++++++++++- tests/communication/ros2/test_connectors.py | 71 +++++-- 10 files changed, 381 insertions(+), 83 deletions(-) diff --git a/docs/API_documentation/connectors/ROS_2_Connectors.md b/docs/API_documentation/connectors/ROS_2_Connectors.md index 77eaf656d..8f46315ce 100644 --- a/docs/API_documentation/connectors/ROS_2_Connectors.md +++ b/docs/API_documentation/connectors/ROS_2_Connectors.md @@ -35,6 +35,8 @@ The `ROS2Connector` is the main interface for publishing, subscribing, and calli ```python from rai.communication.ros2.connectors import ROS2Connector, ROS2Message from std_msgs.msg import String +from std_srvs.srv import SetBool +from nav2_msgs.action import NavigateToPose connector = ROS2Connector() @@ -44,7 +46,7 @@ connector.send_message( target="/my_topic" ) -# Send a message using a dictionary (msg_type is required) +# Send a message using a dictionary (msg_type is required, as a string or class) connector.send_message( message=ROS2Message(payload={"data": "Hello"}), target="/my_topic", @@ -58,18 +60,32 @@ connector.register_callback( msg_type="std_msgs/msg/String" ) -# Call a service +# Call a service with a request instance (msg_type is inferred) response = connector.service_call( - message=my_request_msg, + message=SetBool.Request(data=True), + target="/my_service" +) + +# Call a service using a dictionary (msg_type is required) +response = connector.service_call( + message=ROS2Message(payload={"data": True}), target="/my_service", - msg_type="my_package/srv/MyService" + msg_type=SetBool +) + +# Start an action with a goal instance (msg_type is inferred) +handle = connector.start_action( + action_data=NavigateToPose.Goal(), + target="/my_action", + on_feedback=feedback_cb, + on_done=done_cb ) -# Start an action +# Start an action using a dictionary (msg_type is required) handle = connector.start_action( - action_data=my_goal_msg, + action_data=ROS2Message(payload={}), target="/my_action", - msg_type="my_package/action/MyAction", + msg_type="nav2_msgs/action/NavigateToPose", on_feedback=feedback_cb, on_done=done_cb ) diff --git a/src/rai_core/rai/communication/ros2/api/action.py b/src/rai_core/rai/communication/ros2/api/action.py index 32df25c34..63dcd1255 100644 --- a/src/rai_core/rai/communication/ros2/api/action.py +++ b/src/rai_core/rai/communication/ros2/api/action.py @@ -33,7 +33,6 @@ import rclpy.action import rclpy.node import rclpy.task -import rosidl_runtime_py.set_message from action_msgs.srv import CancelGoal from rclpy.action import ActionClient, CancelResponse, GoalResponse from rclpy.action.client import ClientGoalHandle @@ -55,7 +54,6 @@ BaseROS2API, IROS2Message, ) -from rai.communication.ros2.api.conversion import import_message_from_str from rai.communication.ros2.ros_async import get_future_result @@ -111,7 +109,7 @@ def _safe_callback_wrapper( def create_action_server( self, - action_type: str, + action_type: str | Type[Any], action_name: str, execute_callback: Callable[[ServerGoalHandle], Type[IROS2Message]], *, @@ -164,7 +162,7 @@ def create_action_server( if result_timeout <= 0: raise ValueError(f"result_timeout must be positive, got {result_timeout!r}") handle = self._generate_handle() - action_ros_type = import_message_from_str(action_type) + action_ros_type = self.resolve_interface_type(action_type) try: action_server = ActionServer( node=self.node, @@ -202,8 +200,8 @@ def create_action_server( def send_goal( self, action_name: str, - action_type: str, - goal: Dict[str, Any], + action_type: str | Type[Any] | None = None, + goal: IROS2Message | Dict[str, Any] | None = None, *, feedback_callback: Callable[[Any], None] = lambda _: None, done_callback: Callable[ @@ -220,11 +218,7 @@ def send_goal( feedbacks=[], ) - action_cls = import_message_from_str(action_type) - action_goal = action_cls.Goal() # type: ignore - rosidl_runtime_py.set_message.set_message_fields( - action_goal, copy.deepcopy(goal) - ) + action_goal, action_cls = self.resolve_content(goal, action_type, "Goal") action_client = ActionClient(self.node, action_cls, action_name) if not action_client.wait_for_server(timeout_sec=timeout_sec): # type: ignore diff --git a/src/rai_core/rai/communication/ros2/api/base.py b/src/rai_core/rai/communication/ros2/api/base.py index b138c7d10..88ff233e7 100644 --- a/src/rai_core/rai/communication/ros2/api/base.py +++ b/src/rai_core/rai/communication/ros2/api/base.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +import copy +import importlib import logging from typing import ( Any, @@ -131,6 +133,67 @@ def import_message_from_str(msg_type: str) -> Type[object]: msg_namespaced_type: NamespacedType = get_namespaced_type(msg_type) return import_message_from_namespaced_type(msg_namespaced_type) + @staticmethod + def resolve_interface_type(interface_type: str | Type[Any]) -> Type[Any]: + """Return the interface class for a type string like 'std_srvs/srv/SetBool' or the class itself.""" + if isinstance(interface_type, str): + return import_message_from_str(interface_type) + return interface_type + + @staticmethod + def get_interface_type(instance: IROS2Message) -> Type[Any]: + """Return the interface class an instance belongs to, e.g. SetBool for SetBool.Request().""" + cls = type(instance) + package = importlib.import_module(cls.__module__.rsplit(".", 1)[0]) + return getattr(package, cls.__name__.partition("_")[0]) + + @classmethod + def resolve_content( + cls, + content: IROS2Message | Dict[str, Any], + interface_type: str | Type[Any] | None, + member: str | None = None, + ) -> Tuple[IROS2Message, Type[Any]]: + """Resolve a dictionary or ROS 2 instance into (instance, interface class). + + Args: + content: ROS 2 instance or dictionary of field values. + interface_type: Interface type string or class. Required for dictionaries, + validated against the instance otherwise. + member: Nested interface class the content must be an instance of, + e.g. 'Request' for services or 'Goal' for actions. + + Raises: + ValueError: If content is neither a dictionary nor a ROS 2 instance, + if a dictionary is given without interface_type, or if the instance + does not match interface_type or member. + """ + if isinstance(content, dict): + if interface_type is None: + raise ValueError("Interface type must be provided if content is a dict") + interface_cls = cls.resolve_interface_type(interface_type) + instance_cls = getattr(interface_cls, member) if member else interface_cls + instance = instance_cls() + # set_message_fields mutates nested lists, see ros2/rosidl_runtime_py#33 + rosidl_runtime_py.set_message.set_message_fields( + instance, copy.deepcopy(content) + ) + return instance, interface_cls + if isinstance(content, type) or not is_message(content): + raise ValueError(f"Invalid content type: {type(content)}") + interface_cls = cls.get_interface_type(content) if member else type(content) + instance_cls = getattr(interface_cls, member) if member else interface_cls + if type(content) is not instance_cls: + raise ValueError(f"Expected {instance_cls}, got {type(content)}") + if ( + interface_type is not None + and cls.resolve_interface_type(interface_type) is not interface_cls + ): + raise ValueError( + f"Interface type {interface_type} does not match {type(content)}" + ) + return content, interface_cls + def get_topic_type(self, topic: str) -> str: names_and_types = self.node.get_topic_names_and_types(no_demangle=False) for name, types in names_and_types: diff --git a/src/rai_core/rai/communication/ros2/api/service.py b/src/rai_core/rai/communication/ros2/api/service.py index 06dac1bc1..64158a9bb 100644 --- a/src/rai_core/rai/communication/ros2/api/service.py +++ b/src/rai_core/rai/communication/ros2/api/service.py @@ -21,6 +21,7 @@ Dict, List, Tuple, + Type, ) import rclpy @@ -36,8 +37,8 @@ from rai.communication.ros2.api.base import ( BaseROS2API, + IROS2Message, ) -from rai.communication.ros2.api.conversion import import_message_from_str class ROS2ServiceAPI(BaseROS2API): @@ -57,8 +58,8 @@ def release_client(self, service_name: str) -> bool: def call_service( self, service_name: str, - service_type: str, - request: Any, + service_type: str | Type[Any] | None = None, + request: IROS2Message | Dict[str, Any] | None = None, timeout_sec: float = 5.0, *, reuse_client: bool = True, @@ -68,8 +69,9 @@ def call_service( Args: service_name: Fully-qualified service name. - service_type: ROS 2 service type string (e.g., 'std_srvs/srv/SetBool'). - request: Request payload dict. + service_type: ROS 2 service type string (e.g., 'std_srvs/srv/SetBool') or class. + Required when request is a dict, inferred from the instance otherwise. + request: Request payload dict or request instance (e.g., SetBool.Request()). timeout_sec: Seconds to wait for availability/response. reuse_client: Reuse a cached client. Client creation is synchronized; set False to create a new client per call. @@ -78,7 +80,8 @@ def call_service( Response message instance. Raises: - ValueError: Service not available within the timeout. + ValueError: Service not available within the timeout, request is a dict + without service_type, or request does not match service_type. AttributeError: Service type or request cannot be constructed. Note: @@ -87,7 +90,7 @@ def call_service( through the same client. Use reuse_client=False for per-call clients when concurrent service calls are required. """ - srv_msg, srv_cls = self.build_ros2_service_request(service_type, request) + srv_msg, srv_cls = self.resolve_content(request, service_type, "Request") def _call_service(client: Client, timeout_sec: float) -> Any: is_service_available = client.wait_for_service(timeout_sec=timeout_sec) @@ -118,11 +121,11 @@ def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: def create_service( self, service_name: str, - service_type: str, + service_type: str | Type[Any], callback: Callable[[Any, Any], Any], **kwargs, ) -> str: - srv_cls = import_message_from_str(service_type) + srv_cls = self.resolve_interface_type(service_type) service = self.node.create_service(srv_cls, service_name, callback, **kwargs) handle = str(uuid.uuid4()) self._services[handle] = service diff --git a/src/rai_core/rai/communication/ros2/api/topic.py b/src/rai_core/rai/communication/ros2/api/topic.py index 88ab01a09..138184e11 100644 --- a/src/rai_core/rai/communication/ros2/api/topic.py +++ b/src/rai_core/rai/communication/ros2/api/topic.py @@ -142,7 +142,7 @@ def publish( self, topic: str, msg_content: IROS2Message | Dict[str, Any], - msg_type: str | None = None, + msg_type: str | Type[Any] | None = None, *, auto_qos_matching: bool = True, qos_profile: Optional[QoSProfile] = None, @@ -151,27 +151,22 @@ def publish( Args: topic: Name of the topic to publish to - msg_content: Dictionary containing the message content - msg_type: ROS2 message type as string (e.g. 'std_msgs/msg/String') + msg_content: ROS2 message instance or dictionary containing the message content + msg_type: ROS2 message type as string (e.g. 'std_msgs/msg/String') or class, + required when msg_content is a dictionary auto_qos_matching: Whether to automatically match QoS with subscribers qos_profile: Optional custom QoS profile to use Raises: - ValueError: If neither auto_qos_matching is True nor qos_profile is provided + ValueError: If neither auto_qos_matching is True nor qos_profile is provided, + if msg_content is a dictionary without msg_type, or if msg_type does not + match the ROS2 message instance """ qos_profile = self._resolve_qos_profile( topic, auto_qos_matching, qos_profile, for_publisher=True ) - - if self.is_ros2_message(msg_content): - msg = msg_content - elif isinstance(msg_content, dict) and msg_type is not None: - msg = self.build_ros2_msg(msg_type, msg_content) - elif isinstance(msg_content, dict) and msg_type is None: - raise ValueError("msg_type must be provided if msg_content is a dict") - else: - raise ValueError(f"Invalid message content type: {type(msg_content)}") - publisher = self._get_or_create_publisher(topic, type(msg), qos_profile) + msg, msg_cls = self.resolve_content(msg_content, msg_type) + publisher = self._get_or_create_publisher(topic, msg_cls, qos_profile) publisher.publish(msg) def _verify_receive_args( diff --git a/src/rai_core/rai/communication/ros2/connectors/action_mixin.py b/src/rai_core/rai/communication/ros2/connectors/action_mixin.py index d9489d2de..b5b183bb7 100644 --- a/src/rai_core/rai/communication/ros2/connectors/action_mixin.py +++ b/src/rai_core/rai/communication/ros2/connectors/action_mixin.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Any, Callable, Optional +from typing import Any, Callable, Optional, Type -from rai.communication.ros2.api import ROS2ActionAPI +from rai.communication.ros2.api import IROS2Message, ROS2ActionAPI from rai.communication.ros2.messages import ROS2HRIMessage, ROS2Message @@ -32,21 +32,22 @@ def __post_init__(self, *args: Any, **kwargs: Any) -> None: def start_action( self, - action_data: Optional[ROS2Message | ROS2HRIMessage], + action_data: Optional[ROS2Message | ROS2HRIMessage | IROS2Message], target: str, on_feedback: Callable[[Any], None] = lambda _: None, on_done: Callable[[Any], None] = lambda _: None, timeout_sec: float = 1.0, *, - msg_type: str, + msg_type: str | Type[Any] | None = None, **kwargs: Any, ) -> str: - if not isinstance(action_data, ROS2Message): - raise ValueError("Action data must be of type ROS2Message") + goal = ( + action_data.payload if isinstance(action_data, ROS2Message) else action_data + ) accepted, handle = self._actions_api.send_goal( action_name=target, action_type=msg_type, - goal=action_data.payload, + goal=goal, timeout_sec=timeout_sec, feedback_callback=on_feedback, done_callback=on_done, diff --git a/src/rai_core/rai/communication/ros2/connectors/base.py b/src/rai_core/rai/communication/ros2/connectors/base.py index fa80cd393..fac985211 100644 --- a/src/rai_core/rai/communication/ros2/connectors/base.py +++ b/src/rai_core/rai/communication/ros2/connectors/base.py @@ -25,6 +25,7 @@ Literal, Optional, Tuple, + Type, TypeVar, ) @@ -255,7 +256,7 @@ def send_message( message: T | IROS2Message, target: str, *, - msg_type: str | None = None, + msg_type: str | Type[Any] | None = None, auto_qos_matching: bool = True, qos_profile: Optional[QoSProfile] = None, **kwargs: Any, @@ -265,11 +266,12 @@ def send_message( Parameters ---------- message : T | IROS2Message - The message to send. Can be a subclass of ROS2Message (payload is a dict) or any ROS2 message. + The message to send. Either a ROS2Message with a dictionary payload or a ROS2 message instance. target : str The target topic name. - msg_type : str | None, optional - The ROS2 message type. If None, the message type will be inferred from the message content. Must be provided if msg_content is a ROS2Message subclass. + msg_type : str | Type[Any] | None, optional + The ROS2 message type as string, e.g. 'std_msgs/msg/String', or class. Required when + message is a ROS2Message, inferred from the instance otherwise. auto_qos_matching : bool, optional Whether to automatically match QoS profiles, by default True. qos_profile : Optional[QoSProfile], optional @@ -277,10 +279,7 @@ def send_message( **kwargs : Any Additional keyword arguments. """ - if isinstance(message, ROS2Message): # T class - msg_content = message.payload - else: # An actual ROS 2 message - msg_content = message + msg_content = message.payload if isinstance(message, ROS2Message) else message self._topic_api.publish( topic=target, msg_content=msg_content, diff --git a/src/rai_core/rai/communication/ros2/connectors/service_mixin.py b/src/rai_core/rai/communication/ros2/connectors/service_mixin.py index 7c1597a56..5b4bd5e5f 100644 --- a/src/rai_core/rai/communication/ros2/connectors/service_mixin.py +++ b/src/rai_core/rai/communication/ros2/connectors/service_mixin.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Any +from typing import Any, Type -from rai.communication.ros2.api import ROS2ServiceAPI +from rai.communication.ros2.api import IROS2Message, ROS2ServiceAPI from rai.communication.ros2.messages import ROS2Message @@ -35,18 +35,19 @@ def release_client(self, service_name: str) -> bool: def service_call( self, - message: ROS2Message, + message: ROS2Message | IROS2Message, target: str, timeout_sec: float = 5.0, *, - msg_type: str, + msg_type: str | Type[Any] | None = None, reuse_client: bool = True, **kwargs: Any, ) -> ROS2Message: + request = message.payload if isinstance(message, ROS2Message) else message msg = self._service_api.call_service( service_name=target, service_type=msg_type, - request=message.payload, + request=request, timeout_sec=timeout_sec, reuse_client=reuse_client, ) diff --git a/tests/communication/ros2/test_api.py b/tests/communication/ros2/test_api.py index b2b7d7d21..2afead72e 100644 --- a/tests/communication/ros2/test_api.py +++ b/tests/communication/ros2/test_api.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import copy import logging import threading import time @@ -25,12 +26,13 @@ from geometry_msgs.msg import ( Point, Pose, + PoseArray, PoseStamped, PoseWithCovariance, PoseWithCovarianceStamped, Quaternion, ) -from nav2_msgs.action import NavigateToPose +from nav2_msgs.action import NavigateThroughPoses, NavigateToPose from rai.communication.ros2.api import ( ROS2ActionAPI, ROS2ServiceAPI, @@ -45,7 +47,7 @@ from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from std_msgs.msg import Header, String -from std_srvs.srv import SetBool +from std_srvs.srv import SetBool, Trigger from .helpers import ( MessageSubscriber, @@ -121,6 +123,64 @@ def test_is_message_type( assert is_action == BaseROS2API.is_ros2_action(entity) +@pytest.mark.parametrize( + "instance,interface_type", + [ + (String(), String), + (PoseWithCovarianceStamped(), PoseWithCovarianceStamped), + (SetBool.Request(), SetBool), + (SetBool.Response(), SetBool), + (NavigateToPose.Goal(), NavigateToPose), + (NavigateToPose.Result(), NavigateToPose), + (NavigateToPose.Feedback(), NavigateToPose), + ], +) +def test_get_interface_type( + ros_setup: None, instance: Any, interface_type: type +) -> None: + assert BaseROS2API.get_interface_type(instance) is interface_type + + +@pytest.mark.parametrize( + "interface_type,expected", + [ + ("std_msgs/msg/String", String), + (String, String), + ("std_srvs/srv/SetBool", SetBool), + ("nav2_msgs/action/NavigateToPose", NavigateToPose), + ], +) +def test_resolve_interface_type( + ros_setup: None, interface_type: str | type, expected: type +) -> None: + assert BaseROS2API.resolve_interface_type(interface_type) is expected + + +@pytest.mark.parametrize( + "interface_type,member,content", + [ + (PoseArray, None, {"poses": [{"position": {"x": 1.0}}]}), + ( + NavigateThroughPoses, + "Goal", + { + "poses": [ + {"header": {"frame_id": "map"}, "pose": {"position": {"x": 1.0}}} + ] + }, + ), + ], +) +def test_resolve_content_does_not_mutate_dict( + ros_setup: None, interface_type: type, member: str | None, content: dict +) -> None: + original = copy.deepcopy(content) + instance, resolved = BaseROS2API.resolve_content(content, interface_type, member) + assert resolved is interface_type + assert len(instance.poses) == 1 + assert content == original + + @pytest.mark.parametrize( "message_content,msg_type,actual_type", [ @@ -276,6 +336,10 @@ def test_ros2_single_message_dict_no_type( (None, None), ("data: Hello, ROS2!", "std_msgs/msg/String"), ("data: Hello, ROS2!", None), + (String, "std_msgs/msg/String"), + (String, None), + (String(data="Hello, ROS2!"), "std_msgs/msg/Header"), + (String(data="Hello, ROS2!"), Header), ], ) def test_ros2_single_message_invalid_type( @@ -303,18 +367,90 @@ def test_ros2_single_message_invalid_type( def invoke_set_bool_service( - service_name: str, service_api: ROS2ServiceAPI, reuse_client: bool = True + service_name: str, + service_api: ROS2ServiceAPI, + reuse_client: bool = True, + service_type: str | type | None = "std_srvs/srv/SetBool", + request: Any = None, ): response = service_api.call_service( service_name, - service_type="std_srvs/srv/SetBool", - request={"data": True}, + service_type=service_type, + request={"data": True} if request is None else request, reuse_client=reuse_client, ) assert response.success assert response.message == "Test service called" +@pytest.mark.parametrize( + "service_type,request_content", + [ + ("std_srvs/srv/SetBool", {"data": True}), + (SetBool, {"data": True}), + (None, SetBool.Request(data=True)), + ("std_srvs/srv/SetBool", SetBool.Request(data=True)), + (SetBool, SetBool.Request(data=True)), + ], +) +def test_ros2_service_single_call_request_types( + ros_setup: None, + request: pytest.FixtureRequest, + service_type: str | type | None, + request_content: Any, +) -> None: + service_name = f"{request.node.originalname}_service" # type: ignore + node_name = f"{request.node.originalname}_node" # type: ignore + service_server = ServiceServer(service_name, ReentrantCallbackGroup()) + node = Node(node_name) + executors, threads = multi_threaded_spinner([service_server, node]) + + try: + service_api = ROS2ServiceAPI(node) + invoke_set_bool_service( + service_name, + service_api, + service_type=service_type, + request=request_content, + ) + finally: + shutdown_executors_and_threads(executors, threads) + + +@pytest.mark.parametrize( + "service_type,request_content", + [ + (None, {"data": True}), + (None, None), + (None, SetBool.Request), + (None, SetBool), + (None, SetBool.Response()), + ("std_srvs/srv/Trigger", SetBool.Request(data=True)), + (Trigger, SetBool.Request(data=True)), + ], +) +def test_ros2_service_single_call_invalid_request( + ros_setup: None, + request: pytest.FixtureRequest, + service_type: str | type | None, + request_content: Any, +) -> None: + service_name = f"{request.node.originalname}_service" # type: ignore + node_name = f"{request.node.originalname}_node" # type: ignore + service_server = ServiceServer(service_name, ReentrantCallbackGroup()) + node = Node(node_name) + executors, threads = multi_threaded_spinner([service_server, node]) + + try: + service_api = ROS2ServiceAPI(node) + with pytest.raises(ValueError): + service_api.call_service( + service_name, service_type=service_type, request=request_content + ) + finally: + shutdown_executors_and_threads(executors, threads) + + @pytest.mark.parametrize( "callback_group", [MutuallyExclusiveCallbackGroup(), ReentrantCallbackGroup()], @@ -516,7 +652,22 @@ def test_ros2_service_single_call_wrong_service_name( shutdown_executors_and_threads(executors, threads) -def test_ros2_action_send_goal(ros_setup: None, request: pytest.FixtureRequest) -> None: +@pytest.mark.parametrize( + "action_type,goal", + [ + ("nav2_msgs/action/NavigateToPose", {}), + (NavigateToPose, {}), + (None, NavigateToPose.Goal()), + ("nav2_msgs/action/NavigateToPose", NavigateToPose.Goal()), + (NavigateToPose, NavigateToPose.Goal()), + ], +) +def test_ros2_action_send_goal( + ros_setup: None, + request: pytest.FixtureRequest, + action_type: str | type | None, + goal: Any, +) -> None: action_name = f"{request.node.originalname}_action" # type: ignore node_name = f"{request.node.originalname}_node" # type: ignore action_server = TestActionServer(action_name) @@ -525,9 +676,7 @@ def test_ros2_action_send_goal(ros_setup: None, request: pytest.FixtureRequest) try: action_api = ROS2ActionAPI(node) - accepted, handle = action_api.send_goal( - action_name, "nav2_msgs/action/NavigateToPose", {} - ) + accepted, handle = action_api.send_goal(action_name, action_type, goal) assert accepted assert handle != "" @@ -535,6 +684,38 @@ def test_ros2_action_send_goal(ros_setup: None, request: pytest.FixtureRequest) shutdown_executors_and_threads(executors, threads) +@pytest.mark.parametrize( + "action_type,goal", + [ + (None, {}), + (None, None), + (None, NavigateToPose.Goal), + (None, NavigateToPose), + (None, NavigateToPose.Result()), + ("nav2_msgs/action/NavigateToPose", NavigateToPose.Feedback()), + ("nav2_msgs/action/NavigateThroughPoses", NavigateToPose.Goal()), + ], +) +def test_ros2_action_send_goal_invalid_goal( + ros_setup: None, + request: pytest.FixtureRequest, + action_type: str | type | None, + goal: Any, +) -> None: + action_name = f"{request.node.originalname}_action" # type: ignore + node_name = f"{request.node.originalname}_node" # type: ignore + action_server = TestActionServer(action_name) + node = Node(node_name) + executors, threads = multi_threaded_spinner([action_server, node]) + + try: + action_api = ROS2ActionAPI(node) + with pytest.raises(ValueError): + action_api.send_goal(action_name, action_type, goal) + finally: + shutdown_executors_and_threads(executors, threads) + + def test_ros2_action_send_goal_get_result( ros_setup: None, request: pytest.FixtureRequest ) -> None: diff --git a/tests/communication/ros2/test_connectors.py b/tests/communication/ros2/test_connectors.py index 526d4c46c..9faab9bcb 100644 --- a/tests/communication/ros2/test_connectors.py +++ b/tests/communication/ros2/test_connectors.py @@ -64,7 +64,9 @@ "message_content,msg_type,actual_type", [ (ROS2Message(payload={"data": "Hello, ROS2!"}), "std_msgs/msg/String", String), + (ROS2Message(payload={"data": "Hello, ROS2!"}), String, String), (String(data="Hello, ROS2!"), None, String), + (String(data="Hello, ROS2!"), "std_msgs/msg/String", String), (String(), None, String), (Pose(), None, Pose), (PoseWithCovarianceStamped(), None, PoseWithCovarianceStamped), @@ -90,8 +92,8 @@ def test_ros2_connector_send_message( ros_setup: None, request: pytest.FixtureRequest, - message_content: ROS2Message, - msg_type: str | None, + message_content: Any, + msg_type: str | type | None, actual_type: type, ): topic_name = f"{request.node.originalname}_topic" # type: ignore @@ -125,16 +127,47 @@ def test_ros2_connector_receive_message( shutdown_executors_and_threads(executors, threads) -def service_call_helper(service_name: str, connector: ROS2Connector): - message = ROS2Message(payload={"data": True}) - response = connector.service_call( - message, target=service_name, msg_type="std_srvs/srv/SetBool" - ) +def service_call_helper( + service_name: str, + connector: ROS2Connector, + message: Any = None, + msg_type: str | type | None = "std_srvs/srv/SetBool", +): + if message is None: + message = ROS2Message(payload={"data": True}) + response = connector.service_call(message, target=service_name, msg_type=msg_type) assert response.payload == SetBool.Response( success=True, message="Test service called" ) +@pytest.mark.parametrize( + "message,msg_type", + [ + (ROS2Message(payload={"data": True}), "std_srvs/srv/SetBool"), + (ROS2Message(payload={"data": True}), SetBool), + (SetBool.Request(data=True), None), + (SetBool.Request(data=True), "std_srvs/srv/SetBool"), + (SetBool.Request(data=True), SetBool), + ], +) +def test_ros2_connector_service_call_message_types( + ros_setup: None, + request: pytest.FixtureRequest, + message: Any, + msg_type: str | type | None, +): + service_name = f"{request.node.originalname}_service" # type: ignore + service_server = ServiceServer(service_name, ReentrantCallbackGroup()) + executors, threads = multi_threaded_spinner([service_server]) + connector = ROS2Connector() + try: + service_call_helper(service_name, connector, message, msg_type) + finally: + connector.shutdown() + shutdown_executors_and_threads(executors, threads) + + @pytest.mark.parametrize( "callback_group", [MutuallyExclusiveCallbackGroup(), ReentrantCallbackGroup()], @@ -223,19 +256,31 @@ def test_ros2_connector_service_call_multiple_calls_at_the_same_time_multiproces shutdown_executors_and_threads(executors, threads) -def test_ros2_connector_send_goal(ros_setup: None, request: pytest.FixtureRequest): +@pytest.mark.parametrize( + "action_data,msg_type", + [ + (ROS2Message(payload={}), "nav2_msgs/action/NavigateToPose"), + (ROS2Message(payload={}), NavigateToPose), + (NavigateToPose.Goal(), None), + (NavigateToPose.Goal(), "nav2_msgs/action/NavigateToPose"), + (NavigateToPose.Goal(), NavigateToPose), + ], +) +def test_ros2_connector_send_goal( + ros_setup: None, + request: pytest.FixtureRequest, + action_data: Any, + msg_type: str | type | None, +): action_name = f"{request.node.originalname}_action" # type: ignore action_server = TestActionServer(action_name) executors, threads = multi_threaded_spinner([action_server]) connector = ROS2Connector() try: - message = ROS2Message( - payload={}, - ) handle = connector.start_action( - action_data=message, + action_data=action_data, target=action_name, - msg_type="nav2_msgs/action/NavigateToPose", + msg_type=msg_type, ) assert handle is not None finally: From f5d58f5687a6a5b121749083d4d175fe7ca902a7 Mon Sep 17 00:00:00 2001 From: Maciej Majek Date: Thu, 10 Sep 2026 12:18:43 +0200 Subject: [PATCH 09/10] chore: bump rai_core version to 2.13.0 --- src/rai_core/pyproject.toml | 2 +- uv.lock | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rai_core/pyproject.toml b/src/rai_core/pyproject.toml index e9f1e308c..3f8b8478a 100644 --- a/src/rai_core/pyproject.toml +++ b/src/rai_core/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "rai_core" -version = "2.12.4" +version = "2.13.0" description = "Core functionality for RAI framework" readme = "README.md" requires-python = ">=3.10,<3.13" diff --git a/uv.lock b/uv.lock index aa2918fe1..69d4f8077 100644 --- a/uv.lock +++ b/uv.lock @@ -4714,7 +4714,7 @@ requires-dist = [ [[package]] name = "rai-core" -version = "2.12.4" +version = "2.13.0" source = { editable = "src/rai_core" } dependencies = [ { name = "coloredlogs" }, From 5096316b29b12283900d688bbb51d5a8fe665643 Mon Sep 17 00:00:00 2001 From: Maciej Majek Date: Thu, 10 Sep 2026 12:27:27 +0200 Subject: [PATCH 10/10] test: use nav2 interfaces in service and action tests --- tests/communication/ros2/helpers.py | 99 ++++++++++++++++++- tests/communication/ros2/test_api.py | 103 ++++++++++++-------- tests/communication/ros2/test_connectors.py | 75 +++++++++----- 3 files changed, 211 insertions(+), 66 deletions(-) diff --git a/tests/communication/ros2/helpers.py b/tests/communication/ros2/helpers.py index 226499865..acbabfd65 100644 --- a/tests/communication/ros2/helpers.py +++ b/tests/communication/ros2/helpers.py @@ -23,7 +23,10 @@ import pytest import rclpy from cv_bridge import CvBridge -from nav2_msgs.action import NavigateToPose +from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from nav2_msgs.action import ComputePathThroughPoses, NavigateToPose +from nav2_msgs.msg import CostmapMetaData +from nav2_msgs.srv import GetCostmap from pydub import AudioSegment from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse from rclpy.action.server import ServerGoalHandle @@ -32,7 +35,7 @@ from rclpy.node import Node from rosgraph_msgs.msg import Clock from sensor_msgs.msg import Image -from std_msgs.msg import String +from std_msgs.msg import Header, String from std_srvs.srv import SetBool from tf2_ros import TransformBroadcaster, TransformStamped @@ -120,6 +123,98 @@ def handle_test_message(self, msg: Any) -> None: self.received_messages.append(msg) +COSTMAP_SPECS_DICT = { + "layer": "static_layer", + "resolution": 0.5, + "size_x": 3, + "size_y": 2, + "origin": {"position": {"x": 1.0, "y": 2.0}, "orientation": {"w": 1.0}}, +} +COSTMAP_SPECS_MSG = CostmapMetaData( + layer="static_layer", + resolution=0.5, + size_x=3, + size_y=2, + origin=Pose(position=Point(x=1.0, y=2.0), orientation=Quaternion(w=1.0)), +) + +PATH_GOAL_DICT = { + "start": {"header": {"frame_id": "map"}, "pose": {"orientation": {"w": 1.0}}}, + "goals": [ + { + "header": {"frame_id": "map"}, + "pose": {"position": {"x": 1.0, "y": 2.0}, "orientation": {"w": 1.0}}, + }, + { + "header": {"frame_id": "map"}, + "pose": {"position": {"x": 3.0, "y": 4.0}, "orientation": {"w": 1.0}}, + }, + ], + "planner_id": "GridBased", + "use_start": True, +} +PATH_GOAL_MSG = ComputePathThroughPoses.Goal( + start=PoseStamped( + header=Header(frame_id="map"), pose=Pose(orientation=Quaternion(w=1.0)) + ), + goals=[ + PoseStamped( + header=Header(frame_id="map"), + pose=Pose(position=Point(x=1.0, y=2.0), orientation=Quaternion(w=1.0)), + ), + PoseStamped( + header=Header(frame_id="map"), + pose=Pose(position=Point(x=3.0, y=4.0), orientation=Quaternion(w=1.0)), + ), + ], + planner_id="GridBased", + use_start=True, +) + + +class GetCostmapServer(Node): + def __init__( + self, service_name: str, callback_group: Optional[CallbackGroup] = None + ): + super().__init__("test_get_costmap_server") + self.srv = self.create_service( + GetCostmap, + service_name, + self.handle_get_costmap, + callback_group=callback_group, + ) + + def handle_get_costmap( + self, request: GetCostmap.Request, response: GetCostmap.Response + ) -> GetCostmap.Response: + response.map.header.frame_id = "map" + response.map.metadata = request.specs + response.map.data = [0] * (request.specs.size_x * request.specs.size_y) + return response + + +class ComputePathThroughPosesServer(Node): + def __init__(self, action_name: str): + super().__init__(f"test_compute_path_server_{str(uuid.uuid4())[-12:]}") + self.action_server = ActionServer( + self, + action_type=ComputePathThroughPoses, + action_name=action_name, + execute_callback=self.handle_compute_path, + callback_group=ReentrantCallbackGroup(), + ) + + def handle_compute_path( + self, goal_handle: ServerGoalHandle + ) -> ComputePathThroughPoses.Result: + goal: ComputePathThroughPoses.Goal = goal_handle.request + goal_handle.succeed() + result = ComputePathThroughPoses.Result() + result.path.header.frame_id = goal.start.header.frame_id + result.path.poses = [goal.start, *goal.goals] + return result + + class TestActionServer(Node): __test__ = False diff --git a/tests/communication/ros2/test_api.py b/tests/communication/ros2/test_api.py index 2afead72e..8b81d5065 100644 --- a/tests/communication/ros2/test_api.py +++ b/tests/communication/ros2/test_api.py @@ -32,7 +32,12 @@ PoseWithCovarianceStamped, Quaternion, ) -from nav2_msgs.action import NavigateThroughPoses, NavigateToPose +from nav2_msgs.action import ( + ComputePathThroughPoses, + NavigateThroughPoses, + NavigateToPose, +) +from nav2_msgs.srv import GetCostmap from rai.communication.ros2.api import ( ROS2ActionAPI, ROS2ServiceAPI, @@ -47,9 +52,15 @@ from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from std_msgs.msg import Header, String -from std_srvs.srv import SetBool, Trigger +from std_srvs.srv import SetBool from .helpers import ( + COSTMAP_SPECS_DICT, + COSTMAP_SPECS_MSG, + PATH_GOAL_DICT, + PATH_GOAL_MSG, + ComputePathThroughPosesServer, + GetCostmapServer, MessageSubscriber, ServiceServer, TestActionClient, @@ -367,16 +378,12 @@ def test_ros2_single_message_invalid_type( def invoke_set_bool_service( - service_name: str, - service_api: ROS2ServiceAPI, - reuse_client: bool = True, - service_type: str | type | None = "std_srvs/srv/SetBool", - request: Any = None, + service_name: str, service_api: ROS2ServiceAPI, reuse_client: bool = True ): response = service_api.call_service( service_name, - service_type=service_type, - request={"data": True} if request is None else request, + service_type="std_srvs/srv/SetBool", + request={"data": True}, reuse_client=reuse_client, ) assert response.success @@ -386,11 +393,11 @@ def invoke_set_bool_service( @pytest.mark.parametrize( "service_type,request_content", [ - ("std_srvs/srv/SetBool", {"data": True}), - (SetBool, {"data": True}), - (None, SetBool.Request(data=True)), - ("std_srvs/srv/SetBool", SetBool.Request(data=True)), - (SetBool, SetBool.Request(data=True)), + ("nav2_msgs/srv/GetCostmap", {"specs": COSTMAP_SPECS_DICT}), + (GetCostmap, {"specs": COSTMAP_SPECS_DICT}), + (None, GetCostmap.Request(specs=COSTMAP_SPECS_MSG)), + ("nav2_msgs/srv/GetCostmap", GetCostmap.Request(specs=COSTMAP_SPECS_MSG)), + (GetCostmap, GetCostmap.Request(specs=COSTMAP_SPECS_MSG)), ], ) def test_ros2_service_single_call_request_types( @@ -401,18 +408,18 @@ def test_ros2_service_single_call_request_types( ) -> None: service_name = f"{request.node.originalname}_service" # type: ignore node_name = f"{request.node.originalname}_node" # type: ignore - service_server = ServiceServer(service_name, ReentrantCallbackGroup()) + service_server = GetCostmapServer(service_name, ReentrantCallbackGroup()) node = Node(node_name) executors, threads = multi_threaded_spinner([service_server, node]) try: service_api = ROS2ServiceAPI(node) - invoke_set_bool_service( - service_name, - service_api, - service_type=service_type, - request=request_content, + response = service_api.call_service( + service_name, service_type=service_type, request=request_content ) + assert response.map.header.frame_id == "map" + assert response.map.metadata == COSTMAP_SPECS_MSG + assert len(response.map.data) == 6 finally: shutdown_executors_and_threads(executors, threads) @@ -420,13 +427,13 @@ def test_ros2_service_single_call_request_types( @pytest.mark.parametrize( "service_type,request_content", [ - (None, {"data": True}), + (None, {"specs": COSTMAP_SPECS_DICT}), (None, None), - (None, SetBool.Request), - (None, SetBool), - (None, SetBool.Response()), - ("std_srvs/srv/Trigger", SetBool.Request(data=True)), - (Trigger, SetBool.Request(data=True)), + (None, GetCostmap.Request), + (None, GetCostmap), + (None, GetCostmap.Response()), + ("std_srvs/srv/SetBool", GetCostmap.Request()), + (SetBool, GetCostmap.Request()), ], ) def test_ros2_service_single_call_invalid_request( @@ -437,7 +444,7 @@ def test_ros2_service_single_call_invalid_request( ) -> None: service_name = f"{request.node.originalname}_service" # type: ignore node_name = f"{request.node.originalname}_node" # type: ignore - service_server = ServiceServer(service_name, ReentrantCallbackGroup()) + service_server = GetCostmapServer(service_name, ReentrantCallbackGroup()) node = Node(node_name) executors, threads = multi_threaded_spinner([service_server, node]) @@ -655,11 +662,11 @@ def test_ros2_service_single_call_wrong_service_name( @pytest.mark.parametrize( "action_type,goal", [ - ("nav2_msgs/action/NavigateToPose", {}), - (NavigateToPose, {}), - (None, NavigateToPose.Goal()), - ("nav2_msgs/action/NavigateToPose", NavigateToPose.Goal()), - (NavigateToPose, NavigateToPose.Goal()), + ("nav2_msgs/action/ComputePathThroughPoses", PATH_GOAL_DICT), + (ComputePathThroughPoses, PATH_GOAL_DICT), + (None, PATH_GOAL_MSG), + ("nav2_msgs/action/ComputePathThroughPoses", PATH_GOAL_MSG), + (ComputePathThroughPoses, PATH_GOAL_MSG), ], ) def test_ros2_action_send_goal( @@ -670,16 +677,26 @@ def test_ros2_action_send_goal( ) -> None: action_name = f"{request.node.originalname}_action" # type: ignore node_name = f"{request.node.originalname}_node" # type: ignore - action_server = TestActionServer(action_name) + action_server = ComputePathThroughPosesServer(action_name) node = Node(node_name) executors, threads = multi_threaded_spinner([action_server, node]) try: action_api = ROS2ActionAPI(node) accepted, handle = action_api.send_goal(action_name, action_type, goal) - assert accepted assert handle != "" + + start_time = time.perf_counter() + while not action_api.is_goal_done(handle): + time.sleep(0.01) + if time.perf_counter() - start_time > 1.0: + raise TimeoutError("Goal not done") + result = action_api.get_result(handle) + + assert result.status == GoalStatus.STATUS_SUCCEEDED + assert result.result.path.header.frame_id == "map" + assert result.result.path.poses == [PATH_GOAL_MSG.start, *PATH_GOAL_MSG.goals] finally: shutdown_executors_and_threads(executors, threads) @@ -687,13 +704,17 @@ def test_ros2_action_send_goal( @pytest.mark.parametrize( "action_type,goal", [ - (None, {}), + (None, PATH_GOAL_DICT), (None, None), - (None, NavigateToPose.Goal), - (None, NavigateToPose), - (None, NavigateToPose.Result()), - ("nav2_msgs/action/NavigateToPose", NavigateToPose.Feedback()), - ("nav2_msgs/action/NavigateThroughPoses", NavigateToPose.Goal()), + (None, ComputePathThroughPoses.Goal), + (None, ComputePathThroughPoses), + (None, ComputePathThroughPoses.Result()), + ( + "nav2_msgs/action/ComputePathThroughPoses", + ComputePathThroughPoses.Feedback(), + ), + ("nav2_msgs/action/NavigateToPose", ComputePathThroughPoses.Goal()), + (NavigateToPose, ComputePathThroughPoses.Goal()), ], ) def test_ros2_action_send_goal_invalid_goal( @@ -704,7 +725,7 @@ def test_ros2_action_send_goal_invalid_goal( ) -> None: action_name = f"{request.node.originalname}_action" # type: ignore node_name = f"{request.node.originalname}_node" # type: ignore - action_server = TestActionServer(action_name) + action_server = ComputePathThroughPosesServer(action_name) node = Node(node_name) executors, threads = multi_threaded_spinner([action_server, node]) diff --git a/tests/communication/ros2/test_connectors.py b/tests/communication/ros2/test_connectors.py index 9faab9bcb..f67d508aa 100644 --- a/tests/communication/ros2/test_connectors.py +++ b/tests/communication/ros2/test_connectors.py @@ -19,6 +19,7 @@ from unittest.mock import MagicMock import pytest +from action_msgs.msg import GoalStatus from builtin_interfaces.msg import Time from geometry_msgs.msg import ( Point, @@ -27,7 +28,8 @@ PoseWithCovarianceStamped, Quaternion, ) -from nav2_msgs.action import NavigateToPose +from nav2_msgs.action import ComputePathThroughPoses, NavigateToPose +from nav2_msgs.srv import GetCostmap from PIL import Image from pydub import AudioSegment from rai.communication.ros2 import ( @@ -45,6 +47,12 @@ from std_srvs.srv import SetBool from .helpers import ( + COSTMAP_SPECS_DICT, + COSTMAP_SPECS_MSG, + PATH_GOAL_DICT, + PATH_GOAL_MSG, + ComputePathThroughPosesServer, + GetCostmapServer, HRIMessageSubscriber, MessagePublisher, MessageSubscriber, @@ -127,15 +135,11 @@ def test_ros2_connector_receive_message( shutdown_executors_and_threads(executors, threads) -def service_call_helper( - service_name: str, - connector: ROS2Connector, - message: Any = None, - msg_type: str | type | None = "std_srvs/srv/SetBool", -): - if message is None: - message = ROS2Message(payload={"data": True}) - response = connector.service_call(message, target=service_name, msg_type=msg_type) +def service_call_helper(service_name: str, connector: ROS2Connector): + message = ROS2Message(payload={"data": True}) + response = connector.service_call( + message, target=service_name, msg_type="std_srvs/srv/SetBool" + ) assert response.payload == SetBool.Response( success=True, message="Test service called" ) @@ -144,11 +148,14 @@ def service_call_helper( @pytest.mark.parametrize( "message,msg_type", [ - (ROS2Message(payload={"data": True}), "std_srvs/srv/SetBool"), - (ROS2Message(payload={"data": True}), SetBool), - (SetBool.Request(data=True), None), - (SetBool.Request(data=True), "std_srvs/srv/SetBool"), - (SetBool.Request(data=True), SetBool), + ( + ROS2Message(payload={"specs": COSTMAP_SPECS_DICT}), + "nav2_msgs/srv/GetCostmap", + ), + (ROS2Message(payload={"specs": COSTMAP_SPECS_DICT}), GetCostmap), + (GetCostmap.Request(specs=COSTMAP_SPECS_MSG), None), + (GetCostmap.Request(specs=COSTMAP_SPECS_MSG), "nav2_msgs/srv/GetCostmap"), + (GetCostmap.Request(specs=COSTMAP_SPECS_MSG), GetCostmap), ], ) def test_ros2_connector_service_call_message_types( @@ -158,11 +165,16 @@ def test_ros2_connector_service_call_message_types( msg_type: str | type | None, ): service_name = f"{request.node.originalname}_service" # type: ignore - service_server = ServiceServer(service_name, ReentrantCallbackGroup()) + service_server = GetCostmapServer(service_name, ReentrantCallbackGroup()) executors, threads = multi_threaded_spinner([service_server]) connector = ROS2Connector() try: - service_call_helper(service_name, connector, message, msg_type) + response = connector.service_call( + message, target=service_name, msg_type=msg_type + ) + assert response.payload.map.header.frame_id == "map" + assert response.payload.map.metadata == COSTMAP_SPECS_MSG + assert len(response.payload.map.data) == 6 finally: connector.shutdown() shutdown_executors_and_threads(executors, threads) @@ -259,11 +271,14 @@ def test_ros2_connector_service_call_multiple_calls_at_the_same_time_multiproces @pytest.mark.parametrize( "action_data,msg_type", [ - (ROS2Message(payload={}), "nav2_msgs/action/NavigateToPose"), - (ROS2Message(payload={}), NavigateToPose), - (NavigateToPose.Goal(), None), - (NavigateToPose.Goal(), "nav2_msgs/action/NavigateToPose"), - (NavigateToPose.Goal(), NavigateToPose), + ( + ROS2Message(payload=PATH_GOAL_DICT), + "nav2_msgs/action/ComputePathThroughPoses", + ), + (ROS2Message(payload=PATH_GOAL_DICT), ComputePathThroughPoses), + (PATH_GOAL_MSG, None), + (PATH_GOAL_MSG, "nav2_msgs/action/ComputePathThroughPoses"), + (PATH_GOAL_MSG, ComputePathThroughPoses), ], ) def test_ros2_connector_send_goal( @@ -273,16 +288,30 @@ def test_ros2_connector_send_goal( msg_type: str | type | None, ): action_name = f"{request.node.originalname}_action" # type: ignore - action_server = TestActionServer(action_name) + action_server = ComputePathThroughPosesServer(action_name) executors, threads = multi_threaded_spinner([action_server]) connector = ROS2Connector() + results: List[Any] = [] try: handle = connector.start_action( action_data=action_data, target=action_name, + on_done=lambda future: results.append(future.result()), msg_type=msg_type, ) assert handle is not None + + start_time = time.perf_counter() + while not results: + time.sleep(0.01) + if time.perf_counter() - start_time > 1.0: + raise TimeoutError("Goal not done") + + assert results[0].status == GoalStatus.STATUS_SUCCEEDED + assert results[0].result.path.poses == [ + PATH_GOAL_MSG.start, + *PATH_GOAL_MSG.goals, + ] finally: connector.shutdown() shutdown_executors_and_threads(executors, threads)