diff --git a/src/rai_core/pyproject.toml b/src/rai_core/pyproject.toml index 009a9a15f..e9f1e308c 100644 --- a/src/rai_core/pyproject.toml +++ b/src/rai_core/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "rai_core" -version = "2.12.3" +version = "2.12.4" description = "Core functionality for RAI framework" readme = "README.md" requires-python = ">=3.10,<3.13" diff --git a/src/rai_core/rai/tools/ros2/navigation/bounds.py b/src/rai_core/rai/tools/ros2/navigation/bounds.py new file mode 100644 index 000000000..5c136815d --- /dev/null +++ b/src/rai_core/rai/tools/ros2/navigation/bounds.py @@ -0,0 +1,60 @@ +# Copyright (C) 2026 Robotec.AI +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from math import inf +from typing import Annotated, Optional, Tuple + +from pydantic import BaseModel, Field, model_validator +from typing_extensions import Self + +# a plain float tuple would accept NaN, which silently disables every comparison +Coordinate = Annotated[float, Field(allow_inf_nan=False)] +Bounds = Optional[Tuple[Coordinate, Coordinate, Coordinate]] + + +class WorkspaceBounds(BaseModel): + """Optional (x, y, z) box a navigation goal has to fall inside.""" + + workspace_bounds_min: Bounds = Field( + default=None, + description="Optional minimum (x, y, z) workspace bounds for navigation goals", + ) + workspace_bounds_max: Bounds = Field( + default=None, + description="Optional maximum (x, y, z) workspace bounds for navigation goals", + ) + + @model_validator(mode="after") + def _check_bounds_ordering(self) -> Self: + lo, hi = self.workspace_bounds_min, self.workspace_bounds_max + if ( + lo is not None + and hi is not None + and any(low > high for low, high in zip(lo, hi)) + ): + raise ValueError( + f"workspace_bounds_min {lo} exceeds workspace_bounds_max {hi}" + ) + return self + + def reject_out_of_bounds(self, x: float, y: float, z: float) -> None: + """Raise if the goal falls outside the configured workspace.""" + lo = self.workspace_bounds_min or (-inf, -inf, -inf) + hi = self.workspace_bounds_max or (inf, inf, inf) + for axis, value, low, high in zip("xyz", (x, y, z), lo, hi): + if not low <= value <= high: + raise ValueError( + f"Goal rejected: {axis}={value} outside the workspace " + f"[{low}, {high}]" + ) diff --git a/src/rai_core/rai/tools/ros2/navigation/nav2.py b/src/rai_core/rai/tools/ros2/navigation/nav2.py index 75be15d8e..149df6cfc 100644 --- a/src/rai_core/rai/tools/ros2/navigation/nav2.py +++ b/src/rai_core/rai/tools/ros2/navigation/nav2.py @@ -29,13 +29,14 @@ from rai.communication.ros2.connectors import ROS2Connector from rai.messages import MultimodalArtifact from rai.tools.ros2.base import BaseROS2Tool, BaseROS2Toolkit +from rai.tools.ros2.navigation.bounds import WorkspaceBounds current_action_id: Optional[str] = None current_feedback: Optional[NavigateToPose.Feedback] = None current_result: Optional[NavigateToPose.Result] = None -class Nav2Toolkit(BaseROS2Toolkit): +class Nav2Toolkit(WorkspaceBounds, BaseROS2Toolkit): connector: ROS2Connector frame_id: str = Field( default="map", description="The frame id of the Nav2 stack (map, odom, etc.)" @@ -50,6 +51,8 @@ def get_tools(self) -> List[BaseTool]: connector=self.connector, frame_id=self.frame_id, action_name=self.action_name, + workspace_bounds_min=self.workspace_bounds_min, + workspace_bounds_max=self.workspace_bounds_max, ), CancelNavigateToPoseTool(connector=self.connector), GetNavigateToPoseFeedbackTool(connector=self.connector), @@ -72,7 +75,7 @@ class NavigateToPoseToolInput(BaseModel): ) -class NavigateToPoseTool(BaseROS2Tool): +class NavigateToPoseTool(WorkspaceBounds, BaseROS2Tool): name: str = "navigate_to_pose" description: str = "Navigate to a specific pose" @@ -94,6 +97,7 @@ def on_done(self, result: NavigateToPose.Result) -> None: current_result = result def _run(self, x: float, y: float, z: float, yaw: float) -> str: + self.reject_out_of_bounds(x, y, z) pose = PoseStamped() pose.header.frame_id = self.frame_id pose.header.stamp = self.connector.node.get_clock().now().to_msg() diff --git a/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py b/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py index 52c0b704f..166097ebc 100644 --- a/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py +++ b/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py @@ -22,6 +22,7 @@ from tf_transformations import quaternion_from_euler from rai.tools.ros2.base import BaseROS2Tool +from rai.tools.ros2.navigation.bounds import WorkspaceBounds def _get_status_string(status: int) -> str: @@ -81,13 +82,21 @@ def _run(self) -> str: class NavigateToPoseBlockingToolInput(BaseModel): - x: float = Field(..., description="The x coordinate of the pose") - y: float = Field(..., description="The y coordinate of the pose") - z: float = Field(..., description="The z coordinate of the pose") - yaw: float = Field(..., description="The yaw angle of the pose") + x: float = Field( + ..., allow_inf_nan=False, description="The x coordinate of the pose" + ) + y: float = Field( + ..., allow_inf_nan=False, description="The y coordinate of the pose" + ) + z: float = Field( + ..., allow_inf_nan=False, description="The z coordinate of the pose" + ) + yaw: float = Field( + ..., allow_inf_nan=False, description="The yaw angle of the pose" + ) -class NavigateToPoseBlockingTool(BaseROS2Tool): +class NavigateToPoseBlockingTool(WorkspaceBounds, BaseROS2Tool): name: str = "navigate_to_pose_blocking" description: str = "Navigate to a specific pose" frame_id: str = Field( @@ -99,6 +108,7 @@ class NavigateToPoseBlockingTool(BaseROS2Tool): args_schema: Type[NavigateToPoseBlockingToolInput] = NavigateToPoseBlockingToolInput def _run(self, x: float, y: float, z: float, yaw: float) -> str: + self.reject_out_of_bounds(x, y, z) action_client = ActionClient( self.connector.node, NavigateToPose, self.action_name ) diff --git a/tests/tools/ros2/test_nav2_bounds.py b/tests/tools/ros2/test_nav2_bounds.py new file mode 100644 index 000000000..3a01e2dde --- /dev/null +++ b/tests/tools/ros2/test_nav2_bounds.py @@ -0,0 +1,97 @@ +# Copyright (C) 2026 Robotec.AI +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""A navigation tool configured with workspace bounds must refuse a goal +outside them before it reaches the action server.""" + +import pytest + +try: + import rclpy # noqa: F401 + + _ = rclpy # noqa: F841 +except ImportError: + pytest.skip("ROS2 is not installed", allow_module_level=True) + +from unittest.mock import MagicMock + +from pydantic import ValidationError +from rai.communication.ros2.connectors import ROS2Connector +from rai.tools.ros2.navigation.nav2 import Nav2Toolkit, NavigateToPoseTool +from rai.tools.ros2.navigation.nav2_blocking import NavigateToPoseBlockingTool + +MIN = (-5.0, -5.0, 0.0) +MAX = (5.0, 5.0, 2.0) + + +@pytest.fixture +def connector(): + return MagicMock(spec=ROS2Connector) + + +@pytest.fixture(params=[NavigateToPoseTool, NavigateToPoseBlockingTool]) +def bounded_tool(request, connector): + return request.param( + connector=connector, workspace_bounds_min=MIN, workspace_bounds_max=MAX + ) + + +@pytest.mark.parametrize( + "x,y,z", + [ + (6.0, 0.0, 1.0), + (-6.0, 0.0, 1.0), + (0.0, 6.0, 1.0), + (0.0, 0.0, 3.0), + (0.0, 0.0, -1.0), + ], +) +def test_goal_outside_bounds_is_refused(bounded_tool, x, y, z): + with pytest.raises(ValueError, match="Goal rejected"): + bounded_tool._run(x=x, y=y, z=z, yaw=0.0) + bounded_tool.connector.start_action.assert_not_called() + + +def test_unbounded_tool_accepts_anything(connector): + NavigateToPoseTool(connector=connector).reject_out_of_bounds(1e9, -1e9, 1e9) + + +def test_one_sided_bounds_only_constrain_that_side(connector): + tool = NavigateToPoseTool(connector=connector, workspace_bounds_max=MAX) + tool.reject_out_of_bounds(-1e9, -1e9, 0.0) + with pytest.raises(ValueError, match="outside the workspace"): + tool.reject_out_of_bounds(6.0, 0.0, 0.0) + + +def test_inverted_bounds_are_rejected_at_construction(connector): + with pytest.raises(ValidationError, match="exceeds"): + NavigateToPoseTool( + connector=connector, workspace_bounds_min=MAX, workspace_bounds_max=MIN + ) + + +def test_nan_bounds_are_rejected_at_construction(connector): + with pytest.raises(ValidationError): + NavigateToPoseTool( + connector=connector, workspace_bounds_min=(1.0, 2.0, float("nan")) + ) + + +def test_toolkit_passes_bounds_to_the_goal_tool(connector): + toolkit = Nav2Toolkit( + connector=connector, workspace_bounds_min=MIN, workspace_bounds_max=MAX + ) + tool = next(t for t in toolkit.get_tools() if isinstance(t, NavigateToPoseTool)) + assert tool.workspace_bounds_min == MIN + assert tool.workspace_bounds_max == MAX diff --git a/uv.lock b/uv.lock index 3c0ed6bb8..aa2918fe1 100644 --- a/uv.lock +++ b/uv.lock @@ -4714,7 +4714,7 @@ requires-dist = [ [[package]] name = "rai-core" -version = "2.12.3" +version = "2.12.4" source = { editable = "src/rai_core" } dependencies = [ { name = "coloredlogs" },