feat(nav2): add workspace bounds and coordinate safety validation

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"

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}]"
+                )

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

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
         )

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