From cd1602c90cad467cda0a9f290363f9d273f0f21b Mon Sep 17 00:00:00 2001 From: Maciej Majek Date: Fri, 8 Aug 2025 17:04:38 +0200 Subject: [PATCH 1/5] feat: nav2 blocking tool --- .../rai/tools/ros2/navigation/__init__.py | 2 + .../tools/ros2/navigation/nav2_blocking.py | 70 +++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py diff --git a/src/rai_core/rai/tools/ros2/navigation/__init__.py b/src/rai_core/rai/tools/ros2/navigation/__init__.py index 8c5a94838..4142fcc68 100644 --- a/src/rai_core/rai/tools/ros2/navigation/__init__.py +++ b/src/rai_core/rai/tools/ros2/navigation/__init__.py @@ -20,6 +20,7 @@ Nav2Toolkit, NavigateToPoseTool, ) +from .nav2_blocking import NavigateToPoseBlockingTool __all__ = [ "CancelNavigateToPoseTool", @@ -27,5 +28,6 @@ "GetNavigateToPoseResultTool", "GetOccupancyGridTool", "Nav2Toolkit", + "NavigateToPoseBlockingTool", "NavigateToPoseTool", ] diff --git a/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py b/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py new file mode 100644 index 000000000..cc6ee2b6e --- /dev/null +++ b/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py @@ -0,0 +1,70 @@ +# Copyright (C) 2025 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 typing import Type + +from geometry_msgs.msg import PoseStamped, Quaternion +from nav2_msgs.action import NavigateToPose +from pydantic import BaseModel, Field +from rclpy.action import ActionClient +from tf_transformations import quaternion_from_euler + +from rai.tools.ros2.base import BaseROS2Tool + + +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") + + +class NavigateToPoseBlockingTool(BaseROS2Tool): + name: str = "navigate_to_pose_blocking" + description: str = "Navigate to a specific pose" + frame_id: str = Field( + default="map", description="The frame id of the Nav2 stack (map, odom, etc.)" + ) + action_name: str = Field( + default="navigate_to_pose", description="The name of the Nav2 action" + ) + args_schema: Type[NavigateToPoseBlockingToolInput] = NavigateToPoseBlockingToolInput + + def _run(self, x: float, y: float, z: float, yaw: float) -> str: + action_client = ActionClient( + self.connector.node, NavigateToPose, self.action_name + ) + + pose = PoseStamped() + pose.header.frame_id = self.frame_id + pose.header.stamp = self.connector.node.get_clock().now().to_msg() + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.position.z = z + quat = quaternion_from_euler(0, 0, yaw) + pose.pose.orientation = Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]) + + goal = { + "pose": { + "header": { + "frame_id": self.frame_id, + "stamp": self.connector.node.get_clock().now().to_msg(), + }, + "pose": pose, + } + } + + result = action_client.send_goal(goal) + + return str(result) From 4fc08ac87284095fbe3696657261fbebc31fab34 Mon Sep 17 00:00:00 2001 From: Maciej Majek <46171033+maciejmajek@users.noreply.github.com> Date: Fri, 12 Sep 2025 14:34:08 +0200 Subject: [PATCH 2/5] Update nav2_blocking.py --- .../rai/tools/ros2/navigation/nav2_blocking.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) 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 cc6ee2b6e..34acee981 100644 --- a/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py +++ b/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py @@ -46,6 +46,7 @@ def _run(self, x: float, y: float, z: float, yaw: float) -> str: self.connector.node, NavigateToPose, self.action_name ) + pose = PoseStamped() pose.header.frame_id = self.frame_id pose.header.stamp = self.connector.node.get_clock().now().to_msg() @@ -55,15 +56,8 @@ def _run(self, x: float, y: float, z: float, yaw: float) -> str: quat = quaternion_from_euler(0, 0, yaw) pose.pose.orientation = Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]) - goal = { - "pose": { - "header": { - "frame_id": self.frame_id, - "stamp": self.connector.node.get_clock().now().to_msg(), - }, - "pose": pose, - } - } + goal = NavigateToPose.Goal() + goal.pose = pose result = action_client.send_goal(goal) From ec81f101d86b5d94a0d008d9d60fa28954a6e4fc Mon Sep 17 00:00:00 2001 From: Maciej Majek <46171033+maciejmajek@users.noreply.github.com> Date: Fri, 12 Sep 2025 14:41:15 +0200 Subject: [PATCH 3/5] Update nav2_blocking.py --- src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 34acee981..0e67ccc97 100644 --- a/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py +++ b/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py @@ -61,4 +61,10 @@ def _run(self, x: float, y: float, z: float, yaw: float) -> str: result = action_client.send_goal(goal) - return str(result) + if result is None: + return "Navigate to pose action failed. Please try again." + + if result.result.error_code != 0: + return f"Navigate to pose action failed. Error code: {result.result.error_code}" + + return "Navigate to pose successful." From 84f871c5c3b5614d43df839863d473dd97273ecf Mon Sep 17 00:00:00 2001 From: maciejmajek Date: Fri, 12 Sep 2025 14:41:57 +0200 Subject: [PATCH 4/5] chore: pre-commit --- src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 0e67ccc97..b51a79207 100644 --- a/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py +++ b/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py @@ -46,7 +46,6 @@ def _run(self, x: float, y: float, z: float, yaw: float) -> str: self.connector.node, NavigateToPose, self.action_name ) - pose = PoseStamped() pose.header.frame_id = self.frame_id pose.header.stamp = self.connector.node.get_clock().now().to_msg() @@ -63,7 +62,7 @@ def _run(self, x: float, y: float, z: float, yaw: float) -> str: if result is None: return "Navigate to pose action failed. Please try again." - + if result.result.error_code != 0: return f"Navigate to pose action failed. Error code: {result.result.error_code}" From ffa4cef4f44c616d4b4b7b0a1d17a9abf30528a2 Mon Sep 17 00:00:00 2001 From: Maciej Majek <46171033+maciejmajek@users.noreply.github.com> Date: Fri, 12 Sep 2025 14:46:18 +0200 Subject: [PATCH 5/5] chore: bump rai-core v --- src/rai_core/pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rai_core/pyproject.toml b/src/rai_core/pyproject.toml index 9c8152a88..d13258cbc 100644 --- a/src/rai_core/pyproject.toml +++ b/src/rai_core/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "poetry.core.masonry.api" [tool.poetry] name = "rai_core" -version = "2.3.0" +version = "2.4.0" description = "Core functionality for RAI framework" authors = ["Maciej Majek ", "Bartłomiej Boczek ", "Kajetan Rachwał "] readme = "README.md"