diff --git a/docs/API_documentation/connectors/ROS_2_Connectors.md b/docs/API_documentation/connectors/ROS_2_Connectors.md index afd7ac796..8f46315ce 100644 --- a/docs/API_documentation/connectors/ROS_2_Connectors.md +++ b/docs/API_documentation/connectors/ROS_2_Connectors.md @@ -33,13 +33,22 @@ 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 +from std_srvs.srv import SetBool +from nav2_msgs.action import NavigateToPose 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, as a string or class) +connector.send_message( + message=ROS2Message(payload={"data": "Hello"}), target="/my_topic", msg_type="std_msgs/msg/String" ) @@ -51,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=SetBool.Request(data=True), + target="/my_service" +) + +# Call a service using a dictionary (msg_type is required) response = connector.service_call( - message=my_request_msg, + 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/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/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 7692f5cbd..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, @@ -36,7 +38,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 @@ -126,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: @@ -134,3 +202,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/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 cb55002f6..138184e11 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 | Type[Any] | None = None, *, auto_qos_matching: bool = True, qos_profile: Optional[QoSProfile] = None, @@ -150,20 +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 ) - - msg = self.build_ros2_msg(msg_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 4f0accec1..fac985211 100644 --- a/src/rai_core/rai/communication/ros2/connectors/base.py +++ b/src/rai_core/rai/communication/ros2/connectors/base.py @@ -16,7 +16,18 @@ 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, + Type, + TypeVar, +) import rclpy import rclpy.executors @@ -31,6 +42,7 @@ from rai.communication import BaseConnector from rai.communication.ros2.api import ( + IROS2Message, ROS2ActionAPI, ROS2ServiceAPI, ROS2TopicAPI, @@ -241,10 +253,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 | Type[Any] | None = None, auto_qos_matching: bool = True, qos_profile: Optional[QoSProfile] = None, **kwargs: Any, @@ -253,12 +265,13 @@ def send_message( Parameters ---------- - message : T - The message to send. + message : T | IROS2Message + 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 - The ROS2 message type. + 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 @@ -266,9 +279,10 @@ def send_message( **kwargs : Any Additional keyword arguments. """ + msg_content = message.payload if isinstance(message, ROS2Message) else 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/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/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 64974fb76..8b81d5065 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 @@ -22,12 +23,27 @@ import pytest from action_msgs.msg import GoalStatus from action_msgs.srv import CancelGoal -from nav2_msgs.action import NavigateToPose +from geometry_msgs.msg import ( + Point, + Pose, + PoseArray, + PoseStamped, + PoseWithCovariance, + PoseWithCovarianceStamped, + Quaternion, +) +from nav2_msgs.action import ( + ComputePathThroughPoses, + NavigateThroughPoses, + NavigateToPose, +) +from nav2_msgs.srv import GetCostmap from rai.communication.ros2.api import ( ROS2ActionAPI, ROS2ServiceAPI, ROS2TopicAPI, ) +from rai.communication.ros2.api.base import BaseROS2API from rclpy.callback_groups import ( CallbackGroup, MutuallyExclusiveCallbackGroup, @@ -35,9 +51,16 @@ ) 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 ( + COSTMAP_SPECS_DICT, + COSTMAP_SPECS_MSG, + PATH_GOAL_DICT, + PATH_GOAL_MSG, + ComputePathThroughPosesServer, + GetCostmapServer, MessageSubscriber, ServiceServer, TestActionClient, @@ -51,12 +74,157 @@ _ = 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=PoseStamped( + 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, + False, + False, + ), + (NavigateToPose.Result(), True, False, False), + (NavigateToPose.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( + "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", + [ + ({"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 +232,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 +284,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 +308,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, ) @@ -139,6 +317,66 @@ 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), + (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( + 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 ): @@ -152,6 +390,74 @@ def invoke_set_bool_service( assert response.message == "Test service called" +@pytest.mark.parametrize( + "service_type,request_content", + [ + ("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( + 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 = GetCostmapServer(service_name, ReentrantCallbackGroup()) + node = Node(node_name) + executors, threads = multi_threaded_spinner([service_server, node]) + + try: + service_api = ROS2ServiceAPI(node) + 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) + + +@pytest.mark.parametrize( + "service_type,request_content", + [ + (None, {"specs": COSTMAP_SPECS_DICT}), + (None, None), + (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( + 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 = GetCostmapServer(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()], @@ -353,21 +659,80 @@ 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/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( + 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) + 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, "nav2_msgs/action/NavigateToPose", {} - ) - + 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) + + +@pytest.mark.parametrize( + "action_type,goal", + [ + (None, PATH_GOAL_DICT), + (None, None), + (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( + 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 = ComputePathThroughPosesServer(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) diff --git a/tests/communication/ros2/test_connectors.py b/tests/communication/ros2/test_connectors.py index 9fbf0d4e1..f67d508aa 100644 --- a/tests/communication/ros2/test_connectors.py +++ b/tests/communication/ros2/test_connectors.py @@ -19,7 +19,17 @@ from unittest.mock import MagicMock import pytest -from nav2_msgs.action import NavigateToPose +from action_msgs.msg import GoalStatus +from builtin_interfaces.msg import Time +from geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + PoseWithCovarianceStamped, + Quaternion, +) +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 ( @@ -33,10 +43,16 @@ MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup, ) -from std_msgs.msg import String +from std_msgs.msg import Header, String 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, @@ -52,21 +68,53 @@ _ = 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), + (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), + ( + 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: Any, + msg_type: str | type | 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) @@ -97,6 +145,41 @@ def service_call_helper(service_name: str, connector: ROS2Connector): ) +@pytest.mark.parametrize( + "message,msg_type", + [ + ( + 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( + ros_setup: None, + request: pytest.FixtureRequest, + message: Any, + msg_type: str | type | None, +): + service_name = f"{request.node.originalname}_service" # type: ignore + service_server = GetCostmapServer(service_name, ReentrantCallbackGroup()) + executors, threads = multi_threaded_spinner([service_server]) + connector = ROS2Connector() + try: + 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) + + @pytest.mark.parametrize( "callback_group", [MutuallyExclusiveCallbackGroup(), ReentrantCallbackGroup()], @@ -185,21 +268,50 @@ 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=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( + 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) + action_server = ComputePathThroughPosesServer(action_name) executors, threads = multi_threaded_spinner([action_server]) connector = ROS2Connector() + results: List[Any] = [] try: - message = ROS2Message( - payload={}, - ) handle = connector.start_action( - action_data=message, + action_data=action_data, target=action_name, - msg_type="nav2_msgs/action/NavigateToPose", + 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) 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" },