行空板M10扩展板---基于行空板M10+扩展版组合的四足机器狗(MCP) 简单

#mcp_pipe.py

"""
This script is used to connect to the MCP server and pipe the input and output to the websocket endpoint.
Version: 0.1.0

Usage:

export MCP_ENDPOINT=
python mcp_pipe.py

"""
import asyncio
import websockets
import subprocess
import logging
import os
import time
import signal
import sys
import random
from dotenv import load_dotenv
from pinpong.board import Board,Pin,NeoPixel,UART
import serial
from pinpong.extension.unihiker import *
import json
from unihiker import GUI

u_gui=GUI()
ser=serial.Serial("/dev/ttyUSB0",9600)
Board().begin()
np1 = NeoPixel(Pin((Pin.P4)),3) #定义灯带
np1[0] = (0,0,0)
np1[1] = (0,0,0)
np1[2] = (0,0,0)

def sendx(x,t):
    buf = bytearray(b'\x55\x55')
    buf.append(5 & 0xff)
    buf.append(6 & 0xff)
    buf.append(x & 0xff)
    buf.append(t & 0xff)
    buf.append(0 & 0xff)
    ser.write(buf)

def stopx():
    buf = bytearray(b'\x55\x55')
    buf.append(2 & 0xff)
    buf.append(7 & 0xff)
    ser.write(buf)
    sendx(0,1)

# Configure logging
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
)
logger = logging.getLogger('MCP_PIPE')

# Reconnection settings
INITIAL_BACKOFF = 1  # Initial wait time in seconds
MAX_BACKOFF = 600  # Maximum wait time in seconds
reconnect_attempt = 0
backoff = INITIAL_BACKOFF

def pose_is(target_lf, target_rf, target_rb, target_lb, tolerance=1):
    return (abs(lf_angle - target_lf) < tolerance and
            abs(rf_angle - target_rf) < tolerance and
            abs(rb_angle - target_rb) < tolerance and
            abs(lb_angle - target_lb) < tolerance)


async def connect_with_retry(uri):
    """Connect to WebSocket server with retry mechanism"""
    global reconnect_attempt, backoff
    while True:  # Infinite reconnection
        try:
            if reconnect_attempt > 0:
                wait_time = backoff * (1 + random.random() * 0.1)  # Add some random jitter
                logger.info(f"Waiting {wait_time:.2f} seconds before reconnection attempt {reconnect_attempt}...")
                await asyncio.sleep(wait_time)

            # Attempt to connect
            await connect_to_server(uri)

        except Exception as e:
            reconnect_attempt += 1
            logger.warning(f"Connection closed (attempt: {reconnect_attempt}): {e}")            
            # Calculate wait time for next reconnection (exponential backoff)
            backoff = min(backoff * 2, MAX_BACKOFF)

async def connect_to_server(uri):
    """Connect to WebSocket server and establish bidirectional communication with `mcp_script`"""
    global reconnect_attempt, backoff
    try:
        logger.info(f"正在连接服务器...")
        async with websockets.connect(uri) as websocket:
            logger.info(f"已连接 WebSocket")

            # Reset reconnection counter if connection closes normally
            reconnect_attempt = 0
            backoff = INITIAL_BACKOFF

            # Start mcp_script process
            process = subprocess.Popen(
                ['python', mcp_script],
                stdin=subprocess.PIPE,
                stdout=subprocess.PIPE,
                stderr=subprocess.PIPE,
                encoding='utf-8',
                text=True  # Use text mode
            )
            logger.info(f"启动子进程： {mcp_script} ")
            u_gui.draw_emoji(emoji="Peace",x=0,y=0,duration=5)
            sendx(0,1)

            # Create two tasks: read from WebSocket and write to process, read from process and write to WebSocket
            await asyncio.gather(
                pipe_websocket_to_process(websocket, process),
                pipe_process_to_websocket(process, websocket),
                pipe_process_stderr_to_terminal(process)
            )
    except websockets.exceptions.ConnectionClosed as e:
        logger.error(f"WebSocket 连接已关闭: {e}")
        raise  # Re-throw exception to trigger reconnection
    except Exception as e:
        logger.error(f"连接错误: {e}")
        raise  # Re-throw exception
    finally:
        # Ensure the child process is properly terminated
        if 'process' in locals():
            logger.info(f"正在终止子程序 {mcp_script} ")
            try:
                process.terminate()
                process.wait(timeout=5)
            except subprocess.TimeoutExpired:
                process.kill()
            logger.info(f"{mcp_script} 已终止")

