This page is a result of research, mostly including AI chats

See my notes on Experimenting with pipeline: LLM -> ros-mcp-server -> ROS2/Nav2

Contents:

• Foundation: What is MCP Server
• ROS2/Nav2 robot as an MCP Server
• Using DFRobot HUSKYLENS 2 as an MCP Server
• How HuskyLens 2 built-in MCP server is used
• When HuskyLens 2 cannot use its WiFi interface
• Best MCP Server Solution (general purpose)
• Best MCP Server Solution for Nav2 and HuskyLens 2 over I2C
• Best MCP Server solution for a beginner
• Behavior Trees, Nav2, and LLM Strategy
• Why an AI should never drive the robot's wheels directly

what is MCP server in AI?

Sources: 1 2 3 4

An MCP Server (Model Context Protocol Server) is a specialized backend service that acts as a universal adapter, securely connecting AI models to external data sources, APIs, and software tools. Originally introduced by [Anthropic](https://www.anthropic.com/news/model-context-protocol), it functions like a "USB-C port for AI tools," giving Large Language Models (LLMs) a standardized way to read fresh data and take real-world actions without needing custom, brittle integration code for every single app.

Before MCP, developers faced a massive scalability issue called the N × M integration problem. If you had 5 different AI models (like Claude, GPT-4, Gemini) and 5 external tools (like GitHub, PostgreSQL, Slack), you had to write 25 separate, custom integrations to make them all talk to each other. With MCP, you write one MCP Server for your tool. Any AI client or model that understands the protocol can immediately discover and use that tool, dropping the integrations down to a simple N + M equation.

The architecture utilizes a straightforward client-server setup:

• The MCP Host / Client: This is the AI application you interact with (e.g., an AI-powered IDE like Cursor, or a chatbot). It translates your natural language requests into structured protocol commands.
• The MCP Server: This is a lightweight, standalone service sitting in front of your data. It tells the AI exactly what capabilities it has and handles the heavy lifting of fetching data or running commands.
• The Communication: They talk using standard, lightweight JSON-RPC 2.0 messages over simple command-line streams (stdio) or web connections (HTTP/SSE).

An MCP server serves three core building blocks to an AI model:

• Resources: Raw, read-only data sources that the AI can look at for context (e.g., local files, database logs, API documentation).
• Tools: Executable functions that allow the AI to actively do things (e.g., compile code, write a file, send a Slack message, calculate a budget).
• Prompts: Pre-defined templates or shortcuts that guide how the AI interacts with the tool or frames its questions.

I have a full Nav2 stack ROS2 robot capable of moving around and navigating.

Looks like it can present itself as an MCP server and interact (be commanded) by AI?

Instead of building an MCP server from scratch, you can deploy a ready-made bridge that connects your running Nav2 stack to an LLM:

• nav2_mcp_server (by Alberto Tudela) This is a dedicated, open-source MCP server explicitly tailored for Nav2. It sits on top of your ROS 2 workspace, interacts with rclpy, and exposes Nav2 functionalities directly to MCP clients.
• ros-mcp-server / RobotMCP A broader ecosystem that connects LLMs (like Claude, ChatGPT, or Gemini) to any ROS/ROS 2 robot. It has built-in examples showing an AI agent executing high-level tasks like "Grab a Coke from the fridge and go to the living room" by stringing together Nav2 goals and MoveIt manipulation commands.
• ros2-mcp-server is a Python-based server that integrates the Model Context Protocol (MCP) with ROS 2, enabling AI assistants to control robots via ROS 2 topics. It processes commands through FastMCP and runs as a ROS 2 node, publishing geometry_msgs/Twist messages to the /cmd_vel topic to control robot movement.
• Amazing-ROS-2-MCP A native rclpy implementation that doesn't rely on rosbridge. It handles native ROS 2 graph introspection, Nav2 waypoint following, and includes built-in hardware safety guardrails (like velocity clamping so the AI can't accidentally command your robot to crash at full speed).

When you expose your robot via an MCP server, the protocol maps the capabilities of your robot into concepts the LLM can understand:

