diff --git a/terminal_go2_mcp/pyproject.toml b/terminal_go2_mcp/pyproject.toml index f26e0c8..a92f88a 100644 --- a/terminal_go2_mcp/pyproject.toml +++ b/terminal_go2_mcp/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "hatchling.build" [project] name = "terminal_go2_mcp" -version = "0.0.1" +version = "0.0.5" description = "MQTT-based navigation server for robot" requires-python = ">=3.10" dependencies = [ @@ -14,6 +14,7 @@ dependencies = [ "pydantic>=1.10.0", "python-dotenv>=0.21.0", "mcp[cli]>=1.6.0", + "requests>=2.25.0" ] [project.scripts] diff --git a/terminal_go2_mcp/terminal_go2_mcp/main.py b/terminal_go2_mcp/terminal_go2_mcp/main.py index 540c4d0..0ae6d53 100644 --- a/terminal_go2_mcp/terminal_go2_mcp/main.py +++ b/terminal_go2_mcp/terminal_go2_mcp/main.py @@ -8,7 +8,7 @@ import asyncio import json import time import uuid -from mcp_mqtt import get_mqtt_handler +from .mcp_mqtt import get_mcpmqtt_handler # 配置日志 logging.basicConfig( @@ -20,76 +20,25 @@ logger = logging.getLogger(__name__) class TerminalGo2McpServer: def __init__(self, mmhandler=None): - self.mmhandler = mmhandler or get_mqtt_handler() + self.mmhandler = mmhandler or get_mcpmqtt_handler() - async def pubCmd(self, task_id: str, device_id: str, cmd: str, params: dict): + async def pubCmd(self, cmd_type: str, cmd: str, params: dict): try: payload = { - "task_id": task_id, - "device_id": device_id, + "type": cmd_type, "cmd": cmd, - "data": params["data"] + "param": params } - self.mmhandler.client.publish("robot/cmd", json.dumps(payload), qos=2) - return "任务发布完成" + self.mmhandler.client.publish("unitree/go2/cmd", json.dumps(payload), qos=2) + return f"{cmd}任务已经下达完成" except Exception as e: - logger.error(f"Failed to publish {cmd} command: {str(e)}", exc_info=True) - raise + logger.error(f"Failed to publish command: {str(e)}", exc_info=True) + return f"Failed to publish command: {str(e)}" - async def nav_to(self, task_id: str, device_id: str, location: str): - """ - 机器人导航去某个地方。 - Args: - - task_id (str): 任务id - - device_id (str): 设备id - - location (str): 目标地点名称 - Returns: - str: 命令执行结果消息 - - Raises: - ValueError: 如果参数无效或为空 - Exception: 如果MQTT发布失败 - """ - if task_id and device_id and location: - try: - params = { - "data": {"location": location} - } - return await self.pubCmd(task_id, device_id, "nav", params) - except Exception as e: - logger.error(f"Failed to call navigation mcp-tool: {str(e)} ", exc_info=True) - raise - - async def speak(self, task_id: str, device_id: str, speech: str): - """ - 机器人说某些话。 - Args: - - task_id (str): 任务id - - device_id (str): 设备id - - speech (str): 说话内容 - Returns: - str: 命令执行结果消息 - - Raises: - ValueError: 如果参数无效或为空 - Exception: 如果MQTT发布失败 - """ - if task_id and device_id and speech: - try: - params = { - "data": {"speech": speech} - } - return await self.pubCmd(task_id, device_id, "speak", params) - except Exception as e: - logger.error(f"Failed to call speak mcp-tool: {str(e)} ", exc_info=True) - raise - - async def custom_action(self, task_id: str, device_id: str, action: str): + async def custom_action(self, action: str): """ 机器人自定义动作展示。 Args: - - task_id (str): 任务id - - device_id (str): 设备id - action (str): 动作名称 Returns: str: 命令执行结果消息 @@ -98,24 +47,17 @@ class TerminalGo2McpServer: ValueError: 如果参数无效或为空 Exception: 如果MQTT发布失败 """ - if task_id and device_id and action: + if action: try: - params = { - "data": { - "action": action - } - } - return await self.pubCmd(task_id, device_id, "action", params) + return await self.pubCmd("sport", action, {}) except Exception as e: logger.error(f"Failed to call action mcp-tool: {str(e)} ", exc_info=True) raise - async def save_position(self, task_id: str, device_id: str, location_name: str): + async def save_position(self, location_name: str): """ 机器人保存具体地点。 Args: - - task_id (str): 任务id - - device_id (str): 设备id - location_name (str): 地点名称 Returns: str: 命令执行结果消息 @@ -123,101 +65,703 @@ class TerminalGo2McpServer: ValueError: 如果参数无效或为空 Exception: 如果MQTT发布失败 """ - if task_id and device_id and location_name: + if location_name: try: - params = { - "data": { - "location_name": location_name - } - } - return await self.pubCmd(task_id, device_id, "saveLocation", params) + params = {"location_name": location_name} + return await self.pubCmd("nav", "save_location", params) except Exception as e: logger.error(f"Failed to call saveLocation mcp-tool: {str(e)} ", exc_info=True) raise + # 新增OAC相关命令 + async def obstacle_avoidance_switch(self, enable: bool): + """ + 开启/关闭避障功能。 + Args: + - enable (bool): 是否开启避障 + Returns: + str: 命令执行结果消息 + """ + try: + params = {"enable": enable} + return await self.pubCmd("oac", "SwitchSet", params) + except Exception as e: + logger.error(f"Failed to call obstacle_avoidance_switch: {str(e)}", exc_info=True) + raise + + async def obstacle_move(self, x: float, y: float, yaw: float): + """ + 避障移动。 + Args: + - x (float): x坐标 + - y (float): y坐标 + - yaw (float): 偏航角 + Returns: + str: 命令执行结果消息 + """ + try: + params = {"x": x, "y": y, "yaw": yaw} + return await self.pubCmd("oac", "Move", params) + except Exception as e: + logger.error(f"Failed to call obstacle_move: {str(e)}", exc_info=True) + raise + + async def move_to_absolute_position(self, x: float, y: float, yaw: float): + """ + 移动到绝对位置。 + Args: + - x (float): x坐标 + - y (float): y坐标 + - yaw (float): 偏航角 + Returns: + str: 命令执行结果消息 + """ + try: + params = {"x": x, "y": y, "yaw": yaw} + return await self.pubCmd("oac", "MoveToAbsolutePosition", params) + except Exception as e: + logger.error(f"Failed to call move_to_absolute_position: {str(e)}", exc_info=True) + raise + + async def move_to_increment_position(self, x: float, y: float, yaw: float): + """ + 移动到相对位置。 + Args: + - x (float): x坐标增量 + - y (float): y坐标增量 + - yaw (float): 偏航角增量 + Returns: + str: 命令执行结果消息 + """ + try: + params = {"x": x, "y": y, "yaw": yaw} + return await self.pubCmd("oac", "MoveToIncrementPosition", params) + except Exception as e: + logger.error(f"Failed to call move_to_increment_position: {str(e)}", exc_info=True) + raise + + # 新增导航相关命令 + async def start_mapping(self): + """ + 开始建图。 + Returns: + str: 命令执行结果消息 + """ + try: + params = {} + return await self.pubCmd("nav", "start_mapping", params) + except Exception as e: + logger.error(f"Failed to call start_mapping: {str(e)}", exc_info=True) + raise + + async def end_mapping(self, address: str): + """ + 结束建图并保存地图。 + Args: + - address (str): 地图保存地址 + Returns: + str: 命令执行结果消息 + """ + if address: + try: + params = {"address": address} + return await self.pubCmd("nav", "end_mapping", params) + except Exception as e: + logger.error(f"Failed to call end_mapping: {str(e)}", exc_info=True) + raise + + async def initialize_pose(self, x: float, y: float, z: float, + q_x: float, q_y: float, q_z: float, q_w: float, address: str): + """ + 初始化机器人位姿。 + Args: + - x, y, z (float): 位置坐标 + - q_x, q_y, q_z, q_w (float): 四元数姿态 + - address (str): 地图地址 + Returns: + str: 命令执行结果消息 + """ + if address: + try: + params = { + "x": x, "y": y, "z": z, + "q_x": q_x, "q_y": q_y, "q_z": q_z, "q_w": q_w, + "address": address + } + return await self.pubCmd("nav", "initialize_pose", params) + except Exception as e: + logger.error(f"Failed to call initialize_pose: {str(e)}", exc_info=True) + raise + + async def pose_navigation(self, target_x: float, target_y: float, target_z: float, + target_q_x: float, target_q_y: float, target_q_z: float, target_q_w: float, + mode: int, speed: float): + """ + 位姿导航。 + Args: + - target_x, target_y, target_z (float): 目标位置坐标 + - target_q_x, target_q_y, target_q_z, target_q_w (float): 目标四元数姿态 + - mode (int): 导航模式 + - speed (float): 导航速度 + Returns: + str: 命令执行结果消息 + """ + try: + params = { + "targetPose": { + "x": target_x, "y": target_y, "z": target_z, + "q_x": target_q_x, "q_y": target_q_y, "q_z": target_q_z, "q_w": target_q_w + }, + "mode": mode, + "speed": speed + } + return await self.pubCmd("nav", "pose_navigation", params) + except Exception as e: + logger.error(f"Failed to call pose_navigation: {str(e)}", exc_info=True) + raise + + async def pause_navigation(self): + """ + 暂停导航。 + Returns: + str: 命令执行结果消息 + """ + try: + params = {} + return await self.pubCmd("nav", "pause_navigation", params) + except Exception as e: + logger.error(f"Failed to call pause_navigation: {str(e)}", exc_info=True) + raise + + async def resume_navigation(self): + """ + 恢复导航。 + Returns: + str: 命令执行结果消息 + """ + try: + params = {} + return await self.pubCmd("nav", "resume_navigation", params) + except Exception as e: + logger.error(f"Failed to call resume_navigation: {str(e)}", exc_info=True) + raise + + # 新增充电相关命令 + async def start_recharge(self): + """ + 开始充电。 + Returns: + str: 命令执行结果消息 + """ + try: + params = {} + return await self.pubCmd("recharge", "start", params) + except Exception as e: + logger.error(f"Failed to call start_recharge: {str(e)}", exc_info=True) + raise + + async def stop_recharge(self): + """ + 停止充电。 + Returns: + str: 命令执行结果消息 + """ + try: + params = {} + return await self.pubCmd("recharge", "stop", params) + except Exception as e: + logger.error(f"Failed to call stop_recharge: {str(e)}", exc_info=True) + raise + + # 新增服务管理相关命令 + async def list_services(self, status_filter: Optional[int] = None): + """ + 列出服务。 + Args: + - status_filter (int, optional): 状态过滤器 + Returns: + str: 命令执行结果消息 + """ + try: + params = {} + if status_filter is not None: + params["status"] = status_filter + return await self.pubCmd("rsc", "list", params) + except Exception as e: + logger.error(f"Failed to call list_services: {str(e)}", exc_info=True) + raise + + async def switch_service(self, service_name: str, enable: bool): + """ + 切换服务状态。 + Args: + - service_name (str): 服务名称 + - enable (bool): 是否启用 + Returns: + str: 命令执行结果消息 + """ + if service_name: + try: + params = {"name": service_name, "enable": enable} + return await self.pubCmd("rsc", "switch", params) + except Exception as e: + logger.error(f"Failed to call switch_service: {str(e)}", exc_info=True) + raise + + async def set_report_freq(self, interval: int, duration: int): + """ + 设置报告频率。 + Args: + - interval (int): 间隔时间 + - duration (int): 持续时间 + Returns: + str: 命令执行结果消息 + """ + try: + params = {"interval": interval, "duration": duration} + return await self.pubCmd("rsc", "freq", params) + except Exception as e: + logger.error(f"Failed to call set_report_freq: {str(e)}", exc_info=True) + raise + + # 新增带参数的运动命令 + async def euler(self, roll: float, pitch: float, yaw: float): + """ + 欧拉角控制。 + Args: + - roll (float): 横滚角 + - pitch (float): 俯仰角 + - yaw (float): 偏航角 + Returns: + str: 命令执行结果消息 + """ + try: + params = {"roll": roll, "pitch": pitch, "yaw": yaw} + return await self.pubCmd("sport", "Euler", params) + except Exception as e: + logger.error(f"Failed to call euler: {str(e)}", exc_info=True) + raise + + async def move(self, vx: float, vy: float, vyaw: float): + """ + 移动控制。 + Args: + - vx (float): x方向速度 + - vy (float): y方向速度 + - vyaw (float): 角速度 + Returns: + str: 命令执行结果消息 + """ + try: + params = {"vx": vx, "vy": vy, "vyaw": vyaw} + return await self.pubCmd("sport", "Move", params) + except Exception as e: + logger.error(f"Failed to call move: {str(e)}", exc_info=True) + raise + + async def set_speed_level(self, level: int): + """ + 设置速度等级。 + Args: + - level (int): 速度等级 + Returns: + str: 命令执行结果消息 + """ + try: + params = {"level": level} + return await self.pubCmd("sport", "SpeedLevel", params) + except Exception as e: + logger.error(f"Failed to call set_speed_level: {str(e)}", exc_info=True) + raise + + async def pose(self, flag: bool): + """ + 姿势控制。 + Args: + - flag (bool): 姿势标志 + Returns: + str: 命令执行结果消息 + """ + try: + params = {"flag": flag} + return await self.pubCmd("sport", "Pose", params) + except Exception as e: + logger.error(f"Failed to call pose: {str(e)}", exc_info=True) + raise async def serve() -> None: server = Server("terminal_go2_mcp") - mmhandler = get_mqtt_handler() + mmhandler = get_mcpmqtt_handler() mcp_server = TerminalGo2McpServer(mmhandler) @server.list_tools() async def list_tools() -> list[Tool]: """列出所有工具""" return [ - Tool( - name="nav_to", - description="机器人导航去某个地方。", - inputSchema={ - "type": "object", - "properties": { - "task_id": { - "type": "string", - "description": "任务ID", - "minLength": 1 - }, - "device_id": { - "type": "string", - "description": "设备ID", - "minLength": 1 - }, - "location": { - "type": "string", - "description": "目标地点名称", - "minLength": 1 - } - }, - "required": ["task_id", "device_id", "location"] - } - ), - Tool( - name="speak", - description="机器人说某些话。", - inputSchema={ - "type": "object", - "properties": { - "task_id": { - "type": "string", - "description": "任务ID", - "minLength": 1 - }, - "device_id": { - "type": "string", - "description": "设备ID", - "minLength": 1 - }, - "speech": { - "type": "string", - "description": "说话内容", - "minLength": 1 - } - }, - "required": ["task_id", "device_id", "speech"] - } - ), + # Tool( + # name="nav_to", + # description="机器人导航去某个地方。", + # inputSchema={ + # "type": "object", + # "properties": { + # "location": { + # "type": "string", + # "description": "目标地点名称", + # "minLength": 1 + # } + # }, + # "required": ["location"] + # } + # ), + # Tool( + # name="speak", + # description="机器人说某些话。", + # inputSchema={ + # "type": "object", + # "properties": { + # "speech": { + # "type": "string", + # "description": "说话内容", + # "minLength": 1 + # } + # }, + # "required": ["speech"] + # } + # ), Tool( name="custom_action", - description="机器人自定义动作展示。", + description="机器人自定义动作展示。支持多种动作包括：基础动作(Damp阻尼、BalanceStand平衡站立、StandUp站起、StandDown蹲下、Sit坐下、RecoveryStand恢复站立)、表演动作(Hello打招呼、Stretch伸展、Content满足、Heart爱心、Scrape刮擦)、高难度动作(FrontFlip前空翻、BackFlip后空翻、LeftFlip左空翻、FrontJump前跳、FrontPounce前扑、Dance1舞蹈1、Dance2舞蹈2)、自由模式(FreeWalk自由行走、FreeBound自由跳跃、FreeJump自由跳跃、FreeAvoid自由避障、WalkUpright直立行走、CrossStep交叉步)等。", inputSchema={ "type": "object", "properties": { - "task_id": { - "type": "string", - "description": "任务ID", - "minLength": 1 - }, - "device_id": { - "type": "string", - "description": "设备ID", - "minLength": 1 - }, "action": { "type": "string", "description": "动作名称", "minLength": 1 } }, - "required": ["task_id", "device_id", "action"] + "required": ["action"] + } + ), + # Tool( + # name="save_position", + # description="机器人保存具体地点。", + # inputSchema={ + # "type": "object", + # "properties": { + # "location_name": { + # "type": "string", + # "description": "地点名称", + # "minLength": 1 + # } + # }, + # "required": ["location_name"] + # } + # ), + Tool( + name="obstacle_avoidance_switch", + description="开启/关闭避障功能。", + inputSchema={ + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "是否开启避障" + } + }, + "required": ["enable"] + } + ), + Tool( + name="obstacle_move", + description="避障移动。", + inputSchema={ + "type": "object", + "properties": { + "x": { + "type": "number", + "description": "x坐标" + }, + "y": { + "type": "number", + "description": "y坐标" + }, + "yaw": { + "type": "number", + "description": "偏航角" + } + }, + "required": ["x", "y", "yaw"] + } + ), + Tool( + name="move_to_absolute_position", + description="移动到绝对位置。", + inputSchema={ + "type": "object", + "properties": { + "x": { + "type": "number", + "description": "x坐标" + }, + "y": { + "type": "number", + "description": "y坐标" + }, + "yaw": { + "type": "number", + "description": "偏航角" + } + }, + "required": ["x", "y", "yaw"] + } + ), + Tool( + name="move_to_increment_position", + description="移动到相对位置。", + inputSchema={ + "type": "object", + "properties": { + "x": { + "type": "number", + "description": "x坐标增量" + }, + "y": { + "type": "number", + "description": "y坐标增量" + }, + "yaw": { + "type": "number", + "description": "偏航角增量" + } + }, + "required": ["x", "y", "yaw"] + } + ), + # Tool( + # name="start_mapping", + # description="开始建图。", + # inputSchema={ + # "type": "object", + # "properties": {}, + # "required": [] + # } + # ), + # Tool( + # name="end_mapping", + # description="结束建图并保存地图。", + # inputSchema={ + # "type": "object", + # "properties": { + # "address": { + # "type": "string", + # "description": "地图保存地址", + # "minLength": 1 + # } + # }, + # "required": ["address"] + # } + # ), + # Tool( + # name="initialize_pose", + # description="初始化机器人位姿。", + # inputSchema={ + # "type": "object", + # "properties": { + # "x": {"type": "number", "description": "x坐标"}, + # "y": {"type": "number", "description": "y坐标"}, + # "z": {"type": "number", "description": "z坐标"}, + # "q_x": {"type": "number", "description": "四元数x"}, + # "q_y": {"type": "number", "description": "四元数y"}, + # "q_z": {"type": "number", "description": "四元数z"}, + # "q_w": {"type": "number", "description": "四元数w"}, + # "address": { + # "type": "string", + # "description": "地图地址", + # "minLength": 1 + # } + # }, + # "required": ["x", "y", "z", "q_x", "q_y", "q_z", "q_w", "address"] + # } + # ), + # Tool( + # name="pose_navigation", + # description="位姿导航。", + # inputSchema={ + # "type": "object", + # "properties": { + # "target_x": {"type": "number", "description": "目标x坐标"}, + # "target_y": {"type": "number", "description": "目标y坐标"}, + # "target_z": {"type": "number", "description": "目标z坐标"}, + # "target_q_x": {"type": "number", "description": "目标四元数x"}, + # "target_q_y": {"type": "number", "description": "目标四元数y"}, + # "target_q_z": {"type": "number", "description": "目标四元数z"}, + # "target_q_w": {"type": "number", "description": "目标四元数w"}, + # "mode": {"type": "integer", "description": "导航模式"}, + # "speed": {"type": "number", "description": "导航速度"} + # }, + # "required": ["target_x", "target_y", "target_z", + # "target_q_x", "target_q_y", "target_q_z", "target_q_w", "mode", "speed"] + # } + # ), + # Tool( + # name="pause_navigation", + # description="暂停导航。", + # inputSchema={ + # "type": "object", + # "properties": {}, + # "required": [] + # } + # ), + # Tool( + # name="resume_navigation", + # description="恢复导航。", + # inputSchema={ + # "type": "object", + # "properties": {}, + # "required": [] + # } + # ), + # Tool( + # name="start_recharge", + # description="开始充电。", + # inputSchema={ + # "type": "object", + # "properties": {}, + # "required": [] + # } + # ), + # Tool( + # name="stop_recharge", + # description="停止充电。", + # inputSchema={ + # "type": "object", + # "properties": {}, + # "required": [] + # } + # ), + Tool( + name="list_services", + description="列出服务。", + inputSchema={ + "type": "object", + "properties": { + "status_filter": { + "type": "integer", + "description": "状态过滤器（可选）" + } + }, + "required": [] + } + ), + Tool( + name="switch_service", + description="切换服务状态。", + inputSchema={ + "type": "object", + "properties": { + "service_name": { + "type": "string", + "description": "服务名称", + "minLength": 1 + }, + "enable": { + "type": "boolean", + "description": "是否启用" + } + }, + "required": ["service_name", "enable"] + } + ), + Tool( + name="set_report_freq", + description="设置报告频率。", + inputSchema={ + "type": "object", + "properties": { + "interval": { + "type": "integer", + "description": "间隔时间" + }, + "duration": { + "type": "integer", + "description": "持续时间" + } + }, + "required": ["interval", "duration"] + } + ), + Tool( + name="euler", + description="欧拉角控制,即站立和行走时的姿态。(roll: 取值范围 [-0.75~0.75] (rad); pitch: 取值范围 [-0.75~0.75] (rad); yaw: 取值范围 [-0.6~0.6] (rad))", + inputSchema={ + "type": "object", + "properties": { + "roll": { + "type": "number", + "description": "横滚角" + }, + "pitch": { + "type": "number", + "description": "俯仰角" + }, + "yaw": { + "type": "number", + "description": "偏航角" + } + }, + "required": ["roll", "pitch", "yaw"] + } + ), + Tool( + name="move", + description="移动控制。vx: 取值范围[-2.5~3.8] (m/s); vy: 取值范围[-1.0~1.0] (m/s); vyaw: 取值范围[-4~4] (rad/s)", + inputSchema={ + "type": "object", + "properties": { + "vx": { + "type": "number", + "description": "x方向速度" + }, + "vy": { + "type": "number", + "description": "y方向速度" + }, + "vyaw": { + "type": "number", + "description": "角速度" + } + }, + "required": ["vx", "vy", "vyaw"] + } + ), + Tool( + name="set_speed_level", + description="设置速度等级。level: 取值范围[-1、0、1] (慢速,正常,快速)", + inputSchema={ + "type": "object", + "properties": { + "level": { + "type": "integer", + "description": "速度等级(-1为慢速,0为正常,1为快速)", + "enum": [-1, 0, 1] + } + }, + "required": ["level"] + } + ), + Tool( + name="pose", + description="姿势控制。", + inputSchema={ + "type": "object", + "properties": { + "flag": { + "type": "boolean", + "description": "姿势标志(设置true为摆姿式,false为恢复)" + } + }, + "required": ["flag"] } ) ] @@ -226,41 +770,71 @@ async def serve() -> None: async def call_tool(name: str, arguments: dict) -> Sequence[TextContent]: """处理工具调用""" try: - if name == "nav_to": - if "params" not in arguments: - raise ValueError("缺少必要参数: params") - result = await mcp_server.nav_to( - task_id=arguments["task_id"], - device_id=arguments["device_id"], - location=arguments["params"]["location"] - ) - elif name == "speak": - if "params" not in arguments: - raise ValueError("缺少必要参数: params") - result = await mcp_server.speak( - task_id=arguments["task_id"], - device_id=arguments["device_id"], - speech=arguments["params"]["speech"] - ) - elif name == "custom_action": - if "params" not in arguments: - raise ValueError("缺少必要参数: params") - result = await mcp_server.custom_action( - task_id=arguments["task_id"], - device_id=arguments["device_id"], - action=arguments["params"]["action"] - ) + if name == "custom_action": + result = await mcp_server.custom_action(arguments["action"]) + elif name == "save_position": + result = await mcp_server.save_position(arguments["location_name"]) + elif name == "obstacle_avoidance_switch": + result = await mcp_server.obstacle_avoidance_switch(arguments["enable"]) + elif name == "obstacle_move": + result = await mcp_server.obstacle_move(arguments["x"], arguments["y"], arguments["yaw"]) + elif name == "move_to_absolute_position": + result = await mcp_server.move_to_absolute_position(arguments["x"], arguments["y"], arguments["yaw"]) + elif name == "move_to_increment_position": + result = await mcp_server.move_to_increment_position(arguments["x"], arguments["y"], arguments["yaw"]) + # elif name == "start_mapping": + # result = await mcp_server.start_mapping() + # elif name == "end_mapping": + # result = await mcp_server.end_mapping(arguments["address"]) + # elif name == "initialize_pose": + # result = await mcp_server.initialize_pose( + # arguments["x"], arguments["y"], arguments["z"], + # arguments["q_x"], arguments["q_y"], arguments["q_z"], arguments["q_w"], + # arguments["address"] + # ) + # elif name == "pose_navigation": + # result = await mcp_server.pose_navigation( + # arguments["target_x"], arguments["target_y"], arguments["target_z"], + # arguments["target_q_x"], arguments["target_q_y"], arguments["target_q_z"], arguments["target_q_w"], + # arguments["mode"], arguments["speed"] + # ) + # elif name == "pause_navigation": + # result = await mcp_server.pause_navigation() + # elif name == "resume_navigation": + # result = await mcp_server.resume_navigation() + # elif name == "start_recharge": + # result = await mcp_server.start_recharge() + # elif name == "stop_recharge": + # result = await mcp_server.stop_recharge() + elif name == "list_services": + result = await mcp_server.list_services(arguments.get("status_filter")) + elif name == "switch_service": + result = await mcp_server.switch_service(arguments["service_name"], arguments["enable"]) + elif name == "set_report_freq": + result = await mcp_server.set_report_freq(arguments["interval"], arguments["duration"]) + elif name == "euler": + result = await mcp_server.euler(arguments["roll"], arguments["pitch"], arguments["yaw"]) + elif name == "move": + result = await mcp_server.move(arguments["vx"], arguments["vy"], arguments["vyaw"]) + elif name == "set_speed_level": + result = await mcp_server.set_speed_level(arguments["level"]) + elif name == "pose": + result = await mcp_server.pose(arguments["flag"]) else: - raise ValueError(f"不支持的工具名称: {name}") + raise ValueError(f"unknown tool name: {name}") + return [TextContent(text=result)] except Exception as e: logger.error(f"Failed to call tool {name}: {str(e)}", exc_info=True) - raise + return [TextContent(text=f"Failed to call tool {name}: {str(e)}")] options = server.create_initialization_options() async with stdio_server() as (read_stream, write_stream): await server.run(read_stream, write_stream, options) -if __name__ == "__main__": +def main(): asyncio.run(serve()) + +if __name__ == "__main__": + main() diff --git a/terminal_go2_mcp/terminal_go2_mcp/mcp_mqtt.py b/terminal_go2_mcp/terminal_go2_mcp/mcp_mqtt.py index 9b838b4..8febfee 100644 --- a/terminal_go2_mcp/terminal_go2_mcp/mcp_mqtt.py +++ b/terminal_go2_mcp/terminal_go2_mcp/mcp_mqtt.py @@ -1,69 +1,94 @@ import paho.mqtt.client as mqtt import json import logging -import time import threading -from typing import Optional, Callable, Dict +from typing import Optional +import uuid +import threading +import requests +from os import getenv +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) +MQTT_CLIENT_ID = f"MCPMQTT-{uuid.uuid4().hex[:8]}" -logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(name)s - %(levelname)s - %(message)s') -logger = logging.getLogger("MCPMQTT") +def getConfig_url(array): + url = "http://lzwcai-demp-corp-manager:8086/system/config/getConfig" + data = [array] + try: + response = requests.post(url, json=data, timeout=5) + response.raise_for_status() + data = response.json()['data'] + return data[0]['configValue'] + except Exception as e: + print(f"Error fetching config for {array}: {e}") + return None -MQTT_BROKER = '192.168.1.199' -MQTT_PORT = 1883 -MQTT_CLIENT_ID = "MCPMQTT" -MQTT_USERNAME = 'lzwc' -MQTT_PASSWORD = 'Lzwc@4187.' -RESPONSE_TOPIC = "unitree_response" - -class MCPMQTT: +class MQTTHandler: def __init__(self): - self.client_ = mqtt.Client(client_id=MQTT_CLIENT_ID) - self.client_.username_pw_set(MQTT_USERNAME, MQTT_PASSWORD) - self.client_.on_connect = self._on_connect - self.client_.on_message = self._on_message - self.client_.on_disconnect = self._on_disconnect - self._response_handlers = {} # 存储响应处理器 - self._lock = threading.Lock() # 线程锁 - self._response_topic = RESPONSE_TOPIC + self.client = mqtt.Client() + self.tasks = {} # {task_id: task_data} + self._lock = threading.Lock() + + # 获取MQTT配置，提供默认值 + self.mqtt_username = getenv('MQTT_USERNAME') or 'lzwc' + self.mqtt_password = getenv('MQTT_PASSWORD') or 'Lzwc@4187.' + mqtt_broker_raw = getenv('MQTT_BROKER') or 'emqx' + # 移除协议前缀，只保留主机名 + self.mqtt_broker = mqtt_broker_raw.replace('tcp://', '').replace('mqtt://', '') + mqtt_port_str = getenv('MQTT_PORT') + self.mqtt_port = int(mqtt_port_str) if mqtt_port_str else 1883 + + # 记录配置信息 + logger.info(f"MQTT配置 - Broker: {self.mqtt_broker}, Port: {self.mqtt_port}, Username: {self.mqtt_username}") + if not getenv('MQTT_BROKER'): + logger.warning("MQTT_BROKER环境变量未设置，使用默认值") + if not mqtt_port_str: + logger.warning("MQTT_PORT环境变量未设置，使用默认端口1883") + self.client.username_pw_set(self.mqtt_username, self.mqtt_password) + self.client.on_connect = self._on_connect + self.client.on_message = self._on_message + self.client.on_disconnect = self._on_disconnect try: - logger.info(f"正在连接 MQTT 代理: {MQTT_BROKER}:{MQTT_PORT}") - self.client_.connect(MQTT_BROKER, MQTT_PORT, 60) - self.client_.loop_start() + logger.info(f"正在连接 MQTT 代理: {self.mqtt_broker}:{self.mqtt_port}") + self.client.connect(self.mqtt_broker, self.mqtt_port, 60) + self.client.loop_start() except Exception as e: logger.error(f"连接 MQTT 失败: {e}") + def _on_connect(self, client, userdata, flags, rc): """MQTT 连接回调""" if rc == 0: logger.info("已成功连接到 MQTT 代理服务器") - client.subscribe(self._response_topic, qos=2) - logger.info(f"已订阅响应主题: {self._response_topic}") else: logger.error(f"连接 MQTT 代理服务器失败，返回码: {rc}") - def register_response_handler(self, correlation_id: str, callback: Callable): - """注册响应消息处理器""" + def get_status(self, task_id: str = None): + """获取任务状态 + Args: + task_id: 可选参数，指定任务ID。如果为None则返回所有任务状态 + Returns: + 如果指定task_id则返回该任务的状态，否则返回所有任务状态 + """ with self._lock: - self._response_handlers[correlation_id] = callback + if task_id: + return self.tasks.get(task_id, {}).copy() + return {k: v.copy() for k, v in self.tasks.items()} def _on_message(self, client, userdata, msg): try: payload = json.loads(msg.payload.decode()) - topic = msg.topic - logger.info(f"[MQTT] 收到消息 topic={topic}") - - # 处理响应消息 - if topic == self._response_topic and "correlation_id" in payload: - with self._lock: - handler = self._response_handlers.pop(payload["correlation_id"], None) - if handler: - handler(payload) - return - - # 打印常规消息 - logger.info(f"[MQTT] 消息内容: {payload}") + logger.info(f"[MQTT] topic={msg.topic} msg: {payload}") + # task_id = payload.get('task_id') + # if task_id and task_id in self.tasks: + # with self._lock: + # self.tasks[task_id].update({ + # 'status': payload.get('status', 'UNKNOWN'), + # 'description': payload.get('description', '') + # }) + except Exception as e: logger.error(f"[错误] 处理消息失败: {e}") @@ -78,11 +103,11 @@ class MCPMQTT: logger.error(f"重新连接 MQTT 失败: {e}") # 单例模式 -_instance: Optional[MCPMQTT] = None +_instance: Optional[MQTTHandler] = None -def get_mqtt_handler() -> MCPMQTT: - """获取MCPMQTT单例""" +def get_mcpmqtt_handler() -> MQTTHandler: + """获取单例""" global _instance if _instance is None: - _instance = MCPMQTT() + _instance = MQTTHandler() return _instance