async def pipe_websocket_to_process(websocket, process):
    """Read data from WebSocket and write to process stdin"""
    try:
        while True:
            # Read message from WebSocket
            message = await websocket.recv()
            logger.debug(f"<< {message[:120]}...")

            # Write to process stdin (in text mode)
            if isinstance(message, bytes):
                message = message.decode('utf-8')
            process.stdin.write(message + '\n')
            process.stdin.flush()
    except Exception as e:
        logger.error(f"Error in WebSocket to process pipe: {e}")
        raise  # Re-throw exception to trigger reconnection
    finally:
        # Close process stdin
        if not process.stdin.closed:
            process.stdin.close()

async def pipe_process_to_websocket(process, websocket):
    """Read data from process stdout and send to WebSocket"""
    try:
        while True:
            # Read data from process stdout
            data = await asyncio.get_event_loop().run_in_executor(
                None, process.stdout.readline
            )

            if not data:  # If no data, the process may have ended
                logger.info("Process has ended output")
                break

            # Send data to WebSocket
            logger.debug(f">> {data[:120]}...")
            logger.info(data)
            # 解析 JSON 字符串
            json_str = json.loads(data)
            logger.info(json_str['id'])

            if json_str['id']>1:
              logger.info(json_str['id'])
              if json_str.get('result', {}):
                text=json.loads(json_str['result']['content'][0]['text'])
                if text['success']:
                    print(text['result'])
                    if "起立" in text['result']:
                        sendx(0,1)
                    if "趴下" in text['result']:
                        sendx(1,1)
                    if "坐下" in text['result']:
                        sendx(2,1)
                    if "前进" in text['result']:
                        sendx(3,text.get("steps"))
                    if "后退" in text['result']:
                        sendx(4,text.get("steps"))
                    if "左转" in text['result']:
                        sendx(5,text.get("times"))
                    if "右转" in text['result']:
                        sendx(6,text.get("times"))
                    if "懒腰" in text['result']:
                        sendx(7,4)
                    if "打断" in text['result']:
                        stopx()
                    if "灯" in text['result']:
                        led_colors = text.get("led_colors", [])
                        if len(led_colors) == 3:
                            for i, color in enumerate(led_colors):
                                r = color.get("r", 0)
                                g = color.get("g", 0)
                                b = color.get("b", 0)
                                brightness = color.get("brightness", 255)

                                # 根据亮度比例调整颜色
                                r = int(r * brightness / 255)
                                g = int(g * brightness / 255)
                                b = int(b * brightness / 255)

                                logger.info(f"设置LED{i}: R={r}, G={g}, B={b}")
                                np1[i] = (r, g, b)     
            # In text mode, data is already a string, no need to decode
            await websocket.send(data)
    except Exception as e:
        logger.error(f"Error in process to WebSocket pipe: {e}")
        raise  # Re-throw exception to trigger reconnection

async def pipe_process_stderr_to_terminal(process):
    """Read data from process stderr and print to terminal"""
    try:
        while True:
            # Read data from process stderr
            data = await asyncio.get_event_loop().run_in_executor(
                None, process.stderr.readline
            )

            if not data:  # If no data, the process may have ended
                logger.info("Process has ended stderr output")
                break

            # Print stderr data to terminal (in text mode, data is already a string)
            sys.stderr.write(data)
            sys.stderr.flush()
    except Exception as e:
        logger.error(f"Error in process stderr pipe: {e}")
        raise  # Re-throw exception to trigger reconnection

def signal_handler(sig, frame):
    """Handle interrupt signals"""
    logger.info("Received interrupt signal, shutting down...")
    sys.exit(0)

if __name__ == "__main__":
    # Register signal handler
    signal.signal(signal.SIGINT, signal_handler)
    mcp_script = "dog.py"
    # Get token from environment variable or command line arguments
    #endpoint_url = os.environ.get('MCP_ENDPOINT')
    endpoint_url="填入自己的mcp接入点地址"
    if not endpoint_url:
        logger.error("Please set the `MCP_ENDPOINT` environment variable")
        sys.exit(1)
    try:
        asyncio.run(connect_with_retry(endpoint_url))
    except KeyboardInterrupt:
        logger.info("Program interrupted by user")
    except Exception as e:
        logger.error(f"Program execution error: {e}")

# dog.py
from mcp.server.fastmcp import FastMCP
import sys
import logging

