diff --git a/sim/__init__.py b/sim/__init__.py new file mode 100644 index 0000000..3803095 --- /dev/null +++ b/sim/__init__.py @@ -0,0 +1,24 @@ +""" +HUTB MCP 工具模块 + +包含所有 MCP 工具的实现: +- vehicle_tools: 车辆控制工具 +- air_tools: 无人机控制工具 +- editor_tools: 编辑器控制工具 +- weather_tools: 天气控制工具 +- sensor_tools: 传感器管理工具 +""" + +from .vehicle_tools import register_vehicle_tools +from .air_tools import register_air_tools +from .editor_tools import register_editor_tools +from .weather_tools import register_weather_tools +from .sensor_tools import register_sensor_tools + +__all__ = [ + "register_vehicle_tools", + "register_air_tools", + "register_editor_tools", + "register_weather_tools", + "register_sensor_tools", +] diff --git a/sim/air_tools.py b/sim/air_tools.py new file mode 100644 index 0000000..a285c20 --- /dev/null +++ b/sim/air_tools.py @@ -0,0 +1,303 @@ +""" +无人机控制工具模块 + +提供无人机生成、飞行控制等功能。 +注意:HUTB/CARLA 原生不支持无人机,此模块提供基于 Walker 的模拟实现。 +""" + +import logging +from typing import Dict, List, Any, Optional +from mcp.server.fastmcp import FastMCP, Context + +logger = logging.getLogger("hutb_mcp.tools.air") + + +def register_air_tools(mcp: FastMCP) -> None: + """注册无人机控制工具""" + + @mcp.tool() + def spawn_drone( + ctx: Context, + location: List[float], + rotation: List[float] = None + ) -> Dict[str, Any]: + """ + 生成无人机(模拟实现) + + 注意:CARLA 原生不支持无人机,此功能使用特殊 Actor 模拟。 + + Args: + location: 生成位置 [x, y, z] + rotation: 旋转角度 [pitch, yaw, roll],默认 [0, 0, 0] + + Returns: + 生成结果 + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + rotation = rotation or [0, 0, 0] + + # 使用静态道具模拟无人机 + # 在实际项目中,可以使用自定义的无人机模型 + blueprint = conn.blueprint_library.find("static.prop.box01") + + if not blueprint: + # 尝试其他可用的静态道具 + props = list(conn.blueprint_library.filter("static.prop.*")) + if props: + blueprint = props[0] + else: + return {"success": False, "error": "未找到可用的无人机蓝图"} + + # 创建变换 + transform = carla.Transform( + carla.Location(x=location[0], y=location[1], z=location[2]), + carla.Rotation(pitch=rotation[0], yaw=rotation[1], roll=rotation[2]) + ) + + # 生成无人机 + drone = conn.world.spawn_actor(blueprint, transform) + + if drone: + conn.track_actor(drone.id) + logger.info(f"生成无人机成功: {drone.id}") + return { + "success": True, + "drone_id": drone.id, + "location": location, + "note": "无人机为模拟实现,使用静态道具代替" + } + else: + return {"success": False, "error": "生成无人机失败"} + + except Exception as e: + logger.error(f"生成无人机失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def get_drone_state(ctx: Context, drone_id: int) -> Dict[str, Any]: + """ + 获取无人机状态 + + Args: + drone_id: 无人机 ID + + Returns: + 无人机状态信息 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + drone = conn.world.get_actor(drone_id) + if not drone: + return {"success": False, "error": f"未找到无人机: {drone_id}"} + + transform = drone.get_transform() + velocity = drone.get_velocity() + + result = { + "success": True, + "drone_id": drone_id, + "location": { + "x": transform.location.x, + "y": transform.location.y, + "z": transform.location.z + }, + "rotation": { + "pitch": transform.rotation.pitch, + "yaw": transform.rotation.yaw, + "roll": transform.rotation.roll + }, + "velocity": { + "x": velocity.x, + "y": velocity.y, + "z": velocity.z + }, + "altitude": transform.location.z + } + + logger.info(f"获取无人机 {drone_id} 状态成功") + return result + + except Exception as e: + logger.error(f"获取无人机状态失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def set_drone_destination( + ctx: Context, + drone_id: int, + destination: List[float] + ) -> Dict[str, Any]: + """ + 设置无人机目标位置 + + Args: + drone_id: 无人机 ID + destination: 目标位置 [x, y, z] + + Returns: + 操作结果 + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + drone = conn.world.get_actor(drone_id) + if not drone: + return {"success": False, "error": f"未找到无人机: {drone_id}"} + + # 直接设置位置(简化实现) + # 在实际项目中,应该实现平滑的飞行路径 + current_transform = drone.get_transform() + current_transform.location = carla.Location( + x=destination[0], + y=destination[1], + z=destination[2] + ) + drone.set_transform(current_transform) + + logger.info(f"设置无人机 {drone_id} 目标位置: {destination}") + return { + "success": True, + "drone_id": drone_id, + "destination": destination, + "note": "位置已直接设置,实际项目中应实现平滑飞行" + } + + except Exception as e: + logger.error(f"设置无人机目标位置失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def drone_hover(ctx: Context, drone_id: int) -> Dict[str, Any]: + """ + 无人机悬停 + + Args: + drone_id: 无人机 ID + + Returns: + 操作结果 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + drone = conn.world.get_actor(drone_id) + if not drone: + return {"success": False, "error": f"未找到无人机: {drone_id}"} + + # 静态道具默认就是悬停状态 + transform = drone.get_transform() + + logger.info(f"无人机 {drone_id} 悬停中") + return { + "success": True, + "drone_id": drone_id, + "status": "hovering", + "location": { + "x": transform.location.x, + "y": transform.location.y, + "z": transform.location.z + } + } + + except Exception as e: + logger.error(f"无人机悬停失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def drone_land(ctx: Context, drone_id: int) -> Dict[str, Any]: + """ + 无人机降落 + + Args: + drone_id: 无人机 ID + + Returns: + 操作结果 + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + drone = conn.world.get_actor(drone_id) + if not drone: + return {"success": False, "error": f"未找到无人机: {drone_id}"} + + # 将无人机降落到地面 + transform = drone.get_transform() + transform.location.z = 0.5 # 降落到接近地面的高度 + drone.set_transform(transform) + + logger.info(f"无人机 {drone_id} 降落成功") + return { + "success": True, + "drone_id": drone_id, + "status": "landed", + "location": { + "x": transform.location.x, + "y": transform.location.y, + "z": transform.location.z + } + } + + except Exception as e: + logger.error(f"无人机降落失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def destroy_drone(ctx: Context, drone_id: int) -> Dict[str, Any]: + """ + 销毁无人机 + + Args: + drone_id: 无人机 ID + + Returns: + 操作结果 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + drone = conn.world.get_actor(drone_id) + if not drone: + return {"success": False, "error": f"未找到无人机: {drone_id}"} + + drone.destroy() + conn.untrack_actor(drone_id) + + logger.info(f"销毁无人机成功: {drone_id}") + return {"success": True, "drone_id": drone_id} + + except Exception as e: + logger.error(f"销毁无人机失败: {e}") + return {"success": False, "error": str(e)} + + logger.info("无人机控制工具注册完成") diff --git a/sim/editor_tools.py b/sim/editor_tools.py new file mode 100644 index 0000000..f28ed68 --- /dev/null +++ b/sim/editor_tools.py @@ -0,0 +1,327 @@ +""" +编辑器控制工具模块 + +提供场景编辑、Actor 管理、地图加载等功能。 +""" + +import logging +from typing import Dict, List, Any, Optional +from mcp.server.fastmcp import FastMCP, Context + +logger = logging.getLogger("hutb_mcp.tools.editor") + + +def register_editor_tools(mcp: FastMCP) -> None: + """注册编辑器控制工具""" + + @mcp.tool() + def get_actors_in_level(ctx: Context) -> List[Dict[str, Any]]: + """ + 获取当前场景中所有 Actor 的列表 + + Returns: + Actor 列表,每个 Actor 包含 id、type_id、name 等信息 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + logger.error("未连接到仿真器") + return [] + + actors = conn.world.get_actors() + result = [] + + for actor in actors: + result.append({ + "id": actor.id, + "type_id": actor.type_id, + "name": actor.attributes.get("role_name", ""), + "location": { + "x": actor.get_location().x, + "y": actor.get_location().y, + "z": actor.get_location().z + } + }) + + logger.info(f"获取到 {len(result)} 个 Actor") + return result + + except Exception as e: + logger.error(f"获取 Actor 列表失败: {e}") + return [] + + @mcp.tool() + def get_current_map(ctx: Context) -> str: + """ + 获取当前加载的地图名称 + + Returns: + 地图名称字符串 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + logger.error("未连接到仿真器") + return "" + + map_name = conn.map.name + logger.info(f"当前地图: {map_name}") + return map_name + + except Exception as e: + logger.error(f"获取地图名称失败: {e}") + return "" + + @mcp.tool() + def load_map(ctx: Context, map_name: str) -> Dict[str, Any]: + """ + 加载指定地图 + + Args: + map_name: 地图名称,如 "Town10" 或 "Town01" + + Returns: + 操作结果 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + logger.info(f"正在加载地图: {map_name}") + + # 清理当前资源 + conn.cleanup_all() + + # 加载新地图 + conn.client.load_world(map_name) + + # 更新世界和地图引用 + conn.world = conn.client.get_world() + conn.map = conn.world.get_map() + conn.blueprint_library = conn.world.get_blueprint_library() + + logger.info(f"地图 {map_name} 加载成功") + return {"success": True, "map": map_name} + + except Exception as e: + logger.error(f"加载地图失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def spawn_actor( + ctx: Context, + actor_type: str, + location: List[float], + rotation: List[float] = None + ) -> Dict[str, Any]: + """ + 在场景中生成 Actor + + Args: + actor_type: Actor 类型,如 "static.prop.bench" + location: 位置 [x, y, z] + rotation: 旋转 [pitch, yaw, roll],默认 [0, 0, 0] + + Returns: + 生成结果,包含 Actor ID + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + rotation = rotation or [0, 0, 0] + + # 查找蓝图 + blueprint = conn.blueprint_library.find(actor_type) + if not blueprint: + return {"success": False, "error": f"未找到蓝图: {actor_type}"} + + # 创建变换 + transform = carla.Transform( + carla.Location(x=location[0], y=location[1], z=location[2]), + carla.Rotation(pitch=rotation[0], yaw=rotation[1], roll=rotation[2]) + ) + + # 生成 Actor + actor = conn.world.spawn_actor(blueprint, transform) + + if actor: + conn.track_actor(actor.id) + logger.info(f"生成 Actor 成功: {actor.id} ({actor_type})") + return { + "success": True, + "actor_id": actor.id, + "type": actor_type, + "location": location + } + else: + return {"success": False, "error": "生成 Actor 失败"} + + except Exception as e: + logger.error(f"生成 Actor 失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def delete_actor(ctx: Context, actor_id: int) -> Dict[str, Any]: + """ + 删除指定 Actor + + Args: + actor_id: Actor ID + + Returns: + 操作结果 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + actor = conn.world.get_actor(actor_id) + if not actor: + return {"success": False, "error": f"未找到 Actor: {actor_id}"} + + actor.destroy() + conn.untrack_actor(actor_id) + + logger.info(f"删除 Actor 成功: {actor_id}") + return {"success": True, "actor_id": actor_id} + + except Exception as e: + logger.error(f"删除 Actor 失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def set_actor_transform( + ctx: Context, + actor_id: int, + location: List[float] = None, + rotation: List[float] = None + ) -> Dict[str, Any]: + """ + 设置 Actor 的位置和旋转 + + Args: + actor_id: Actor ID + location: 新位置 [x, y, z] + rotation: 新旋转 [pitch, yaw, roll] + + Returns: + 操作结果 + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + actor = conn.world.get_actor(actor_id) + if not actor: + return {"success": False, "error": f"未找到 Actor: {actor_id}"} + + current_transform = actor.get_transform() + + # 更新位置 + if location: + current_transform.location = carla.Location( + x=location[0], y=location[1], z=location[2] + ) + + # 更新旋转 + if rotation: + current_transform.rotation = carla.Rotation( + pitch=rotation[0], yaw=rotation[1], roll=rotation[2] + ) + + actor.set_transform(current_transform) + + logger.info(f"设置 Actor {actor_id} 变换成功") + return { + "success": True, + "actor_id": actor_id, + "location": [ + current_transform.location.x, + current_transform.location.y, + current_transform.location.z + ], + "rotation": [ + current_transform.rotation.pitch, + current_transform.rotation.yaw, + current_transform.rotation.roll + ] + } + + except Exception as e: + logger.error(f"设置 Actor 变换失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def get_actor_properties(ctx: Context, actor_id: int) -> Dict[str, Any]: + """ + 获取 Actor 的属性信息 + + Args: + actor_id: Actor ID + + Returns: + Actor 属性字典 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + actor = conn.world.get_actor(actor_id) + if not actor: + return {"success": False, "error": f"未找到 Actor: {actor_id}"} + + transform = actor.get_transform() + velocity = actor.get_velocity() + + result = { + "success": True, + "id": actor.id, + "type_id": actor.type_id, + "location": { + "x": transform.location.x, + "y": transform.location.y, + "z": transform.location.z + }, + "rotation": { + "pitch": transform.rotation.pitch, + "yaw": transform.rotation.yaw, + "roll": transform.rotation.roll + }, + "velocity": { + "x": velocity.x, + "y": velocity.y, + "z": velocity.z + }, + "attributes": dict(actor.attributes) + } + + logger.info(f"获取 Actor {actor_id} 属性成功") + return result + + except Exception as e: + logger.error(f"获取 Actor 属性失败: {e}") + return {"success": False, "error": str(e)} + + logger.info("编辑器控制工具注册完成") diff --git a/sim/pyproject.toml b/sim/pyproject.toml index a20df47..5005503 100644 --- a/sim/pyproject.toml +++ b/sim/pyproject.toml @@ -1,10 +1,63 @@ [project] -name = "carla-mcp" +name = "hutb-mcp" version = "0.1.0" -description = "Add your description here" +description = "基于 FastMCP 的 HUTB 人车仿真器交互控制系统" readme = "README.md" requires-python = ">=3.10" +license = {text = "MIT"} +authors = [ + {name = "徐杨杨", email = "xuyangyang@hutb.edu.cn"} +] +keywords = ["mcp", "carla", "simulator", "fastmcp", "autonomous-driving"] +classifiers = [ + "Development Status :: 3 - Alpha", + "Intended Audience :: Developers", + "License :: OSI Approved :: MIT License", + "Programming Language :: Python :: 3", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", +] + dependencies = [ - "carla>=0.9.15", - "mcp>=1.6.0", + "mcp[cli]>=1.4.1", + "python-dotenv>=1.0.0", + "pydantic>=2.0.0", +] + +# 注意: carla 需要从本地 wheel 文件安装 +# pip install D:/hutb/PythonAPI/carla/dist/hutb-2.9.16-cp310-cp310-win_amd64.whl + +[project.optional-dependencies] +dev = [ + "pytest>=7.0.0", + "pytest-asyncio>=0.21.0", + "hypothesis>=6.0.0", + "black>=23.0.0", + "isort>=5.0.0", + "mypy>=1.0.0", ] + +[project.scripts] +hutb-mcp = "hutb_mcp:main" + +[build-system] +requires = ["setuptools>=61.0", "wheel"] +build-backend = "setuptools.build_meta" + +[tool.setuptools.packages.find] +where = ["."] +include = ["hutb_mcp*"] + +[tool.black] +line-length = 100 +target-version = ['py310'] + +[tool.isort] +profile = "black" +line_length = 100 + +[tool.mypy] +python_version = "3.10" +warn_return_any = true +warn_unused_configs = true diff --git a/sim/sensor_tools.py b/sim/sensor_tools.py new file mode 100644 index 0000000..6c7646f --- /dev/null +++ b/sim/sensor_tools.py @@ -0,0 +1,347 @@ +""" +传感器管理工具模块 + +提供相机、激光雷达、雷达等传感器的创建和管理功能。 +""" + +import logging +from typing import Dict, List, Any, Optional +from mcp.server.fastmcp import FastMCP, Context + +logger = logging.getLogger("hutb_mcp.tools.sensor") + + +def register_sensor_tools(mcp: FastMCP) -> None: + """注册传感器管理工具""" + + @mcp.tool() + def attach_camera( + ctx: Context, + vehicle_id: int, + camera_type: str = "rgb", + location: List[float] = None, + rotation: List[float] = None, + image_size_x: int = 800, + image_size_y: int = 600, + fov: float = 90.0 + ) -> Dict[str, Any]: + """ + 为车辆附加相机传感器 + + Args: + vehicle_id: 车辆 ID + camera_type: 相机类型,可选值: rgb, depth, semantic_segmentation + location: 相对于车辆的位置 [x, y, z],默认 [0, 0, 2] + rotation: 相对于车辆的旋转 [pitch, yaw, roll],默认 [0, 0, 0] + image_size_x: 图像宽度 + image_size_y: 图像高度 + fov: 视场角 + + Returns: + 传感器信息 + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + location = location or [0, 0, 2] + rotation = rotation or [0, 0, 0] + + # 获取车辆 + vehicle = conn.world.get_actor(vehicle_id) + if not vehicle: + return {"success": False, "error": f"未找到车辆: {vehicle_id}"} + + # 确定相机蓝图 + camera_blueprints = { + "rgb": "sensor.camera.rgb", + "depth": "sensor.camera.depth", + "semantic_segmentation": "sensor.camera.semantic_segmentation" + } + + if camera_type not in camera_blueprints: + return { + "success": False, + "error": f"未知相机类型: {camera_type},可选值: {list(camera_blueprints.keys())}" + } + + # 获取蓝图 + blueprint = conn.blueprint_library.find(camera_blueprints[camera_type]) + + # 设置属性 + blueprint.set_attribute("image_size_x", str(image_size_x)) + blueprint.set_attribute("image_size_y", str(image_size_y)) + blueprint.set_attribute("fov", str(fov)) + + # 创建变换 + transform = carla.Transform( + carla.Location(x=location[0], y=location[1], z=location[2]), + carla.Rotation(pitch=rotation[0], yaw=rotation[1], roll=rotation[2]) + ) + + # 附加传感器 + sensor = conn.world.spawn_actor(blueprint, transform, attach_to=vehicle) + + if sensor: + conn.track_sensor(sensor.id) + logger.info(f"附加相机成功: {sensor.id} ({camera_type})") + return { + "success": True, + "sensor_id": sensor.id, + "type": camera_type, + "parent_id": vehicle_id, + "image_size": [image_size_x, image_size_y], + "fov": fov + } + else: + return {"success": False, "error": "附加相机失败"} + + except Exception as e: + logger.error(f"附加相机失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def attach_lidar( + ctx: Context, + vehicle_id: int, + location: List[float] = None, + rotation: List[float] = None, + channels: int = 32, + range: float = 100.0, + points_per_second: int = 56000, + rotation_frequency: float = 10.0, + upper_fov: float = 10.0, + lower_fov: float = -30.0 + ) -> Dict[str, Any]: + """ + 为车辆附加激光雷达传感器 + + Args: + vehicle_id: 车辆 ID + location: 相对于车辆的位置 [x, y, z],默认 [0, 0, 2.5] + rotation: 相对于车辆的旋转 [pitch, yaw, roll],默认 [0, 0, 0] + channels: 激光通道数 + range: 探测范围(米) + points_per_second: 每秒点数 + rotation_frequency: 旋转频率(Hz) + upper_fov: 上视场角 + lower_fov: 下视场角 + + Returns: + 传感器信息 + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + location = location or [0, 0, 2.5] + rotation = rotation or [0, 0, 0] + + # 获取车辆 + vehicle = conn.world.get_actor(vehicle_id) + if not vehicle: + return {"success": False, "error": f"未找到车辆: {vehicle_id}"} + + # 获取蓝图 + blueprint = conn.blueprint_library.find("sensor.lidar.ray_cast") + + # 设置属性 + blueprint.set_attribute("channels", str(channels)) + blueprint.set_attribute("range", str(range)) + blueprint.set_attribute("points_per_second", str(points_per_second)) + blueprint.set_attribute("rotation_frequency", str(rotation_frequency)) + blueprint.set_attribute("upper_fov", str(upper_fov)) + blueprint.set_attribute("lower_fov", str(lower_fov)) + + # 创建变换 + transform = carla.Transform( + carla.Location(x=location[0], y=location[1], z=location[2]), + carla.Rotation(pitch=rotation[0], yaw=rotation[1], roll=rotation[2]) + ) + + # 附加传感器 + sensor = conn.world.spawn_actor(blueprint, transform, attach_to=vehicle) + + if sensor: + conn.track_sensor(sensor.id) + logger.info(f"附加激光雷达成功: {sensor.id}") + return { + "success": True, + "sensor_id": sensor.id, + "type": "lidar", + "parent_id": vehicle_id, + "channels": channels, + "range": range + } + else: + return {"success": False, "error": "附加激光雷达失败"} + + except Exception as e: + logger.error(f"附加激光雷达失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def attach_radar( + ctx: Context, + vehicle_id: int, + location: List[float] = None, + rotation: List[float] = None, + horizontal_fov: float = 30.0, + vertical_fov: float = 10.0, + range: float = 100.0, + points_per_second: int = 1500 + ) -> Dict[str, Any]: + """ + 为车辆附加雷达传感器 + + Args: + vehicle_id: 车辆 ID + location: 相对于车辆的位置 [x, y, z],默认 [2, 0, 1] + rotation: 相对于车辆的旋转 [pitch, yaw, roll],默认 [0, 0, 0] + horizontal_fov: 水平视场角 + vertical_fov: 垂直视场角 + range: 探测范围(米) + points_per_second: 每秒点数 + + Returns: + 传感器信息 + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + location = location or [2, 0, 1] + rotation = rotation or [0, 0, 0] + + # 获取车辆 + vehicle = conn.world.get_actor(vehicle_id) + if not vehicle: + return {"success": False, "error": f"未找到车辆: {vehicle_id}"} + + # 获取蓝图 + blueprint = conn.blueprint_library.find("sensor.other.radar") + + # 设置属性 + blueprint.set_attribute("horizontal_fov", str(horizontal_fov)) + blueprint.set_attribute("vertical_fov", str(vertical_fov)) + blueprint.set_attribute("range", str(range)) + blueprint.set_attribute("points_per_second", str(points_per_second)) + + # 创建变换 + transform = carla.Transform( + carla.Location(x=location[0], y=location[1], z=location[2]), + carla.Rotation(pitch=rotation[0], yaw=rotation[1], roll=rotation[2]) + ) + + # 附加传感器 + sensor = conn.world.spawn_actor(blueprint, transform, attach_to=vehicle) + + if sensor: + conn.track_sensor(sensor.id) + logger.info(f"附加雷达成功: {sensor.id}") + return { + "success": True, + "sensor_id": sensor.id, + "type": "radar", + "parent_id": vehicle_id, + "horizontal_fov": horizontal_fov, + "vertical_fov": vertical_fov, + "range": range + } + else: + return {"success": False, "error": "附加雷达失败"} + + except Exception as e: + logger.error(f"附加雷达失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def destroy_sensor(ctx: Context, sensor_id: int) -> Dict[str, Any]: + """ + 销毁传感器 + + Args: + sensor_id: 传感器 ID + + Returns: + 操作结果 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + sensor = conn.world.get_actor(sensor_id) + if not sensor: + return {"success": False, "error": f"未找到传感器: {sensor_id}"} + + sensor.destroy() + conn.untrack_sensor(sensor_id) + + logger.info(f"销毁传感器成功: {sensor_id}") + return {"success": True, "sensor_id": sensor_id} + + except Exception as e: + logger.error(f"销毁传感器失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def list_vehicle_sensors(ctx: Context, vehicle_id: int) -> List[Dict[str, Any]]: + """ + 列出车辆上的所有传感器 + + Args: + vehicle_id: 车辆 ID + + Returns: + 传感器列表 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return [] + + # 获取所有传感器类型的 Actor + sensors = conn.world.get_actors().filter("sensor.*") + + result = [] + for sensor in sensors: + # 检查是否附加到指定车辆 + parent = sensor.parent + if parent and parent.id == vehicle_id: + transform = sensor.get_transform() + result.append({ + "id": sensor.id, + "type_id": sensor.type_id, + "location": { + "x": transform.location.x, + "y": transform.location.y, + "z": transform.location.z + }, + "attributes": dict(sensor.attributes) + }) + + logger.info(f"车辆 {vehicle_id} 有 {len(result)} 个传感器") + return result + + except Exception as e: + logger.error(f"列出传感器失败: {e}") + return [] + + logger.info("传感器管理工具注册完成") diff --git a/sim/vehicle_tools.py b/sim/vehicle_tools.py new file mode 100644 index 0000000..242336f --- /dev/null +++ b/sim/vehicle_tools.py @@ -0,0 +1,317 @@ +""" +车辆控制工具模块 + +提供车辆生成、控制、状态获取等功能。 +""" + +import logging +from typing import Dict, List, Any, Optional +from mcp.server.fastmcp import FastMCP, Context + +logger = logging.getLogger("hutb_mcp.tools.vehicle") + + +def register_vehicle_tools(mcp: FastMCP) -> None: + """注册车辆控制工具""" + + @mcp.tool() + def get_vehicle_blueprints(ctx: Context) -> List[str]: + """ + 获取所有可用的车辆蓝图列表 + + Returns: + 车辆蓝图 ID 列表 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + logger.error("未连接到仿真器") + return [] + + blueprints = conn.blueprint_library.filter("vehicle.*") + result = [bp.id for bp in blueprints] + + logger.info(f"获取到 {len(result)} 个车辆蓝图") + return result + + except Exception as e: + logger.error(f"获取车辆蓝图失败: {e}") + return [] + + @mcp.tool() + def spawn_vehicle( + ctx: Context, + blueprint_id: str, + location: List[float], + rotation: List[float] = None + ) -> Dict[str, Any]: + """ + 生成车辆 + + Args: + blueprint_id: 车辆蓝图 ID,如 "vehicle.tesla.model3" + location: 生成位置 [x, y, z] + rotation: 旋转角度 [pitch, yaw, roll],默认 [0, 0, 0] + + Returns: + 生成结果,包含车辆 ID + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + rotation = rotation or [0, 0, 0] + + # 查找蓝图 + blueprint = conn.blueprint_library.find(blueprint_id) + if not blueprint: + return {"success": False, "error": f"未找到车辆蓝图: {blueprint_id}"} + + # 设置随机颜色(如果支持) + if blueprint.has_attribute("color"): + color = blueprint.get_attribute("color").recommended_values + if color: + blueprint.set_attribute("color", color[0]) + + # 创建变换 + transform = carla.Transform( + carla.Location(x=location[0], y=location[1], z=location[2]), + carla.Rotation(pitch=rotation[0], yaw=rotation[1], roll=rotation[2]) + ) + + # 生成车辆 + vehicle = conn.world.spawn_actor(blueprint, transform) + + if vehicle: + conn.track_vehicle(vehicle.id) + logger.info(f"生成车辆成功: {vehicle.id} ({blueprint_id})") + return { + "success": True, + "vehicle_id": vehicle.id, + "blueprint": blueprint_id, + "location": location + } + else: + return {"success": False, "error": "生成车辆失败,可能位置有碰撞"} + + except Exception as e: + logger.error(f"生成车辆失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def destroy_vehicle(ctx: Context, vehicle_id: int) -> Dict[str, Any]: + """ + 销毁车辆 + + Args: + vehicle_id: 车辆 ID + + Returns: + 操作结果 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + vehicle = conn.world.get_actor(vehicle_id) + if not vehicle: + return {"success": False, "error": f"未找到车辆: {vehicle_id}"} + + vehicle.destroy() + conn.untrack_vehicle(vehicle_id) + + logger.info(f"销毁车辆成功: {vehicle_id}") + return {"success": True, "vehicle_id": vehicle_id} + + except Exception as e: + logger.error(f"销毁车辆失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def set_vehicle_autopilot( + ctx: Context, + vehicle_id: int, + enabled: bool + ) -> Dict[str, Any]: + """ + 设置车辆自动驾驶模式 + + Args: + vehicle_id: 车辆 ID + enabled: 是否启用自动驾驶 + + Returns: + 操作结果 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + vehicle = conn.world.get_actor(vehicle_id) + if not vehicle: + return {"success": False, "error": f"未找到车辆: {vehicle_id}"} + + vehicle.set_autopilot(enabled) + + logger.info(f"设置车辆 {vehicle_id} 自动驾驶: {enabled}") + return { + "success": True, + "vehicle_id": vehicle_id, + "autopilot": enabled + } + + except Exception as e: + logger.error(f"设置自动驾驶失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def apply_vehicle_control( + ctx: Context, + vehicle_id: int, + throttle: float = 0.0, + steer: float = 0.0, + brake: float = 0.0, + hand_brake: bool = False, + reverse: bool = False + ) -> Dict[str, Any]: + """ + 应用车辆控制 + + Args: + vehicle_id: 车辆 ID + throttle: 油门 (0.0-1.0) + steer: 转向 (-1.0 到 1.0,负值左转,正值右转) + brake: 刹车 (0.0-1.0) + hand_brake: 手刹 + reverse: 倒车 + + Returns: + 操作结果 + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + vehicle = conn.world.get_actor(vehicle_id) + if not vehicle: + return {"success": False, "error": f"未找到车辆: {vehicle_id}"} + + # 限制参数范围 + throttle = max(0.0, min(1.0, throttle)) + steer = max(-1.0, min(1.0, steer)) + brake = max(0.0, min(1.0, brake)) + + # 创建控制对象 + control = carla.VehicleControl( + throttle=throttle, + steer=steer, + brake=brake, + hand_brake=hand_brake, + reverse=reverse + ) + + vehicle.apply_control(control) + + logger.info(f"应用车辆 {vehicle_id} 控制: throttle={throttle}, steer={steer}, brake={brake}") + return { + "success": True, + "vehicle_id": vehicle_id, + "control": { + "throttle": throttle, + "steer": steer, + "brake": brake, + "hand_brake": hand_brake, + "reverse": reverse + } + } + + except Exception as e: + logger.error(f"应用车辆控制失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def get_vehicle_state(ctx: Context, vehicle_id: int) -> Dict[str, Any]: + """ + 获取车辆状态 + + Args: + vehicle_id: 车辆 ID + + Returns: + 车辆状态信息 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + vehicle = conn.world.get_actor(vehicle_id) + if not vehicle: + return {"success": False, "error": f"未找到车辆: {vehicle_id}"} + + transform = vehicle.get_transform() + velocity = vehicle.get_velocity() + acceleration = vehicle.get_acceleration() + angular_velocity = vehicle.get_angular_velocity() + + # 计算速度大小 (km/h) + speed = 3.6 * (velocity.x**2 + velocity.y**2 + velocity.z**2)**0.5 + + result = { + "success": True, + "vehicle_id": vehicle_id, + "type_id": vehicle.type_id, + "location": { + "x": transform.location.x, + "y": transform.location.y, + "z": transform.location.z + }, + "rotation": { + "pitch": transform.rotation.pitch, + "yaw": transform.rotation.yaw, + "roll": transform.rotation.roll + }, + "velocity": { + "x": velocity.x, + "y": velocity.y, + "z": velocity.z + }, + "speed_kmh": speed, + "acceleration": { + "x": acceleration.x, + "y": acceleration.y, + "z": acceleration.z + }, + "angular_velocity": { + "x": angular_velocity.x, + "y": angular_velocity.y, + "z": angular_velocity.z + } + } + + logger.info(f"获取车辆 {vehicle_id} 状态成功") + return result + + except Exception as e: + logger.error(f"获取车辆状态失败: {e}") + return {"success": False, "error": str(e)} + + logger.info("车辆控制工具注册完成") diff --git a/sim/weather_tools.py b/sim/weather_tools.py new file mode 100644 index 0000000..e96d69b --- /dev/null +++ b/sim/weather_tools.py @@ -0,0 +1,385 @@ +""" +天气控制工具模块 + +提供天气参数设置、预设天气、时间控制等功能。 +""" + +import logging +from typing import Dict, Any +from mcp.server.fastmcp import FastMCP, Context + +logger = logging.getLogger("hutb_mcp.tools.weather") + +# 天气预设 +WEATHER_PRESETS = { + "clear": { + "cloudiness": 0.0, + "precipitation": 0.0, + "precipitation_deposits": 0.0, + "wind_intensity": 0.0, + "sun_altitude_angle": 70.0, + "fog_density": 0.0, + "wetness": 0.0 + }, + "cloudy": { + "cloudiness": 80.0, + "precipitation": 0.0, + "precipitation_deposits": 0.0, + "wind_intensity": 20.0, + "sun_altitude_angle": 45.0, + "fog_density": 0.0, + "wetness": 0.0 + }, + "rainy": { + "cloudiness": 100.0, + "precipitation": 80.0, + "precipitation_deposits": 50.0, + "wind_intensity": 30.0, + "sun_altitude_angle": 30.0, + "fog_density": 10.0, + "wetness": 80.0 + }, + "foggy": { + "cloudiness": 50.0, + "precipitation": 0.0, + "precipitation_deposits": 0.0, + "wind_intensity": 10.0, + "sun_altitude_angle": 30.0, + "fog_density": 80.0, + "wetness": 20.0 + }, + "stormy": { + "cloudiness": 100.0, + "precipitation": 100.0, + "precipitation_deposits": 100.0, + "wind_intensity": 100.0, + "sun_altitude_angle": 10.0, + "fog_density": 30.0, + "wetness": 100.0 + } +} + + +def register_weather_tools(mcp: FastMCP) -> None: + """注册天气控制工具""" + + @mcp.tool() + def get_weather(ctx: Context) -> Dict[str, Any]: + """ + 获取当前天气参数 + + Returns: + 天气参数字典 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + weather = conn.world.get_weather() + + result = { + "success": True, + "cloudiness": weather.cloudiness, + "precipitation": weather.precipitation, + "precipitation_deposits": weather.precipitation_deposits, + "wind_intensity": weather.wind_intensity, + "sun_azimuth_angle": weather.sun_azimuth_angle, + "sun_altitude_angle": weather.sun_altitude_angle, + "fog_density": weather.fog_density, + "fog_distance": weather.fog_distance, + "wetness": weather.wetness, + "fog_falloff": weather.fog_falloff, + "scattering_intensity": weather.scattering_intensity, + "mie_scattering_scale": weather.mie_scattering_scale, + "rayleigh_scattering_scale": weather.rayleigh_scattering_scale + } + + logger.info("获取天气参数成功") + return result + + except Exception as e: + logger.error(f"获取天气参数失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def set_weather( + ctx: Context, + cloudiness: float = None, + precipitation: float = None, + precipitation_deposits: float = None, + wind_intensity: float = None, + sun_azimuth_angle: float = None, + sun_altitude_angle: float = None, + fog_density: float = None, + fog_distance: float = None, + wetness: float = None + ) -> Dict[str, Any]: + """ + 设置天气参数 + + Args: + cloudiness: 云量 (0-100) + precipitation: 降水量 (0-100) + precipitation_deposits: 降水沉积 (0-100) + wind_intensity: 风力强度 (0-100) + sun_azimuth_angle: 太阳方位角 (0-360) + sun_altitude_angle: 太阳高度角 (-90 到 90) + fog_density: 雾密度 (0-100) + fog_distance: 雾距离 + wetness: 湿度 (0-100) + + Returns: + 操作结果 + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + # 获取当前天气 + weather = conn.world.get_weather() + + # 更新指定参数(带范围限制) + if cloudiness is not None: + weather.cloudiness = max(0.0, min(100.0, cloudiness)) + if precipitation is not None: + weather.precipitation = max(0.0, min(100.0, precipitation)) + if precipitation_deposits is not None: + weather.precipitation_deposits = max(0.0, min(100.0, precipitation_deposits)) + if wind_intensity is not None: + weather.wind_intensity = max(0.0, min(100.0, wind_intensity)) + if sun_azimuth_angle is not None: + weather.sun_azimuth_angle = sun_azimuth_angle % 360.0 + if sun_altitude_angle is not None: + weather.sun_altitude_angle = max(-90.0, min(90.0, sun_altitude_angle)) + if fog_density is not None: + weather.fog_density = max(0.0, min(100.0, fog_density)) + if fog_distance is not None: + weather.fog_distance = max(0.0, fog_distance) + if wetness is not None: + weather.wetness = max(0.0, min(100.0, wetness)) + + # 应用天气 + conn.world.set_weather(weather) + + logger.info("设置天气参数成功") + return { + "success": True, + "cloudiness": weather.cloudiness, + "precipitation": weather.precipitation, + "sun_altitude_angle": weather.sun_altitude_angle, + "fog_density": weather.fog_density, + "wetness": weather.wetness + } + + except Exception as e: + logger.error(f"设置天气参数失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def set_weather_preset(ctx: Context, preset: str) -> Dict[str, Any]: + """ + 应用天气预设 + + Args: + preset: 预设名称,可选值: clear, cloudy, rainy, foggy, stormy + + Returns: + 操作结果 + """ + from ..connection import get_connection + import carla + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + preset = preset.lower() + if preset not in WEATHER_PRESETS: + return { + "success": False, + "error": f"未知预设: {preset},可选值: {list(WEATHER_PRESETS.keys())}" + } + + params = WEATHER_PRESETS[preset] + + # 创建天气对象 + weather = carla.WeatherParameters( + cloudiness=params["cloudiness"], + precipitation=params["precipitation"], + precipitation_deposits=params["precipitation_deposits"], + wind_intensity=params["wind_intensity"], + sun_altitude_angle=params["sun_altitude_angle"], + fog_density=params["fog_density"], + wetness=params["wetness"] + ) + + conn.world.set_weather(weather) + + logger.info(f"应用天气预设成功: {preset}") + return { + "success": True, + "preset": preset, + "parameters": params + } + + except Exception as e: + logger.error(f"应用天气预设失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def set_time_of_day(ctx: Context, hour: int, minute: int = 0) -> Dict[str, Any]: + """ + 设置一天中的时间(通过调整太阳位置) + + Args: + hour: 小时 (0-23) + minute: 分钟 (0-59) + + Returns: + 操作结果 + """ + from ..connection import get_connection + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + # 限制范围 + hour = max(0, min(23, hour)) + minute = max(0, min(59, minute)) + + # 计算太阳高度角 + # 简化模型:6:00 日出 (0°), 12:00 正午 (90°), 18:00 日落 (0°) + time_decimal = hour + minute / 60.0 + + if 6 <= time_decimal <= 18: + # 白天 + sun_altitude = 90.0 * (1 - abs(time_decimal - 12) / 6) + else: + # 夜晚 + sun_altitude = -45.0 + + # 计算太阳方位角 + sun_azimuth = (time_decimal - 6) * 15 # 每小时15度 + + weather = conn.world.get_weather() + weather.sun_altitude_angle = sun_altitude + weather.sun_azimuth_angle = sun_azimuth % 360 + conn.world.set_weather(weather) + + logger.info(f"设置时间成功: {hour:02d}:{minute:02d}") + return { + "success": True, + "time": f"{hour:02d}:{minute:02d}", + "sun_altitude_angle": sun_altitude, + "sun_azimuth_angle": sun_azimuth % 360 + } + + except Exception as e: + logger.error(f"设置时间失败: {e}") + return {"success": False, "error": str(e)} + + @mcp.tool() + def set_extreme_weather(ctx: Context, weather_type: str) -> Dict[str, Any]: + """ + 设置极端天气(用于压力测试) + + Args: + weather_type: 极端天气类型,可选值: + - heavy_rain: 暴雨 + - dense_fog: 浓雾 + - blizzard: 暴风雪 + - sandstorm: 沙尘暴 + + Returns: + 操作结果 + """ + from ..connection import get_connection + import carla + + extreme_presets = { + "heavy_rain": { + "cloudiness": 100.0, + "precipitation": 100.0, + "precipitation_deposits": 100.0, + "wind_intensity": 80.0, + "sun_altitude_angle": 5.0, + "fog_density": 20.0, + "wetness": 100.0 + }, + "dense_fog": { + "cloudiness": 100.0, + "precipitation": 0.0, + "precipitation_deposits": 0.0, + "wind_intensity": 5.0, + "sun_altitude_angle": 20.0, + "fog_density": 100.0, + "wetness": 50.0 + }, + "blizzard": { + "cloudiness": 100.0, + "precipitation": 100.0, + "precipitation_deposits": 100.0, + "wind_intensity": 100.0, + "sun_altitude_angle": 10.0, + "fog_density": 50.0, + "wetness": 0.0 + }, + "sandstorm": { + "cloudiness": 80.0, + "precipitation": 0.0, + "precipitation_deposits": 50.0, + "wind_intensity": 100.0, + "sun_altitude_angle": 30.0, + "fog_density": 70.0, + "wetness": 0.0 + } + } + + try: + conn = get_connection() + if not conn.is_connected(): + return {"success": False, "error": "未连接到仿真器"} + + weather_type = weather_type.lower() + if weather_type not in extreme_presets: + return { + "success": False, + "error": f"未知极端天气类型: {weather_type},可选值: {list(extreme_presets.keys())}" + } + + params = extreme_presets[weather_type] + + weather = carla.WeatherParameters( + cloudiness=params["cloudiness"], + precipitation=params["precipitation"], + precipitation_deposits=params["precipitation_deposits"], + wind_intensity=params["wind_intensity"], + sun_altitude_angle=params["sun_altitude_angle"], + fog_density=params["fog_density"], + wetness=params["wetness"] + ) + + conn.world.set_weather(weather) + + logger.info(f"设置极端天气成功: {weather_type}") + return { + "success": True, + "weather_type": weather_type, + "parameters": params + } + + except Exception as e: + logger.error(f"设置极端天气失败: {e}") + return {"success": False, "error": str(e)} + + logger.info("天气控制工具注册完成")