1. The User Prompt: You tell an MCP-enabled AI client (like Claude Desktop or an AI agent framework): "Check if the loading dock is clear. If it's blocked, navigate to the secondary inspection bay."
2. The Discovery: The AI checks its tools and sees it has an MCP server connected to your robot with tools like get_camera_feed(), get_robot_pose(), and navigate_to_pose().
3. Execution Part 1 (Observation): The AI calls navigate_to_pose for the loading dock coordinates. It polls the action feedback until it arrives, then calls get_camera_feed().
4. Reasoning: The vision model looks at the image matrix returned by MCP, notices a pallet blocking the dock, and decides: "The loading dock is blocked."
5. Execution Part 2 (Action): The AI calls navigate_to_pose again with the coordinates for the secondary inspection bay.

Allowing a generative AI model to send raw commands directly to hardware is inherently risky. When setting this up on a physical robot:

• Enforce Middleware Clamping: Ensure your MCP server or a separate ROS node acts as a safety guardrail—clamping the maximum linear/angular velocities (cmd_vel) and checking for immediate proximity to obstacles.
• Semantic Constraints: Rather than letting the AI input arbitrary $(x,y)$ coordinates, consider exposing tools that only accept pre-defined semantic locations (e.g., go_to_location(zone="kitchen")) so the AI cannot command the robot to drive off a ledge or into a restricted area.

How does DFRobot HUSKYLENS 2 fit into this architecture?

The DFRobot HUSKYLENS 2 fits into this setup perfectly by serving as an edge-AI hardware accelerator and specialized sensor node for your robot. It acts as a dedicated bridge for the robot's physical perception, while your Nav2 workspace acts as the bridge for its physical actions.

Instead of your robot's main computer (like a Raspberry Pi or Jetson) burning CPU cycles running heavy computer vision models, the HuskyLens 2 uses its onboard 6 TOPS AI processor to detect objects, faces, or gestures entirely on-device.

The device fits into your AI-commanded robot ecosystem in two distinct ways:

When you connect an LLM to a robot, sending a raw, continuous HD video feed over the network to the AI cloud is slow, expensive, and latency-heavy. The HuskyLens 2 has a built-in MCP service designed specifically to solve this. Instead of sending a raw image snapshot, it processes the video locally and exposes structured MCP Resources to the LLM.

• The Raw View: A standard camera sends a massive grid of pixels.
• The HuskyLens MCP View: The sensor tells the LLM through the Model Context Protocol: "I currently see a delivery_box at bounding box [X, Y] and User_John standing 2 meters away."

This structured text/JSON data is exactly what LLMs need to make fast, context-aware decisions.

If you combine your Nav2 MCP server with the HuskyLens 2 MCP sensor, you create a highly efficient, bidirectional loop where the AI can seamlessly observe and act:

                    ┌──────────────────────────────┐
                    │      AI Agent / LLM          │
                    │  (e.g., Claude 3.5 Sonnet)   │
                    └──────────────┬───────────────┘
                                   │
              ┌────────────────────┴────────────────────┐
       MCP Resources (Perception)                 MCP Tools (Action)
              │                                         │
   ┌──────────▼──────────┐                   ┌──────────▼──────────┐
   │    HUSKYLENS 2      │                   │  ROS 2 / Nav2 Stack │
   │  AI Vision Sensor   │                   │  Robot Movement     │
   └─────────────────────┘                   └─────────────────────┘

1. The Observation: The LLM queries the HuskyLens 2 MCP resource: get_visible_objects().
2. The Processing: The HuskyLens 2 uses its local object-tracking or custom YOLO model to scan the room. It replies to the LLM: {"objects": ["couch", "coffee_table"], "keys_detected": false}.
3. The Logic: The LLM reasons: "I don't see the keys in the living room. I need to check the kitchen."
4. The Action: The LLM invokes your Nav2 MCP tool: navigate_to_pose(kitchen_x, kitchen_y).
5. The Loop Repeats: 🔄 Once Nav2 signals that the robot has arrived, the LLM queries the HuskyLens 2 again to scan the new room.

To get this hardware talking to your robot, you would typically connect the HuskyLens 2 to your main robot controller via its standard UART or I2C interface.

Because the HuskyLens 2 handles all the heavy 6 TOPS AI lifting locally, your main robot CPU stays completely free to handle real-time Nav2 costmap updates, SLAM, and localization.

HuskyLens 2 has an MCP server built-in. How will it be used in this architecture?

Because the HuskyLens 2 has a built-in MCP server, it does not need to pass raw image pixels or data over I2C/UART to a middleman just to talk to an AI. Instead, it becomes a first-class network citizen in your architecture.

