Rosbridge MCP Server

""" Publish Action Tool Sends goals to ROS action servers via rosbridge. """ import asyncio from typing import Any import uuid import roslibpy import roslibpy.actionlib from mcp.types import Tool PUBLISH_ACTION_TOOL = Tool( name="publish_action", description="Send a goal to a ROS action server", inputSchema={ "type": "object", "properties": { "action_name": { "type": "string", "description": "The ROS action server name (e.g., '/move_base')", }, "action_type": { "type": "string", "description": "The ROS action type (e.g., 'move_base_msgs/MoveBaseAction')", }, "goal": { "type": "object", "description": "The goal data as a JSON object", }, "timeout": { "type": "number", "description": "Timeout in seconds to wait for result (default: 30)", "default": 30, }, }, "required": ["action_name", "action_type", "goal"], }, ) async def publish_action(ros: roslibpy.Ros, action_name: str, action_type: str, goal: dict[str, Any], timeout: float = 30.0) -> dict[str, Any]: """ Send a goal to a ROS action server. Args: ros: The ROS connection instance action_name: The ROS action server name action_type: The ROS action type goal: The goal data as a dictionary timeout: Timeout in seconds to wait for result Returns: A dictionary containing the result or error information """ try: # Create action client action_client = roslibpy.actionlib.ActionClient(ros, action_name, action_type) # Generate a unique goal ID goal_id = str(uuid.uuid4()) # Create futures for tracking the action result_future = asyncio.Future() feedback_messages = [] def handle_result(result): result_future.set_result({"success": True, "result": result, "goal_id": goal_id, "feedback": feedback_messages}) def handle_feedback(feedback): feedback_messages.append(feedback) def handle_failure(error): result_future.set_result({"success": False, "error": str(error), "goal_id": goal_id, "feedback": feedback_messages}) # Create goal message with roslibpy.Message goal_msg = roslibpy.Message(goal) # Create and configure the goal goal_handle = roslibpy.actionlib.Goal(action_client, goal_msg) goal_handle.on("result", handle_result) goal_handle.on("feedback", handle_feedback) goal_handle.on("status", lambda status: None) # Status updates goal_handle.on("timeout", lambda: handle_failure("Goal timeout")) # Send goal and wait for result goal_handle.send() try: result = await asyncio.wait_for(result_future, timeout=timeout) return result except asyncio.TimeoutError: # Cancel the goal on timeout goal_handle.cancel() return {"success": False, "error": f"Timeout waiting for action result after {timeout} seconds", "goal_id": goal_id, "feedback": feedback_messages} finally: # Clean up the action client action_client.dispose() except Exception as e: return {"success": False, "error": f"Failed to send action goal: {str(e)}"}