From 08d5d82d50dd05aa375c3a745938adb55ac018f7 Mon Sep 17 00:00:00 2001 From: Eddy Zhou Date: Thu, 23 Oct 2025 17:05:12 +0000 Subject: [PATCH 1/2] checkpoint, in case of dangerous --- ARCHITECTURE.md | 890 +++++++++++++++++++++++++++++++++++++++++++++ PHASE3_4_STATUS.md | 296 +++++++++++++++ 2 files changed, 1186 insertions(+) create mode 100644 ARCHITECTURE.md create mode 100644 PHASE3_4_STATUS.md diff --git a/ARCHITECTURE.md b/ARCHITECTURE.md new file mode 100644 index 0000000..a3886b5 --- /dev/null +++ b/ARCHITECTURE.md @@ -0,0 +1,890 @@ +# robot_mcp Architecture & Implementation Plan + +**Last Updated:** 2025-10-23 + +--- + +## Table of Contents + +1. [Overview](#overview) +2. [Terminology](#terminology) +3. [Repository Structure](#repository-structure) +4. [Core Architecture](#core-architecture) +5. [Component Breakdown](#component-breakdown) +6. [Implementation Plan](#implementation-plan) +7. [Design Decisions](#design-decisions) + +--- + +## Overview + +`robot_mcp` is a Model Context Protocol (MCP) server for ROS2 robots that enables AI assistants (like Claude) to control robots through natural language via HTTP. + +**Key Features:** +- Configuration-driven ROS2 HTTP server +- Pure C++ implementation using `rclcpp_lifecycle` +- HTTP-based (not stdio) for network accessibility +- **Generic plugin system** - 3 plugins support ALL message types +- Runtime type introspection for automatic JSON conversion +- Resource conflict management for concurrency control +- Multi-distro ROS2 support + +--- + +## Terminology + +**Primitive**: A configured MCP-ROS2 connection (logical concept) +- Defined in YAML config with name, ROS2 resource, type, and plugin +- Each primitive becomes one MCP tool or resource +- Example: `{name: "battery", topic: "/battery_state", msg_type: "sensor_msgs/msg/BatteryState", plugin: "BasicTopicPlugin"}` + +**Plugin**: Backend implementation class (C++ code) +- Example: `BasicTopicPlugin`, `BasicServicePlugin`, `BasicActionPlugin` +- One plugin class can power many primitives +- Loaded via pluginlib from .so files +- Generic plugins support ALL message types via runtime introspection + +**Plugin Instance**: Runtime object (one per primitive) +- Each primitive needs its own plugin instance for state (subscriptions, cache, etc.) +- Example: 3 topics using BasicTopicPlugin → 3 separate plugin instances +- Managed by `MCPPrimitiveRegistry` + +--- + +## Repository Structure + +``` +robot_mcp/ +├── robot_mcp_server/ # Main HTTP MCP server (C++) +| ├── include/robot_mcp_server/ +| │ ├── mcp_http_server/ # HTTP server layer +| │ │ ├── http_server.hpp # HTTPServer class +| │ │ ├── auth_middleware.hpp # API key authentication +| │ │ └── json_rpc_handler.hpp # JSON-RPC 2.0 parsing +| │ ├── mcp_handler/ # MCP protocol handler +| │ │ └── mcp_handler.hpp # MCPHandler functor (initialize, ping, tools/*, resources/*) +| │ ├── mcp_router/ # Request routing +| │ │ ├── mcp_router.hpp # MCPRouter class +| │ │ └── operation_tracker.hpp # Operation ID tracking for actions +| │ ├── mcp_primitive/ # Plugin infrastructure +| │ │ ├── mcp_primitive_factory.hpp # Creates registry from config +| │ │ ├── mcp_primitive_registry.hpp # Owns plugin instances +| │ │ └── mcp_primitive_loader.hpp # Pluginlib wrapper +| │ ├── mcp_config/ # Configuration +| │ │ ├── config_types.hpp # Struct definitions +| │ │ └── config_parser.hpp # ROS2 parameter parsing +| │ └── robot_mcp_server_node.hpp # Main lifecycle node +│ ├── src/ +│ ├── test/ # Catch2 unit tests + launch tests +│ ├── package.xml +│ └── CMakeLists.txt +│ +├── robot_mcp_msg_pluginlib/ # Base plugin interface (CORE) +│ ├── include/robot_mcp_msg_pluginlib/ +│ │ ├── message_plugin.hpp # Base interface for all plugins +│ │ ├── basic_topic_plugin.hpp # Generic topic plugin (all msg types) +│ │ ├── basic_service_plugin.hpp # Generic service plugin (all srv types) +│ │ ├── basic_action_plugin.hpp # Generic action plugin (all action types) +│ │ ├── message_conversion.hpp # JSON ↔ ROS conversion utilities +│ │ └── visibility_control.h +│ ├── src/ # Implementation of generic plugins +│ ├── plugins.xml # Pluginlib manifest for 3 basic plugins +│ ├── package.xml +│ └── CMakeLists.txt +│ +└── robot_mcp_bringup/ # Launch files and example configs + ├── config/ + │ ├── minimal.yaml # Minimal example config + │ └── presets/ # Example configurations + ├── launch/ + │ └── robot_mcp.launch.yaml # YAML launch file + ├── package.xml + └── CMakeLists.txt +``` + +**Key Structural Notes:** +- **3 Generic Plugins**: `BasicTopicPlugin`, `BasicServicePlugin`, `BasicActionPlugin` in `robot_mcp_msg_pluginlib` +- **No type-specific plugin packages**: Generic plugins use runtime introspection to support all message types +- **Custom plugins**: Users can create custom plugin packages if they need specialized behavior + +--- + +## Core Architecture + +### Component Flow + +``` +┌─────────────────────────────────────────────────────────────┐ +│ HTTPServer (cpp-httplib) │ +│ Handles HTTP requests/responses (httplib::Request/Response) │ +│ ┌────────────────┐ ┌─────────────────┐ │ +│ │ AuthMiddleware │ → │ JSONRPCHandler │ │ +│ │ (API Key) │ │ (HTTP → JSON) │ │ +│ └────────────────┘ └─────────────────┘ │ +└────────────────────────────┬────────────────────────────────┘ + │ nlohmann::json + │ +┌────────────────────────────▼────────────────────────────────┐ +│ MCPHandler (Functor) │ +│ Single class handling all 6 MCP methods: │ +│ - initialize: Return server info │ +│ - ping: Health check │ +│ - tools/list: List available tools (topics/services/actions)│ +│ - tools/call: Execute tool (delegate to router) │ +│ - resources/list: List available resources (topics) │ +│ - resources/read: Read resource (delegate to router) │ +│ │ +│ asFunction() → std::function for HTTPServer │ +└────────────────────────────┬────────────────────────────────┘ + │ Route to appropriate plugin + │ +┌────────────────────────────▼────────────────────────────────┐ +│ MCPRouter │ +│ Routes requests to correct plugin instance │ +│ ┌────────────────────────────────────────────────┐ │ +│ │ OperationTracker (owned by router) │ │ +│ │ - Tracks active action operation IDs │ │ +│ │ - Maps operation_id → plugin instance │ │ +│ └────────────────────────────────────────────────┘ │ +│ │ +│ Non-owning reference to MCPPrimitiveRegistry │ +└────────────────────────────┬────────────────────────────────┘ + │ Lookup plugin by primitive name + │ +┌────────────────────────────▼────────────────────────────────┐ +│ MCPPrimitiveRegistry │ +│ Owns all plugin instances (unique_ptr) │ +│ Maps: primitive name → plugin instance │ +│ │ +│ Primitive 1: "/chatter" → BasicTopicPlugin instance #1 │ +│ Primitive 2: "/battery" → BasicTopicPlugin instance #2 │ +│ Primitive 3: "/my_trigger" → BasicServicePlugin inst #1 │ +│ Primitive 4: "/navigate" → BasicActionPlugin inst #1 │ +└────────────────────────────┬────────────────────────────────┘ + │ Call methods on plugin instances + ┌──────────────────┼──────────────────┐ + │ │ │ + ▼ ▼ ▼ +┌─────────────────┐ ┌─────────────────┐ ┌─────────────────┐ +│ BasicTopicPlugin│ │BasicServicePlugin│ │BasicActionPlugin│ +│ (Generic) │ │ (Generic) │ │ (Generic) │ +│ │ │ │ │ │ +│ - Uses Generic │ │ - Uses Generic │ │ - Uses Generic │ +│ Subscription │ │ Client │ │ Action Client │ +│ - Runtime type │ │ - Runtime type │ │ - Runtime type │ +│ introspection │ │ introspection │ │ introspection │ +│ - JSON ↔ ROS │ │ - JSON ↔ ROS │ │ - JSON ↔ ROS │ +│ conversion │ │ conversion │ │ conversion │ +│ - Caching │ │ │ │ - Op tracking │ +└────────┬────────┘ └────────┬─────────┘ └────────┬────────┘ + │ │ │ + ▼ ▼ ▼ +┌─────────────────┐ ┌──────────────────┐ ┌──────────────────┐ +│ ROS2 Topics │ │ ROS2 Services │ │ ROS2 Actions │ +│ (pub/sub) │ │ (client) │ │ (action client) │ +└─────────────────┘ └──────────────────┘ └──────────────────┘ +``` + +### MCPServerNode Ownership + +```cpp +class MCPServerNode : public rclcpp_lifecycle::LifecycleNode { + std::unique_ptr http_server_; + std::unique_ptr mcp_handler_; + std::unique_ptr mcp_router_; + std::unique_ptr mcp_primitive_registry_; + + // MCPRouter internally owns: + // - OperationTracker operation_tracker_ + + // MCPPrimitiveRegistry internally owns: + // - std::map> plugins_ +}; +``` + +**Key Architectural Principles:** + +1. **Generic Plugins**: 3 plugins support ALL message types via runtime introspection +2. **Thin Router**: Router only routes, doesn't handle ROS2 operations +3. **Functor Pattern**: MCPHandler is a functor with `asFunction()` for clean HTTP integration +4. **Clear Ownership**: Each component owns its dependencies, non-owning references for loose coupling +5. **Data Conversion at Plugin Boundary**: JSON ↔ ROS conversion happens entirely within plugins + +### Plugin System Flow + +**At Startup (Plugin Loading via Pluginlib):** +``` +1. MCPPrimitiveFactory uses pluginlib to discover available plugins +2. Scans packages with pluginlib manifests (plugins.xml): + - robot_mcp_msg_pluginlib::BasicTopicPlugin + - robot_mcp_msg_pluginlib::BasicServicePlugin + - robot_mcp_msg_pluginlib::BasicActionPlugin + - [optional] custom_plugins::MyCustomPlugin +3. Factory.createRegistry(config) instantiates plugin instances per primitive +``` + +**At Configuration (Factory Pattern):** +``` +1. Parse YAML config: + topics: + - name: "battery" + topic: "/battery_state" + msg_type: "sensor_msgs/msg/BatteryState" + plugin: "BasicTopicPlugin" + description: "Robot battery status and charge level" + +2. For each primitive in config: + a. Factory loads plugin class via pluginlib + b. Validates plugin supports msg_type (getSupportedTypes()) + c. Instantiates plugin with primitive config + d. Calls plugin->initialize(node, primitive_config) + e. Plugin creates ROS2 constructs (subscribers, clients, etc.) + +3. Factory returns MCPPrimitiveRegistry with all plugin instances + +4. MCPRouter created with reference to registry +5. MCPHandler created with reference to router +``` + +**At Runtime (Request Handling):** +``` +Example: Claude reads battery status + +1. HTTP POST /mcp → {"method": "resources/read", "params": {"uri": "battery"}} +2. HTTPServer → AuthMiddleware → JSONRPCHandler (HTTP → JSON) +3. MCPHandler receives JSON-RPC request +4. MCPHandler routes to appropriate method handler (resources/read) +5. MCPHandler calls MCPRouter.readResource("battery") +6. MCPRouter looks up "battery" in MCPPrimitiveRegistry +7. MCPRouter calls plugin->readCachedMessage() +8. BasicTopicPlugin returns cached JSON message +9. Response flows back: Plugin → Router → Handler → HTTP +``` + +### Data Flow Example + +**User Request:** "Navigate to pose X" + +``` +1. Claude Desktop → HTTP POST /mcp + {"method": "tools/call", "params": {"name": "navigate", "arguments": {"x": 1.0, "y": 2.0, ...}}} + +2. HTTPServer → AuthMiddleware (validates API key) → JSONRPCHandler (parses JSON-RPC) + +3. MCPHandler receives request, routes to tools/call handler + +4. MCPHandler calls MCPRouter.callTool("navigate", arguments) + +5. MCPRouter: + a. Looks up "navigate" primitive in MCPPrimitiveRegistry + b. Checks resource group conflicts (Phase 5 - deferred for now) + c. Generates operation_id for tracking + +6. MCPRouter calls plugin->sendGoal(arguments, operation_id) + +7. BasicActionPlugin: + a. Validates goal structure (JSON schema check) + b. Converts JSON → ROS2 action goal using runtime introspection + c. Creates/reuses action client for "/navigate_to_pose" + d. Sends goal to action server + e. Stores goal handle with operation_id + +8. MCPRouter's OperationTracker records: operation_id → plugin instance + +9. Response returns to Claude: {"operation_id": "uuid-1234", "status": "pending"} + +10. Claude later queries status: tools/call {"name": "get_status", "arguments": {"operation_id": "uuid-1234"}} + Router → OperationTracker → Plugin → Returns status + feedback + +11. Claude can cancel: tools/call {"name": "cancel", "arguments": {"operation_id": "uuid-1234"}} + Router → OperationTracker → Plugin → Cancels action +``` + +**Key Data Flow Principles:** + +1. **JSON ↔ ROS conversion happens entirely in plugins** - Router never sees ROS messages +2. **Router is thin** - Only routing, lookup, and operation tracking +3. **Plugins are stateful** - Each instance owns ROS2 clients and cached data +4. **Type information flows through config** - msg_type specified in YAML, used for introspection + +**Plugin Base Classes (`robot_mcp_msg_pluginlib/`):** + +Our plugin system uses **3 generic plugins** that support ALL message types via runtime introspection. + +```cpp +namespace robot_mcp_msg_pluginlib { + +// ============================================================================ +// Tag Dispatch for Service/Action Message Parts +// ============================================================================ +namespace message_part { + struct Request {}; // For service requests and action goals + struct Response {}; // For service responses and action results + struct Feedback {}; // For action feedback only +} + +// ============================================================================ +// Base interface - all plugins must implement +// ============================================================================ +class MessagePlugin { +public: + virtual ~MessagePlugin() = default; + + // Initialize plugin with node and primitive config + virtual void initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const config::PrimitiveConfig& config) = 0; + + // Returns empty vector = supports ALL types (generic) + // Returns non-empty = only supports specific types (custom plugins) + virtual std::vector getSupportedTypes() const = 0; + + // Get description for MCP tools/resources list + virtual std::string getDescription() const = 0; +}; + +// ============================================================================ +// BasicTopicPlugin - Generic plugin for ALL topic types +// ============================================================================ +class BasicTopicPlugin : public MessagePlugin { +public: + // Initialization + void initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const config::TopicConfig& config) override; + + // Generic support - handles ALL message types + std::vector getSupportedTypes() const override { + return {}; // Empty = ALL types supported + } + + std::string getDescription() const override { + return description_; // From config + } + + // Publish a message (tools/call for topics with publish=true) + virtual void publishMessage(const nlohmann::json& msg); + + // Read cached message (resources/read) + virtual nlohmann::json readCachedMessage() const; + +protected: + // Uses rclcpp::GenericSubscription for runtime type handling + std::shared_ptr subscription_; + + // Uses rclcpp::GenericPublisher for runtime type handling + std::shared_ptr publisher_; + + // Cached latest message(s) as JSON + std::deque message_cache_; + + // Message type info for introspection + std::string msg_type_; + const rosidl_message_type_support_t* type_support_; + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::string description_; +}; + +// ============================================================================ +// BasicServicePlugin - Generic plugin for ALL service types +// ============================================================================ +class BasicServicePlugin : public MessagePlugin { +public: + // Initialization + void initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const config::ServiceConfig& config) override; + + // Generic support - handles ALL service types + std::vector getSupportedTypes() const override { + return {}; // Empty = ALL types supported + } + + std::string getDescription() const override { + return description_; + } + + // Call service (tools/call) + virtual nlohmann::json callService(const nlohmann::json& request); + + // Tag dispatch for request/response conversion + virtual nlohmann::json toJson(const void* msg_ptr, message_part::Request) const; + virtual nlohmann::json toJson(const void* msg_ptr, message_part::Response) const; + virtual void fromJson(const nlohmann::json& j, void* msg_ptr, message_part::Request) const; + virtual void fromJson(const nlohmann::json& j, void* msg_ptr, message_part::Response) const; + +protected: + // Uses rclcpp::GenericClient for runtime type handling + std::shared_ptr client_; + + // Service type info for introspection + std::string srv_type_; + const rosidl_service_type_support_t* type_support_; + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::string description_; + int timeout_ms_; +}; + +// ============================================================================ +// BasicActionPlugin - Generic plugin for ALL action types +// ============================================================================ +class BasicActionPlugin : public MessagePlugin { +public: + // Initialization + void initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const config::ActionConfig& config) override; + + // Generic support - handles ALL action types + std::vector getSupportedTypes() const override { + return {}; // Empty = ALL types supported + } + + std::string getDescription() const override { + return description_; + } + + // Send goal (tools/call) + virtual nlohmann::json sendGoal(const nlohmann::json& goal, std::string& operation_id); + + // Get status (tools/call with operation_id) + virtual nlohmann::json getStatus(const std::string& operation_id) const; + + // Cancel action (tools/call) + virtual void cancel(const std::string& operation_id); + + // Tag dispatch for goal/result/feedback conversion + virtual nlohmann::json toJson(const void* msg_ptr, message_part::Request) const; // Goal + virtual nlohmann::json toJson(const void* msg_ptr, message_part::Response) const; // Result + virtual nlohmann::json toJson(const void* msg_ptr, message_part::Feedback) const; // Feedback + virtual void fromJson(const nlohmann::json& j, void* msg_ptr, message_part::Request) const; // Goal + +protected: + // Uses rclcpp_action::Client with generic types (Phase 4) + // For now: std::shared_ptr for type-erased action client + std::shared_ptr action_client_; + + // Track active goals by operation_id + std::map> active_goals_; + + // Action type info for introspection + std::string action_type_; + const rosidl_action_type_support_t* type_support_; + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + std::string description_; + bool cancellable_; +}; + +} // namespace robot_mcp_msg_pluginlib +``` + +**Key Architectural Insight: Generic Plugins with Runtime Introspection** + +The plugin architecture uses **3 generic plugins** that support ALL message types via runtime introspection: + +1. **Generic by Default**: `BasicTopicPlugin`, `BasicServicePlugin`, `BasicActionPlugin` + - Support ANY ROS2 message/service/action type + - Use `rclcpp::GenericSubscription`, `rclcpp::GenericClient`, etc. + - JSON ↔ ROS conversion via `rosidl_typesupport_introspection_cpp` + +2. **Runtime Type Handling**: No compile-time knowledge of message types needed + - Type information comes from config (`msg_type: "sensor_msgs/msg/BatteryState"`) + - Plugins use type support introspection to walk message structure + - Automatic JSON conversion for all field types (primitives, arrays, nested) + +3. **Tag Dispatch for Message Parts**: Services and actions have multiple message parts + - `toJson(void*, message_part::Request)` for service requests/action goals + - `toJson(void*, message_part::Response)` for service responses/action results + - `toJson(void*, message_part::Feedback)` for action feedback + - Compile-time overload resolution with runtime polymorphism + +4. **Extensibility**: Users can create custom plugins for specialized behavior + - Custom plugins declare `getSupportedTypes()` to limit scope + - Example: `ImagePlugin` might only support `sensor_msgs/msg/Image` + - Factory validates type compatibility at startup + +5. **Configuration-Driven**: Each primitive specifies its plugin + - Primitives using same plugin type → separate instances + - Each instance owns its own ROS2 clients and state + - Description field in config used for MCP tools/resources list + +**Example Configuration:** + +```yaml +topics: + - name: "battery" + topic: "/battery_state" + msg_type: "sensor_msgs/msg/BatteryState" + plugin: "BasicTopicPlugin" + description: "Robot battery status including voltage, current, and charge percentage" + subscribe: true + publish: false + + - name: "cmd_vel" + topic: "/cmd_vel" + msg_type: "geometry_msgs/msg/Twist" + plugin: "BasicTopicPlugin" + description: "Velocity commands for robot base motion (linear and angular)" + subscribe: false + publish: true + +services: + - name: "reset" + service: "/reset" + srv_type: "std_srvs/srv/Trigger" + plugin: "BasicServicePlugin" + description: "Reset robot state to initial configuration" + timeout_ms: 3000 + +actions: + - name: "navigate" + action: "/navigate_to_pose" + action_type: "nav2_msgs/action/NavigateToPose" + plugin: "BasicActionPlugin" + description: "Navigate robot to a target pose in the map frame" + timeout_ms: 60000 + cancellable: true +``` + +**Plugin Manifest (plugins.xml):** +```xml + + + Generic plugin for all ROS2 topic types + + + + Generic plugin for all ROS2 service types + + + + Generic plugin for all ROS2 action types + + +``` + +**Runtime Introspection Approach:** + +```cpp +// Simplified example of JSON ↔ ROS conversion using introspection +nlohmann::json BasicTopicPlugin::messageToJson(const void* ros_msg) { + auto members = rosidl_typesupport_introspection_cpp::get_message_members(type_support_); + nlohmann::json result; + + for (size_t i = 0; i < members->member_count_; ++i) { + const auto& member = members->members_[i]; + const void* field_ptr = static_cast(ros_msg) + member.offset_; + + switch (member.type_id_) { + case rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: + result[member.name_] = *static_cast(field_ptr); + break; + case rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + result[member.name_] = *static_cast(field_ptr); + break; + // ... handle all types, arrays, nested messages recursively + } + } + return result; +} +``` + +## Implementation Plan (Phase 3+4) + +### 1. Plugin Base Classes ✅ (Phase 1-2 Complete) +- **Already Implemented**: `config_types.hpp`, `config_parser.hpp`, HTTPServer, AuthMiddleware, JSONRPCHandler +- **Phase 3+4**: Create `robot_mcp_msg_pluginlib` package + - `message_plugin.hpp`: Base interface with `getSupportedTypes()`, `getDescription()` + - `basic_topic_plugin.hpp`: Generic topic plugin using `rclcpp::GenericSubscription` + - `basic_service_plugin.hpp`: Generic service plugin with tag dispatch + - `basic_action_plugin.hpp`: Generic action plugin with operation tracking + - `message_conversion.hpp`: Introspection-based JSON ↔ ROS utilities + +### 2. Plugin Infrastructure +- **MCPPrimitiveLoader**: Wraps pluginlib for loading plugin classes +- **MCPPrimitiveFactory**: Creates plugin instances and returns registry + - Validates `getSupportedTypes()` compatibility at startup + - Instantiates plugins with primitive config (name, description, etc.) +- **MCPPrimitiveRegistry**: Owns plugin instances via `std::map>` + - Provides lookup by primitive name + +### 3. MCP Handler +- **MCPHandler**: Single functor class handling all 6 MCP methods + - `initialize()`: Return server info + - `ping()`: Health check + - `tools/list`: List primitives with descriptions + - `tools/call`: Dispatch to router + - `resources/list`: List topics with subscribe=true + - `resources/read`: Dispatch to router + - `asFunction()`: Returns `std::function` for HTTPServer + +### 4. Router & Operation Tracker +- **MCPRouter**: Routes requests to correct plugin in registry + - Non-owning reference to `MCPPrimitiveRegistry` + - Internally owns `OperationTracker` + - Methods: `callTool()`, `readResource()`, `getOperationStatus()`, `cancelOperation()` +- **OperationTracker**: Tracks active action operations + - Maps `operation_id` → plugin instance reference + - Used for status queries and cancellation + +### 5. Integration in MCPServerNode +- **on_configure()**: + - Create `MCPPrimitiveFactory` + - `mcp_primitive_registry_ = factory.createRegistry(shared_from_this(), config_)` + - `mcp_router_ = make_unique(*mcp_primitive_registry_)` + - `mcp_handler_ = make_unique(config_, *mcp_router_)` +- **on_activate()**: + - `http_server_->start(config_.server, mcp_handler_->asFunction())` +- **Remove**: `handleMCPRequest()` method (MCPHandler owns data path) + +### 6. Testing Strategy +- **Catch2 Unit Tests**: + - Test each plugin class with mock message types + - Test introspection-based JSON conversion + - Test MCPRouter routing logic + - Test OperationTracker + - Test MCPHandler method dispatch +- **Launch Tests**: + - End-to-end test with real ROS2 topics/services/actions + - Test all 6 MCP methods + - Test operation tracking for long-running actions + +--- + +## Design Decisions + +### Multi-Distro ROS2 Support + +**Decision:** Target all ROS2 distributions (Humble, Iron, Jazzy, etc.) + +**Implementation:** +- Use standard `rclcpp` APIs available in all distros +- Avoid distro-specific features +- Test on Humble (LTS) and Jazzy (latest LTS) +- Document any known compatibility issues + +### Authentication Default + +**Decision:** Auth disabled by default in example configs + +**Rationale:** +- Easier development and testing experience +- Most users will be in trusted networks initially +- Document security setup clearly for production +- Easy to enable via config when needed + +### Server-Sent Events (SSE) + +**Decision:** Defer SSE to Phase 13+ (post-MVP) + +**Rationale:** +- Polling-based status checking is sufficient for MVP +- MCP client support for SSE is still maturing +- Adds ~2-3 weeks of complexity +- Can be added later without breaking changes + +**Future Implementation:** +- Add SSE endpoint `/mcp/events` +- Stream action feedback in real-time +- Push topic updates at configured rate +- Notification on operation completion + +### Message Type System: Generic Plugins with Runtime Introspection + +**Decision:** 3 generic plugins support ALL message types via runtime introspection using `rosidl_typesupport_introspection_cpp` + +**Key Innovation:** No type-specific plugins needed. Plugins use runtime type information from config to handle any ROS2 message/service/action. + +**Advantages:** +- **Zero plugin proliferation** - 3 plugins handle all message types +- **No maintenance burden** - Don't need to create/maintain hundreds of type-specific plugins +- **Automatic support for new types** - Custom messages work immediately without new plugins +- **Configuration-driven** - Type information comes from YAML, not compilation +- **Extensible** - Users can still create custom plugins for specialized behavior +- **Follows ROS2 patterns** - Uses same introspection APIs as ros2bag, ros2 topic echo, etc. + +**Why Generic Over Type-Specific:** + +| Aspect | Type-Specific Plugins | Generic Plugins | +|--------|----------------------|-----------------| +| **Plugin count** | Hundreds (one per type) | 3 (topic/service/action) | +| **Maintenance** | High (update for each type) | Low (one implementation) | +| **New message types** | Requires new plugin | Works immediately | +| **Testing** | Test each plugin | Test once | +| **Compile dependencies** | All message packages | None (runtime only) | +| **Custom behavior** | Easy (per-type) | Possible (custom plugins) | + +**Implementation Approach:** + +1. **Generic ROS2 Clients:** + - `rclcpp::GenericSubscription` for topics + - `rclcpp::GenericClient` for services + - Generic action client (Phase 4 work needed) + +2. **Runtime Introspection:** + - Use `rosidl_typesupport_introspection_cpp::get_message_members()` + - Walk message structure recursively + - Convert each field based on `type_id_` (float, string, nested message, array, etc.) + +3. **Tag Dispatch:** + - Services have Request/Response parts + - Actions have Goal/Result/Feedback parts + - Use empty tag structs for compile-time overload resolution + - Virtual functions provide runtime polymorphism for router + +4. **Type Compatibility Validation:** + - Plugins declare `getSupportedTypes()`: empty = ALL, non-empty = specific types + - Factory validates compatibility at startup + - Allows custom plugins to restrict scope + +**Configuration Example:** +```yaml +# All use the same BasicTopicPlugin class +topics: + - name: "battery" + topic: "/battery_state" + msg_type: "sensor_msgs/msg/BatteryState" + plugin: "BasicTopicPlugin" + description: "Robot battery status" + + - name: "odom" + topic: "/odom" + msg_type: "nav_msgs/msg/Odometry" + plugin: "BasicTopicPlugin" + description: "Robot odometry" + + - name: "custom_msg" + topic: "/my_custom_topic" + msg_type: "my_pkg/msg/MyCustomMessage" + plugin: "BasicTopicPlugin" # Works without creating new plugin! + description: "Custom message type" +``` + +**When to Create Custom Plugins:** +- **Specialized validation**: Image size limits, velocity bounds, etc. +- **Performance optimization**: Zero-copy for large messages +- **Complex transformations**: Coordinate frame conversions, compression +- **Domain-specific logic**: Robot-specific safety checks + +**Custom Plugin Example:** +```cpp +class ImagePlugin : public BasicTopicPlugin { + std::vector getSupportedTypes() const override { + return {"sensor_msgs/msg/Image", "sensor_msgs/msg/CompressedImage"}; + } + + // Override to add compression before sending over network + nlohmann::json readCachedMessage() const override { + auto full_json = BasicTopicPlugin::readCachedMessage(); + return compressImageData(full_json); // Custom compression + } +}; +``` + +### Resource Conflict Management + +**Decision:** Explicit resource group system with configurable resolution + +**Rationale:** +- AI assistants may send parallel conflicting requests +- Need deterministic behavior, not race conditions +- Configuration allows robot-specific policies +- Three strategies handle different use cases: + - `error`: Safe default, user handles retry + - `cancel`: Convenient for interruptible actions + - `queue`: For sequential operations + +### HTTP vs stdio Transport + +**Decision:** HTTP-only (no stdio support) + +**Advantages:** +- Network accessible (control robot remotely) +- Multiple concurrent clients +- Standard debugging tools (curl, Postman) +- Better for production deployment +- No SSH tunneling needed + +**Tradeoff:** +- Cannot use with local-only MCP clients +- Requires network setup +- Decided acceptable for robotic use case + +--- + +## Open Questions / Future Enhancements + +### Phase 13+: Advanced Features + +1. **Server-Sent Events (SSE)** + - Real-time action feedback streaming + - Topic update push notifications + - Reduces polling overhead + +2. **Web Dashboard** + - Monitor active operations + - View robot state + - Test tools manually + - Debug interface issues + +3. **Advanced Retry Logic** + - Automatic retry for transient failures + - Exponential backoff + - Circuit breaker pattern + +4. **Performance Optimizations** + - Zero-copy message handling + - Connection pooling + - Response caching + - Compression support + +5. **Security Enhancements** + - TLS/HTTPS support + - OAuth2 authentication + - Role-based access control + - Audit logging + +6. **Observability** + - Prometheus metrics export + - OpenTelemetry tracing + - Structured logging to stdout + +--- + +## Success Metrics + +**MVP (Phases 1-8):** +- Can control TurtleBot3 via Claude Desktop +- Navigate, read sensors, publish velocity commands +- Basic status tracking works +- Configuration via YAML + +**Production Ready (Phases 1-12):** +- 80%+ test coverage +- Documentation complete +- Debian package available +- Docker image published +- Sub-10ms HTTP latency +- Handles 1000+ req/s + +**Community Adoption:** +- 100+ GitHub stars +- Used in 5+ different robot platforms +- Active community contributions +- Plugins for custom message types + +--- + +## Contributing + +See `CONTRIBUTING.md` for development guidelines, coding standards, and how to submit PRs. + +## License + +Apache 2.0 - See `LICENSE` file for details. diff --git a/PHASE3_4_STATUS.md b/PHASE3_4_STATUS.md new file mode 100644 index 0000000..f189ddb --- /dev/null +++ b/PHASE3_4_STATUS.md @@ -0,0 +1,296 @@ +# Phase 3+4 Implementation Status + +**Date:** 2025-10-23 +**Status:** In Progress - Building Plugin Infrastructure +**Next:** Switch to Jazzy for better generic client support + +--- + +## What We've Completed + +### 1. ✅ Config Updates +- **File:** `robot_mcp_server/include/robot_mcp_server/mcp_config/config_types.hpp` +- **Changes:** Added `description` field to `TopicConfig`, `ServiceConfig`, `ActionConfig` +- **Purpose:** Descriptions will flow through to MCP tools/resources list for Claude to reason about + +### 2. ✅ Base Plugin Interface +- **File:** `robot_mcp_msg_pluginlib/include/robot_mcp_msg_pluginlib/message_plugin.hpp` +- **Changes:** Completely rewritten for generic plugin design +- **Key Methods:** + - `initialize(node, primitive_name, description)` - Setup with lifecycle node + - `getSupportedTypes()` - Returns empty vector for generic plugins, non-empty for custom + - `getDescription()` - Returns description from config + - `getPrimitiveName()` - Returns primitive name +- **Tag Dispatch:** Added `message_part::Request`, `message_part::Response`, `message_part::Feedback` for service/action parts + +### 3. ✅ New Package: robot_mcp_generic_plugins +Created complete package structure: + +``` +robot_mcp_generic_plugins/ +├── CMakeLists.txt ✅ Modern CMake with target_link_libraries +├── package.xml ✅ Dependencies on robot_mcp_msg_pluginlib + introspection +├── plugins.xml ✅ Pluginlib manifest for 3 generic plugins +├── include/robot_mcp_generic_plugins/ +│ ├── basic_topic_plugin.hpp ✅ Generic topic plugin (all msg types) +│ ├── basic_service_plugin.hpp ✅ Generic service plugin (stub - needs work) +│ ├── basic_action_plugin.hpp ✅ Generic action plugin (stub - needs work) +│ └── message_conversion.hpp ✅ Introspection utilities (stub) +└── src/ + ├── basic_topic_plugin.cpp ✅ Stub implementation + ├── basic_service_plugin.cpp ✅ Stub implementation + ├── basic_action_plugin.cpp ✅ Stub implementation with UUID generation + └── message_conversion.cpp ✅ Stub implementation +``` + +### 4. ✅ Plugin Implementations (Stubs) + +**BasicTopicPlugin:** +- Uses `rclcpp::GenericSubscription` and `rclcpp::GenericPublisher` ✅ +- Methods: `configureTopic()`, `publishMessage()`, `readCachedMessage()` +- Caching: Latest 10 messages in JSON format +- Status: Compiles but needs introspection conversion implementation + +**BasicServicePlugin:** +- Placeholder for generic service client (doesn't exist in Humble) +- Methods: `configureService()`, `callService()` +- Status: ⚠️ Needs GenericClient (may exist in Jazzy) + +**BasicActionPlugin:** +- Placeholder for generic action client +- Methods: `configureAction()`, `sendGoal()`, `getStatus()`, `cancel()` +- UUID generation for operation IDs ✅ +- Status: ⚠️ Needs generic action client + +**message_conversion.hpp/cpp:** +- Stub functions for introspection-based conversion: + - `rosMessageToJson()` - TODO + - `jsonToRosMessage()` - TODO + - `getMessageTypeSupport()` - TODO + - `getServiceTypeSupport()` - TODO + - `getActionTypeSupport()` - TODO + +--- + +## Current Build Status + +**Last Build Attempt:** Failed on Humble +**Issue:** `rclcpp/generic_client.hpp` does not exist in Humble +**Packages Built:** +- ✅ `robot_mcp_msg_pluginlib` - Rebuilt successfully with new interface +- ❌ `robot_mcp_generic_plugins` - Failed due to missing GenericClient + +**Warnings to Fix:** +- Unused parameter warnings (use `(void)param` to suppress) +- Parentheses suggestion in UUID generation + +--- + +## What's Next (After Jazzy Switch) + +### Immediate Tasks + +1. **Fix Compilation on Jazzy** + - Check if `GenericClient` exists in Jazzy + - If not, implement manual service client creation using type support + - Fix all compilation warnings + +2. **Implement Introspection Conversion** (Critical Path) + - File: `robot_mcp_generic_plugins/src/message_conversion.cpp` + - Implement `rosMessageToJson()` using `rosidl_typesupport_introspection_cpp` + - Implement `jsonToRosMessage()` + - Implement `getMessageTypeSupport()`, `getServiceTypeSupport()`, `getActionTypeSupport()` + - Walk message members recursively + - Handle all field types: primitives, strings, arrays, nested messages + - Reference: `rosbag2_cpp` or `ros2 topic echo` for examples + +3. **Complete Plugin Implementations** + - Update `basic_topic_plugin.cpp` to use conversion utilities + - Update `basic_service_plugin.cpp` once GenericClient is available + - Update `basic_action_plugin.cpp` once generic action client is available + +4. **Build and Test Plugins Package** + ```bash + colcon build --packages-select robot_mcp_generic_plugins + ``` + +### Phase 3+4 Remaining Work + +5. **Create MCPPrimitiveFactory** + - File: `robot_mcp_server/include/robot_mcp_server/mcp_primitive/mcp_primitive_factory.hpp` + - Uses `pluginlib::ClassLoader` directly (no wrapper) + - Validates `getSupportedTypes()` compatibility at startup + - Returns `MCPPrimitiveRegistry` populated with plugin instances + +6. **Create MCPPrimitiveRegistry** + - File: `robot_mcp_server/include/robot_mcp_server/mcp_primitive/mcp_primitive_registry.hpp` + - Owns plugin instances via `std::map>` + - Provides lookup by primitive name + - Methods: `getPlugin(name)`, `getAllPrimitives()`, `hasPrimitive(name)` + +7. **Create OperationTracker** + - File: `robot_mcp_server/include/robot_mcp_server/mcp_router/operation_tracker.hpp` + - Maps `operation_id` → plugin instance reference + - Used for action status queries and cancellation + - Thread-safe for concurrent access + +8. **Create MCPRouter** + - File: `robot_mcp_server/include/robot_mcp_server/mcp_router/mcp_router.hpp` + - Routes requests to correct plugin in registry + - Non-owning reference to `MCPPrimitiveRegistry` + - Internally owns `OperationTracker` + - Methods: + - `callTool(primitive_name, args)` - Dispatch to plugin + - `readResource(primitive_name)` - Read cached topic data + - `getOperationStatus(operation_id)` - Query action status + - `cancelOperation(operation_id)` - Cancel action + +9. **Create MCPHandler** + - File: `robot_mcp_server/include/robot_mcp_server/mcp_handler/mcp_handler.hpp` + - Single functor class handling all 6 MCP methods: + - `initialize()` - Return server info + - `ping()` - Health check + - `tools/list` - List all primitives with descriptions + - `tools/call` - Dispatch to router + - `resources/list` - List topics with subscribe=true + - `resources/read` - Dispatch to router + - `asFunction()` returns `std::function` for HTTPServer + - Non-owning reference to `MCPRouter` + +10. **Update MCPServerNode Integration** + - File: `robot_mcp_server/src/robot_mcp_server_node.cpp` + - **on_configure():** + ```cpp + MCPPrimitiveFactory factory; + mcp_primitive_registry_ = factory.createRegistry(shared_from_this(), config_); + mcp_router_ = make_unique(*mcp_primitive_registry_); + mcp_handler_ = make_unique(config_, *mcp_router_); + ``` + - **on_activate():** + ```cpp + http_server_->start(config_.server, mcp_handler_->asFunction()); + ``` + - **REMOVE:** `handleMCPRequest()` method (MCPHandler owns data path) + +11. **Write End-to-End Launch Test** + - Test all 6 MCP methods with real ROS2 topics/services/actions + - Test operation tracking for long-running actions + - Add to `robot_mcp_test` package + +12. **Verify All Tests Pass** + ```bash + colcon test + pre-commit run --all-files + ``` + +--- + +## Architecture Decisions Summary + +### Generic Plugin Design +- **3 plugins total:** BasicTopicPlugin, BasicServicePlugin, BasicActionPlugin +- **Runtime introspection:** No compile-time knowledge of message types +- **Configuration-driven:** Type info comes from YAML config +- **Extensibility:** Users can create custom plugins for specialized behavior + +### Ownership Structure +``` +MCPServerNode owns: +├── http_server_ (HTTPServer) +├── mcp_handler_ (MCPHandler - functor) +├── mcp_router_ (MCPRouter - owns OperationTracker) +└── mcp_primitive_registry_ (MCPPrimitiveRegistry - owns all plugin instances) +``` + +### Factory Pattern +- Factory creates registry from config +- Factory validates plugin-type compatibility via `getSupportedTypes()` +- Factory is transient (local variable in on_configure) +- Registry is owned by MCPServerNode + +### Data Flow +``` +HTTP Request → HTTPServer → MCPHandler → MCPRouter → MCPPrimitiveRegistry → Plugin + ↓ +JSON ← HTTP Response ← MCPHandler ← MCPRouter ← JSON ← Plugin ← ROS2 Message +``` + +--- + +## Known Issues & Limitations + +1. **Humble ROS2:** No GenericClient for services/actions + - **Solution:** Switch to Jazzy or implement manual client creation + +2. **Introspection Not Implemented:** Core conversion logic TODO + - **Impact:** Plugins are stubs, can't actually convert messages + - **Priority:** Critical path for Phase 3+4 + +3. **Generic Action Client:** May not exist even in Jazzy + - **Fallback:** Manual action client creation using type support + - **Complexity:** High - action client setup is non-trivial + +4. **Type Support Lookup:** Need runtime lookup mechanism + - **Challenge:** Parsing type strings like "sensor_msgs/msg/BatteryState" + - **Approach:** Use rosidl_typesupport APIs or dlopen() for .so files + +--- + +## Testing Strategy + +### Unit Tests (Catch2 in robot_mcp_test) +- Test introspection conversion with known message types +- Test plugin initialization and configuration +- Test MCPRouter routing logic +- Test OperationTracker +- Test MCPHandler method dispatch + +### Launch Tests (launch_testing) +- End-to-end test with real ROS2 topics/services/actions +- Test all 6 MCP methods +- Test operation tracking +- Test error handling + +--- + +## References + +- **ARCHITECTURE.md:** Complete design doc (updated 2025-10-23) +- **Introspection Examples:** + - `ros2 topic echo` implementation + - `rosbag2_cpp` serialization + - `rosidl_typesupport_introspection_cpp` examples +- **Generic Subscription:** `/opt/ros/jazzy/include/rclcpp/rclcpp/generic_subscription.hpp` + +--- + +## Command to Resume + +After switching to Jazzy: + +```bash +# Source Jazzy workspace +source /opt/ros/jazzy/setup.bash + +# Rebuild all packages +colcon build --packages-select robot_mcp_msg_pluginlib robot_mcp_generic_plugins + +# Check if GenericClient exists +find /opt/ros/jazzy/include -name "*generic*" + +# Continue with introspection implementation +# Edit: robot_mcp_generic_plugins/src/message_conversion.cpp +``` + +--- + +## Questions to Resolve + +1. **Jazzy GenericClient:** Does it exist? What's the header? +2. **Type Support Lookup:** Best approach for runtime type lookup? +3. **Action Client:** Generic or manual creation? +4. **Resource Groups:** Defer to Phase 5 or stub in Phase 3+4? + +--- + +**Next Session:** Start by checking Jazzy's generic client support, then implement introspection conversion. From 66bf224316621f2aea18ae5fa30f28c55e302b5a Mon Sep 17 00:00:00 2001 From: Eddy Zhou Date: Thu, 23 Oct 2025 18:06:52 +0000 Subject: [PATCH 2/2] checkpoint, for tomorrow :( --- .devcontainer/Dockerfile | 15 +- .../robot_common_msg_plugins/CMakeLists.txt | 19 - .../robot_common_msg_plugins/COLCON_IGNORE | 0 .../robot_common_msg_plugins/package.xml | 24 - .../CMakeLists.txt | 75 --- .../COLCON_IGNORE | 0 .../package.xml | 23 - .../plugins.xml | 3 - .../robot_mcp_nav_msg_plugins/CMakeLists.txt | 75 --- .../robot_mcp_nav_msg_plugins/COLCON_IGNORE | 0 .../robot_mcp_nav_msg_plugins/package.xml | 23 - .../robot_mcp_nav_msg_plugins/plugins.xml | 3 - .../CMakeLists.txt | 75 --- .../COLCON_IGNORE | 0 .../robot_mcp_sensor_msg_plugins/package.xml | 23 - .../robot_mcp_sensor_msg_plugins/plugins.xml | 3 - .../robot_mcp_std_msg_plugins/CMakeLists.txt | 75 --- .../robot_mcp_std_msg_plugins/COLCON_IGNORE | 0 .../robot_mcp_std_msg_plugins/package.xml | 30 - .../robot_mcp_std_msg_plugins/plugins.xml | 9 - robot_mcp_generic_plugins/CMakeLists.txt | 114 ++++ .../generic_service_plugin.hpp | 122 +++++ .../generic_topic_plugin.hpp | 152 ++++++ .../message_conversion.hpp | 112 ++++ robot_mcp_generic_plugins/package.xml | 29 + robot_mcp_generic_plugins/plugins.xml | 25 + .../src/generic_service_plugin.cpp | 161 ++++++ .../src/generic_topic_plugin.cpp | 182 +++++++ .../src/message_conversion.cpp | 511 ++++++++++++++++++ .../message_plugin.hpp | 93 ++-- .../mcp_config/config_types.hpp | 3 + .../test/test_http_components.cpp | 2 +- robot_mcp_server/test/test_lifecycle.cpp | 1 - robot_mcp_test/README.md | 1 - robot_mcp_test/cmake/add_robot_mcp_test.cmake | 14 +- .../include/robot_mcp_test/robot_mcp_test.hpp | 9 +- robot_mcp_test/test/test_basic_example.cpp | 3 +- robot_mcp_test/test/test_with_ros.cpp | 3 +- 38 files changed, 1501 insertions(+), 511 deletions(-) delete mode 100644 robot_mcp_common_msg_plugins/robot_common_msg_plugins/CMakeLists.txt delete mode 100644 robot_mcp_common_msg_plugins/robot_common_msg_plugins/COLCON_IGNORE delete mode 100644 robot_mcp_common_msg_plugins/robot_common_msg_plugins/package.xml delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/CMakeLists.txt delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/COLCON_IGNORE delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/package.xml delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/plugins.xml delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/CMakeLists.txt delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/COLCON_IGNORE delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/package.xml delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/plugins.xml delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/CMakeLists.txt delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/COLCON_IGNORE delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/package.xml delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/plugins.xml delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/CMakeLists.txt delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/COLCON_IGNORE delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/package.xml delete mode 100644 robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/plugins.xml create mode 100644 robot_mcp_generic_plugins/CMakeLists.txt create mode 100644 robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/generic_service_plugin.hpp create mode 100644 robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/generic_topic_plugin.hpp create mode 100644 robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/message_conversion.hpp create mode 100644 robot_mcp_generic_plugins/package.xml create mode 100644 robot_mcp_generic_plugins/plugins.xml create mode 100644 robot_mcp_generic_plugins/src/generic_service_plugin.cpp create mode 100644 robot_mcp_generic_plugins/src/generic_topic_plugin.cpp create mode 100644 robot_mcp_generic_plugins/src/message_conversion.cpp diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 043a53d..203e13f 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -9,6 +9,7 @@ FROM ros:${ROS_DISTRO}-ros-base AS dev-tools # Install development tools not in base image RUN apt-get update && apt-get install -y --no-install-recommends \ python3-pip \ + python3-venv \ python3-colcon-common-extensions \ python3-rosdep \ openssh-client \ @@ -16,8 +17,9 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ curl \ && rm -rf /var/lib/apt/lists/* -# Install pre-commit -RUN python3 -m pip install --no-cache-dir pre-commit==3.8.0 +# Create a system-wide virtual environment and install pre-commit +RUN python3 -m venv --system-site-packages /opt/venv && \ + /opt/venv/bin/pip install --no-cache-dir pre-commit==3.8.0 # Set working dir (matches VSCode workspace) WORKDIR /deep_ros_ws @@ -58,8 +60,8 @@ ARG ROS_DISTRO # Cater image to user SHELL ["/bin/bash", "-c"] # hadolint ignore=SC2086 -RUN groupadd --gid ${USER_GID} ${USERNAME} \ - && useradd --uid ${USER_UID} --gid ${USER_GID} -m $USERNAME --shell /bin/bash \ +RUN groupadd --gid ${USER_GID} ${USERNAME} || groupmod -n ${USERNAME} $(getent group ${USER_GID} | cut -d: -f1) \ + && useradd --uid ${USER_UID} --gid ${USER_GID} -m $USERNAME --shell /bin/bash || usermod -l ${USERNAME} -d /home/${USERNAME} -m $(getent passwd ${USER_UID} | cut -d: -f1) \ && apt-get update \ && apt-get install -y --no-install-recommends sudo \ && echo $USERNAME ALL=\(ALL\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ @@ -76,5 +78,6 @@ USER $USERNAME SHELL ["/bin/bash", "-o", "pipefail", "-c"] RUN curl -fsSL https://claude.ai/install.sh | bash -# Source ROS in user's bashrc -RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /home/$USERNAME/.bashrc +# Source ROS and Python venv in user's bashrc +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /home/$USERNAME/.bashrc && \ + echo "source /opt/venv/bin/activate" >> /home/$USERNAME/.bashrc diff --git a/robot_mcp_common_msg_plugins/robot_common_msg_plugins/CMakeLists.txt b/robot_mcp_common_msg_plugins/robot_common_msg_plugins/CMakeLists.txt deleted file mode 100644 index 2b9c3dc..0000000 --- a/robot_mcp_common_msg_plugins/robot_common_msg_plugins/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -# Copyright (c) 2025-present WATonomous. All rights reserved. -# -# 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. -cmake_minimum_required(VERSION 3.8) -project(robot_mcp_common_msg_plugins) - -find_package(ament_cmake REQUIRED) - -ament_package() diff --git a/robot_mcp_common_msg_plugins/robot_common_msg_plugins/COLCON_IGNORE b/robot_mcp_common_msg_plugins/robot_common_msg_plugins/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/robot_mcp_common_msg_plugins/robot_common_msg_plugins/package.xml b/robot_mcp_common_msg_plugins/robot_common_msg_plugins/package.xml deleted file mode 100644 index 5de8f89..0000000 --- a/robot_mcp_common_msg_plugins/robot_common_msg_plugins/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - robot_mcp_common_msg_plugins - 0.1.0 - - Metapackage that aggregates common ROS2 message plugins for robot_mcp. - - - Eddy Zhou - Apache 2.0 - WATonomous - - ament_cmake - - robot_mcp_std_msg_plugins - robot_mcp_geometry_msg_plugins - robot_mcp_sensor_msg_plugins - robot_mcp_nav_msg_plugins - - - ament_cmake - - diff --git a/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/CMakeLists.txt b/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/CMakeLists.txt deleted file mode 100644 index b01d9cc..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/CMakeLists.txt +++ /dev/null @@ -1,75 +0,0 @@ -# Copyright (c) 2025-present WATonomous. All rights reserved. -# -# 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. -cmake_minimum_required(VERSION 3.22) -project(robot_mcp_geometry_msg_plugins) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) - add_link_options(-Wl,-no-undefined) -endif() - -find_package(ament_cmake REQUIRED) -find_package(robot_mcp_msg_pluginlib REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nlohmann_json REQUIRED) - -set(include_dir ${CMAKE_CURRENT_SOURCE_DIR}/include) - -# Plugin library (will be populated in Phase 3) -# add_library(${PROJECT_NAME} SHARED -# src/plugins/pose_plugin.cpp -# # More plugin implementations... -# ) -# target_include_directories(${PROJECT_NAME} PUBLIC -# $ -# $ -# ) -# target_link_libraries(${PROJECT_NAME} -# PUBLIC -# robot_mcp_msg_pluginlib::robot_mcp_msg_pluginlib -# PRIVATE -# ${geometry_msgs_TARGETS} -# pluginlib::pluginlib -# nlohmann_json::nlohmann_json -# ) -# -# install(TARGETS ${PROJECT_NAME} -# EXPORT ${PROJECT_NAME}Targets -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION bin -# ) -# -# install(EXPORT ${PROJECT_NAME}Targets -# NAMESPACE ${PROJECT_NAME}:: -# DESTINATION share/${PROJECT_NAME}/cmake -# ) - -install(DIRECTORY include/ - DESTINATION include -) - -install(FILES plugins.xml - DESTINATION share/${PROJECT_NAME} -) - -pluginlib_export_plugin_description_file(robot_mcp_msg_pluginlib plugins.xml) - -# ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) -ament_package() diff --git a/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/COLCON_IGNORE b/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/package.xml b/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/package.xml deleted file mode 100644 index 2fd6e30..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - robot_mcp_geometry_msg_plugins - 0.1.0 - Message plugins for geometry_msgs types - - Eddy Zhou - Apache 2.0 - WATonomous - - ament_cmake - - robot_mcp_msg_pluginlib - geometry_msgs - pluginlib - nlohmann-json-dev - - - ament_cmake - - - diff --git a/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/plugins.xml b/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/plugins.xml deleted file mode 100644 index 8c052b6..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_geometry_msg_plugins/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/CMakeLists.txt b/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/CMakeLists.txt deleted file mode 100644 index cca5e08..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/CMakeLists.txt +++ /dev/null @@ -1,75 +0,0 @@ -# Copyright (c) 2025-present WATonomous. All rights reserved. -# -# 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. -cmake_minimum_required(VERSION 3.22) -project(robot_mcp_nav_msg_plugins) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) - add_link_options(-Wl,-no-undefined) -endif() - -find_package(ament_cmake REQUIRED) -find_package(robot_mcp_msg_pluginlib REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nlohmann_json REQUIRED) - -set(include_dir ${CMAKE_CURRENT_SOURCE_DIR}/include) - -# Plugin library (will be populated in Phase 3) -# add_library(${PROJECT_NAME} SHARED -# src/plugins/odometry_plugin.cpp -# # More plugin implementations... -# ) -# target_include_directories(${PROJECT_NAME} PUBLIC -# $ -# $ -# ) -# target_link_libraries(${PROJECT_NAME} -# PUBLIC -# robot_mcp_msg_pluginlib::robot_mcp_msg_pluginlib -# PRIVATE -# ${nav_msgs_TARGETS} -# pluginlib::pluginlib -# nlohmann_json::nlohmann_json -# ) -# -# install(TARGETS ${PROJECT_NAME} -# EXPORT ${PROJECT_NAME}Targets -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION bin -# ) -# -# install(EXPORT ${PROJECT_NAME}Targets -# NAMESPACE ${PROJECT_NAME}:: -# DESTINATION share/${PROJECT_NAME}/cmake -# ) - -install(DIRECTORY include/ - DESTINATION include -) - -install(FILES plugins.xml - DESTINATION share/${PROJECT_NAME} -) - -pluginlib_export_plugin_description_file(robot_mcp_msg_pluginlib plugins.xml) - -# ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) -ament_package() diff --git a/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/COLCON_IGNORE b/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/package.xml b/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/package.xml deleted file mode 100644 index 09c0398..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - robot_mcp_nav_msg_plugins - 0.1.0 - Message plugins for nav_msgs types - - Eddy Zhou - Apache 2.0 - WATonomous - - ament_cmake - - robot_mcp_msg_pluginlib - nav_msgs - pluginlib - nlohmann-json-dev - - - ament_cmake - - - diff --git a/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/plugins.xml b/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/plugins.xml deleted file mode 100644 index e54fd10..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_nav_msg_plugins/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/CMakeLists.txt b/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/CMakeLists.txt deleted file mode 100644 index 4e8ec87..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/CMakeLists.txt +++ /dev/null @@ -1,75 +0,0 @@ -# Copyright (c) 2025-present WATonomous. All rights reserved. -# -# 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. -cmake_minimum_required(VERSION 3.22) -project(robot_mcp_sensor_msg_plugins) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) - add_link_options(-Wl,-no-undefined) -endif() - -find_package(ament_cmake REQUIRED) -find_package(robot_mcp_msg_pluginlib REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nlohmann_json REQUIRED) - -set(include_dir ${CMAKE_CURRENT_SOURCE_DIR}/include) - -# Plugin library (will be populated in Phase 3) -# add_library(${PROJECT_NAME} SHARED -# src/plugins/battery_state_plugin.cpp -# # More plugin implementations... -# ) -# target_include_directories(${PROJECT_NAME} PUBLIC -# $ -# $ -# ) -# target_link_libraries(${PROJECT_NAME} -# PUBLIC -# robot_mcp_msg_pluginlib::robot_mcp_msg_pluginlib -# PRIVATE -# ${sensor_msgs_TARGETS} -# pluginlib::pluginlib -# nlohmann_json::nlohmann_json -# ) -# -# install(TARGETS ${PROJECT_NAME} -# EXPORT ${PROJECT_NAME}Targets -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION bin -# ) -# -# install(EXPORT ${PROJECT_NAME}Targets -# NAMESPACE ${PROJECT_NAME}:: -# DESTINATION share/${PROJECT_NAME}/cmake -# ) - -install(DIRECTORY include/ - DESTINATION include -) - -install(FILES plugins.xml - DESTINATION share/${PROJECT_NAME} -) - -pluginlib_export_plugin_description_file(robot_mcp_msg_pluginlib plugins.xml) - -# ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) -ament_package() diff --git a/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/COLCON_IGNORE b/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/package.xml b/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/package.xml deleted file mode 100644 index 4db64ac..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - robot_mcp_sensor_msg_plugins - 0.1.0 - Message plugins for sensor_msgs types - - Eddy Zhou - Apache 2.0 - WATonomous - - ament_cmake - - robot_mcp_msg_pluginlib - sensor_msgs - pluginlib - nlohmann-json-dev - - - ament_cmake - - - diff --git a/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/plugins.xml b/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/plugins.xml deleted file mode 100644 index 0563185..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_sensor_msg_plugins/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/CMakeLists.txt b/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/CMakeLists.txt deleted file mode 100644 index 6be2f83..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/CMakeLists.txt +++ /dev/null @@ -1,75 +0,0 @@ -# Copyright (c) 2025-present WATonomous. All rights reserved. -# -# 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. -cmake_minimum_required(VERSION 3.22) -project(robot_mcp_std_msg_plugins) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) - add_link_options(-Wl,-no-undefined) -endif() - -find_package(ament_cmake REQUIRED) -find_package(robot_mcp_msg_pluginlib REQUIRED) -find_package(std_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nlohmann_json REQUIRED) - -set(include_dir ${CMAKE_CURRENT_SOURCE_DIR}/include) - -# Plugin library (will be populated in Phase 3) -# add_library(${PROJECT_NAME} SHARED -# src/plugins/string_plugin.cpp -# # More plugin implementations... -# ) -# target_include_directories(${PROJECT_NAME} PUBLIC -# $ -# $ -# ) -# target_link_libraries(${PROJECT_NAME} -# PUBLIC -# robot_mcp_msg_pluginlib::robot_mcp_msg_pluginlib -# PRIVATE -# ${std_msgs_TARGETS} -# pluginlib::pluginlib -# nlohmann_json::nlohmann_json -# ) -# -# install(TARGETS ${PROJECT_NAME} -# EXPORT ${PROJECT_NAME}Targets -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION bin -# ) -# -# install(EXPORT ${PROJECT_NAME}Targets -# NAMESPACE ${PROJECT_NAME}:: -# DESTINATION share/${PROJECT_NAME}/cmake -# ) - -install(DIRECTORY include/ - DESTINATION include -) - -install(FILES plugins.xml - DESTINATION share/${PROJECT_NAME} -) - -pluginlib_export_plugin_description_file(robot_mcp_msg_pluginlib plugins.xml) - -# ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) -ament_package() diff --git a/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/COLCON_IGNORE b/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/package.xml b/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/package.xml deleted file mode 100644 index 5306f23..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - robot_mcp_std_msg_plugins - 0.1.0 - Message plugins for std_msgs types - - Eddy Zhou - Apache 2.0 - WATonomous - - ament_cmake - - - robot_mcp_msg_pluginlib - - - std_msgs - - - pluginlib - - - nlohmann-json-dev - - - ament_cmake - - - diff --git a/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/plugins.xml b/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/plugins.xml deleted file mode 100644 index 1fc62b7..0000000 --- a/robot_mcp_common_msg_plugins/robot_mcp_std_msg_plugins/plugins.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - diff --git a/robot_mcp_generic_plugins/CMakeLists.txt b/robot_mcp_generic_plugins/CMakeLists.txt new file mode 100644 index 0000000..2dfa869 --- /dev/null +++ b/robot_mcp_generic_plugins/CMakeLists.txt @@ -0,0 +1,114 @@ +# Copyright (c) 2025-present WATonomous. All rights reserved. +# +# 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. + +cmake_minimum_required(VERSION 3.22) +project(robot_mcp_generic_plugins) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + add_link_options(-Wl,-no-undefined) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(pluginlib REQUIRED) +find_package(nlohmann_json REQUIRED) +find_package(robot_mcp_msg_pluginlib REQUIRED) +find_package(rosidl_typesupport_introspection_cpp REQUIRED) +find_package(rosidl_runtime_cpp REQUIRED) + +set(include_dir ${CMAKE_CURRENT_SOURCE_DIR}/include) + +# Detect ROS distro for conditional compilation +if($ENV{ROS_DISTRO} STREQUAL "humble") + set(HAS_GENERIC_CLIENT FALSE) + message(STATUS "Building for ROS2 Humble - GenericClient not available") +elseif($ENV{ROS_DISTRO} STREQUAL "jazzy" OR $ENV{ROS_DISTRO} STREQUAL "rolling") + set(HAS_GENERIC_CLIENT TRUE) + message(STATUS "Building for ROS2 ${ROS_DISTRO} - GenericClient available") +else() + # Default: assume recent distro has GenericClient + set(HAS_GENERIC_CLIENT TRUE) + message(WARNING "Unknown ROS distro: $ENV{ROS_DISTRO} - assuming GenericClient is available") +endif() + +# Plugin library - always include topic plugin and conversion utilities +set(PLUGIN_SOURCES + src/generic_topic_plugin.cpp + src/message_conversion.cpp +) + +# Add service plugin only if GenericClient is available (Jazzy/Rolling+) +if(HAS_GENERIC_CLIENT) + list(APPEND PLUGIN_SOURCES src/generic_service_plugin.cpp) + message(STATUS "Including GenericServicePlugin (GenericClient available in ${ROS_DISTRO})") +else() + message(STATUS "Excluding GenericServicePlugin (GenericClient not available in ${ROS_DISTRO})") +endif() + +# NOTE: Generic action plugin is NOT implemented +# Generic action client API doesn't exist even in Jazzy/Rolling +# Will need custom implementation or typed action clients in the future + +add_library(${PROJECT_NAME} SHARED ${PLUGIN_SOURCES}) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC + robot_mcp_msg_pluginlib::robot_mcp_msg_pluginlib + PRIVATE + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + pluginlib::pluginlib + nlohmann_json::nlohmann_json + ${rosidl_typesupport_introspection_cpp_TARGETS} + ${rosidl_runtime_cpp_TARGETS} +) + +# Export plugin library +pluginlib_export_plugin_description_file(robot_mcp_msg_pluginlib plugins.xml) + +# Install +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(EXPORT ${PROJECT_NAME}Targets + NAMESPACE ${PROJECT_NAME}:: + DESTINATION share/${PROJECT_NAME}/cmake +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(FILES plugins.xml + DESTINATION share/${PROJECT_NAME} +) + +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_package() diff --git a/robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/generic_service_plugin.hpp b/robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/generic_service_plugin.hpp new file mode 100644 index 0000000..fb4a444 --- /dev/null +++ b/robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/generic_service_plugin.hpp @@ -0,0 +1,122 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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. + +#ifndef ROBOT_MCP_GENERIC_PLUGINS__GENERIC_SERVICE_PLUGIN_HPP_ +#define ROBOT_MCP_GENERIC_PLUGINS__GENERIC_SERVICE_PLUGIN_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include "robot_mcp_msg_pluginlib/message_plugin.hpp" +#include "robot_mcp_generic_plugins/message_conversion.hpp" + +namespace robot_mcp_generic_plugins +{ + +/** + * @brief Generic plugin for ALL ROS2 service types + * + * This plugin uses rclcpp::GenericClient and runtime type introspection + * to support any ROS2 service type without compile-time knowledge. + * + * Features: + * - Calls services with JSON request, returns JSON response + * - Automatic JSON ↔ ROS conversion via introspection + * - Tag dispatch for Request/Response parts + * - Exposed as MCP tools via tools/call + */ +class GenericServicePlugin : public robot_mcp_msg_pluginlib::MessagePlugin +{ +public: + GenericServicePlugin(); + virtual ~GenericServicePlugin(); + + // ============================================================================ + // MessagePlugin interface + // ============================================================================ + + void initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::string & primitive_name, + const std::string & description) override; + + std::vector getSupportedTypes() const override + { + return {}; // Empty = supports ALL types + } + + std::string getDescription() const override { return description_; } + + std::string getPrimitiveName() const override { return primitive_name_; } + + // ============================================================================ + // Service-specific methods (called by MCPRouter) + // ============================================================================ + + /** + * @brief Initialize service-specific configuration + * + * This is called after initialize() to provide service-specific config. + * + * @param service_name ROS2 service name (e.g., "/reset") + * @param srv_type ROS2 service type (e.g., "std_srvs/srv/Trigger") + * @param timeout_ms Service call timeout in milliseconds + */ + void configureService( + const std::string & service_name, + const std::string & srv_type, + int timeout_ms); + + /** + * @brief Call the service + * + * Called by MCPRouter when MCP client uses tools/call. + * Blocks until response is received or timeout occurs. + * + * @param request JSON service request + * @return JSON service response + * @throws std::runtime_error on timeout or service failure + */ + nlohmann::json callService(const nlohmann::json & request); + +protected: + // Node handle + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + + // Primitive identification + std::string primitive_name_; + std::string description_; + + // Service configuration + std::string service_name_; + std::string srv_type_; + int timeout_ms_{5000}; + + // Generic ROS2 client (runtime type handling) + std::shared_ptr client_; + + // Type support for introspection + const rosidl_service_type_support_t * type_support_{nullptr}; + rosidl_message_type_support_t request_type_support_{}; + rosidl_message_type_support_t response_type_support_{}; +}; + +} // namespace robot_mcp_generic_plugins + +#endif // ROBOT_MCP_GENERIC_PLUGINS__GENERIC_SERVICE_PLUGIN_HPP_ diff --git a/robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/generic_topic_plugin.hpp b/robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/generic_topic_plugin.hpp new file mode 100644 index 0000000..725b4ba --- /dev/null +++ b/robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/generic_topic_plugin.hpp @@ -0,0 +1,152 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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. + +#ifndef ROBOT_MCP_GENERIC_PLUGINS__GENERIC_TOPIC_PLUGIN_HPP_ +#define ROBOT_MCP_GENERIC_PLUGINS__GENERIC_TOPIC_PLUGIN_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "robot_mcp_msg_pluginlib/message_plugin.hpp" +#include "robot_mcp_generic_plugins/message_conversion.hpp" + +namespace robot_mcp_generic_plugins +{ + +/** + * @brief Generic plugin for ALL ROS2 topic types + * + * This plugin uses rclcpp::GenericSubscription and runtime type introspection + * to support any ROS2 message type without compile-time knowledge. + * + * Features: + * - Subscribes to topics and caches latest N messages + * - Publishes messages to topics + * - Automatic JSON ↔ ROS conversion via introspection + * - Exposed as MCP resources (for subscribe=true) and/or tools (for publish=true) + */ +class GenericTopicPlugin : public robot_mcp_msg_pluginlib::MessagePlugin +{ +public: + GenericTopicPlugin(); + virtual ~GenericTopicPlugin(); + + // ============================================================================ + // MessagePlugin interface + // ============================================================================ + + void initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::string & primitive_name, + const std::string & description) override; + + std::vector getSupportedTypes() const override + { + return {}; // Empty = supports ALL types + } + + std::string getDescription() const override { return description_; } + + std::string getPrimitiveName() const override { return primitive_name_; } + + // ============================================================================ + // Topic-specific methods (called by MCPRouter) + // ============================================================================ + + /** + * @brief Initialize topic-specific configuration + * + * This is called after initialize() to provide topic-specific config. + * + * @param topic_name ROS2 topic name (e.g., "/battery_state") + * @param msg_type ROS2 message type (e.g., "sensor_msgs/msg/BatteryState") + * @param subscribe Whether to subscribe and cache messages + * @param publish Whether to allow publishing + */ + void configureTopic( + const std::string & topic_name, + const std::string & msg_type, + bool subscribe, + bool publish); + + /** + * @brief Publish a message to the topic + * + * Called by MCPRouter when MCP client uses tools/call to publish. + * + * @param msg JSON message to publish + * @throws std::runtime_error if publish is not enabled + */ + void publishMessage(const nlohmann::json & msg); + + /** + * @brief Read the latest cached message + * + * Called by MCPRouter when MCP client uses resources/read. + * + * @return JSON representation of latest message + * @throws std::runtime_error if subscribe is not enabled or no messages cached + */ + nlohmann::json readCachedMessage() const; + + /** + * @brief Check if plugin has cached messages available + * + * @return true if at least one message is cached + */ + bool hasCachedMessages() const { return !message_cache_.empty(); } + +protected: + // Node handle + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + + // Primitive identification + std::string primitive_name_; + std::string description_; + + // Topic configuration + std::string topic_name_; + std::string msg_type_; + bool subscribe_enabled_{false}; + bool publish_enabled_{false}; + + // Generic ROS2 clients (runtime type handling) + std::shared_ptr subscription_; + std::shared_ptr publisher_; + + // Message cache (JSON format for fast access) + static constexpr size_t MAX_CACHE_SIZE = 10; + mutable std::deque message_cache_; + + // Type support for introspection + const rosidl_message_type_support_t * type_support_{nullptr}; + + /** + * @brief Callback for generic subscription + * + * Converts ROS message to JSON and caches it. + */ + void subscriptionCallback(std::shared_ptr serialized_msg); +}; + +} // namespace robot_mcp_generic_plugins + +#endif // ROBOT_MCP_GENERIC_PLUGINS__GENERIC_TOPIC_PLUGIN_HPP_ diff --git a/robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/message_conversion.hpp b/robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/message_conversion.hpp new file mode 100644 index 0000000..18fbf28 --- /dev/null +++ b/robot_mcp_generic_plugins/include/robot_mcp_generic_plugins/message_conversion.hpp @@ -0,0 +1,112 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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. + +#ifndef ROBOT_MCP_GENERIC_PLUGINS__MESSAGE_CONVERSION_HPP_ +#define ROBOT_MCP_GENERIC_PLUGINS__MESSAGE_CONVERSION_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace robot_mcp_generic_plugins +{ +namespace conversion +{ + +/** + * @brief Convert ROS2 message to JSON using runtime introspection + * + * This function uses rosidl_typesupport_introspection_cpp to walk the message + * structure at runtime and convert each field to JSON. + * + * @param ros_msg Pointer to ROS2 message + * @param type_support Type support structure for introspection + * @return JSON representation of the message + * + * TODO: Implement full introspection-based conversion + */ +nlohmann::json rosMessageToJson( + const void * ros_msg, + const rosidl_message_type_support_t * type_support); + +/** + * @brief Convert JSON to ROS2 message using runtime introspection + * + * This function uses rosidl_typesupport_introspection_cpp to walk the message + * structure at runtime and populate each field from JSON. + * + * @param json JSON object to deserialize + * @param ros_msg Pointer to ROS2 message to populate + * @param type_support Type support structure for introspection + * + * TODO: Implement full introspection-based conversion + */ +void jsonToRosMessage( + const nlohmann::json & json, + void * ros_msg, + const rosidl_message_type_support_t * type_support); + +/** + * @brief Get message type support from type string + * + * Resolves message type support at runtime from a string like + * "sensor_msgs/msg/BatteryState". + * + * @param msg_type Message type string + * @return Type support structure + * @throws std::runtime_error if type not found + * + * TODO: Implement type support lookup + */ +const rosidl_message_type_support_t * getMessageTypeSupport( + const std::string & msg_type); + +/** + * @brief Get service type support from type string + * + * Resolves service type support at runtime from a string like + * "std_srvs/srv/Trigger". + * + * @param srv_type Service type string + * @return Type support structure + * @throws std::runtime_error if type not found + * + * TODO: Implement type support lookup + */ +const rosidl_service_type_support_t * getServiceTypeSupport( + const std::string & srv_type); + +/** + * @brief Get action type support from type string + * + * Resolves action type support at runtime from a string like + * "nav2_msgs/action/NavigateToPose". + * + * @param action_type Action type string + * @return Type support structure + * @throws std::runtime_error if type not found + * + * TODO: Implement type support lookup + */ +const rosidl_action_type_support_t * getActionTypeSupport( + const std::string & action_type); + +} // namespace conversion +} // namespace robot_mcp_generic_plugins + +#endif // ROBOT_MCP_GENERIC_PLUGINS__MESSAGE_CONVERSION_HPP_ diff --git a/robot_mcp_generic_plugins/package.xml b/robot_mcp_generic_plugins/package.xml new file mode 100644 index 0000000..9b99525 --- /dev/null +++ b/robot_mcp_generic_plugins/package.xml @@ -0,0 +1,29 @@ + + + + robot_mcp_generic_plugins + 0.1.0 + Generic ROS2 message plugins for robot_mcp that support all message types via runtime introspection + Eddy + Apache-2.0 + + ament_cmake + + + rclcpp + rclcpp_lifecycle + pluginlib + nlohmann-json-dev + + + robot_mcp_msg_pluginlib + + + rosidl_typesupport_introspection_cpp + rosidl_runtime_cpp + + + ament_cmake + + + diff --git a/robot_mcp_generic_plugins/plugins.xml b/robot_mcp_generic_plugins/plugins.xml new file mode 100644 index 0000000..5ac3615 --- /dev/null +++ b/robot_mcp_generic_plugins/plugins.xml @@ -0,0 +1,25 @@ + + + + + Generic plugin for all ROS2 topic types using runtime introspection. + Works for: geometry_msgs, nav_msgs, std_msgs, sensor_msgs (except Image/PointCloud). + Use specialized plugins for images and point clouds. + + + + + + + Generic plugin for all ROS2 service types using GenericClient. + Only available in ROS2 Jazzy and Rolling (not available in Humble). + + + + + diff --git a/robot_mcp_generic_plugins/src/generic_service_plugin.cpp b/robot_mcp_generic_plugins/src/generic_service_plugin.cpp new file mode 100644 index 0000000..dfa5751 --- /dev/null +++ b/robot_mcp_generic_plugins/src/generic_service_plugin.cpp @@ -0,0 +1,161 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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. + +#include "robot_mcp_generic_plugins/generic_service_plugin.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace robot_mcp_generic_plugins +{ + +GenericServicePlugin::GenericServicePlugin() +{ +} + +GenericServicePlugin::~GenericServicePlugin() +{ +} + +void GenericServicePlugin::initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::string & primitive_name, + const std::string & description) +{ + node_ = node; + primitive_name_ = primitive_name; + description_ = description; +} + +void GenericServicePlugin::configureService( + const std::string & service_name, + const std::string & srv_type, + int timeout_ms) +{ + service_name_ = service_name; + srv_type_ = srv_type; + timeout_ms_ = timeout_ms; + + // Get type support from runtime introspection + type_support_ = conversion::getServiceTypeSupport(srv_type); + + // Extract request and response message members from service type support + const auto * service_members = static_cast( + type_support_->data); + + // Create type support structs for request and response + request_type_support_.typesupport_identifier = rosidl_typesupport_introspection_cpp::typesupport_identifier; + request_type_support_.data = service_members->request_members_; + request_type_support_.func = nullptr; + + response_type_support_.typesupport_identifier = rosidl_typesupport_introspection_cpp::typesupport_identifier; + response_type_support_.data = service_members->response_members_; + response_type_support_.func = nullptr; + + // Create generic client using the factory function + client_ = rclcpp::create_generic_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + service_name_, + srv_type_, + rclcpp::ServicesQoS(), + nullptr // callback group + ); +} + +nlohmann::json GenericServicePlugin::callService(const nlohmann::json & request) +{ + if (!client_) { + throw std::runtime_error( + "Service client not initialized for primitive: " + primitive_name_); + } + + // Wait for service to be available + if (!client_->wait_for_service(std::chrono::seconds(5))) { + throw std::runtime_error( + "Service not available: " + service_name_); + } + + // Get request/response message members for allocation + const auto * request_members = static_cast( + request_type_support_.data); + const auto * response_members = static_cast( + response_type_support_.data); + + // Allocate ROS request message + std::vector ros_request_buffer(request_members->size_of_); + void * ros_request = ros_request_buffer.data(); + request_members->init_function(ros_request, rosidl_runtime_cpp::MessageInitialization::ZERO); + + // Allocate ROS response message + std::vector ros_response_buffer(response_members->size_of_); + void * ros_response = ros_response_buffer.data(); + response_members->init_function(ros_response, rosidl_runtime_cpp::MessageInitialization::ZERO); + + try { + // Convert JSON to ROS request using introspection + conversion::jsonToRosMessage(request, ros_request, &request_type_support_); + + // Serialize request + rclcpp::SerializedMessage serialized_request; + rclcpp::SerializationBase request_serializer(&request_type_support_); + request_serializer.serialize_message(ros_request, &serialized_request); + + // Call service (GenericClient expects void* to serialized data) + auto future = client_->async_send_request(static_cast(&serialized_request)); + auto status = future.wait_for(std::chrono::milliseconds(timeout_ms_)); + + if (status != std::future_status::ready) { + // Cleanup + request_members->fini_function(ros_request); + response_members->fini_function(ros_response); + throw std::runtime_error("Service call timeout: " + service_name_); + } + + // Get serialized response (returns std::shared_ptr which is actually a SerializedMessage*) + auto serialized_response_ptr = future.get(); + auto * serialized_response = static_cast(serialized_response_ptr.get()); + + // Deserialize response + rclcpp::SerializationBase response_serializer(&response_type_support_); + response_serializer.deserialize_message(serialized_response, ros_response); + + // Convert ROS response to JSON using introspection + nlohmann::json json_response = conversion::rosMessageToJson(ros_response, &response_type_support_); + + // Cleanup + request_members->fini_function(ros_request); + response_members->fini_function(ros_response); + + return json_response; + + } catch (...) { + // Cleanup on exception + request_members->fini_function(ros_request); + response_members->fini_function(ros_response); + throw; + } +} + +} // namespace robot_mcp_generic_plugins + +#include +PLUGINLIB_EXPORT_CLASS(robot_mcp_generic_plugins::GenericServicePlugin, robot_mcp_msg_pluginlib::MessagePlugin) diff --git a/robot_mcp_generic_plugins/src/generic_topic_plugin.cpp b/robot_mcp_generic_plugins/src/generic_topic_plugin.cpp new file mode 100644 index 0000000..3f00edc --- /dev/null +++ b/robot_mcp_generic_plugins/src/generic_topic_plugin.cpp @@ -0,0 +1,182 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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. + +#include "robot_mcp_generic_plugins/generic_topic_plugin.hpp" + +#include + +#include +#include +#include + +namespace robot_mcp_generic_plugins +{ + +GenericTopicPlugin::GenericTopicPlugin() +{ +} + +GenericTopicPlugin::~GenericTopicPlugin() +{ +} + +void GenericTopicPlugin::initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::string & primitive_name, + const std::string & description) +{ + node_ = node; + primitive_name_ = primitive_name; + description_ = description; +} + +void GenericTopicPlugin::configureTopic( + const std::string & topic_name, + const std::string & msg_type, + bool subscribe, + bool publish) +{ + topic_name_ = topic_name; + msg_type_ = msg_type; + subscribe_enabled_ = subscribe; + publish_enabled_ = publish; + + // Get type support from runtime introspection + type_support_ = conversion::getMessageTypeSupport(msg_type); + + if (subscribe_enabled_) { + // Create generic subscription + auto callback = [this](std::shared_ptr msg) { + this->subscriptionCallback(msg); + }; + + subscription_ = node_->create_generic_subscription( + topic_name_, + msg_type_, + rclcpp::QoS(10), + callback); + } + + if (publish_enabled_) { + // Create generic publisher + publisher_ = node_->create_generic_publisher( + topic_name_, + msg_type_, + rclcpp::QoS(10)); + } +} + +void GenericTopicPlugin::publishMessage(const nlohmann::json & msg) +{ + if (!publish_enabled_) { + throw std::runtime_error( + "Publishing not enabled for primitive: " + primitive_name_); + } + + if (!publisher_) { + throw std::runtime_error( + "Publisher not initialized for primitive: " + primitive_name_); + } + + // Get message members for allocation + const auto * members = static_cast( + type_support_->data); + + // Allocate ROS message + std::vector ros_msg_buffer(members->size_of_); + void * ros_msg = ros_msg_buffer.data(); + + // Initialize the message + members->init_function(ros_msg, rosidl_runtime_cpp::MessageInitialization::ZERO); + + try { + // Convert JSON to ROS message using introspection + conversion::jsonToRosMessage(msg, ros_msg, type_support_); + + // Serialize the message + rclcpp::SerializedMessage serialized_msg; + rclcpp::SerializationBase serializer(type_support_); + serializer.serialize_message(ros_msg, &serialized_msg); + + // Publish + publisher_->publish(serialized_msg); + + // Cleanup + members->fini_function(ros_msg); + } catch (...) { + // Cleanup on exception + members->fini_function(ros_msg); + throw; + } +} + +nlohmann::json GenericTopicPlugin::readCachedMessage() const +{ + if (!subscribe_enabled_) { + throw std::runtime_error( + "Subscription not enabled for primitive: " + primitive_name_); + } + + if (message_cache_.empty()) { + throw std::runtime_error( + "No cached messages available for primitive: " + primitive_name_); + } + + return message_cache_.front(); +} + +void GenericTopicPlugin::subscriptionCallback( + std::shared_ptr serialized_msg) +{ + // Get message members for allocation + const auto * members = static_cast( + type_support_->data); + + // Allocate ROS message + std::vector ros_msg_buffer(members->size_of_); + void * ros_msg = ros_msg_buffer.data(); + + // Initialize the message + members->init_function(ros_msg, rosidl_runtime_cpp::MessageInitialization::ZERO); + + try { + // Deserialize + rclcpp::SerializationBase serializer(type_support_); + serializer.deserialize_message(serialized_msg.get(), ros_msg); + + // Convert to JSON using introspection + nlohmann::json json_msg = conversion::rosMessageToJson(ros_msg, type_support_); + + // Add to cache + message_cache_.push_front(json_msg); + if (message_cache_.size() > MAX_CACHE_SIZE) { + message_cache_.pop_back(); + } + + // Cleanup + members->fini_function(ros_msg); + } catch (const std::exception & e) { + // Cleanup on exception + members->fini_function(ros_msg); + RCLCPP_ERROR( + node_->get_logger(), + "Failed to deserialize/convert message on topic %s: %s", + topic_name_.c_str(), e.what()); + } +} + +} // namespace robot_mcp_generic_plugins + +#include +PLUGINLIB_EXPORT_CLASS(robot_mcp_generic_plugins::GenericTopicPlugin, robot_mcp_msg_pluginlib::MessagePlugin) diff --git a/robot_mcp_generic_plugins/src/message_conversion.cpp b/robot_mcp_generic_plugins/src/message_conversion.cpp new file mode 100644 index 0000000..ebb034c --- /dev/null +++ b/robot_mcp_generic_plugins/src/message_conversion.cpp @@ -0,0 +1,511 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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. + +#include "robot_mcp_generic_plugins/message_conversion.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace robot_mcp_generic_plugins +{ +namespace conversion +{ + +namespace +{ + +// Forward declarations for recursive conversion +nlohmann::json fieldToJson( + const void * field_ptr, + const rosidl_typesupport_introspection_cpp::MessageMember & member, + const rosidl_message_type_support_t * type_support); + +void jsonToField( + const nlohmann::json & json, + void * field_ptr, + const rosidl_typesupport_introspection_cpp::MessageMember & member, + const rosidl_message_type_support_t * type_support); + +// Convert primitive field to JSON +nlohmann::json primitiveToJson( + const void * field_ptr, + uint8_t type_id) +{ + using namespace rosidl_typesupport_introspection_cpp; + + switch (type_id) { + case ROS_TYPE_BOOL: + return *static_cast(field_ptr); + case ROS_TYPE_BYTE: + case ROS_TYPE_UINT8: + return *static_cast(field_ptr); + case ROS_TYPE_CHAR: + case ROS_TYPE_INT8: + return *static_cast(field_ptr); + case ROS_TYPE_UINT16: + return *static_cast(field_ptr); + case ROS_TYPE_INT16: + return *static_cast(field_ptr); + case ROS_TYPE_UINT32: + return *static_cast(field_ptr); + case ROS_TYPE_INT32: + return *static_cast(field_ptr); + case ROS_TYPE_UINT64: + return *static_cast(field_ptr); + case ROS_TYPE_INT64: + return *static_cast(field_ptr); + case ROS_TYPE_FLOAT: + return *static_cast(field_ptr); + case ROS_TYPE_DOUBLE: + return *static_cast(field_ptr); + case ROS_TYPE_STRING: + return *static_cast(field_ptr); + default: + throw std::runtime_error("Unknown primitive type: " + std::to_string(type_id)); + } +} + +// Convert JSON to primitive field +void jsonToPrimitive( + const nlohmann::json & json, + void * field_ptr, + uint8_t type_id) +{ + using namespace rosidl_typesupport_introspection_cpp; + + switch (type_id) { + case ROS_TYPE_BOOL: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_BYTE: + case ROS_TYPE_UINT8: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_CHAR: + case ROS_TYPE_INT8: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_UINT16: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_INT16: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_UINT32: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_INT32: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_UINT64: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_INT64: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_FLOAT: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_DOUBLE: + *static_cast(field_ptr) = json.get(); + break; + case ROS_TYPE_STRING: + *static_cast(field_ptr) = json.get(); + break; + default: + throw std::runtime_error("Unknown primitive type: " + std::to_string(type_id)); + } +} + +// Convert array/sequence field to JSON +nlohmann::json arrayToJson( + const void * field_ptr, + const rosidl_typesupport_introspection_cpp::MessageMember & member, + const rosidl_message_type_support_t * type_support) +{ + using namespace rosidl_typesupport_introspection_cpp; + + nlohmann::json array = nlohmann::json::array(); + + // Get array/sequence size using the proper introspection function + size_t array_size = 0; + if (member.is_array_) { + if (member.is_upper_bound_) { + // Dynamic-size bounded sequence - use size function + array_size = member.size_function(field_ptr); + } else { + // Fixed-size array + array_size = member.array_size_; + } + } else { + // Unbounded sequence - use size function + array_size = member.size_function(field_ptr); + } + + // Process each element using get_const_function for proper type-safe access + for (size_t i = 0; i < array_size; ++i) { + const void * element_ptr = member.get_const_function(field_ptr, i); + + if (member.type_id_ == ROS_TYPE_MESSAGE) { + // Nested message array + const auto * nested_members = static_cast(member.members_->data); + nlohmann::json element_json; + for (size_t j = 0; j < nested_members->member_count_; ++j) { + const auto & nested_member = nested_members->members_[j]; + const void * nested_field_ptr = static_cast(element_ptr) + nested_member.offset_; + element_json[nested_member.name_] = fieldToJson(nested_field_ptr, nested_member, type_support); + } + array.push_back(element_json); + } else { + // Primitive array - element_ptr points directly to the element + array.push_back(primitiveToJson(element_ptr, member.type_id_)); + } + } + + return array; +} + +// Convert JSON to array/sequence field +void jsonToArray( + const nlohmann::json & json, + void * field_ptr, + const rosidl_typesupport_introspection_cpp::MessageMember & member, + const rosidl_message_type_support_t * type_support) +{ + using namespace rosidl_typesupport_introspection_cpp; + + if (!json.is_array()) { + throw std::runtime_error("Expected JSON array for field: " + std::string(member.name_)); + } + + size_t json_size = json.size(); + + if (member.is_array_) { + if (!member.is_upper_bound_) { + // Fixed-size array - size must match exactly + if (json_size != member.array_size_) { + throw std::runtime_error( + "Array size mismatch for field " + std::string(member.name_) + + ": expected " + std::to_string(member.array_size_) + + ", got " + std::to_string(json_size)); + } + } else { + // Bounded sequence - resize if needed + if (json_size > member.array_size_) { + throw std::runtime_error( + "Array size exceeds upper bound for field " + std::string(member.name_) + + ": max " + std::to_string(member.array_size_) + + ", got " + std::to_string(json_size)); + } + if (member.resize_function) { + member.resize_function(field_ptr, json_size); + } + } + } else { + // Unbounded sequence - resize to JSON array size + if (member.resize_function) { + member.resize_function(field_ptr, json_size); + } + } + + // Populate elements using get_function for proper type-safe access + for (size_t i = 0; i < json_size; ++i) { + void * element_ptr = member.get_function(field_ptr, i); + + if (member.type_id_ == ROS_TYPE_MESSAGE) { + // Nested message array/sequence + const auto * nested_members = static_cast(member.members_->data); + for (size_t j = 0; j < nested_members->member_count_; ++j) { + const auto & nested_member = nested_members->members_[j]; + void * nested_field_ptr = static_cast(element_ptr) + nested_member.offset_; + if (json[i].contains(nested_member.name_)) { + jsonToField(json[i][nested_member.name_], nested_field_ptr, nested_member, type_support); + } + } + } else { + // Primitive array/sequence - element_ptr points directly to the element + jsonToPrimitive(json[i], element_ptr, member.type_id_); + } + } +} + +// Convert nested message field to JSON +nlohmann::json messageToJson( + const void * field_ptr, + const rosidl_typesupport_introspection_cpp::MessageMember & member) +{ + using namespace rosidl_typesupport_introspection_cpp; + + const auto * nested_members = static_cast(member.members_->data); + nlohmann::json result; + + for (size_t i = 0; i < nested_members->member_count_; ++i) { + const auto & nested_member = nested_members->members_[i]; + const void * nested_field_ptr = static_cast(field_ptr) + nested_member.offset_; + result[nested_member.name_] = fieldToJson(nested_field_ptr, nested_member, nullptr); + } + + return result; +} + +// Convert JSON to nested message field +void jsonToMessage( + const nlohmann::json & json, + void * field_ptr, + const rosidl_typesupport_introspection_cpp::MessageMember & member) +{ + using namespace rosidl_typesupport_introspection_cpp; + + if (!json.is_object()) { + throw std::runtime_error("Expected JSON object for field: " + std::string(member.name_)); + } + + const auto * nested_members = static_cast(member.members_->data); + + for (size_t i = 0; i < nested_members->member_count_; ++i) { + const auto & nested_member = nested_members->members_[i]; + void * nested_field_ptr = static_cast(field_ptr) + nested_member.offset_; + + if (json.contains(nested_member.name_)) { + jsonToField(json[nested_member.name_], nested_field_ptr, nested_member, nullptr); + } + } +} + +// Convert any field to JSON (recursive) +nlohmann::json fieldToJson( + const void * field_ptr, + const rosidl_typesupport_introspection_cpp::MessageMember & member, + const rosidl_message_type_support_t * type_support) +{ + using namespace rosidl_typesupport_introspection_cpp; + + if (member.is_array_ || !member.is_upper_bound_) { + // Array or sequence + return arrayToJson(field_ptr, member, type_support); + } else if (member.type_id_ == ROS_TYPE_MESSAGE) { + // Nested message + return messageToJson(field_ptr, member); + } else { + // Primitive + return primitiveToJson(field_ptr, member.type_id_); + } +} + +// Convert JSON to any field (recursive) +void jsonToField( + const nlohmann::json & json, + void * field_ptr, + const rosidl_typesupport_introspection_cpp::MessageMember & member, + const rosidl_message_type_support_t * type_support) +{ + using namespace rosidl_typesupport_introspection_cpp; + + if (member.is_array_ || !member.is_upper_bound_) { + // Array or sequence + jsonToArray(json, field_ptr, member, type_support); + } else if (member.type_id_ == ROS_TYPE_MESSAGE) { + // Nested message + jsonToMessage(json, field_ptr, member); + } else { + // Primitive + jsonToPrimitive(json, field_ptr, member.type_id_); + } +} + +} // anonymous namespace + +nlohmann::json rosMessageToJson( + const void * ros_msg, + const rosidl_message_type_support_t * type_support) +{ + if (!ros_msg || !type_support) { + throw std::runtime_error("Null pointer provided to rosMessageToJson"); + } + + // Ensure we have introspection type support + if (type_support->typesupport_identifier != + rosidl_typesupport_introspection_cpp::typesupport_identifier) { + throw std::runtime_error( + "Type support must be from rosidl_typesupport_introspection_cpp"); + } + + const auto * members = static_cast( + type_support->data); + + nlohmann::json result; + + // Walk all message fields + for (size_t i = 0; i < members->member_count_; ++i) { + const auto & member = members->members_[i]; + const void * field_ptr = static_cast(ros_msg) + member.offset_; + + result[member.name_] = fieldToJson(field_ptr, member, type_support); + } + + return result; +} + +void jsonToRosMessage( + const nlohmann::json & json, + void * ros_msg, + const rosidl_message_type_support_t * type_support) +{ + if (!ros_msg || !type_support) { + throw std::runtime_error("Null pointer provided to jsonToRosMessage"); + } + + if (!json.is_object()) { + throw std::runtime_error("JSON must be an object to convert to ROS message"); + } + + // Ensure we have introspection type support + if (type_support->typesupport_identifier != + rosidl_typesupport_introspection_cpp::typesupport_identifier) { + throw std::runtime_error( + "Type support must be from rosidl_typesupport_introspection_cpp"); + } + + const auto * members = static_cast( + type_support->data); + + // Walk all message fields + for (size_t i = 0; i < members->member_count_; ++i) { + const auto & member = members->members_[i]; + void * field_ptr = static_cast(ros_msg) + member.offset_; + + if (json.contains(member.name_)) { + jsonToField(json[member.name_], field_ptr, member, type_support); + } + } +} + +const rosidl_message_type_support_t * getMessageTypeSupport( + const std::string & msg_type) +{ + // Parse msg_type string: "package_name/msg/MessageName" + std::istringstream iss(msg_type); + std::string package, msg_namespace, message_name; + + std::getline(iss, package, '/'); + std::getline(iss, msg_namespace, '/'); + std::getline(iss, message_name); + + if (package.empty() || msg_namespace != "msg" || message_name.empty()) { + throw std::runtime_error("Invalid message type format: " + msg_type + " (expected: package/msg/MessageName)"); + } + + // Build symbol name for type support lookup + // Format: rosidl_typesupport_introspection_cpp__get_message_type_support_handle__package_name__msg__MessageName + std::string symbol_name = + "rosidl_typesupport_introspection_cpp__get_message_type_support_handle__" + + package + "__msg__" + message_name; + + // Load type support using dlsym from RTLD_DEFAULT (searches all loaded libraries) + void * symbol = dlsym(RTLD_DEFAULT, symbol_name.c_str()); + if (!symbol) { + throw std::runtime_error( + "Failed to find type support for " + msg_type + + ": symbol " + symbol_name + " not found. " + + "Error: " + (dlerror() ? dlerror() : "unknown")); + } + + // Call the function to get type support + using TypeSupportFunction = const rosidl_message_type_support_t * (*)(); + auto get_type_support = reinterpret_cast(symbol); + + return get_type_support(); +} + +const rosidl_service_type_support_t * getServiceTypeSupport( + const std::string & srv_type) +{ + // Parse srv_type string: "package_name/srv/ServiceName" + std::istringstream iss(srv_type); + std::string package, srv_namespace, service_name; + + std::getline(iss, package, '/'); + std::getline(iss, srv_namespace, '/'); + std::getline(iss, service_name); + + if (package.empty() || srv_namespace != "srv" || service_name.empty()) { + throw std::runtime_error("Invalid service type format: " + srv_type + " (expected: package/srv/ServiceName)"); + } + + // Build symbol name for type support lookup + std::string symbol_name = + "rosidl_typesupport_introspection_cpp__get_service_type_support_handle__" + + package + "__srv__" + service_name; + + // Load type support using dlsym from RTLD_DEFAULT + void * symbol = dlsym(RTLD_DEFAULT, symbol_name.c_str()); + if (!symbol) { + throw std::runtime_error( + "Failed to find type support for " + srv_type + + ": symbol " + symbol_name + " not found. " + + "Error: " + (dlerror() ? dlerror() : "unknown")); + } + + // Call the function to get type support + using TypeSupportFunction = const rosidl_service_type_support_t * (*)(); + auto get_type_support = reinterpret_cast(symbol); + + return get_type_support(); +} + +const rosidl_action_type_support_t * getActionTypeSupport( + const std::string & action_type) +{ + // Parse action_type string: "package_name/action/ActionName" + std::istringstream iss(action_type); + std::string package, action_namespace, action_name; + + std::getline(iss, package, '/'); + std::getline(iss, action_namespace, '/'); + std::getline(iss, action_name); + + if (package.empty() || action_namespace != "action" || action_name.empty()) { + throw std::runtime_error("Invalid action type format: " + action_type + " (expected: package/action/ActionName)"); + } + + // Build symbol name for type support lookup + std::string symbol_name = + "rosidl_typesupport_introspection_cpp__get_action_type_support_handle__" + + package + "__action__" + action_name; + + // Load type support using dlsym from RTLD_DEFAULT + void * symbol = dlsym(RTLD_DEFAULT, symbol_name.c_str()); + if (!symbol) { + throw std::runtime_error( + "Failed to find type support for " + action_type + + ": symbol " + symbol_name + " not found. " + + "Error: " + (dlerror() ? dlerror() : "unknown")); + } + + // Call the function to get type support + using TypeSupportFunction = const rosidl_action_type_support_t * (*)(); + auto get_type_support = reinterpret_cast(symbol); + + return get_type_support(); +} + +} // namespace conversion +} // namespace robot_mcp_generic_plugins diff --git a/robot_mcp_msg_pluginlib/include/robot_mcp_msg_pluginlib/message_plugin.hpp b/robot_mcp_msg_pluginlib/include/robot_mcp_msg_pluginlib/message_plugin.hpp index 8833827..b6ec26d 100644 --- a/robot_mcp_msg_pluginlib/include/robot_mcp_msg_pluginlib/message_plugin.hpp +++ b/robot_mcp_msg_pluginlib/include/robot_mcp_msg_pluginlib/message_plugin.hpp @@ -12,39 +12,46 @@ // See the License for the specific language governing permissions and // limitations under the License. -// Copyright 2025 WATonomous -// -// 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. - #ifndef ROBOT_MCP_MSG_PLUGINLIB__MESSAGE_PLUGIN_HPP_ #define ROBOT_MCP_MSG_PLUGINLIB__MESSAGE_PLUGIN_HPP_ -#include - +#include #include +#include +#include #include namespace robot_mcp_msg_pluginlib { +// Forward declarations for config types +namespace config +{ +struct TopicConfig; +struct ServiceConfig; +struct ActionConfig; +} // namespace config + +/** + * @brief Tag dispatch types for service/action message parts + * + * These empty structs are used for compile-time function overload resolution + * while maintaining runtime polymorphism through virtual functions. + */ +namespace message_part +{ +struct Request {}; // For service requests and action goals +struct Response {}; // For service responses and action results +struct Feedback {}; // For action feedback only +} // namespace message_part + /** - * @brief Base class for all message plugins + * @brief Base interface for all MCP message plugins * - * Each plugin handles a specific ROS2 message type and provides: - * - JSON serialization (ROS2 message -> JSON) - * - JSON deserialization (JSON -> ROS2 message) - * - Message type information for dynamic type handling + * This is the minimal interface that all plugins must implement for pluginlib discovery. + * Derived plugins (BasicTopicPlugin, BasicServicePlugin, BasicActionPlugin) extend this + * with specific methods for their operation type. * * Plugins are discovered automatically at runtime using pluginlib. */ @@ -54,30 +61,46 @@ class MessagePlugin virtual ~MessagePlugin() = default; /** - * @brief Get the ROS2 message type this plugin handles - * @return Message type string (e.g., "sensor_msgs/msg/BatteryState") + * @brief Initialize plugin with lifecycle node and primitive configuration + * + * This is called once during startup after the plugin is loaded. + * Plugins should create ROS2 clients (subscribers, service clients, etc.) here. + * + * @param node Shared pointer to the lifecycle node + * @param primitive_name Name of the primitive (e.g., "battery", "navigate") + * @param description Human-readable description for MCP tools/resources list */ - virtual std::string getMessageType() const = 0; + virtual void initialize( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::string & primitive_name, + const std::string & description) = 0; /** - * @brief Convert ROS2 message to JSON - * @param msg_ptr Pointer to the ROS2 message (must be correct type) - * @return JSON representation of the message + * @brief Get list of supported message types + * + * @return Empty vector = supports ALL types (generic plugin) + * Non-empty vector = only supports specific types (custom plugin) + * + * Used by MCPPrimitiveFactory to validate plugin compatibility at startup. */ - virtual nlohmann::json toJson(const void * msg_ptr) const = 0; + virtual std::vector getSupportedTypes() const = 0; /** - * @brief Convert JSON to ROS2 message - * @param j JSON object to deserialize - * @param msg_ptr Pointer to the ROS2 message to populate (must be correct type) + * @brief Get human-readable description for MCP protocol + * + * This description is shown to Claude in tools/list and resources/list responses, + * allowing Claude to reason about what each primitive does. + * + * @return Description string (from config) */ - virtual void fromJson(const nlohmann::json & j, void * msg_ptr) const = 0; + virtual std::string getDescription() const = 0; /** - * @brief Get the rosidl message type support structure - * @return Pointer to the type support structure for dynamic message handling + * @brief Get the primitive name + * + * @return Primitive name (e.g., "battery", "cmd_vel", "navigate") */ - virtual const rosidl_message_type_support_t * getTypeSupport() const = 0; + virtual std::string getPrimitiveName() const = 0; }; } // namespace robot_mcp_msg_pluginlib diff --git a/robot_mcp_server/include/robot_mcp_server/mcp_config/config_types.hpp b/robot_mcp_server/include/robot_mcp_server/mcp_config/config_types.hpp index 32ccc31..ce8e72b 100644 --- a/robot_mcp_server/include/robot_mcp_server/mcp_config/config_types.hpp +++ b/robot_mcp_server/include/robot_mcp_server/mcp_config/config_types.hpp @@ -78,6 +78,7 @@ struct TopicConfig std::string topic; ///< ROS2 topic name std::string msg_type; ///< ROS2 message type (e.g., "std_msgs/msg/String") std::string plugin; ///< Plugin class name + std::string description; ///< Human-readable description for MCP tools/resources list bool subscribe{true}; ///< Subscribe and cache messages bool publish{true}; ///< Allow publishing to this topic std::vector resource_groups; ///< Resource groups this topic belongs to @@ -92,6 +93,7 @@ struct ServiceConfig std::string service; ///< ROS2 service name std::string srv_type; ///< ROS2 service type std::string plugin; ///< Plugin class name + std::string description; ///< Human-readable description for MCP tools list int timeout_ms{5000}; ///< Service call timeout std::vector resource_groups; ///< Resource groups this service belongs to }; @@ -105,6 +107,7 @@ struct ActionConfig std::string action; ///< ROS2 action name std::string action_type; ///< ROS2 action type std::string plugin; ///< Plugin class name + std::string description; ///< Human-readable description for MCP tools list int timeout_ms{0}; ///< Action timeout (0 = no timeout) bool cancellable{true}; ///< Whether action can be cancelled std::vector resource_groups; ///< Resource groups this action belongs to diff --git a/robot_mcp_server/test/test_http_components.cpp b/robot_mcp_server/test/test_http_components.cpp index cd344b8..fb81fd7 100644 --- a/robot_mcp_server/test/test_http_components.cpp +++ b/robot_mcp_server/test/test_http_components.cpp @@ -14,8 +14,8 @@ #include -#include #include +#include #include "robot_mcp_server/mcp_http_server/json_rpc_handler.hpp" diff --git a/robot_mcp_server/test/test_lifecycle.cpp b/robot_mcp_server/test/test_lifecycle.cpp index 11bde65..60a8876 100644 --- a/robot_mcp_server/test/test_lifecycle.cpp +++ b/robot_mcp_server/test/test_lifecycle.cpp @@ -31,7 +31,6 @@ #include #include -#include #include #include #include diff --git a/robot_mcp_test/README.md b/robot_mcp_test/README.md index b0141c4..d24ed23 100644 --- a/robot_mcp_test/README.md +++ b/robot_mcp_test/README.md @@ -12,7 +12,6 @@ A ROS 2 testing framework built on Catch2 that provides test fixtures and utilit ## Quick Start ```cpp -#include #include TEST_CASE_METHOD(robot_mcp::test::TestExecutorFixture, "My ROS Test", "[ros]") { diff --git a/robot_mcp_test/cmake/add_robot_mcp_test.cmake b/robot_mcp_test/cmake/add_robot_mcp_test.cmake index 32b5fa8..a178d1a 100644 --- a/robot_mcp_test/cmake/add_robot_mcp_test.cmake +++ b/robot_mcp_test/cmake/add_robot_mcp_test.cmake @@ -31,11 +31,18 @@ function(add_robot_mcp_test TEST_NAME TEST_SOURCE) "LIBRARIES" # Multi-value keywords ) - # Find dependencies - find_package(Catch2 2 REQUIRED) + # Find dependencies (version-agnostic for Ubuntu 22.04/24.04 compatibility) + find_package(Catch2 REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) + # Detect Catch2 version and set compile definition + if(Catch2_VERSION VERSION_GREATER_EQUAL "3.0.0") + set(CATCH2_COMPILE_DEF "CATCH2_V3") + else() + set(CATCH2_COMPILE_DEF "CATCH2_V2") + endif() + # Create the test executable add_executable(${TEST_NAME} ${TEST_SOURCE}) @@ -60,6 +67,9 @@ function(add_robot_mcp_test TEST_NAME TEST_SOURCE) # Add test include directory target_include_directories(${TEST_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test) + # Add Catch2 version compile definition for cross-compatibility + target_compile_definitions(${TEST_NAME} PRIVATE ${CATCH2_COMPILE_DEF}) + # Register the test with ament ament_add_test( ${TEST_NAME} diff --git a/robot_mcp_test/include/robot_mcp_test/robot_mcp_test.hpp b/robot_mcp_test/include/robot_mcp_test/robot_mcp_test.hpp index cddc75e..aee16f1 100644 --- a/robot_mcp_test/include/robot_mcp_test/robot_mcp_test.hpp +++ b/robot_mcp_test/include/robot_mcp_test/robot_mcp_test.hpp @@ -15,7 +15,14 @@ #pragma once // Main header for robot_mcp_test library -// Include this to access all robot_mcp_test functionality +// Include this to access all robot_mcp_test functionality including Catch2 + +// Cross-compatible Catch2 include for Ubuntu 22.04 (Catch2 v2) and Ubuntu 24.04 (Catch2 v3) +#ifdef CATCH2_V3 +#include +#else +#include +#endif // Test fixtures #include "test_fixtures/test_executor_fixture.hpp" diff --git a/robot_mcp_test/test/test_basic_example.cpp b/robot_mcp_test/test/test_basic_example.cpp index 69071aa..985a834 100644 --- a/robot_mcp_test/test/test_basic_example.cpp +++ b/robot_mcp_test/test/test_basic_example.cpp @@ -17,9 +17,8 @@ #include #include -#include -#include #include +#include #include #include diff --git a/robot_mcp_test/test/test_with_ros.cpp b/robot_mcp_test/test/test_with_ros.cpp index 95975f5..9044e6b 100644 --- a/robot_mcp_test/test/test_with_ros.cpp +++ b/robot_mcp_test/test/test_with_ros.cpp @@ -19,9 +19,8 @@ #include #include -#include -#include #include +#include #include #include "test_nodes/publisher_test_node.hpp"