diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 62913fe..16eafd5 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -16,6 +16,7 @@ Unreleased **Changed** **Fixed** +* Fixed KeyError in ROS2 ActionClient when receiving action results with different message formats from rosbridge. **Deprecated** diff --git a/src/roslibpy/comm/comm.py b/src/roslibpy/comm/comm.py index 8a33453..0ba45a7 100644 --- a/src/roslibpy/comm/comm.py +++ b/src/roslibpy/comm/comm.py @@ -138,12 +138,12 @@ def send_ros_action_goal(self, message, resultback, feedback, errback): def _handle_action_request(self, message): if "action" not in message: raise ValueError("Expected action name missing in action request") - raise RosBridgeException('Action server capabilities not yet implemented') + raise RosBridgeException("Action server capabilities not yet implemented") def _handle_action_cancel(self, message): if "action" not in message: raise ValueError("Expected action name missing in action request") - raise RosBridgeException('Action server capabilities not yet implemented') + raise RosBridgeException("Action server capabilities not yet implemented") def _handle_action_feedback(self, message): if "action" not in message: @@ -160,12 +160,21 @@ def _handle_action_result(self, message): if not action_handlers: raise RosBridgeException('No handler registered for action request ID: "%s"' % request_id) - resultback, _ , errback = action_handlers + resultback, _, errback = action_handlers del self._pending_action_requests[request_id] - LOGGER.debug("Received Action result with status: %s", message["status"]) + # Handle different message formats for status field + # ROS2 rosbridge may send status at top level or inside values + status = message.get("status") + if status is None and "values" in message: + status = message["values"].get("status") + if status is None: + # Default to UNKNOWN if status is not found + status = ActionGoalStatus.UNKNOWN.value - results = {"status": ActionGoalStatus(message["status"]).name, "values": message["values"]} + LOGGER.debug("Received Action result with status: %s", status) + + results = {"status": ActionGoalStatus(status).name, "values": message.get("values", {})} if "result" in message and message["result"] is False: if errback: diff --git a/tests/ros2/test_action_client.py b/tests/ros2/test_action_client.py new file mode 100644 index 0000000..bb35f17 --- /dev/null +++ b/tests/ros2/test_action_client.py @@ -0,0 +1,141 @@ +"""Tests for ROS2 ActionClient message handling.""" + +from unittest.mock import Mock + +from roslibpy.comm.comm import RosBridgeProtocol +from roslibpy.core import ActionGoalStatus, ActionResult + + +def test_action_result_with_status_at_top_level(): + """Test handling of action results with status at the top level of the message.""" + protocol = RosBridgeProtocol() + protocol.factory = Mock() + protocol.send_message = Mock() + + result_callback = Mock() + feedback_callback = Mock() + error_callback = Mock() + + request_id = "send_action_goal:/test_action:1" + protocol._pending_action_requests[request_id] = (result_callback, feedback_callback, error_callback) + + # ROS2 rosbridge message format with status at top level + message = { + "op": "action_result", + "action": "/test_action", + "id": request_id, + "status": 4, # SUCCEEDED + "values": {"result": {"success": True, "message": "Action completed"}}, + "result": True, + } + + protocol._handle_action_result(message) + + assert result_callback.called + result = result_callback.call_args[0][0] + assert isinstance(result, ActionResult) + assert result["status"] == ActionGoalStatus.SUCCEEDED.name + assert result["values"] == message["values"] + + +def test_action_result_with_status_in_values(): + """Test handling of action results with status inside the values field. + + This reproduces the KeyError issue reported in GitHub issue. + """ + protocol = RosBridgeProtocol() + protocol.factory = Mock() + protocol.send_message = Mock() + + result_callback = Mock() + feedback_callback = Mock() + error_callback = Mock() + + request_id = "send_action_goal:/test_action:2" + protocol._pending_action_requests[request_id] = (result_callback, feedback_callback, error_callback) + + # Alternative message format with status inside values + message = { + "op": "action_result", + "action": "/test_action", + "id": request_id, + "values": { + "status": 4, # SUCCEEDED - status is here instead + "result": {"success": True, "message": "Action completed"}, + }, + "result": True, + } + + # This should not raise KeyError + protocol._handle_action_result(message) + + assert result_callback.called + result = result_callback.call_args[0][0] + assert isinstance(result, ActionResult) + assert result["status"] == ActionGoalStatus.SUCCEEDED.name + + +def test_action_result_failure_with_status_at_top_level(): + """Test handling of failed action results.""" + protocol = RosBridgeProtocol() + protocol.factory = Mock() + protocol.send_message = Mock() + + result_callback = Mock() + feedback_callback = Mock() + error_callback = Mock() + + request_id = "send_action_goal:/test_action:3" + protocol._pending_action_requests[request_id] = (result_callback, feedback_callback, error_callback) + + message = { + "op": "action_result", + "action": "/test_action", + "id": request_id, + "status": 6, # ABORTED + "values": {"result": {"success": False, "message": "Action failed"}}, + "result": False, + } + + protocol._handle_action_result(message) + + assert error_callback.called + result = error_callback.call_args[0][0] + assert result["status"] == ActionGoalStatus.ABORTED.name + assert result["values"] == message["values"] + + +def test_action_result_without_status_field(): + """Test handling of action results when status field is missing entirely. + + This is a defensive test for cases where neither top-level nor values contain status. + In such cases, we should use a default status. + """ + protocol = RosBridgeProtocol() + protocol.factory = Mock() + protocol.send_message = Mock() + + result_callback = Mock() + feedback_callback = Mock() + error_callback = Mock() + + request_id = "send_action_goal:/test_action:4" + protocol._pending_action_requests[request_id] = (result_callback, feedback_callback, error_callback) + + # Message without status anywhere + message = { + "op": "action_result", + "action": "/test_action", + "id": request_id, + "values": {"result": {"success": True}}, + "result": True, + } + + # Should not raise KeyError, should use default status + protocol._handle_action_result(message) + + assert result_callback.called + result = result_callback.call_args[0][0] + assert isinstance(result, ActionResult) + # Should have some status value (likely UNKNOWN) + assert "status" in result