By installing the optional Wi-Fi 6 module, the HuskyLens 2 connects directly to your local network and hosts a Server-Sent Events (SSE) HTTP endpoint (typically http://<huskylens_ip>:3000/sse).

Here is exactly how this built-in server is used and integrated into your robot's architecture:

Instead of having one massive server, your architecture will use a Multi-Server Client Setup. Your central AI agent framework (running in the cloud or on a powerful local machine using an MCP client like Cherry Studio or an agent framework like LangChain) connects to both servers simultaneously:

1. Your Nav2 MCP Server: Connected via stdio/network to handle robot movement (navigate_to_pose).
2. The HuskyLens 2 Built-in MCP Server: Connected via Wi-Fi SSE to handle visual perception.

                      ┌────────────────────────┐
                      │    AI Central Client   │
                      │  (e.g., Cherry Studio) │
                      └───────────┬────────────┘
                                  │
         ┌────────────────────────┴────────────────────────┐
 (Via Local Network)                               (Via Wi-Fi SSE)
         │                                                 │
┌────────▼──────────────┐                         ┌────────▼──────────────┐
│  Robot PC (Nav2 Stack)│                         │      HUSKYLENS 2      │
│  Exposes Action Tools │                         │ Exposes Vision Tools  │
└───────────────────────┘                         └───────────────────────┘

When the AI client hooks into the HuskyLens 2 URL, the device natively feeds the LLM a set of pre-defined, executable Tools:

• get_result: This is the primary perception tool. When called by the AI, HuskyLens 2 bypasses raw streaming and returns a clean, structured JSON "brief" summarizing exactly what its 6 TOPS chip is seeing:

{
  "timestamp": 17183920,
  "current_algorithm": "object_recognition",
  "matches": [
    {"label": "delivery_box", "confidence": 0.92, "bbox": [120, 45, 50, 50]},
    {"label": "human_face", "id": 1, "name": "John", "emotion": "neutral"}
  ]
}

• switch_application & application_list: The AI can actively reconfigure the camera on the fly. If the AI is looking for lines, it commands the camera to switch to line_tracking. If it's verifying an ID card, it commands it to switch to ocr or qr_code.
• take_photo: Triggers the onboard camera to capture an image and store it to its SD card, or feeds the snapshot back to a Vision-LLM for complex reasoning.

Because both your navigation stack and your camera speak the exact same language (MCP), the AI agent can intelligently orchestrate them in a sequential chain.

1. The Prompt: You tell the AI: "Go check if John is in the office, and if he looks happy."
2. Navigation: The AI scans its tools, chooses the Nav2 server, and calls navigate_to_pose(office_x, office_y). It waits for the robot to arrive.
3. Configuration: Once at the office, the AI calls the HuskyLens tool switch_application(application="face_recognition").
4. Perception: The AI calls get_result. The HuskyLens 2 returns: {"name": "John", "emotion": "happy"}.
5. The Final Report: The AI concludes the loop and responds to you in plain English: "I have navigated to the office. I found John, and he appears to be in a good mood!"

Traditionally, to achieve this, you had to write a massive ROS node that listened to custom camera topics, filtered out bounding boxes, mapped IDs to names, and then sent a custom payload to an AI.

With the built-in MCP server on the HuskyLens 2, the sensor handles its own AI communication layer. Your ROS 2 robot stack is completely unburdened by video processing pipelines, allowing your main robot CPU to focus entirely on deterministic control, safety limits, and navigation.

Useful links:

• https://www.dfrobot.com/huskylens/huskylens2
• https://www.youtube.com/watch?v=mrrSIrVURC8&t=138
• https://www.electromaker.io/blog/article/huskylens-2-plus-vs-huskylens-2-are-the-add-ons-worth-it
• https://wiki.dfrobot.com/sen0638/docs/22605
• https://www.hackster.io/roni-bandini/huskylens-2-model-context-protocol-mcp-3675ac
• https://www.youtube.com/watch?v=wylEehOJMps&t=360
• https://www.youtube.com/watch?v=buPT0hNFsJ8
• https://www.dfrobot.com/blog-18898.html

What if I don't want to use HuskyLens 2 WiFi and it is connected to I2C, but I need to use its MCP Server?

If you want to keep the HuskyLens 2 isolated from your Wi-Fi network and connected purely via I2C or UART, you can still use its MCP features! However, because the built-in MCP server relies on the Wi-Fi module to host its network endpoints, you will need to use a Proxy Architecture.

Your main Robot PC (the one running Nav2) will act as a bridge: it reads the raw data over I2C, wraps it, and hosts a combined local MCP server on behalf of the camera.

Here is how you can set this up efficiently:

Instead of the AI Client talking directly to two separate network devices, it talks to a single, unified MCP Server running locally on your Robot PC.

┌────────────────────────┐
│    AI Central Client   │ (e.g., Claude Desktop, Cherry Studio)
└───────────┬────────────┘
            │
            │ (Local network / stdio transport)
┌───────────▼────────────┐
│      Robot PC          │ ◄─── Runs your Unified MCP Server (Python/C++)
└─────┬────────────▲─────┘
      │            │
      │ (Internal) │ (Physical I2C / UART Bus)
      ▼            ▼
┌───────────┐┌───────────┐
│Nav2 Stack ││HUSKYLENS 2│
└───────────┘└───────────┘

To build this, you will write a lightweight Python MCP server on your robot using the official Python MCP SDK.

On your robot PC, install the MCP library and the HuskyLens Python library (which handles reading the I2C registers):

pip install mcp huskylib

You can write a short Python script that registers both your Nav2 actions and your HuskyLens I2C readings as MCP tools.

Here is a conceptual example using the FastMCP framework:

# --- Unified MCP Server: HUSKYLENS and Nav2 Tools ---

import rclpyfrom geometry_msgs.msg
import PoseStamped
from mcp.server.fastmcp import FastMCP
# Standard library to talk to HuskyLens over I2C/UART
from huskylib import HuskyLensLibrary

# Initialize MCP Server
mcp = FastMCP("Robot_Unified_Server")
# Initialize HuskyLens via I2C (adjust protocol/address as needed)
hl = HuskyLensLibrary("I2C", "", address=0x32) 

# --- HUSKYLENS TOOLS (Bridged over I2C) ---

@mcp.tool()def get_camera_vision_results() -> str:
    """Gets the tracked objects, faces, or lines currently seen by the HuskyLens camera."""
    # Read the hardware directly over I2C
    blocks = hl.command_request_blocks()
    
    if not blocks:
        return "Camera sees nothing in its current mode."
        
    # Format the physical I2C payload into clean JSON text for the LLM
    results = []
    for block in blocks:
        results.append({
            "id": block.ID,
            "x_center": block.xCenter,
            "y_center": block.yCenter,
            "width": block.width,
            "height": block.height
        })
    return str(results)

@mcp.tool()def change_camera_mode(mode: str) -> str:
    """Changes the HuskyLens AI algorithm. Options: face_recognition, object_tracking, line_tracking."""
    # Map the LLM's text request to the camera's hardware commands
    mode_mapping = {
        "face_recognition": "ALGORITHM_FACE_RECOGNITION",
        "object_tracking": "ALGORITHM_OBJECT_TRACKING",
        "line_tracking": "ALGORITHM_LINE_TRACKING"
    }
    if mode in mode_mapping:
        hl.algorithmSelect(mode_mapping[mode])
        return f"Successfully switched camera to {mode} mode."
    return "Invalid mode requested."

# --- NAV2 TOOLS (Bridged over ROS 2) ---

@mcp.tool()def navigate_to_waypoint(x: float, y: float) -> str:
    """Commands the robot to drive to a specific map coordinate."""
    # Your existing ROS2 rclpy code to send a Nav2 Goal goes here...
    return f"Robot navigation to X:{x}, Y:{y} initiated."

if __name__ == "__main__":
    mcp.run(transport="stdio")

• Air-gapped / Secure: Your camera has zero network footprint. It cannot be scanned, hacked, or lose Wi-Fi connection in a dead zone.
• Unified State: Because one python script controls both Nav2 and the camera, you can write "macro" tools. For example, you could create a tool called search_room_for_object() that rotates the robot 360 degrees while continuously reading the HuskyLens I2C data, returning a single final answer to the LLM.
• Lower Latency: I2C/UART communication between the camera and your single-board computer is incredibly fast and immune to local Wi-Fi congestion.

Out of the four Existing Open-Source Options you found before - which one is the most likely to survive long term?

Based on the current open-source landscape for integrating robotics (specifically ROS 2 / Nav2) with the Model Context Protocol (MCP), the three primary options that have emerged in the community are:

1. ros-mcp-server (by RobotMCP / Contoro)
2. ros2-mcp-server (by kakimochi)
3. amazing-ros2-mcp (by proxi666)

Out of these three, amazing-ros2-mcp is the most likely to survive and thrive long-term, closely followed by ros-mcp-server for different strategic reasons.

Note: I skipped nav2_mcp_server by ajtudela because it serves a fundamentally different purpose in your architecture rather than competing with the three options mentioned. It isn't a general-purpose ROS 2-to-MCP bridge; it is a highly specialized, high-level application server built exclusively for Nav2 operations.

In robotics, architecture and performance dictate longevity. amazing-ros2-mcp addresses fundamental engineering flaws present in early implementations:

• Native rclpy Integration: Early alternatives (like ros-mcp-server) heavily relied on rosbridge and WebSockets to pass data between the MCP layer and the robot. This introduced severe serialization overhead, network latency, and complexity. amazing-ros2-mcp embeds a native ROS 2 node using a background spin-thread pattern. This delivers single-digit millisecond latency for topic and action discovery, which is essential for real-time edge hardware like the HuskyLens. [1]
• Auto-Workspace Discovery: It automatically injects AMENT_PREFIX_PATH. This means as your custom Nav2 workspace or HuskyLens tracking nodes evolve, the LLM can immediately inspect and import your custom message types without manual re-configuration. [1]
• Built-in Safety Guardrails: It is the only option explicitly built with hardware safeguards (velocity clamping for cmd_vel, blocked topics, and dry-run modes). Giving an AI agent autonomous control over physical actuators is inherently risky, making safety middleware a prerequisite for long-term real-world deployment. [1, 2]

While technically outpaced by native DDS performance, ros-mcp-server (by the RobotMCP team) has high survival odds because it is backed by a corporate entity (Contoro). Corporate backing ensures dedicated maintenance, documentation, and funding, which community-driven solo projects sometimes lack.

However, it is structurally designed to handle broad cloud-to-robot actions rather than tightly coupled edge-hardware loops.

For an edge-AI setup using a localized proxy (where your local computer bridges the HuskyLens 2 over I2C and commands Nav2 directly), amazing-ros2-mcp provides the exact asynchronous performance and workspace discovery required to ensure your camera readings and navigation commands remain tightly synchronized without crashing.

Sources:

• https://www.reddit.com/r/ROS/comments/1t8gr99/built_a_native_ros_2_mcp_server_for_ai_agents_no/
• https://discourse.openrobotics.org/t/amazing-ros-2-mcp-native-ros-2-mcp-server-for-ai-assisted-robotics-agents/55270
• https://github.com/robotmcp/ros-mcp-server
• https://robotmcp.ai/blog
• https://github.com/kakimochi/ros2-mcp-server
• https://mcpmarket.com/server/ros2
• https://www.trossenrobotics.com/post/introducing-the-trossen-mcp-server

I skipped nav2_mcp_server by ajtudela in the previous section because it serves a fundamentally different purpose in your architecture rather than competing with the three options mentioned.

It isn't a general-purpose ROS 2-to-MCP bridge; it is a highly specialized, high-level application server built exclusively for Nav2 operations.

If we look at how this repository is built, it actually changes the long-term outlook of your specific architecture significantly.

The previous three options (amazing-ros2-mcp, ros-mcp-server, and ros2-mcp-server) act as open-ended translation layers. They look at any generic ROS 2 topic, service, or action on your robot and expose it dynamically to an LLM.

In contrast, nav2_mcp_server hardcodes deep, native integration for the Nav2 lifecycle, costmaps, and action servers. Instead of forcing an LLM to figure out how to manually construct a nav2_msgs/action/NavigateToPose goal payload, this server exposes clean, explicitly engineered tools like:

• navigate_to_pose(x, y, yaw)
• follow_waypoints(waypoints)
• clear_costmaps(costmap_type)
• nav2_lifecycle(action)

It handles the asynchronous ROS 2 action states, cancellations, and feedback internally, exposing only clean primitives to the Model Context Protocol.

Because nav2_mcp_server is a dedicated navigation gateway, it shouldn't be compared as an "either/or" choice against a general ROS 2 bridge. In fact, nav2_mcp_server is the absolute best choice to survive long-term for your navigation stack specifically, because it is built on fastmcp and handles the complex asynchronous states of Nav2 out of the box.

However, because it is hyper-focused on Nav2, it cannot see your HuskyLens 2 I2C data. It has no tools to read arbitrary hardware registers or generic topics.

Therefore, your ultimate long-term architecture changes from choosing one server to running a Dual-Server Layout:

                       ┌────────────────────────┐
                       │    AI Central Client   │
                       │  (e.g., Claude Desktop)│
                       └───────────┬────────────┘
                                   │
         ┌─────────────────────────┴─────────────────────────┐
  (Local Stdio / HTTP)                                (Local Stdio / HTTP)
         │                                                   │
┌────────▼─────────────────────┐                    ┌────────▼────────────────┐
│    nav2_mcp_server           │                    │   Your Custom Python    │
│     (by ajtudela)            │                    │    I2C HuskyLens Server │
├──────────────────────────────┤                    ├─────────────────────────┤
│ Exposes Nav2 tools natively; |                    │ Exposes HuskyLens tools │
│ Handles actions, costmaps,   |                    │ via direct I2C readings │
│ and waypoints.               │                    │ (`huskylib`).           │
└──────────────────────────────┘                    └─────────────────────────┘

1. Zero Reinvented Wheels: You don't have to write a single line of ROS 2 action client boilerplate to handle navigation. nav2_mcp_server handles the entire navigation lifecycle flawlessly.
2. Clean Separation of Concerns: Your navigation stack logic is cleanly isolated from your vision sensor hardware logic. If you change your camera to a different sensor later, your navigation MCP server configuration remains completely untouched.
3. Ease of Setup: You can spin up nav2_mcp_server instantly using its pre-built Docker image or uv run, leaving you to only write the lightweight Python I2C wrapper scripts for the camera hardware.

After examining the repositories of currently available Model Context Protocol (MCP) servers, I made the following observations:

• Stale Development: Most repositories show few signs of active development, with minor commits dating back several months.
• Sparse Documentation: Documentation is often limited to a single root README file, and installation instructions are sparse.

In contrast, the ros-mcp-server (by RobotMCP / Contoro) offers comprehensive documentation and a rich collection of well-described examples. The trade-off is that it is not tightly integrated with native ROS2, relying instead on a Websockets interface.

Conclusion: For my current use case (learning and prototyping), ros-mcp-server is the clear winner. I can revisit this architectural choice in the future as my requirements change.

How do Behavior trees fit in the picture? What if I have custom behaviors which are also part of Nav2 sometimes?

In this architecture, Behavior Trees (BTs) act as the operational brain of the robot, while the LLM acts as the strategic planner.

Instead of the LLM micromanaging every motor command or recovery maneuver, the LLM selects a high-level behavioral goal via an MCP tool.

The Nav2 Behavior Tree then takes over to execute that goal safely, handling real-time sensor updates, obstacle avoidance, and recovery behaviors locally on the physical hardware.

To keep your robot stable and prevent the LLM from crashing into walls due to network latency, duties are split cleanly into specialized layers:

• Strategic Layer (LLM via MCP)
  • Responsibility: High-level reasoning, semantic understanding, and task sequencing.
  • Examples: "Find the red delivery box and bring it to John."
• Operational Layer (Nav2 Behavior Tree)
  • Responsibility: Deterministic execution, path planning, safety middleware, and local autonomy.
  • Examples: Compute path, track global costmap, retry navigation if blocked by a human, or trigger recovery behaviors.
• Perception Layer (HuskyLens 2 via I2C Proxy)
  • Responsibility: Edge feature extraction and object localization.
  • Examples: Detect a delivery_box at relative coordinates [X, Y, Z] from the camera sensor frame.

If you have custom actions—such as waiting for a physical button press on the robot, or executing a precise infrared docking routine—they should be compiled as Nav2 BT Plugins (Action or Condition Nodes).

Once registered within your Nav2 workspace, you expose these complex behaviors to the LLM by mapping them to explicit tools inside your local nav2_mcp_server.

Here is how the LLM, MCP, Nav2 Behavior Tree, and HuskyLens 2 handle a complex command like "Go fetch the box":

1. Strategic Command: The LLM client calls the custom MCP tool: fetch_object(target="red_box").
2. BT Trigger: The nav2_mcp_server sends a standard ROS 2 action goal to Nav2, but instead of specifying a generic coordinate, it specifies a custom Behavior Tree XML file (e.g., fetch_object_tree.xml).
3. Local Tree Autonomy: The Nav2 BT engine begins ticking at its local execution frequency (e.g., 20Hz):

• Node A (Camera Condition): Ticks a node that checks the local proxy topic to see if the HuskyLens 2 currently detects the target "red_box".
  • Node B (ComputePathToPose): Once spotted, it extracts the coordinates and generates a collision-free path.
  • Node C (FollowPath): Commands the local controllers to move the wheels.
  • Node D (Custom Docking/Grab Action): Once close, it runs your custom behavior plugin to secure the box.

4. MCP Response: Once the entire Behavior Tree succeeds or fails, the nav2_mcp_server catches the result and returns a single structured response back to the LLM: {"status": "success", "message": "Object fetched successfully"}.

The nav2_mcp_server project allows you to pass a custom behavior_tree file path as part of your navigation goals. Here is how your Python MCP bridge registers and maps a custom behavior tree execution tool:

from mcp.server.fastmcp import FastMCP
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient

# Initialize MCP Servermcp = FastMCP("Robot_Custom_Behavior_Server")

@mcp.tool()def execute_custom_behavior(behavior_name: str, target_location: str) -> str:
    """Triggers a complex local behavior tree (e.g., 'patrol', 'dock', 'fetch')."""
    
    # Map semantic names to your compiled Nav2 BT XML paths
    bt_mappings = {
        "fetch": "/path/to/my_robot_ws/src/nav2_trees/fetch_behavior.xml",
        "dock": "/path/to/my_robot_ws/src/nav2_trees/dock_charging_station.xml"
    }
    
    if behavior_name not in bt_mappings:
        return f"Unknown behavior: {behavior_name}"
        
    # Construct the Nav2 NavigateToPose action goal, specifying the custom BT
    goal_msg = NavigateToPose.Goal()
    goal_msg.behavior_tree = bt_mappings[behavior_name]
    
    # Note: Parse your target_location semantic string to real coordinates here
    
    # Send asynchronously to Nav2 and wait for the tree to complete
    # action_client.send_goal_async(goal_msg)
    
    return f"Successfully initiated the local Behavior Tree for {behavior_name}."

if __name__ == "__main__":
    mcp.run(transport="stdio")

Routing custom behaviors through Nav2 Behavior Trees rather than coding raw loops directly into the LLM protects your hardware. If the network drops or the cloud AI stalls mid-task, your robot will not get stuck blindly driving forward into an obstacle.

The local Nav2 BT will immediately detect the timeout or costmap obstacle and safely execute a braking or recovery sequence.

Explain to a 5-year-old why directly commanding cmd_vel by LLM via MCP is a really bad idea.

Imagine a large, heavy robot driving around the house. Now imagine a tiny, real living puppy sleeping on the rug. If the cloud AI (the LLM) commands the wheels directly by screaming instructions like "Go forward at full speed!" over the network (via MCP), it creates a critical safety hazard.

Here is exactly why bypassing the robot's local safety systems risks running over and injuring a puppy:

When the AI sends a movement command, that message has to travel across the internet, through your router, and into the robot. This takes time (latency).

• The Trap: The AI scans the room, sees an empty hallway, and commands: "Drive forward for 5 seconds."
• The Danger: One second later, a puppy bolts into the hallway right in front of the robot.
• The Disaster: Even if the robot's camera catches the puppy and alerts the cloud AI, the AI's emergency "STOP!" command is still stuck traveling over the internet. The robot will blindly execute the original instruction and run right over the puppy.

Large Language Models do not possess instant reflexes. They process information by reading data, predicting tokens, and generating responses. This pipeline can take anywhere from 500 milliseconds to several seconds.

In the physical world, a safety reflex must happen in less than 50 milliseconds. If a puppy rolls over or steps in front of a moving robot, the AI cannot process the visual frame and calculate a braking response fast enough to stop the heavy momentum of the hardware.

To protect pets and people, the AI must never be allowed to touch the raw wheel controls (cmd_vel). Instead, the AI must only suggest a destination, while a local, real-time computer system inside the robot (the Nav2 Behavior Tree) actually drives.

• How it saves the puppy: If the AI says "Go to the kitchen," the local Nav2 system begins driving the wheels.
• Instant Reflexes: The moment the robot's laser sensors or local camera (like the HuskyLens 2) detect a tiny living obstacle on the floor, the local Behavior Tree ticks a Safety Brake node.
• The Result: The robot halts its wheels instantly on a hardware level, completely overriding the cloud AI's path until the puppy safely walks away.

To Be Continued

Back to the Top