From f6c92afe5b139148522830c5fd173fcc1c225b79 Mon Sep 17 00:00:00 2001 From: Stefano Dalla Gasperina Date: Sun, 30 Nov 2025 20:26:44 -0600 Subject: [PATCH 1/2] Add MCP resources for ROS metadata and robot specs - Add resources/ros_metadata.py: Resource for getting all ROS metadata (topics, services, nodes, parameters) - Add resources/robot_specs.py: Resource for listing all available robot specifications - Fix rosapi parameters service: Use /rosapi/get_param_names instead of /rosapi/parameters - Add resources/__init__.py: Register all resources with MCP server - Update server.py: Register MCP resources on startup --- resources/__init__.py | 11 +++ resources/robot_specs.py | 48 ++++++++++++ resources/ros_metadata.py | 156 ++++++++++++++++++++++++++++++++++++++ server.py | 3 + 4 files changed, 218 insertions(+) create mode 100644 resources/__init__.py create mode 100644 resources/robot_specs.py create mode 100644 resources/ros_metadata.py diff --git a/resources/__init__.py b/resources/__init__.py new file mode 100644 index 0000000..88a67dd --- /dev/null +++ b/resources/__init__.py @@ -0,0 +1,11 @@ +# Resources package for ROS MCP Server +# Functions to register resources with the MCP server instance +from resources.robot_specs import register_robot_spec_resources +from resources.ros_metadata import register_ros_metadata_resources + + +def register_all_resources(mcp, ws_manager): + """Register all resources with the MCP server instance.""" + register_robot_spec_resources(mcp) + register_ros_metadata_resources(mcp, ws_manager) + diff --git a/resources/robot_specs.py b/resources/robot_specs.py new file mode 100644 index 0000000..6268252 --- /dev/null +++ b/resources/robot_specs.py @@ -0,0 +1,48 @@ +"""Resources for robot specifications.""" + +import json +from pathlib import Path + + +def register_robot_spec_resources(mcp): + """Register robot specification resources with the MCP server.""" + + # Get the robot_specifications directory path + specs_dir = Path(__file__).parent.parent / "robot_specifications" + + @mcp.resource("ros-mcp://robot-specs/get_all_robots") + def get_all_robots() -> str: + """ + Get all available robot specifications. + + Returns: + str: JSON string with list of available robot names + """ + try: + if not specs_dir.exists(): + return json.dumps({ + "error": f"Robot specifications directory not found: {specs_dir}", + "robot_specifications": [] + }) + + # Find all YAML files + yaml_files = list(specs_dir.glob("*.yaml")) + + # Filter out template files only + robot_names = [ + file.stem for file in yaml_files + if not file.stem.startswith("YOUR_") + ] + robot_names.sort() + + return json.dumps({ + "robot_specifications": robot_names, + "count": len(robot_names) + }) + + except Exception as e: + return json.dumps({ + "error": f"Failed to list robot specifications: {str(e)}", + "robot_specifications": [] + }) + diff --git a/resources/ros_metadata.py b/resources/ros_metadata.py new file mode 100644 index 0000000..7b1bdd2 --- /dev/null +++ b/resources/ros_metadata.py @@ -0,0 +1,156 @@ +"""Resources for ROS metadata and discovery information.""" + +import json + +from utils.websocket_manager import WebSocketManager + + +def register_ros_metadata_resources(mcp, ws_manager: WebSocketManager): + """Register ROS metadata resources with the MCP server.""" + + @mcp.resource("ros-mcp://ros-metadata/all") + def get_all_ros_metadata() -> str: + """ + Get all ROS metadata including topics, services, nodes, and parameters. + + Returns: + str: JSON string with comprehensive ROS system information + """ + try: + metadata = { + "topics": [], + "services": [], + "nodes": [], + "parameters": [], + "ros_version": None, + "errors": [] + } + + # Get ROS version + try: + ros2_request = { + "op": "call_service", + "id": "ros2_version_check", + "service": "/rosapi/get_ros_version", + "args": {}, + } + with ws_manager: + response = ws_manager.request(ros2_request) + values = response.get("values") if response else None + if isinstance(values, dict) and "version" in values: + metadata["ros_version"] = { + "version": values.get("version"), + "distro": values.get("distro") + } + else: + # Try ROS1 + ros1_request = { + "op": "call_service", + "id": "ros1_distro_check", + "service": "/rosapi/get_param", + "args": {"name": "/rosdistro"}, + } + response = ws_manager.request(ros1_request) + value = response.get("values") if response else None + if value: + distro = value.get("value") if isinstance(value, dict) else value + distro_clean = str(distro).strip('"').replace("\\n", "").replace("\n", "") + metadata["ros_version"] = {"version": "1", "distro": distro_clean} + except Exception as e: + metadata["errors"].append(f"Failed to get ROS version: {str(e)}") + + # Get topics + try: + topics_message = { + "op": "call_service", + "service": "/rosapi/topics", + "type": "rosapi/Topics", + "args": {}, + "id": "get_topics_request", + } + with ws_manager: + response = ws_manager.request(topics_message) + if response and "values" in response: + topics = response["values"].get("topics", []) + types = response["values"].get("types", []) + metadata["topics"] = [ + {"name": topic, "type": topic_type} + for topic, topic_type in zip(topics, types) + ] + except Exception as e: + metadata["errors"].append(f"Failed to get topics: {str(e)}") + + # Get services + try: + services_message = { + "op": "call_service", + "service": "/rosapi/services", + "type": "rosapi/Services", + "args": {}, + "id": "get_services_request", + } + with ws_manager: + response = ws_manager.request(services_message) + if response and "values" in response: + services = response["values"].get("services", []) + types = response["values"].get("types", []) + metadata["services"] = [ + {"name": service, "type": service_type} + for service, service_type in zip(services, types) + ] + except Exception as e: + metadata["errors"].append(f"Failed to get services: {str(e)}") + + # Get nodes + try: + nodes_message = { + "op": "call_service", + "service": "/rosapi/nodes", + "type": "rosapi/Nodes", + "args": {}, + "id": "get_nodes_request", + } + with ws_manager: + response = ws_manager.request(nodes_message) + if response and "values" in response: + metadata["nodes"] = response["values"].get("nodes", []) + except Exception as e: + metadata["errors"].append(f"Failed to get nodes: {str(e)}") + + # Get parameters (ROS 2 only) + try: + params_message = { + "op": "call_service", + "service": "/rosapi/get_param_names", + "type": "rosapi/GetParamNames", + "args": {}, + "id": "get_parameters_request", + } + with ws_manager: + response = ws_manager.request(params_message) + if response and "values" in response: + metadata["parameters"] = response["values"].get("names", []) + except Exception as e: + # Parameters might not be available in ROS1 or if service doesn't exist + pass + + # Add summary counts + metadata["summary"] = { + "total_topics": len(metadata["topics"]), + "total_services": len(metadata["services"]), + "total_nodes": len(metadata["nodes"]), + "total_parameters": len(metadata["parameters"]), + "has_errors": len(metadata["errors"]) > 0 + } + + return json.dumps(metadata, indent=2) + + except Exception as e: + return json.dumps({ + "error": f"Failed to get ROS metadata: {str(e)}", + "topics": [], + "services": [], + "nodes": [], + "parameters": [] + }) + diff --git a/server.py b/server.py index 4106566..10e38b7 100644 --- a/server.py +++ b/server.py @@ -41,6 +41,9 @@ ROSBRIDGE_IP, ROSBRIDGE_PORT, default_timeout=5.0 ) # Increased default timeout for ROS operations +# Register MCP resources +from resources import register_all_resources +register_all_resources(mcp, ws_manager) def convert_expects_image_hint(expects_image: str) -> bool | None: """ From eeca67d181784251ed8f32993c9776f6214e2428 Mon Sep 17 00:00:00 2001 From: Stefano Dalla Gasperina Date: Sun, 30 Nov 2025 20:37:36 -0600 Subject: [PATCH 2/2] ruff format and check --- resources/__init__.py | 1 - resources/robot_specs.py | 45 +++++++++++++++++------------------ resources/ros_metadata.py | 49 +++++++++++++++++++++------------------ server.py | 5 ++-- 4 files changed, 50 insertions(+), 50 deletions(-) diff --git a/resources/__init__.py b/resources/__init__.py index 88a67dd..54a39f1 100644 --- a/resources/__init__.py +++ b/resources/__init__.py @@ -8,4 +8,3 @@ def register_all_resources(mcp, ws_manager): """Register all resources with the MCP server instance.""" register_robot_spec_resources(mcp) register_ros_metadata_resources(mcp, ws_manager) - diff --git a/resources/robot_specs.py b/resources/robot_specs.py index 6268252..8bd1735 100644 --- a/resources/robot_specs.py +++ b/resources/robot_specs.py @@ -6,43 +6,40 @@ def register_robot_spec_resources(mcp): """Register robot specification resources with the MCP server.""" - + # Get the robot_specifications directory path specs_dir = Path(__file__).parent.parent / "robot_specifications" - + @mcp.resource("ros-mcp://robot-specs/get_all_robots") def get_all_robots() -> str: """ Get all available robot specifications. - + Returns: str: JSON string with list of available robot names """ try: if not specs_dir.exists(): - return json.dumps({ - "error": f"Robot specifications directory not found: {specs_dir}", - "robot_specifications": [] - }) - + return json.dumps( + { + "error": f"Robot specifications directory not found: {specs_dir}", + "robot_specifications": [], + } + ) + # Find all YAML files yaml_files = list(specs_dir.glob("*.yaml")) - + # Filter out template files only - robot_names = [ - file.stem for file in yaml_files - if not file.stem.startswith("YOUR_") - ] + robot_names = [file.stem for file in yaml_files if not file.stem.startswith("YOUR_")] robot_names.sort() - - return json.dumps({ - "robot_specifications": robot_names, - "count": len(robot_names) - }) - - except Exception as e: - return json.dumps({ - "error": f"Failed to list robot specifications: {str(e)}", - "robot_specifications": [] - }) + return json.dumps({"robot_specifications": robot_names, "count": len(robot_names)}) + + except Exception as e: + return json.dumps( + { + "error": f"Failed to list robot specifications: {str(e)}", + "robot_specifications": [], + } + ) diff --git a/resources/ros_metadata.py b/resources/ros_metadata.py index 7b1bdd2..ebf5cca 100644 --- a/resources/ros_metadata.py +++ b/resources/ros_metadata.py @@ -7,12 +7,12 @@ def register_ros_metadata_resources(mcp, ws_manager: WebSocketManager): """Register ROS metadata resources with the MCP server.""" - + @mcp.resource("ros-mcp://ros-metadata/all") def get_all_ros_metadata() -> str: """ Get all ROS metadata including topics, services, nodes, and parameters. - + Returns: str: JSON string with comprehensive ROS system information """ @@ -23,9 +23,9 @@ def get_all_ros_metadata() -> str: "nodes": [], "parameters": [], "ros_version": None, - "errors": [] + "errors": [], } - + # Get ROS version try: ros2_request = { @@ -40,7 +40,7 @@ def get_all_ros_metadata() -> str: if isinstance(values, dict) and "version" in values: metadata["ros_version"] = { "version": values.get("version"), - "distro": values.get("distro") + "distro": values.get("distro"), } else: # Try ROS1 @@ -54,11 +54,13 @@ def get_all_ros_metadata() -> str: value = response.get("values") if response else None if value: distro = value.get("value") if isinstance(value, dict) else value - distro_clean = str(distro).strip('"').replace("\\n", "").replace("\n", "") + distro_clean = ( + str(distro).strip('"').replace("\\n", "").replace("\n", "") + ) metadata["ros_version"] = {"version": "1", "distro": distro_clean} except Exception as e: metadata["errors"].append(f"Failed to get ROS version: {str(e)}") - + # Get topics try: topics_message = { @@ -79,7 +81,7 @@ def get_all_ros_metadata() -> str: ] except Exception as e: metadata["errors"].append(f"Failed to get topics: {str(e)}") - + # Get services try: services_message = { @@ -100,7 +102,7 @@ def get_all_ros_metadata() -> str: ] except Exception as e: metadata["errors"].append(f"Failed to get services: {str(e)}") - + # Get nodes try: nodes_message = { @@ -116,7 +118,7 @@ def get_all_ros_metadata() -> str: metadata["nodes"] = response["values"].get("nodes", []) except Exception as e: metadata["errors"].append(f"Failed to get nodes: {str(e)}") - + # Get parameters (ROS 2 only) try: params_message = { @@ -130,27 +132,28 @@ def get_all_ros_metadata() -> str: response = ws_manager.request(params_message) if response and "values" in response: metadata["parameters"] = response["values"].get("names", []) - except Exception as e: + except Exception: # Parameters might not be available in ROS1 or if service doesn't exist pass - + # Add summary counts metadata["summary"] = { "total_topics": len(metadata["topics"]), "total_services": len(metadata["services"]), "total_nodes": len(metadata["nodes"]), "total_parameters": len(metadata["parameters"]), - "has_errors": len(metadata["errors"]) > 0 + "has_errors": len(metadata["errors"]) > 0, } - + return json.dumps(metadata, indent=2) - - except Exception as e: - return json.dumps({ - "error": f"Failed to get ROS metadata: {str(e)}", - "topics": [], - "services": [], - "nodes": [], - "parameters": [] - }) + except Exception as e: + return json.dumps( + { + "error": f"Failed to get ROS metadata: {str(e)}", + "topics": [], + "services": [], + "nodes": [], + "parameters": [], + } + ) diff --git a/server.py b/server.py index 10e38b7..5d8c42d 100644 --- a/server.py +++ b/server.py @@ -12,6 +12,7 @@ from fastmcp.utilities.types import Image from PIL import Image as PILImage +from resources import register_all_resources from utils.config_utils import get_verified_robot_spec_util, get_verified_robots_list_util from utils.network_utils import ping_ip_and_port from utils.websocket_manager import WebSocketManager, parse_input @@ -41,10 +42,10 @@ ROSBRIDGE_IP, ROSBRIDGE_PORT, default_timeout=5.0 ) # Increased default timeout for ROS operations -# Register MCP resources -from resources import register_all_resources + register_all_resources(mcp, ws_manager) + def convert_expects_image_hint(expects_image: str) -> bool | None: """ Convert string-based expects_image hint to boolean for internal use.