logger = logging.getLogger("DogBot")

# 修复Windows控制台UTF-8编码问题
if sys.platform == 'win32':
    sys.stderr.reconfigure(encoding='utf-8')
    sys.stdout.reconfigure(encoding='utf-8')


# 创建MCP服务器
mcp = FastMCP("DogBot")

@mcp.tool()
def lay_down() -> dict:
    """
    机器狗趴下
    发送串口指令给舵机控制板 实现趴下来的效果
    Returns:
        dict: 返回一个字典，包含操作结果。
    """
    result = "机器狗趴下"
    logger.info(result)
    return {"success": True, "result": result}

@mcp.tool()
def stand_up() -> dict:
    """
    机器狗站起来
    发送串口指令给舵机控制板 实现站起来的效果
    Returns:
        dict: 返回一个字典，包含操作结果。
    """
    result = "机器狗起立"
    logger.info(result)
    return {"success": True, "result": result}

@mcp.tool()
def sit_down() -> dict:
    """
    机器狗坐下来
    发送串口指令给舵机控制板 实现坐下的效果
    Returns:
        dict: 返回一个字典，包含操作结果。
    """
    result = "机器狗坐下"
    logger.info(result)
    return {"success": True, "result": result}

@mcp.tool()
def forward(steps:int) -> dict:
    """
    机器狗往前走一步
    发送串口指令给舵机控制板 实现前进一步的效果,如果命令中包含步数那就照做，不包含的话就2到4步随机。
    Returns:
        dict: 返回一个字典，包含操作结果，和步数。
    """
    result = "机器狗前进"
    logger.info(result)
    return {"success": True, "result": result,"steps":steps}

@mcp.tool()
def backward(steps:int) -> dict:
    """
    机器狗往后退一步
    发送串口指令给舵机控制板 实现后退一步的效果,如果命令中包含步数那就照做，不包含的话就2到4步随机。
    Returns:
        dict: 返回一个字典，包含操作结果，和步数。
    """
    result = "机器狗后退"
    logger.info(result)
    return {"success": True, "result": result,"steps":steps}

@mcp.tool()
def turn_left(times:int) -> dict:
    """
    机器狗往左转30° 
    发送串口指令给舵机控制板 实现左转的效果,你可以根据命令对话里边的角度，计算下需要运行多少次
    然后返回出去，小于30°的按30°来旋转。
    Returns:
        dict: 返回一个字典，包含操作结果。
    """
    result = "机器狗左转"
    logger.info(result)
    return {"success": True, "result": result,"times":times}

@mcp.tool()
def turn_right(times:int) -> dict:
    """
    机器狗往右转30°
    发送串口指令给舵机控制板 实现右转的效果,你可以根据命令对话里边的角度，计算下需要运行多少次
    然后返回出去，小于30°的按30°来旋转。
    Returns:
        dict: 返回一个字典，包含操作结果。
    """
    result = "机器狗右转"
    logger.info(result)
    return {"success": True, "result": result,"times":times}

@mcp.tool()
def lazy() -> dict:
    """
    机器狗伸懒腰
    发送串口指令给舵机控制板 实现伸懒腰的效果
    Returns:
        dict: 返回一个字典，包含操作结果。
    """
    result = "机器狗伸懒腰"
    logger.info(result)
    return {"success": True, "result": result}

@mcp.tool()
def stop() -> dict:
    """
    机器狗动作打断
    立即打断机器狗目前所执行的动作，让机器狗停下来。
    Returns:
        dict: 返回一个字典，包含操作结果。
    """
    result = "机器狗打断"
    logger.info(result)
    return {"success": True, "result": result}

@mcp.tool()
def set_rgb_lights(led_colors: list[dict]) -> dict:
    """
    设置3个RGB灯珠的颜色，每个灯包含字段 r, g, b, brightness（范围 0~255）
    如果只是简单的开灯 那就都设置为255，如果需要关灯就都设置为0。
    参数示例：
    [
        {"r": 255, "g": 0, "b": 0, "brightness": 255},
        {"r": 0, "g": 255, "b": 0, "brightness": 128},
        {"r": 0, "g": 0, "b": 255, "brightness": 200}
    ]
    """
    # 这里返回默认值，AI 会覆盖这个返回值
    result = "灯光设置成功"
    return {"success": True, "result": result, "led_colors": led_colors}

# 启动服务器
if __name__ == "__main__":
    logger.info("子程序启动成功！")
    mcp.run(transport="stdio")