From 942332a912aa4a33a8ba686e2e61c008ac47a024 Mon Sep 17 00:00:00 2001 From: juliaj Date: Sun, 25 Jan 2026 19:36:18 -0800 Subject: [PATCH 1/7] Fix NavigateToPose_Result object has no attribute error_code and log error message --- .../tools/ros2/navigation/nav2_blocking.py | 48 +++++++++- tests/tools/ros2/test_nav2_tools.py | 90 ++++++++++++++++++- 2 files changed, 135 insertions(+), 3 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 aaff5668c..3c325cba9 100644 --- a/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py +++ b/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py @@ -23,6 +23,49 @@ from rai.tools.ros2.base import BaseROS2Tool +def _get_error_code_string(error_code: int) -> str: + """Convert NavigateToPose error code to human-readable string.""" + error_code_map = { + 0: "NONE", + 1: "UNKNOWN", + 2: "TIMEOUT", + 3: "CANCELED", + 4: "FAILED", + 5: "INVALID_POSE", + 6: "PLANNER_FAILED", + 7: "CONTROLLER_FAILED", + 8: "RECOVERY_FAILED", + } + return error_code_map.get(error_code, f"UNKNOWN_ERROR_CODE_{error_code}") + + +def _get_error_message(result) -> str: + """Extract error message from NavigateToPose result.""" + error_parts = [] + + # Get error code string for context + error_code_str = _get_error_code_string(result.error_code) + error_parts.append(f"({error_code_str})") + + # Check for additional error message fields (if they exist) + # These may vary between ROS2 versions, so we check safely + if hasattr(result, "error_message") and result.error_message: + error_parts.append(f"Error message: {result.error_message}") + elif hasattr(result, "error_msg") and result.error_msg: + error_parts.append(f"Error message: {result.error_msg}") + elif hasattr(result, "message") and result.message: + error_parts.append(f"Message: {result.message}") + + # Include full result string representation for debugging + result_str = str(result) + if result_str and result_str != str(result.error_code): + # Only add if it provides additional info beyond just the error code + if len(result_str) > 50: # Only if it's substantial + error_parts.append(f"Full result: {result_str}") + + return ". ".join(error_parts) + + class GetCurrentPoseToolInput(BaseModel): pass @@ -86,7 +129,8 @@ 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}" + if result.error_code != 0: + error_msg = _get_error_message(result) + return f"Navigate to pose action failed. Error code: {result.error_code}. {error_msg}" return "Navigate to pose successful." diff --git a/tests/tools/ros2/test_nav2_tools.py b/tests/tools/ros2/test_nav2_tools.py index ede015c0e..274da31a1 100644 --- a/tests/tools/ros2/test_nav2_tools.py +++ b/tests/tools/ros2/test_nav2_tools.py @@ -22,12 +22,16 @@ pytest.skip("ROS2 is not installed", allow_module_level=True) -from unittest.mock import MagicMock +from unittest.mock import MagicMock, patch from geometry_msgs.msg import TransformStamped +from nav2_msgs.action import NavigateToPose from rai.communication.ros2.connectors import ROS2Connector from rai.tools.ros2.navigation.nav2_blocking import ( GetCurrentPoseTool, + NavigateToPoseBlockingTool, + _get_error_code_string, + _get_error_message, ) from tests.communication.ros2.helpers import ( @@ -55,3 +59,87 @@ def test_get_current_pose_tool(ros_setup: None, request: pytest.FixtureRequest) connector.get_transform.assert_called_once_with("map", "base_link") assert result == str(transform) + + +def test_get_error_code_string() -> None: + """Test error code to string conversion.""" + assert _get_error_code_string(0) == "NONE" + assert _get_error_code_string(2) == "TIMEOUT" + assert _get_error_code_string(6) == "PLANNER_FAILED" + assert _get_error_code_string(99) == "UNKNOWN_ERROR_CODE_99" + + +def test_get_error_message() -> None: + """Test error message extraction with error code and optional message.""" + # Test with error code only + result = NavigateToPose.Result() + result.error_code = 2 + error_msg = _get_error_message(result) + assert "(TIMEOUT)" in error_msg + + # Test with error message field + result_with_msg = MagicMock() + result_with_msg.error_code = 6 + result_with_msg.error_message = "Planner failed to find path" + error_msg = _get_error_message(result_with_msg) + assert "(PLANNER_FAILED)" in error_msg + assert "Error message: Planner failed to find path" in error_msg + + +def _create_mock_connector() -> MagicMock: + """Helper to create a mock connector with node and clock.""" + connector = MagicMock(spec=ROS2Connector) + mock_node = MagicMock() + mock_clock = MagicMock() + mock_time = MagicMock() + mock_clock.now.return_value.to_msg.return_value = mock_time + connector.node = mock_node + mock_node.get_clock.return_value = mock_clock + return connector + + +def test_navigate_to_pose_blocking_tool( + ros_setup: None, request: pytest.FixtureRequest +) -> None: + """Test navigation tool success, error, and None result scenarios.""" + connector = _create_mock_connector() + tool = NavigateToPoseBlockingTool(connector=connector) + + with patch( + "rai.tools.ros2.navigation.nav2_blocking.ActionClient" + ) as mock_action_client_class: + mock_action_client = MagicMock() + mock_action_client_class.return_value = mock_action_client + + # Test success + success_result = NavigateToPose.Result() + success_result.error_code = 0 + mock_action_client.send_goal.return_value = success_result + result = tool._run(x=1.0, y=2.0, z=0.0, yaw=0.5) + assert result == "Navigate to pose successful." + + # Test error with error code + error_result = NavigateToPose.Result() + error_result.error_code = 2 + mock_action_client.send_goal.return_value = error_result + result = tool._run(x=1.0, y=2.0, z=0.0, yaw=0.5) + assert "Navigate to pose action failed" in result + assert "Error code: 2" in result + assert "(TIMEOUT)" in result + + # Test error with message + error_result_with_msg = MagicMock() + error_result_with_msg.error_code = 6 + error_result_with_msg.error_message = "Planner failed" + mock_action_client.send_goal.return_value = error_result_with_msg + result = tool._run(x=1.0, y=2.0, z=0.0, yaw=0.5) + assert "Error code: 6" in result + assert "(PLANNER_FAILED)" in result + assert "Error message: Planner failed" in result + + # Test None result + mock_action_client.send_goal.return_value = None + result = tool._run(x=1.0, y=2.0, z=0.0, yaw=0.5) + assert result == "Navigate to pose action failed. Please try again." + + assert mock_action_client.send_goal.call_count == 4 From 90c2db827430d116d42c6ea39f8c2ca2be28ff20 Mon Sep 17 00:00:00 2001 From: juliaj Date: Sun, 25 Jan 2026 19:39:57 -0800 Subject: [PATCH 2/7] Version bump --- 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 f84f1ad55..613e20b11 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.8.1" +version = "2.8.2" description = "Core functionality for RAI framework" authors = ["Maciej Majek ", "Bartłomiej Boczek ", "Kajetan Rachwał "] readme = "README.md" From 0fa992b95adb156f5f13d97fa5ed059808a3a875 Mon Sep 17 00:00:00 2001 From: juliaj Date: Sun, 25 Jan 2026 19:50:02 -0800 Subject: [PATCH 3/7] Fix test failure on humble --- tests/tools/ros2/test_nav2_tools.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tests/tools/ros2/test_nav2_tools.py b/tests/tools/ros2/test_nav2_tools.py index 274da31a1..6526d010c 100644 --- a/tests/tools/ros2/test_nav2_tools.py +++ b/tests/tools/ros2/test_nav2_tools.py @@ -25,7 +25,6 @@ from unittest.mock import MagicMock, patch from geometry_msgs.msg import TransformStamped -from nav2_msgs.action import NavigateToPose from rai.communication.ros2.connectors import ROS2Connector from rai.tools.ros2.navigation.nav2_blocking import ( GetCurrentPoseTool, @@ -72,7 +71,7 @@ def test_get_error_code_string() -> None: def test_get_error_message() -> None: """Test error message extraction with error code and optional message.""" # Test with error code only - result = NavigateToPose.Result() + result = MagicMock() result.error_code = 2 error_msg = _get_error_message(result) assert "(TIMEOUT)" in error_msg @@ -112,14 +111,14 @@ def test_navigate_to_pose_blocking_tool( mock_action_client_class.return_value = mock_action_client # Test success - success_result = NavigateToPose.Result() + success_result = MagicMock() success_result.error_code = 0 mock_action_client.send_goal.return_value = success_result result = tool._run(x=1.0, y=2.0, z=0.0, yaw=0.5) assert result == "Navigate to pose successful." # Test error with error code - error_result = NavigateToPose.Result() + error_result = MagicMock() error_result.error_code = 2 mock_action_client.send_goal.return_value = error_result result = tool._run(x=1.0, y=2.0, z=0.0, yaw=0.5) From e4a6c4a0c843669bd0dfa398489a1858ce059790 Mon Sep 17 00:00:00 2001 From: juliaj Date: Sun, 25 Jan 2026 20:03:20 -0800 Subject: [PATCH 4/7] Fix test failure due to time to msg handling on humble --- tests/communication/ros2/helpers.py | 57 +++++++++++++++++++++++++++++ tests/tools/ros2/test_nav2_tools.py | 15 +------- 2 files changed, 59 insertions(+), 13 deletions(-) diff --git a/tests/communication/ros2/helpers.py b/tests/communication/ros2/helpers.py index 98965115c..df9898592 100644 --- a/tests/communication/ros2/helpers.py +++ b/tests/communication/ros2/helpers.py @@ -17,6 +17,7 @@ import time import uuid from typing import Any, Generator, List, Optional, Tuple +from unittest.mock import MagicMock import numpy as np import pytest @@ -396,6 +397,62 @@ def shutdown_executors_and_threads( print(f"Error destroying node: {e}") +def _create_time_with_to_msg_class(): + """Create a TimeWithToMsg class for ROS2 Humble/Jazzy compatibility. + + ROS2 Humble vs Jazzy difference: + - Humble: Strict type checking in __debug__ mode requires actual BuiltinTime + instances, not MagicMock objects. Using MagicMock causes AssertionError. + - Jazzy: More lenient with MagicMock, but BuiltinTime instances don't allow + dynamically adding methods (AttributeError when accessing to_msg). + + Solution: Create a wrapper class that inherits from BuiltinTime and adds to_msg(). + This is used by both create_mock_connector_with_clock() and setup_mock_clock() + in test_grounding_dino.py. + """ + from builtin_interfaces.msg import Time as BuiltinTime + + class TimeWithToMsg(BuiltinTime): + """BuiltinTime wrapper that adds to_msg() method for compatibility.""" + + def to_msg(self): + return self + + return TimeWithToMsg + + +def create_mock_connector_with_clock() -> Any: + """Create a mock ROS2Connector with properly configured clock for testing. + + This helper creates a mock connector that works with ROS2 Humble's strict + type checking for Time messages. The clock's now().to_msg() returns a real + builtin_interfaces.msg.Time instance that can be assigned to message stamps. + + This is similar to setup_mock_clock() in test_grounding_dino.py but creates + a new mock connector instead of modifying an existing agent's connector. + + Returns: + MagicMock: A mock ROS2Connector with node and clock configured. + """ + from rai.communication.ros2.connectors import ROS2Connector + + TimeWithToMsg = _create_time_with_to_msg_class() + + connector = MagicMock(spec=ROS2Connector) + mock_node = MagicMock() + mock_clock = MagicMock() + mock_time = MagicMock() + + # Create a TimeWithToMsg instance (passes isinstance checks and has to_msg()) + mock_ts = TimeWithToMsg() + mock_time.to_msg.return_value = mock_ts + mock_clock.now.return_value = mock_time + mock_node.get_clock.return_value = mock_clock + + connector.node = mock_node + return connector + + @pytest.fixture(scope="function") def ros_setup() -> Generator[None, None, None]: rclpy.init() diff --git a/tests/tools/ros2/test_nav2_tools.py b/tests/tools/ros2/test_nav2_tools.py index 6526d010c..f99364f8f 100644 --- a/tests/tools/ros2/test_nav2_tools.py +++ b/tests/tools/ros2/test_nav2_tools.py @@ -34,6 +34,7 @@ ) from tests.communication.ros2.helpers import ( + create_mock_connector_with_clock, ros_setup, ) @@ -85,23 +86,11 @@ def test_get_error_message() -> None: assert "Error message: Planner failed to find path" in error_msg -def _create_mock_connector() -> MagicMock: - """Helper to create a mock connector with node and clock.""" - connector = MagicMock(spec=ROS2Connector) - mock_node = MagicMock() - mock_clock = MagicMock() - mock_time = MagicMock() - mock_clock.now.return_value.to_msg.return_value = mock_time - connector.node = mock_node - mock_node.get_clock.return_value = mock_clock - return connector - - def test_navigate_to_pose_blocking_tool( ros_setup: None, request: pytest.FixtureRequest ) -> None: """Test navigation tool success, error, and None result scenarios.""" - connector = _create_mock_connector() + connector = create_mock_connector_with_clock() tool = NavigateToPoseBlockingTool(connector=connector) with patch( From cf1a50be16e44466adf3b9c00d7a09637b7f87e9 Mon Sep 17 00:00:00 2001 From: juliaj Date: Sun, 25 Jan 2026 20:12:06 -0800 Subject: [PATCH 5/7] Fix test failure due to segfault during shutdown --- tests/communication/ros2/helpers.py | 76 ++++++++++++++++++++--------- 1 file changed, 53 insertions(+), 23 deletions(-) diff --git a/tests/communication/ros2/helpers.py b/tests/communication/ros2/helpers.py index df9898592..1e7a7b047 100644 --- a/tests/communication/ros2/helpers.py +++ b/tests/communication/ros2/helpers.py @@ -340,61 +340,91 @@ def shutdown_executors_and_threads( all_nodes = [] for executor in executors: try: - all_nodes.extend(executor.get_nodes()) - except Exception: + if executor is not None: + nodes = executor.get_nodes() + if nodes is not None: + all_nodes.extend(nodes) + except (AttributeError, RuntimeError, Exception): + # Executor may be in invalid state, skip pass # Signal action servers to stop executing action_servers = [] for node in all_nodes: try: - if isinstance(node, TestActionServer): + if node is not None and isinstance(node, TestActionServer): node._shutdown_requested = True action_servers.append(node) - except Exception: + except (AttributeError, RuntimeError, Exception): pass # Wait for actions to complete gracefully (with timeout as fallback) if action_servers: for server in action_servers: - server._all_actions_complete.wait(timeout=0.15) + try: + if server is not None and hasattr(server, "_all_actions_complete"): + server._all_actions_complete.wait(timeout=0.15) + except (AttributeError, RuntimeError, Exception): + pass # Cancel all timers BEFORE shutting down executors # This prevents executors from trying to call timers after they're canceled for node in all_nodes: try: - # Try to access and cancel timers through node's internal structure - if hasattr(node, "_timers"): - timers = node._timers - if isinstance(timers, dict): - for timer in list(timers.values()): - try: - timer.cancel() - except Exception: - pass - except Exception: + if node is not None: + # Try to access and cancel timers through node's internal structure + if hasattr(node, "_timers"): + timers = node._timers + if timers is not None and isinstance(timers, dict): + for timer in list(timers.values()): + try: + if timer is not None: + timer.cancel() + except (AttributeError, RuntimeError, Exception): + pass + except (AttributeError, RuntimeError, Exception): pass # Give executor a moment to process timer cancellations and remove them from wait list - time.sleep(0.1) + # Use shorter sleep to reduce chance of accessing invalid state + try: + time.sleep(0.05) + except Exception: + pass # Now shutdown executors to stop spinning threads for executor in executors: try: - executor.shutdown() - except Exception as e: - print(f"Error shutting down executor: {e}") + if executor is not None: + executor.shutdown() + except (AttributeError, RuntimeError, Exception): + # Executor may already be shut down or in invalid state + pass # Wait for threads to actually finish (shutdown() is async) for thread in threads: - thread.join(timeout=2.0) + try: + if thread is not None and thread.is_alive(): + thread.join(timeout=2.0) + except (AttributeError, RuntimeError, Exception): + pass # Clean up any remaining nodes (after executors are shut down and threads joined) + # Only destroy nodes that haven't been destroyed yet for node in all_nodes: try: - node.destroy_node() - except Exception as e: - print(f"Error destroying node: {e}") + if node is not None: + # Check if node is still valid before destroying + # In ROS2 Humble, accessing destroyed nodes can cause segfaults + try: + # Try a safe operation to check if node is still valid + _ = node.get_name() + node.destroy_node() + except (AttributeError, RuntimeError): + # Node already destroyed or invalid, skip + pass + except (AttributeError, RuntimeError, Exception): + pass def _create_time_with_to_msg_class(): From 73ab353c9cebe29df110f7261a6aa17f2a76fea1 Mon Sep 17 00:00:00 2001 From: juliaj Date: Mon, 26 Jan 2026 09:43:18 -0800 Subject: [PATCH 6/7] Update the fix based on GetResultService.Response not NavigateToPose.Result --- .../tools/ros2/navigation/nav2_blocking.py | 72 ++++++++--------- tests/tools/ros2/test_nav2_tools.py | 80 ++++++++++++------- 2 files changed, 82 insertions(+), 70 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 3c325cba9..841e13c6b 100644 --- a/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py +++ b/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py @@ -14,6 +14,7 @@ from typing import Type +from action_msgs.msg import GoalStatus from geometry_msgs.msg import PoseStamped, Quaternion from nav2_msgs.action import NavigateToPose from pydantic import BaseModel, Field @@ -23,47 +24,37 @@ from rai.tools.ros2.base import BaseROS2Tool -def _get_error_code_string(error_code: int) -> str: - """Convert NavigateToPose error code to human-readable string.""" - error_code_map = { - 0: "NONE", - 1: "UNKNOWN", - 2: "TIMEOUT", - 3: "CANCELED", - 4: "FAILED", - 5: "INVALID_POSE", - 6: "PLANNER_FAILED", - 7: "CONTROLLER_FAILED", - 8: "RECOVERY_FAILED", - } - return error_code_map.get(error_code, f"UNKNOWN_ERROR_CODE_{error_code}") +def _get_status_string(status: int) -> str: + """Convert GoalStatus to human-readable string. + Reference: https://github.com/ros2/rcl_interfaces/blob/rolling/action_msgs/msg/GoalStatus.msg + """ + status_map = { + GoalStatus.STATUS_UNKNOWN: "UNKNOWN", + GoalStatus.STATUS_ACCEPTED: "ACCEPTED", + GoalStatus.STATUS_EXECUTING: "EXECUTING", + GoalStatus.STATUS_CANCELING: "CANCELING", + GoalStatus.STATUS_SUCCEEDED: "SUCCEEDED", + GoalStatus.STATUS_CANCELED: "CANCELED", + GoalStatus.STATUS_ABORTED: "ABORTED", + } + return status_map.get(status, f"UNKNOWN_STATUS_{status}") -def _get_error_message(result) -> str: - """Extract error message from NavigateToPose result.""" - error_parts = [] - # Get error code string for context - error_code_str = _get_error_code_string(result.error_code) - error_parts.append(f"({error_code_str})") +def _get_error_message(result_response) -> str: + """Extract error message from NavigateToPose result response. - # Check for additional error message fields (if they exist) - # These may vary between ROS2 versions, so we check safely - if hasattr(result, "error_message") and result.error_message: - error_parts.append(f"Error message: {result.error_message}") - elif hasattr(result, "error_msg") and result.error_msg: - error_parts.append(f"Error message: {result.error_msg}") - elif hasattr(result, "message") and result.message: - error_parts.append(f"Message: {result.message}") + Args: + result_response: GetResultService.Response containing status and result fields + - status (int8): GoalStatus value (how action ended) + - result.error_msg (string): Nav2 error details (why it failed) + """ + status_str = _get_status_string(result_response.status) - # Include full result string representation for debugging - result_str = str(result) - if result_str and result_str != str(result.error_code): - # Only add if it provides additional info beyond just the error code - if len(result_str) > 50: # Only if it's substantial - error_parts.append(f"Full result: {result_str}") + if result_response.result.error_msg: + return f"Status: {status_str}. {result_response.result.error_msg}" - return ". ".join(error_parts) + return f"Status: {status_str}" class GetCurrentPoseToolInput(BaseModel): @@ -124,13 +115,14 @@ def _run(self, x: float, y: float, z: float, yaw: float) -> str: goal = NavigateToPose.Goal() goal.pose = pose - result = action_client.send_goal(goal) + result_response = action_client.send_goal(goal) - if result is None: + if result_response is None: return "Navigate to pose action failed. Please try again." - if result.error_code != 0: - error_msg = _get_error_message(result) - return f"Navigate to pose action failed. Error code: {result.error_code}. {error_msg}" + # Check goal status (SUCCEEDED = 4) + if result_response.status != GoalStatus.STATUS_SUCCEEDED: + error_msg = _get_error_message(result_response) + return f"Navigate to pose action failed. {error_msg}" return "Navigate to pose successful." diff --git a/tests/tools/ros2/test_nav2_tools.py b/tests/tools/ros2/test_nav2_tools.py index f99364f8f..f9a2a0698 100644 --- a/tests/tools/ros2/test_nav2_tools.py +++ b/tests/tools/ros2/test_nav2_tools.py @@ -29,8 +29,8 @@ from rai.tools.ros2.navigation.nav2_blocking import ( GetCurrentPoseTool, NavigateToPoseBlockingTool, - _get_error_code_string, _get_error_message, + _get_status_string, ) from tests.communication.ros2.helpers import ( @@ -61,35 +61,52 @@ def test_get_current_pose_tool(ros_setup: None, request: pytest.FixtureRequest) assert result == str(transform) -def test_get_error_code_string() -> None: - """Test error code to string conversion.""" - assert _get_error_code_string(0) == "NONE" - assert _get_error_code_string(2) == "TIMEOUT" - assert _get_error_code_string(6) == "PLANNER_FAILED" - assert _get_error_code_string(99) == "UNKNOWN_ERROR_CODE_99" +def test_get_status_string() -> None: + """Test GoalStatus to string conversion.""" + from action_msgs.msg import GoalStatus + + assert _get_status_string(GoalStatus.STATUS_UNKNOWN) == "UNKNOWN" + assert _get_status_string(GoalStatus.STATUS_SUCCEEDED) == "SUCCEEDED" + assert _get_status_string(GoalStatus.STATUS_ABORTED) == "ABORTED" + assert _get_status_string(GoalStatus.STATUS_CANCELED) == "CANCELED" + assert _get_status_string(999) == "UNKNOWN_STATUS_999" def test_get_error_message() -> None: - """Test error message extraction with error code and optional message.""" - # Test with error code only - result = MagicMock() - result.error_code = 2 - error_msg = _get_error_message(result) - assert "(TIMEOUT)" in error_msg - - # Test with error message field + """Test error message extraction with status and error_msg.""" + from action_msgs.msg import GoalStatus + + # Test with ABORTED status and no error message + result_response = MagicMock() + result_response.status = GoalStatus.STATUS_ABORTED + result_response.result = MagicMock() + result_response.result.error_msg = "" + error_msg = _get_error_message(result_response) + assert error_msg == "Status: ABORTED" + + # Test with ABORTED status and error message result_with_msg = MagicMock() - result_with_msg.error_code = 6 - result_with_msg.error_message = "Planner failed to find path" + result_with_msg.status = GoalStatus.STATUS_ABORTED + result_with_msg.result = MagicMock() + result_with_msg.result.error_msg = "Planner failed to find path" error_msg = _get_error_message(result_with_msg) - assert "(PLANNER_FAILED)" in error_msg - assert "Error message: Planner failed to find path" in error_msg + assert error_msg == "Status: ABORTED. Planner failed to find path" + + # Test with CANCELED status + canceled_response = MagicMock() + canceled_response.status = GoalStatus.STATUS_CANCELED + canceled_response.result = MagicMock() + canceled_response.result.error_msg = "" + error_msg = _get_error_message(canceled_response) + assert error_msg == "Status: CANCELED" def test_navigate_to_pose_blocking_tool( ros_setup: None, request: pytest.FixtureRequest ) -> None: """Test navigation tool success, error, and None result scenarios.""" + from action_msgs.msg import GoalStatus + connector = create_mock_connector_with_clock() tool = NavigateToPoseBlockingTool(connector=connector) @@ -101,29 +118,32 @@ def test_navigate_to_pose_blocking_tool( # Test success success_result = MagicMock() - success_result.error_code = 0 + success_result.status = GoalStatus.STATUS_SUCCEEDED + success_result.result = MagicMock() + success_result.result.error_msg = "" mock_action_client.send_goal.return_value = success_result result = tool._run(x=1.0, y=2.0, z=0.0, yaw=0.5) assert result == "Navigate to pose successful." - # Test error with error code + # Test error without message error_result = MagicMock() - error_result.error_code = 2 + error_result.status = GoalStatus.STATUS_ABORTED + error_result.result = MagicMock() + error_result.result.error_msg = "" mock_action_client.send_goal.return_value = error_result result = tool._run(x=1.0, y=2.0, z=0.0, yaw=0.5) - assert "Navigate to pose action failed" in result - assert "Error code: 2" in result - assert "(TIMEOUT)" in result + assert result == "Navigate to pose action failed. Status: ABORTED" # Test error with message error_result_with_msg = MagicMock() - error_result_with_msg.error_code = 6 - error_result_with_msg.error_message = "Planner failed" + error_result_with_msg.status = GoalStatus.STATUS_ABORTED + error_result_with_msg.result = MagicMock() + error_result_with_msg.result.error_msg = "Planner failed" mock_action_client.send_goal.return_value = error_result_with_msg result = tool._run(x=1.0, y=2.0, z=0.0, yaw=0.5) - assert "Error code: 6" in result - assert "(PLANNER_FAILED)" in result - assert "Error message: Planner failed" in result + assert ( + result == "Navigate to pose action failed. Status: ABORTED. Planner failed" + ) # Test None result mock_action_client.send_goal.return_value = None From 394154cfcb9646fe679e1b5c9de6cecd1c760ba2 Mon Sep 17 00:00:00 2001 From: juliaj Date: Mon, 26 Jan 2026 10:07:45 -0800 Subject: [PATCH 7/7] Add runtime type checking for returned response --- .../tools/ros2/navigation/nav2_blocking.py | 37 ++++++++++++++++--- 1 file changed, 32 insertions(+), 5 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 841e13c6b..52c0b704f 100644 --- a/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py +++ b/src/rai_core/rai/tools/ros2/navigation/nav2_blocking.py @@ -115,14 +115,41 @@ def _run(self, x: float, y: float, z: float, yaw: float) -> str: goal = NavigateToPose.Goal() goal.pose = pose - result_response = action_client.send_goal(goal) + try: + result_response = action_client.send_goal(goal) + except Exception as e: + return f"Navigate to pose action failed with exception: {type(e).__name__}: {e}" if result_response is None: return "Navigate to pose action failed. Please try again." - # Check goal status (SUCCEEDED = 4) - if result_response.status != GoalStatus.STATUS_SUCCEEDED: - error_msg = _get_error_message(result_response) - return f"Navigate to pose action failed. {error_msg}" + # Validate response structure for debugging + try: + # Check status field (used for success/failure) + if not hasattr(result_response, "status"): + return ( + f"Navigate to pose action failed. Response missing 'status' field. " + f"Response type: {type(result_response).__name__}" + ) + + # Check error_code field (original error source) + if hasattr(result_response, "result") and not hasattr( + result_response.result, "error_code" + ): + return ( + f"Navigate to pose action failed. Result missing 'error_code' field. " + f"Result type: {type(result_response.result).__name__}" + ) + + # Check goal status (SUCCEEDED = 4) + if result_response.status != GoalStatus.STATUS_SUCCEEDED: + error_msg = _get_error_message(result_response) + return f"Navigate to pose action failed. {error_msg}" + + except AttributeError as e: + return ( + f"Navigate to pose action failed. AttributeError: {e}. " + f"Response type: {type(result_response).__name__}" + ) return "Navigate to pose successful."