diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000000..b151230f23
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/simple_wheeled_vehicle/README.md b/src/simple_wheeled_vehicle/README.md
index d38afd39ea..c3d33dc29a 100644
--- a/src/simple_wheeled_vehicle/README.md
+++ b/src/simple_wheeled_vehicle/README.md
@@ -1,129 +1,139 @@
-# 简易轮式小车智能代理
-
-## 项目简介
-一个基于PyTorch神经网络与ROS的**简易轮式小车智能代理**,主要面向Carla自动驾驶模拟器。项目实现了从感知到控制的基本自动驾驶流程,模块化设计清晰,便于学习、扩展与二次开发。
-
-## 功能特性
-- **环境感知**:处理相机图像,识别车道线、可行驶区域及动态障碍物。
-- **决策规划**:基于感知结果进行局部路径规划与动态避障决策。
-- **运动控制**:输出转向、油门及刹车指令,控制小车沿规划路径行驶。
-- **训练接口**:提供数据采集与模型训练接口,支持强化学习与模仿学习。
-- **ROS集成**:通过ROS节点进行模块间通信,支持传感器数据发布与控制指令订阅。
-
-## 环境要求
-- **操作系统**:Ubuntu 20.04/22.04(推荐)或 Windows 10/11(WSL2)
-- **Python**:3.7 - 3.12
-- **核心框架**:PyTorch >= 1.9.0(CPU/GPU版本均可)
-- **模拟器**:Carla 0.9.14+
-- **可选依赖**:ROS Noetic 或 ROS2 Foxy(用于ROS通信)
-- **基础包**:numpy, opencv-python, matplotlib
-
-## 项目结构
-```
-src/simple_wheeled_agent/
-├── main.py # 模块主入口 (非ROS启动)
-├── main.launch # ROS启动配置文件
-├── requirements.txt
-├── config/
-│ └── default.yaml # 配置文件
-├── docs/ # 模块文档
-├── models/ # 预训练模型存放处
-├── scripts/ # 工具脚本
-├── src/ # 核心源代码
-│ ├── perception/
-│ │ ├── __init__.py
-│ │ ├── image_processor.py # 图像处理与神经网络推理
-│ │ └── data_collector.py # 训练数据采集
-│ ├── planning/
-│ │ ├── __init__.py
-│ │ └── local_planner.py # 局部路径规划器
-│ ├── control/
-│ │ ├── __init__.py
-│ │ └── pid_controller.py # 运动控制器
-│ └── utils/ # 通用工具函数
-└── tests/ # 单元测试
-```
-
-## 快速开始
-1. **安装依赖**:
- ```bash
- cd src/simple_wheeled_agent
- pip install -r requirements.txt -i https://mirrors.aliyun.com/pypi/simple/
- ```
-
-2. **启动Carla服务端** (在另一个终端):
- ```bash
- ./CarlaUE4.sh -quality-level=Low # 可根据硬件调整画质
- ```
-
-3. **运行代理**:
- ```bash
- python main.py --config config/default.yaml
- ```
- 程序将连接Carla,生成车辆,并开始自主驾驶。
-
-4. **(可选) ROS模式**:
- ```bash
- roslaunch simple_wheeled_agent main.launch
- ```
-
-## 主要类和方法
-- **`PerceptionAgent` (感知模块)**:
- - `process_image(raw_image)`: 处理原始图像,输出车道与障碍物信息。
- - `collect_training_data()`: 采集带标签的训练数据。
-
-- **`LocalPlanner` (规划模块)**:
- - `compute_path(perception_result, goal)`: 根据感知结果和全局目标计算局部路径。
- - `dynamic_obstacle_avoidance()`: 动态避障算法。
-
-- **`VehicleController` (控制模块)**:
- - `update_control(target_path, current_state)`: 根据目标路径和车辆状态计算控制量。
- - `get_control()`: 返回当前的控制指令(转向、油门、刹车)。
-
-## 配置参数
-关键参数可在 `config/default.yaml` 中调整:
-```yaml
-carla:
- host: localhost
- port: 2000
- timeout: 10.0
-vehicle:
- target_speed: 30.0 # 目标速度 (km/h)
- max_steer_angle: 70.0 # 最大转向角 (度)
-perception:
- model_path: models/perception_model.pth
- image_width: 640
- image_height: 480
-```
-
-## 数据输出
-运行过程中会生成以下数据:
-1. **控制台输出**:车辆状态、控制指令、故障信息。
-2. **日志文件** (位于 `logs/`): 运行日志与性能统计。
-3. **数据记录** (训练模式下): 图像与对应控制指令,保存于 `data/train/`。
-4. **可视化窗口**:实时显示处理后的图像与规划路径。
-
-## 扩展建议
-- **更换模拟器**:参考现有Carla接口,为AirSim或MuJoCo实现对应的环境包装器。
-- **改进感知模型**:替换 `perception` 模块中的网络结构(如使用ResNet、Transformer)。
-- **尝试新算法**:在 `planning` 模块中实现强化学习(PPO、SAC)或更先进的规划算法。
-- **增加传感器**:集成激光雷达(Lidar)或雷达(Radar)数据进行多传感器融合。
-
-## 常见问题
-1. **Q: 无法连接到Carla服务器**
- A: 请确认CarlaUE4正在运行,且端口未被占用。检查配置中的 `host` 和 `port`。
-
-2. **Q: 运行时报错 “No module named ‘torch’”**
- A: 请单独安装与你的Python及CUDA版本匹配的PyTorch。
-
-3. **Q: 车辆控制不平稳,抖动严重**
- A: 尝试调整 `config/default.yaml` 中的PID控制参数,或降低 `target_speed`。
-
-4. **Q: 如何用自己的数据训练模型?**
- A: 运行 `python main.py --mode collect` 采集数据,然后使用 `scripts/train_model.py` 进行训练。
-
-5. **Q: ROS节点无法启动**
- A: 确保ROS环境已正确配置 (`source /opt/ros//setup.bash`),且所有ROS依赖已安装。
-
----
-*更详细的API文档请使用 `mkdocs serve` 在本地构建和查看。*
\ No newline at end of file
+MuJoCo 简易轮式小车智能代理
+项目简介
+一个基于PyTorch神经网络与MuJoCo物理引擎的简易轮式小车智能代理,实现了在连续控制环境下的感知、决策与控制一体化解决方案。项目采用模块化设计,便于在仿真环境中进行机器人控制算法研究、强化学习训练和控制系统验证。
+
+功能特性
+物理仿真:基于MuJoCo的高保真物理模拟,支持关节约束、碰撞检测和接触力计算
+
+神经网络控制:使用PyTorch实现的深度强化学习或模仿学习策略
+
+多模态感知:集成视觉、本体感知和惯性测量单元(IMU)等多种传感器输入
+
+自适应控制:针对不同地形和任务需求调整控制策略
+
+训练框架:提供完整的强化学习训练循环和评估接口
+
+可视化工具:实时显示机器人状态、传感器数据和决策过程
+
+环境要求
+操作系统:Ubuntu 20.04/22.04(推荐)或 Windows 10/11(需配置WSL2)
+
+Python:3.7 - 3.10(MuJoCo官方支持范围)
+
+MuJoCo版本:2.3.0+(需要获取许可证)
+
+核心框架:PyTorch ≥ 1.9.0,MuJoCo-Py ≥ 2.3.0
+
+渲染支持:OpenGL或EGL(GPU加速推荐)
+
+推荐硬件:支持CUDA的NVIDIA显卡(用于神经网络加速)
+
+快速开始
+安装MuJoCo:
+
+bash
+# 获取MuJoCo许可证并下载
+pip install mujoco
+# 设置许可证密钥(按官方指引操作)
+安装依赖:
+
+bash
+cd src/mujoco_wheeled_robot
+pip install -r requirements.txt
+测试环境:
+
+bash
+python -c "import mujoco; print('MuJoCo version:', mujoco.__version__)"
+运行示例:
+
+bash
+# 启动随机策略演示
+python main.py --mode demo
+# 开始训练
+python main.py --mode train --config config/training_config.yaml
+主要类和方法
+WheeledRobotEnv (环境类):
+
+reset(): 重置环境到初始状态
+
+step(action): 执行动作并返回新状态、奖励和终止标志
+
+render(): 渲染当前环境状态
+
+PolicyNetwork (策略网络):
+
+forward(observation): 根据观测计算动作分布
+
+sample(observation): 从分布中采样动作
+
+evaluate(observation, action): 计算动作的对数概率和熵
+
+DataLogger (数据记录器):
+
+log_step(state, action, reward): 记录单步数据
+
+save_episode(episode_id): 保存回合数据
+
+export_trajectory(): 导出轨迹供分析使用
+
+配置参数
+关键配置位于 config/ 目录下:
+
+yaml
+# robot_config.yaml
+robot:
+ mass: 2.5 # 小车质量 (kg)
+ wheel_radius: 0.1 # 轮子半径 (m)
+ max_velocity: 3.0 # 最大速度 (m/s)
+ control_frequency: 50 # 控制频率 (Hz)
+
+# training_config.yaml
+training:
+ algorithm: "sac" # 使用的算法
+ total_timesteps: 1000000
+ learning_rate: 3e-4
+ gamma: 0.99 # 折扣因子
+ batch_size: 256
+ buffer_size: 1000000
+数据输出
+训练日志:TensorBoard格式,包含奖励曲线、策略熵等指标
+
+模型检查点:定期保存的策略网络参数
+
+轨迹数据:.npz格式,包含状态、动作、奖励序列
+
+评估结果:性能指标统计(平均奖励、成功率等)
+
+视频记录:.mp4格式的演示视频
+
+扩展建议
+添加传感器:在模型文件中增加激光雷达或深度相机
+
+更换任务:修改奖励函数实现不同任务(如避障、路径跟踪)
+
+集成真实数据:使用真实机器人数据微调仿真模型
+
+多机器人协同:扩展环境支持多机器人交互
+
+分布式训练:使用Ray等框架实现并行训练加速
+
+模型预测控制:结合学习的动力学模型实现MPC控制器
+
+常见问题
+Q: MuJoCo许可证如何获取?
+A: 访问MuJoCo官网获取个人或机构许可证,按照官方指引配置环境变量。
+
+Q: 仿真运行速度慢
+A: 检查是否启用了GPU渲染,降低渲染分辨率,或使用 headless 模式进行训练。
+
+Q: 训练不稳定,奖励不收敛
+A: 尝试调整超参数(如学习率、折扣因子),增加网络容量,或检查奖励函数设计。
+
+Q: 如何导入自定义机器人模型?
+A: 将 .xml 模型文件放入 models/ 目录,在环境初始化时指定模型路径。
+
+Q: 在Windows上运行问题
+A: 建议使用WSL2或Linux环境,Windows原生支持有限且可能遇到兼容性问题。
+
+Q: 模型无法学习有效策略
+A: 确保观测空间包含足够信息,检查动作空间是否合理,尝试更简单的任务作为起点。
diff --git a/src/simple_wheeled_vehicle/model.py b/src/simple_wheeled_vehicle/model.py
new file mode 100644
index 0000000000..81241f851a
--- /dev/null
+++ b/src/simple_wheeled_vehicle/model.py
@@ -0,0 +1,618 @@
+import pygame
+import math
+import sys
+import numpy as np
+from typing import Tuple, List
+
+# 初始化pygame
+pygame.init()
+
+# 颜色定义
+BACKGROUND = (30, 30, 40)
+CAR_BODY = (70, 130, 180)
+CAR_WHEEL = (50, 50, 60)
+SENSOR_COLOR = (255, 200, 0, 150)
+TEXT_COLOR = (220, 220, 220)
+GRID_COLOR = (60, 60, 70)
+OBSTACLE_COLOR = (180, 80, 80)
+PATH_COLOR = (100, 200, 100, 150)
+BUTTON_COLOR = (80, 80, 100)
+BUTTON_HOVER_COLOR = (100, 100, 120)
+
+# 小车参数
+CAR_WIDTH = 60
+CAR_HEIGHT = 40
+WHEEL_RADIUS = 12
+WHEEL_WIDTH = 6
+SENSOR_RANGE = 150
+SENSOR_ANGLE = 45 # 度数
+
+# 创建窗口
+WIDTH, HEIGHT = 1000, 700
+screen = pygame.display.set_mode((WIDTH, HEIGHT))
+pygame.display.set_caption("轮式小车模型模拟器")
+clock = pygame.time.Clock()
+
+
+class Button:
+ """按钮类"""
+
+ def __init__(self, x, y, width, height, text):
+ self.rect = pygame.Rect(x, y, width, height)
+ self.text = text
+ self.color = BUTTON_COLOR
+ self.hover_color = BUTTON_HOVER_COLOR
+ self.current_color = self.color
+ self.font = pygame.font.SysFont(None, 24)
+
+ def draw(self, surface):
+ pygame.draw.rect(surface, self.current_color, self.rect, border_radius=5)
+ pygame.draw.rect(surface, (120, 120, 140), self.rect, 2, border_radius=5)
+
+ text_surf = self.font.render(self.text, True, TEXT_COLOR)
+ text_rect = text_surf.get_rect(center=self.rect.center)
+ surface.blit(text_surf, text_rect)
+
+ def check_hover(self, pos):
+ if self.rect.collidepoint(pos):
+ self.current_color = self.hover_color
+ return True
+ else:
+ self.current_color = self.color
+ return False
+
+ def is_clicked(self, pos, event):
+ if event.type == pygame.MOUSEBUTTONDOWN and event.button == 1:
+ return self.rect.collidepoint(pos)
+ return False
+
+
+class Obstacle:
+ """障碍物类"""
+
+ def __init__(self, x, y, radius):
+ self.x = x
+ self.y = y
+ self.radius = radius
+
+ def draw(self, surface):
+ pygame.draw.circle(surface, OBSTACLE_COLOR, (self.x, self.y), self.radius)
+ pygame.draw.circle(surface, (150, 60, 60), (self.x, self.y), self.radius, 2)
+
+
+class WheeledCar:
+ """轮式小车类"""
+
+ def __init__(self, x, y):
+ self.x = x
+ self.y = y
+ self.width = CAR_WIDTH
+ self.height = CAR_HEIGHT
+ self.angle = 0 # 朝向角度(弧度)
+ self.speed = 0
+ self.max_speed = 4
+ self.angular_speed = 0
+ self.max_angular_speed = 0.08
+ self.left_wheel_speed = 0
+ self.right_wheel_speed = 0
+ self.sensor_data = [0, 0, 0] # 左、前、右传感器
+ self.path = [] # 记录轨迹
+ self.max_path_length = 100
+
+ def update(self, obstacles):
+ """更新小车状态"""
+ # 更新位置
+ self.x += self.speed * math.cos(self.angle)
+ self.y += self.speed * math.sin(self.angle)
+
+ # 更新角度
+ self.angle += self.angular_speed
+
+ # 归一化角度到0-2π
+ self.angle %= 2 * math.pi
+
+ # 更新传感器数据
+ self.update_sensors(obstacles)
+
+ # 记录轨迹
+ self.path.append((self.x, self.y))
+ if len(self.path) > self.max_path_length:
+ self.path.pop(0)
+
+ def update_sensors(self, obstacles):
+ """更新传感器数据"""
+ sensor_angles = [
+ self.angle - math.radians(SENSOR_ANGLE), # 左传感器
+ self.angle, # 前传感器
+ self.angle + math.radians(SENSOR_ANGLE) # 右传感器
+ ]
+
+ for i, sensor_angle in enumerate(sensor_angles):
+ min_distance = SENSOR_RANGE
+
+ # 传感器射线终点
+ end_x = self.x + SENSOR_RANGE * math.cos(sensor_angle)
+ end_y = self.y + SENSOR_RANGE * math.sin(sensor_angle)
+
+ # 检查与所有障碍物的交点
+ for obstacle in obstacles:
+ distance = self.line_obstacle_intersection(
+ self.x, self.y, end_x, end_y, obstacle
+ )
+ if distance < min_distance:
+ min_distance = distance
+
+ # 存储传感器数据(归一化到0-1)
+ self.sensor_data[i] = 1.0 - (min_distance / SENSOR_RANGE)
+
+ def line_obstacle_intersection(self, x1, y1, x2, y2, obstacle):
+ """计算线段与障碍物的交点距离"""
+ # 从障碍物对象获取属性
+ ox = obstacle.x
+ oy = obstacle.y
+ radius = obstacle.radius
+
+ # 线段向量
+ dx = x2 - x1
+ dy = y2 - y1
+
+ # 圆心到线段起点的向量
+ fx = x1 - ox
+ fy = y1 - oy
+
+ # 二次方程系数
+ a = dx * dx + dy * dy
+ b = 2 * (fx * dx + fy * dy)
+ c = (fx * fx + fy * fy) - radius * radius
+
+ # 判别式
+ discriminant = b * b - 4 * a * c
+
+ if discriminant < 0:
+ # 没有交点
+ return SENSOR_RANGE
+
+ discriminant = math.sqrt(discriminant)
+
+ # 两个解
+ t1 = (-b - discriminant) / (2 * a)
+ t2 = (-b + discriminant) / (2 * a)
+
+ # 检查交点是否在线段上
+ hits = []
+ if 0 <= t1 <= 1:
+ hits.append(t1)
+ if 0 <= t2 <= 1:
+ hits.append(t2)
+
+ if not hits:
+ return SENSOR_RANGE
+
+ # 返回最近交点的距离
+ t = min(hits)
+ intersection_x = x1 + t * dx
+ intersection_y = y1 + t * dy
+
+ return math.sqrt((intersection_x - x1) ** 2 + (intersection_y - y1) ** 2)
+
+ def set_control(self, forward, backward, left, right):
+ """设置控制输入"""
+ # 计算线速度
+ if forward and not backward:
+ self.speed = self.max_speed
+ elif backward and not forward:
+ self.speed = -self.max_speed / 2
+ else:
+ self.speed = 0
+
+ # 计算角速度
+ if left and not right:
+ self.angular_speed = -self.max_angular_speed
+ elif right and not left:
+ self.angular_speed = self.max_angular_speed
+ else:
+ self.angular_speed = 0
+
+ # 模拟差速驱动
+ if self.speed != 0:
+ if left:
+ self.left_wheel_speed = self.speed * 0.5
+ self.right_wheel_speed = self.speed
+ elif right:
+ self.left_wheel_speed = self.speed
+ self.right_wheel_speed = self.speed * 0.5
+ else:
+ self.left_wheel_speed = self.speed
+ self.right_wheel_speed = self.speed
+ else:
+ self.left_wheel_speed = 0
+ self.right_wheel_speed = 0
+
+ def draw(self, surface):
+ """绘制小车"""
+ # 绘制轨迹
+ if len(self.path) > 1:
+ pygame.draw.lines(surface, PATH_COLOR, False, self.path, 2)
+
+ # 计算小车四个角的坐标
+ cos_angle = math.cos(self.angle)
+ sin_angle = math.sin(self.angle)
+
+ half_width = self.width / 2
+ half_height = self.height / 2
+
+ # 车身四个顶点
+ corners = [
+ (-half_width, -half_height),
+ (half_width, -half_height),
+ (half_width, half_height),
+ (-half_width, half_height)
+ ]
+
+ # 旋转和平移顶点
+ rotated_corners = []
+ for x, y in corners:
+ # 旋转
+ rx = x * cos_angle - y * sin_angle
+ ry = x * sin_angle + y * cos_angle
+ # 平移
+ tx = rx + self.x
+ ty = ry + self.y
+ rotated_corners.append((tx, ty))
+
+ # 绘制车身
+ pygame.draw.polygon(surface, CAR_BODY, rotated_corners)
+ pygame.draw.polygon(surface, (40, 80, 130), rotated_corners, 2)
+
+ # 绘制轮子
+ wheel_offsets = [
+ (-half_width + 5, -half_height - WHEEL_WIDTH / 2), # 左前轮
+ (-half_width + 5, half_height - WHEEL_WIDTH / 2), # 左后轮
+ (half_width - 5, -half_height - WHEEL_WIDTH / 2), # 右前轮
+ (half_width - 5, half_height - WHEEL_WIDTH / 2) # 右后轮
+ ]
+
+ for wx, wy in wheel_offsets:
+ # 旋转轮子位置
+ rx = wx * cos_angle - wy * sin_angle
+ ry = wx * sin_angle + wy * cos_angle
+ tx = rx + self.x
+ ty = ry + self.y
+
+ # 绘制轮子
+ pygame.draw.ellipse(surface, CAR_WHEEL,
+ (tx - WHEEL_RADIUS, ty - WHEEL_WIDTH / 2,
+ WHEEL_RADIUS * 2, WHEEL_WIDTH))
+
+ # 轮子上的条纹(表示转动)
+ stripe_angle = self.angle + pygame.time.get_ticks() / 100
+ stripe_x = tx + WHEEL_RADIUS * 0.7 * math.cos(stripe_angle)
+ stripe_y = ty + WHEEL_RADIUS * 0.7 * math.sin(stripe_angle)
+ pygame.draw.circle(surface, (30, 30, 40), (int(stripe_x), int(stripe_y)), WHEEL_WIDTH // 2)
+
+ # 绘制传感器
+ sensor_angles = [
+ self.angle - math.radians(SENSOR_ANGLE),
+ self.angle,
+ self.angle + math.radians(SENSOR_ANGLE)
+ ]
+
+ for i, sensor_angle in enumerate(sensor_angles):
+ # 传感器探测距离基于传感器数据
+ sensor_length = SENSOR_RANGE * (1 - self.sensor_data[i])
+ end_x = self.x + sensor_length * math.cos(sensor_angle)
+ end_y = self.y + sensor_length * math.sin(sensor_angle)
+
+ # 绘制传感器线
+ sensor_surf = pygame.Surface((WIDTH, HEIGHT), pygame.SRCALPHA)
+ pygame.draw.line(sensor_surf, SENSOR_COLOR,
+ (self.x, self.y), (end_x, end_y), 2)
+ surface.blit(sensor_surf, (0, 0))
+
+ # 绘制传感器终点
+ if self.sensor_data[i] > 0:
+ pygame.draw.circle(surface, (255, 100, 100),
+ (int(end_x), int(end_y)), 5)
+
+ # 绘制小车前方的箭头
+ front_x = self.x + half_width * cos_angle
+ front_y = self.y + half_width * sin_angle
+ arrow_length = 20
+
+ arrow_left_x = front_x - arrow_length * math.cos(self.angle - math.pi / 6)
+ arrow_left_y = front_y - arrow_length * math.sin(self.angle - math.pi / 6)
+ arrow_right_x = front_x - arrow_length * math.cos(self.angle + math.pi / 6)
+ arrow_right_y = front_y - arrow_length * math.sin(self.angle + math.pi / 6)
+
+ pygame.draw.polygon(surface, (255, 255, 255), [
+ (front_x, front_y),
+ (arrow_left_x, arrow_left_y),
+ (arrow_right_x, arrow_right_y)
+ ])
+
+
+def draw_grid(surface):
+ """绘制网格"""
+ grid_size = 50
+
+ # 垂直线
+ for x in range(0, WIDTH, grid_size):
+ pygame.draw.line(surface, GRID_COLOR, (x, 0), (x, HEIGHT), 1)
+
+ # 水平线
+ for y in range(0, HEIGHT, grid_size):
+ pygame.draw.line(surface, GRID_COLOR, (0, y), (WIDTH, y), 1)
+
+
+def draw_info_panel(surface, car, control_state, mode):
+ """绘制信息面板"""
+ panel_width = 300
+ panel_rect = pygame.Rect(WIDTH - panel_width, 0, panel_width, HEIGHT)
+
+ # 绘制面板背景
+ pygame.draw.rect(surface, (40, 40, 50), panel_rect)
+ pygame.draw.line(surface, (70, 70, 90), (WIDTH - panel_width, 0), (WIDTH - panel_width, HEIGHT), 2)
+
+ font = pygame.font.SysFont(None, 28)
+ small_font = pygame.font.SysFont(None, 24)
+
+ # 标题
+ title = font.render("小车状态", True, TEXT_COLOR)
+ surface.blit(title, (WIDTH - panel_width + 20, 20))
+
+ # 位置信息
+ info_y = 60
+ infos = [
+ f"位置: ({car.x:.1f}, {car.y:.1f})",
+ f"角度: {math.degrees(car.angle):.1f}°",
+ f"速度: {car.speed:.2f}",
+ f"角速度: {math.degrees(car.angular_speed):.2f}°/s",
+ f"左轮速度: {car.left_wheel_speed:.2f}",
+ f"右轮速度: {car.right_wheel_speed:.2f}"
+ ]
+
+ for info in infos:
+ text = small_font.render(info, True, TEXT_COLOR)
+ surface.blit(text, (WIDTH - panel_width + 20, info_y))
+ info_y += 30
+
+ # 传感器数据
+ info_y += 20
+ sensor_title = font.render("传感器数据", True, TEXT_COLOR)
+ surface.blit(sensor_title, (WIDTH - panel_width + 20, info_y))
+
+ info_y += 40
+ sensor_labels = ["左传感器", "前传感器", "右传感器"]
+ for i, label in enumerate(sensor_labels):
+ # 文本
+ text = small_font.render(f"{label}: {car.sensor_data[i]:.2f}", True, TEXT_COLOR)
+ surface.blit(text, (WIDTH - panel_width + 20, info_y))
+
+ # 进度条
+ bar_width = 150
+ bar_height = 15
+ bar_x = WIDTH - panel_width + 120
+ bar_y = info_y + 2
+
+ # 背景条
+ pygame.draw.rect(surface, (60, 60, 70), (bar_x, bar_y, bar_width, bar_height), border_radius=3)
+
+ # 填充条
+ fill_width = bar_width * car.sensor_data[i]
+ color_value = int(255 * car.sensor_data[i])
+ bar_color = (255, 255 - color_value, 100)
+ pygame.draw.rect(surface, bar_color, (bar_x, bar_y, fill_width, bar_height), border_radius=3)
+
+ info_y += 30
+
+ # 控制状态
+ info_y += 20
+ control_title = font.render("控制状态", True, TEXT_COLOR)
+ surface.blit(control_title, (WIDTH - panel_width + 20, info_y))
+
+ info_y += 40
+ control_keys = [
+ ("前进", "W / ↑", control_state["forward"]),
+ ("后退", "S / ↓", control_state["backward"]),
+ ("左转", "A / ←", control_state["left"]),
+ ("右转", "D / →", control_state["right"])
+ ]
+
+ for label, key, active in control_keys:
+ color = (100, 255, 100) if active else (150, 150, 150)
+ text = small_font.render(f"{label} ({key}): {'激活' if active else '未激活'}", True, color)
+ surface.blit(text, (WIDTH - panel_width + 20, info_y))
+ info_y += 30
+
+ # 模式信息
+ info_y += 20
+ mode_text = font.render(f"模式: {'键盘控制' if mode == 'keyboard' else '自动巡逻'}", True, TEXT_COLOR)
+ surface.blit(mode_text, (WIDTH - panel_width + 20, info_y))
+
+ # 操作说明
+ info_y = HEIGHT - 150
+ instructions_title = font.render("操作说明", True, TEXT_COLOR)
+ surface.blit(instructions_title, (WIDTH - panel_width + 20, info_y))
+
+ info_y += 30
+ instructions = [
+ "W/↑: 前进",
+ "S/↓: 后退",
+ "A/←: 左转",
+ "D/→: 右转",
+ "R: 重置小车位置",
+ "M: 切换控制模式",
+ "C: 清除轨迹",
+ "ESC: 退出"
+ ]
+
+ for instruction in instructions:
+ text = small_font.render(instruction, True, (180, 180, 200))
+ surface.blit(text, (WIDTH - panel_width + 20, info_y))
+ info_y += 25
+
+
+def main():
+ """主函数"""
+ # 创建小车
+ car = WheeledCar(WIDTH // 4, HEIGHT // 2)
+
+ # 创建障碍物
+ obstacles = [
+ Obstacle(300, 200, 30),
+ Obstacle(500, 300, 40),
+ Obstacle(200, 500, 35),
+ Obstacle(600, 150, 25),
+ Obstacle(400, 450, 30),
+ Obstacle(700, 400, 45)
+ ]
+
+ # 控制状态
+ control_state = {
+ "forward": False,
+ "backward": False,
+ "left": False,
+ "right": False
+ }
+
+ # 控制模式:keyboard(键盘控制)或 auto(自动巡逻)
+ control_mode = "keyboard"
+
+ # 创建按钮
+ reset_button = Button(WIDTH - 280, HEIGHT - 90, 120, 40, "重置位置")
+ clear_path_button = Button(WIDTH - 280, HEIGHT - 40, 120, 40, "清除轨迹")
+ mode_button = Button(WIDTH - 150, HEIGHT - 90, 120, 40, "切换模式")
+
+ running = True
+ auto_timer = 0
+
+ # 主循环
+ while running:
+ mouse_pos = pygame.mouse.get_pos()
+
+ # 处理事件
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT:
+ running = False
+
+ # 键盘按下事件
+ elif event.type == pygame.KEYDOWN:
+ if event.key == pygame.K_ESCAPE:
+ running = False
+ elif event.key in (pygame.K_w, pygame.K_UP):
+ control_state["forward"] = True
+ elif event.key in (pygame.K_s, pygame.K_DOWN):
+ control_state["backward"] = True
+ elif event.key in (pygame.K_a, pygame.K_LEFT):
+ control_state["left"] = True
+ elif event.key in (pygame.K_d, pygame.K_RIGHT):
+ control_state["right"] = True
+ elif event.key == pygame.K_r:
+ # 重置小车位置
+ car = WheeledCar(WIDTH // 4, HEIGHT // 2)
+ elif event.key == pygame.K_c:
+ # 清除轨迹
+ car.path = []
+ elif event.key == pygame.K_m:
+ # 切换控制模式
+ control_mode = "auto" if control_mode == "keyboard" else "keyboard"
+
+ # 键盘释放事件
+ elif event.type == pygame.KEYUP:
+ if event.key in (pygame.K_w, pygame.K_UP):
+ control_state["forward"] = False
+ elif event.key in (pygame.K_s, pygame.K_DOWN):
+ control_state["backward"] = False
+ elif event.key in (pygame.K_a, pygame.K_LEFT):
+ control_state["left"] = False
+ elif event.key in (pygame.K_d, pygame.K_RIGHT):
+ control_state["right"] = False
+
+ # 鼠标事件
+ elif event.type == pygame.MOUSEBUTTONDOWN:
+ if reset_button.is_clicked(mouse_pos, event):
+ car = WheeledCar(WIDTH // 4, HEIGHT // 2)
+ elif clear_path_button.is_clicked(mouse_pos, event):
+ car.path = []
+ elif mode_button.is_clicked(mouse_pos, event):
+ control_mode = "auto" if control_mode == "keyboard" else "keyboard"
+
+ # 更新按钮状态
+ reset_button.check_hover(mouse_pos)
+ clear_path_button.check_hover(mouse_pos)
+ mode_button.check_hover(mouse_pos)
+
+ # 自动巡逻模式
+ if control_mode == "auto":
+ auto_timer += 1
+
+ # 简单的自动巡逻逻辑:前进,遇到障碍物转向
+ if car.sensor_data[1] > 0.3: # 前方有障碍物
+ if car.sensor_data[0] < car.sensor_data[2]: # 左边障碍物更远
+ control_state["left"] = True
+ control_state["right"] = False
+ else: # 右边障碍物更远
+ control_state["left"] = False
+ control_state["right"] = True
+ control_state["forward"] = False
+ control_state["backward"] = True # 先后退
+ else:
+ # 前进
+ control_state["forward"] = True
+ control_state["backward"] = False
+
+ # 随机转向以避免卡住
+ if auto_timer % 120 == 0: # 每2秒
+ if np.random.random() > 0.5:
+ control_state["left"] = True
+ control_state["right"] = False
+ else:
+ control_state["left"] = False
+ control_state["right"] = True
+ elif auto_timer % 60 == 0: # 每1秒重置转向
+ control_state["left"] = False
+ control_state["right"] = False
+
+ # 更新小车控制
+ car.set_control(
+ control_state["forward"],
+ control_state["backward"],
+ control_state["left"],
+ control_state["right"]
+ )
+
+ # 更新小车状态
+ car.update(obstacles)
+
+ # 绘制背景
+ screen.fill(BACKGROUND)
+
+ # 绘制网格
+ draw_grid(screen)
+
+ # 绘制障碍物
+ for obstacle in obstacles:
+ obstacle.draw(screen)
+
+ # 绘制小车
+ car.draw(screen)
+
+ # 绘制信息面板
+ draw_info_panel(screen, car, control_state, control_mode)
+
+ # 绘制按钮
+ reset_button.draw(screen)
+ clear_path_button.draw(screen)
+ mode_button.draw(screen)
+
+ # 更新显示
+ pygame.display.flip()
+
+ # 控制帧率
+ clock.tick(60)
+ pygame.quit()
+ sys.exit()
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file