diff --git a/README.md b/README.md
new file mode 100644
index 0000000000..c198e51470
--- /dev/null
+++ b/README.md
@@ -0,0 +1,48 @@
+# 神经网络实现代理
+
+利用神经网络/ROS 实现 Carla(车辆、行人的感知、规划、控制)、AirSim、Mujoco 中人和载具的代理。
+
+## 环境配置
+
+* 平台:Windows 10/11,Ubuntu 20.04/22.04
+* 软件:Python 3.7-3.12(需支持3.7)、Pytorch(尽量不使用Tensorflow)
+* 相关软件下载 [链接](https://pan.baidu.com/s/1IFhCd8X9lI24oeYQm5-Edw?pwd=hutb)
+
+
+## 贡献指南
+
+准备提交代码之前,请阅读 [贡献指南](https://github.com/OpenHUTB/.github/blob/master/CONTRIBUTING.md) 。
+代码的优化包括:注释、[PEP 8 风格调整](https://peps.pythonlang.cn/pep-0008/) 、将神经网络应用到Carla模拟器中、撰写对应 [文档](https://openhutb.github.io/nn/) 、添加 [源代码对应的自动化测试](https://docs.github.com/zh/actions/use-cases-and-examples/building-and-testing/building-and-testing-python) 等(从Carla场景中获取神经网络所需数据或将神经网络的结果输出到场景中)。
+
+### 约定
+
+* 每个模块位于`src/{模块名}`目录下,`模块名`需要用2-3个单词表示,首字母不需要大写,下划线`_`分隔,不能宽泛,越具体越好
+* 每个模块的入口须为`main.`开头,比如:main.py、main.cpp、main.bat、main.sh等,提供的ROS功能以`main.launch`文件作为启动配置文件
+* 每次pull request都需要保证能够通过main脚本直接运行整个模块,在提交信息中提供运行动图或截图;Pull Request的标题不能随意,需要概括具体的修改内容;README.md文档中提供运行环境和运行步骤的说明
+* 仓库尽量保存文本文件,二进制文件需要慎重,如运行需要示例数据,可以保存少量数据,大量数据可以通过提供网盘链接并说明下载链接和运行说明
+
+
+### 文档生成
+
+测试生成的文档:
+1. 安装python 3.11,并使用以下命令安装`mkdocs`和相关依赖:
+```shell
+pip install mkdocs -i http://mirrors.aliyun.com/pypi/simple --trusted-host mirrors.aliyun.com
+pip install -r requirements.txt
+```
+(可选)安装完成后使用`mkdocs --version`查看是否安装成功。
+
+2. 在命令行中进入`nn`目录下,运行:
+```shell
+mkdocs build
+mkdocs serve
+```
+然后使用浏览器打开 [http://127.0.0.1:8000](http://127.0.0.1:8000),查看文档页面能否正常显示。
+
+## 参考
+
+* [代理模拟器文档](https://openhutb.github.io)
+* 已有相关 [无人车](https://openhutb.github.io/doc/used_by/) 、[无人机](https://openhutb.github.io/air_doc/third/used_by/) 、[具身人](https://openhutb.github.io/doc/pedestrian/humanoid/) 的实现
+* [神经网络原理](https://github.com/OpenHUTB/neuro)
+
+
diff --git a/requirements.txt b/requirements.txt
new file mode 100644
index 0000000000..2d3d95d455
--- /dev/null
+++ b/requirements.txt
@@ -0,0 +1,5 @@
+python-markdown-math
+imageio
+numpy
+matplotlib
+tensorflow
\ No newline at end of file
diff --git a/src/AI_Drone_Face_Recognition_Human_Tracking/detection_module.py b/src/AI_Drone_Face_Recognition_Human_Tracking/detection_module.py
new file mode 100644
index 0000000000..b5976246be
--- /dev/null
+++ b/src/AI_Drone_Face_Recognition_Human_Tracking/detection_module.py
@@ -0,0 +1,258 @@
+# -*- coding: utf-8 -*-
+"""
+无人机检测模块
+功能:
+1. 独立运行:模拟检测无人机状态、环境障碍物
+2. 被主程序导入:提供检测接口,返回预警信息
+"""
+import numpy as np
+import random
+from enum import Enum
+import time
+
+
+# ===================== 检测类型枚举(便于主程序调用) =====================
+class DetectionType(Enum):
+ OBSTACLE = "障碍物检测"
+ BATTERY = "电量检测"
+ POSITION = "位置检测"
+ COLLISION = "碰撞预警"
+ STATE = "状态检测"
+
+
+# ===================== 核心检测类 =====================
+class DroneDetection:
+ def __init__(self, drone=None):
+ """
+ 初始化检测模块
+ :param drone: 可选,传入虚拟无人机对象(主程序调用时传)
+ """
+ self.drone = drone # 关联无人机对象
+ self.obstacle_list = self._generate_obstacles() # 生成模拟障碍物
+ self.warning_threshold = {
+ "battery": 20.0, # 电量预警阈值(%)
+ "height": 0.5, # 高度过低预警阈值(m)
+ "distance": 1.0 # 障碍物距离预警阈值(m)
+ }
+
+ def _generate_obstacles(self):
+ """生成模拟环境障碍物(随机坐标)"""
+ obstacles = []
+ for _ in range(10): # 生成10个随机障碍物
+ x = random.uniform(-10, 10)
+ y = random.uniform(-10, 10)
+ z = random.uniform(0, 8)
+ obstacles.append(np.array([x, y, z]))
+ return obstacles
+
+ def detect_obstacle(self):
+ """
+ 障碍物检测:计算无人机与最近障碍物的距离
+ :return: dict - 检测结果(距离、是否预警、最近障碍物坐标)
+ """
+ if self.drone is None:
+ return {
+ "type": DetectionType.OBSTACLE.value,
+ "status": "未关联无人机",
+ "distance": None,
+ "warning": False,
+ "nearest_obstacle": None
+ }
+
+ # 计算无人机与每个障碍物的欧氏距离
+ drone_pos = self.drone.position
+ distances = [np.linalg.norm(drone_pos - obs) for obs in self.obstacle_list]
+ min_distance = min(distances)
+ nearest_idx = np.argmin(distances)
+ nearest_obs = self.obstacle_list[nearest_idx]
+
+ # 判断是否触发预警
+ warning = min_distance < self.warning_threshold["distance"]
+
+ return {
+ "type": DetectionType.OBSTACLE.value,
+ "status": "检测完成",
+ "distance": round(min_distance, 2),
+ "warning": warning,
+ "nearest_obstacle": nearest_obs.round(2),
+ "message": f"⚠️ 距离障碍物仅{min_distance:.2f}m,请注意避让!" if warning else "✅ 无障碍物风险"
+ }
+
+ def detect_battery(self):
+ """
+ 电量检测:判断是否低于预警阈值
+ :return: dict - 检测结果
+ """
+ if self.drone is None:
+ return {
+ "type": DetectionType.BATTERY.value,
+ "status": "未关联无人机",
+ "battery": None,
+ "warning": False,
+ "message": "未关联无人机,无法检测电量"
+ }
+
+ battery = self.drone.battery
+ warning = battery < self.warning_threshold["battery"]
+
+ return {
+ "type": DetectionType.BATTERY.value,
+ "status": "检测完成",
+ "battery": round(battery, 2),
+ "warning": warning,
+ "message": f"⚠️ 电量低({battery:.1f}%),请尽快返航!" if warning else f"✅ 电量充足({battery:.1f}%)"
+ }
+
+ def detect_position(self):
+ """
+ 位置检测:判断高度是否过低/超出边界
+ :return: dict - 检测结果
+ """
+ if self.drone is None:
+ return {
+ "type": DetectionType.POSITION.value,
+ "status": "未关联无人机",
+ "position": None,
+ "warning": False,
+ "message": "未关联无人机,无法检测位置"
+ }
+
+ pos = self.drone.position
+ # 兼容主程序枚举状态和独立运行字符串状态
+ drone_state = self.drone.state.value if hasattr(self.drone.state, "value") else self.drone.state
+ height_warning = pos[2] < self.warning_threshold["height"] and drone_state == "FLYING"
+ boundary_warning = abs(pos[0]) > 15 or abs(pos[1]) > 15 # 水平边界±15m
+
+ warning = height_warning or boundary_warning
+ messages = []
+ if height_warning:
+ messages.append(f"⚠️ 飞行高度过低({pos[2]:.1f}m),请注意!")
+ if boundary_warning:
+ messages.append(f"⚠️ 超出安全边界(坐标:{pos[:2].round(1)}),请返航!")
+ if not warning:
+ messages.append(f"✅ 位置正常({pos.round(1)})")
+
+ return {
+ "type": DetectionType.POSITION.value,
+ "status": "检测完成",
+ "position": pos.round(2),
+ "warning": warning,
+ "message": " | ".join(messages)
+ }
+
+ def detect_collision(self):
+ """
+ 碰撞预警:预测未来1秒是否有碰撞风险
+ :return: dict - 检测结果
+ """
+ if self.drone is None:
+ return {
+ "type": DetectionType.COLLISION.value,
+ "status": "未关联无人机",
+ "risk": False,
+ "message": "未关联无人机,无法预测碰撞风险"
+ }
+
+ # 预测1秒后无人机位置(基于当前速度)
+ future_pos = self.drone.position + self.drone.velocity * 1.0
+ # 计算与障碍物的距离
+ distances = [np.linalg.norm(future_pos - obs) for obs in self.obstacle_list]
+ min_future_dist = min(distances)
+ risk = min_future_dist < 0.5 # 距离<0.5m判定为碰撞风险
+
+ return {
+ "type": DetectionType.COLLISION.value,
+ "status": "检测完成",
+ "risk": risk,
+ "future_position": future_pos.round(2),
+ "message": "🚨 1秒后有碰撞风险!请立即调整方向!" if risk else "✅ 无碰撞风险"
+ }
+
+ def detect_state(self):
+ """
+ 状态检测:判断无人机当前状态是否正常
+ :return: dict - 检测结果
+ """
+ if self.drone is None:
+ return {
+ "type": DetectionType.STATE.value,
+ "status": "未关联无人机",
+ "drone_state": None,
+ "message": "未关联无人机,无法检测状态"
+ }
+
+ # 兼容主程序枚举状态(DroneState)和独立运行字符串状态
+ state = self.drone.state.value if hasattr(self.drone.state, "value") else self.drone.state
+ if state == "LANDED":
+ message = "✅ 无人机处于落地状态,状态正常"
+ elif state == "FLYING" and self.drone.battery > 10:
+ message = "✅ 无人机处于飞行状态,状态正常"
+ else:
+ message = f"⚠️ 无人机飞行状态异常(电量{self.drone.battery:.1f}%)"
+
+ return {
+ "type": DetectionType.STATE.value,
+ "status": "检测完成",
+ "drone_state": state,
+ "warning": state == "FLYING" and self.drone.battery <= 10,
+ "message": message
+ }
+
+ def full_detection(self):
+ """
+ 全量检测:执行所有检测项
+ :return: list - 所有检测结果
+ """
+ results = [
+ self.detect_state(),
+ self.detect_battery(),
+ self.detect_position(),
+ self.detect_obstacle(),
+ self.detect_collision()
+ ]
+ return results
+
+
+# ===================== 独立运行测试代码 =====================
+def main():
+ """模块独立运行时的测试逻辑"""
+ print("=" * 60)
+ print("📊 无人机检测模块 - 独立测试模式")
+ print("=" * 60)
+
+ # 模拟无人机对象(独立运行时使用,简化状态为字符串,避免枚举类错误)
+ class MockDrone:
+ def __init__(self):
+ self.position = np.array([2.0, 1.5, 1.0]) # 模拟位置
+ self.velocity = np.array([0.8, 0.5, 0.0]) # 模拟速度
+ self.state = "FLYING" # 直接用字符串标识状态,替代枚举类
+ self.battery = 18.5 # 模拟电量
+
+ # 初始化检测模块
+ mock_drone = MockDrone()
+ detector = DroneDetection(drone=mock_drone)
+
+ # 循环检测(模拟实时监测)
+ try:
+ while True:
+ print(f"\n⏰ 检测时间:{time.strftime('%Y-%m-%d %H:%M:%S')}")
+ # 执行全量检测
+ all_results = detector.full_detection()
+ for res in all_results:
+ print(f"[{res['type']}] {res['message']}")
+
+ # 模拟无人机位置变化
+ mock_drone.position += mock_drone.velocity * 0.5
+ mock_drone.battery -= 0.5 # 模拟电量消耗
+ # 模拟飞行状态切换(每10秒落地一次)
+ if mock_drone.battery < 10:
+ mock_drone.state = "LANDED"
+ time.sleep(2) # 每2秒检测一次
+
+ except KeyboardInterrupt:
+ print("\n\n🛑 检测模块独立测试已停止")
+
+
+# 仅当模块独立运行时执行测试
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/AI_Drone_Face_Recognition_Human_Tracking/human_face_detection_engine.py b/src/AI_Drone_Face_Recognition_Human_Tracking/human_face_detection_engine.py
new file mode 100644
index 0000000000..51ff11fe20
--- /dev/null
+++ b/src/AI_Drone_Face_Recognition_Human_Tracking/human_face_detection_engine.py
@@ -0,0 +1,336 @@
+from ultralytics import YOLO
+import cv2
+import numpy as np
+import os
+import json
+from typing import Optional, Tuple, List, Dict
+
+
+class FaceDatabase:
+ # 【复用之前的FaceDatabase类代码,此处省略(保持不变)】
+ def __init__(self, data_dir: str = "face_database", threshold: float = 0.6):
+ self.data_dir = data_dir
+ self.feat_dir = os.path.join(data_dir, "features")
+ self.meta_dir = os.path.join(data_dir, "metadata")
+ self.threshold = threshold
+ os.makedirs(self.data_dir, exist_ok=True)
+ os.makedirs(self.feat_dir, exist_ok=True)
+ os.makedirs(self.meta_dir, exist_ok=True)
+ self.face_features: Dict[str, np.ndarray] = {}
+ self.face_metadata: Dict[str, dict] = {}
+
+ @staticmethod
+ def preprocess_face(face_roi: np.ndarray) -> Optional[np.ndarray]:
+ if face_roi.size == 0:
+ return None
+ gray = cv2.cvtColor(face_roi, cv2.COLOR_BGR2GRAY)
+ gray = cv2.resize(gray, (128, 128))
+ gray = cv2.equalizeHist(gray)
+ feature = gray.flatten() / 255.0
+ return feature
+
+ def save_face(self, name: str, face_roi: np.ndarray, remark: str = "", overwrite: bool = True) -> bool:
+ feature = self.preprocess_face(face_roi)
+ if feature is None:
+ print(f"❌ 人脸预处理失败,无法保存{name}")
+ return False
+ feat_path = os.path.join(self.feat_dir, f"{name}.npy")
+ meta_path = os.path.join(self.meta_dir, f"{name}.json")
+ if os.path.exists(feat_path) and not overwrite:
+ print(f"⚠️ {name}已存在,跳过保存(如需覆盖请设置overwrite=True)")
+ return False
+ np.save(feat_path, feature)
+ metadata = {
+ "name": name,
+ "remark": remark,
+ "feature_shape": feature.shape,
+ "add_time": str(np.datetime64('now')),
+ "version": "1.0"
+ }
+ with open(meta_path, "w", encoding="utf-8") as f:
+ json.dump(metadata, f, ensure_ascii=False, indent=2)
+ self.face_features[name] = feature
+ self.face_metadata[name] = metadata
+ print(f"✅ {name}人脸数据保存成功")
+ return True
+
+ def load_face(self, name: str) -> bool:
+ feat_path = os.path.join(self.feat_dir, f"{name}.npy")
+ meta_path = os.path.join(self.meta_dir, f"{name}.json")
+ if not os.path.exists(feat_path) or not os.path.exists(meta_path):
+ print(f"❌ {name}人脸数据不存在")
+ return False
+ feature = np.load(feat_path)
+ with open(meta_path, "r", encoding="utf-8") as f:
+ metadata = json.load(f)
+ self.face_features[name] = feature
+ self.face_metadata[name] = metadata
+ print(f"✅ 加载{name}人脸数据成功")
+ return True
+
+ def load_all_faces(self) -> int:
+ loaded_count = 0
+ for feat_file in os.listdir(self.feat_dir):
+ if not feat_file.endswith(".npy"):
+ continue
+ name = os.path.splitext(feat_file)[0]
+ if self.load_face(name):
+ loaded_count += 1
+ print(f"📊 批量加载完成,共加载{loaded_count}个人脸数据")
+ return loaded_count
+
+ def delete_face(self, name: str) -> bool:
+ feat_path = os.path.join(self.feat_dir, f"{name}.npy")
+ meta_path = os.path.join(self.meta_dir, f"{name}.json")
+ for path in [feat_path, meta_path]:
+ if os.path.exists(path):
+ os.remove(path)
+ if name in self.face_features:
+ del self.face_features[name]
+ if name in self.face_metadata:
+ del self.face_metadata[name]
+ print(f"🗑️ {name}人脸数据已删除")
+ return True
+
+ def list_faces(self) -> List[str]:
+ face_list = [os.path.splitext(f)[0] for f in os.listdir(self.feat_dir) if f.endswith(".npy")]
+ print(f"📋 已保存的人脸列表:{face_list}")
+ return face_list
+
+ @staticmethod
+ def calculate_similarity(feature1: np.ndarray, feature2: np.ndarray) -> float:
+ if len(feature1) != len(feature2):
+ return 0.0
+ dot = np.dot(feature1, feature2)
+ norm1 = np.linalg.norm(feature1)
+ norm2 = np.linalg.norm(feature2)
+ return dot / (norm1 * norm2) if (norm1 * norm2) != 0 else 0.0
+
+ def match_face(self, face_roi: np.ndarray) -> Optional[str]:
+ query_feat = self.preprocess_face(face_roi)
+ if query_feat is None or not self.face_features:
+ return None
+ max_sim = 0.0
+ matched_name = None
+ for name, feat in self.face_features.items():
+ sim = self.calculate_similarity(query_feat, feat)
+ if sim > max_sim and sim > self.threshold:
+ max_sim = sim
+ matched_name = name
+ return matched_name
+
+
+class DetectionEngine:
+ # 【复用之前的DetectionEngine类代码,此处省略(保持不变)】
+ def __init__(self,
+ model_path: str = "yolov8n.pt",
+ conf_thres: float = 0.5,
+ track_thres: float = 0.4,
+ is_face_model: bool = False):
+ self.model = YOLO(model_path)
+ self.conf_thres = conf_thres
+ self.track_thres = track_thres
+ self.class_names = self.model.names
+ self.human_class_id = 0
+ self.face_class_id = 0 if is_face_model else None
+
+ def detect(self, frame: np.ndarray) -> List:
+ if frame is None or frame.size == 0:
+ return []
+ results = self.model(
+ frame,
+ conf=self.conf_thres,
+ iou=self.track_thres,
+ show=False,
+ verbose=False
+ )
+ return results
+
+ def get_largest_human(self, results: List) -> Optional[Tuple[int, int, int, int]]:
+ largest_bbox = None
+ max_area = 0
+ for r in results:
+ if not hasattr(r, 'boxes') or r.boxes is None:
+ continue
+ for box in r.boxes:
+ cls = int(box.cls[0])
+ if cls == self.human_class_id:
+ x1, y1, x2, y2 = map(int, box.xyxy[0])
+ x1 = max(0, x1)
+ y1 = max(0, y1)
+ x2 = min(frame.shape[1], x2)
+ y2 = min(frame.shape[0], y2)
+ area = (x2 - x1) * (y2 - y1)
+ if area > max_area and area > 100:
+ max_area = area
+ largest_bbox = (x1, y1, x2, y2)
+ return largest_bbox
+
+ def match_faces(self, frame: np.ndarray, results: List, face_db: FaceDatabase) -> np.ndarray:
+ frame_copy = frame.copy()
+ h, w = frame_copy.shape[:2]
+ for r in results:
+ if not hasattr(r, 'boxes') or r.boxes is None:
+ continue
+ for box in r.boxes:
+ cls = int(box.cls[0])
+ conf = float(box.conf[0])
+ x1, y1, x2, y2 = map(int, box.xyxy[0])
+ x1 = max(0, x1)
+ y1 = max(0, y1)
+ x2 = min(w, x2)
+ y2 = min(h, y2)
+ if self.face_class_id is not None and cls == self.face_class_id:
+ face_roi = frame_copy[y1:y2, x1:x2]
+ name = face_db.match_face(face_roi) or "未知人脸"
+ frame_copy = self.draw_detection_box(frame_copy, (x1, y1, x2, y2), name, conf)
+ elif cls == self.human_class_id:
+ frame_copy = self.draw_detection_box(frame_copy, (x1, y1, x2, y2), "人体", conf)
+ return frame_copy
+
+ @staticmethod
+ def draw_detection_box(frame: np.ndarray,
+ bbox: Tuple[int, int, int, int],
+ label: str,
+ confidence: float) -> np.ndarray:
+ x1, y1, x2, y2 = bbox
+ cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
+ label_text = f"{label} ({confidence:.2f})"
+ font = cv2.FONT_HERSHEY_SIMPLEX
+ text_size, _ = cv2.getTextSize(label_text, font, 0.6, 1)
+ text_w, text_h = text_size
+ bg_x1, bg_y1 = x1, max(y1 - text_h - 10, 0)
+ bg_x2, bg_y2 = x1 + text_w, y1
+ cv2.rectangle(frame, (bg_x1, bg_y1), (bg_x2, bg_y2), (0, 255, 0), -1)
+ cv2.putText(frame, label_text, (x1, bg_y1 + text_h + 2),
+ font, 0.6, (255, 255, 255), 1)
+ return frame
+
+
+# ------------------------------
+# 主程序(集成按键操作)
+# ------------------------------
+if __name__ == "__main__":
+ # 1. 初始化组件
+ engine = DetectionEngine(model_path="yolov8n.pt", is_face_model=False) # 如需人脸检测,切换为yolov8n-face.pt
+ face_db = FaceDatabase(data_dir="my_face_db", threshold=0.6)
+ face_db.load_all_faces() # 启动时加载所有已保存的人脸
+
+ # 2. 初始化状态变量
+ cap = cv2.VideoCapture(0)
+ is_paused = False # 暂停状态
+ show_visual = True # 可视化检测框
+ screenshot_dir = "screenshots"
+ os.makedirs(screenshot_dir, exist_ok=True)
+
+ # 3. 打印操作提示
+ print("=" * 50)
+ print("📱 操作按键说明:")
+ print(" q - 退出程序")
+ print(" s - 保存当前最大人脸到库(需先检测到人脸)")
+ print(" d - 删除指定姓名的人脸数据")
+ print(" l - 重新加载所有人脸数据")
+ print(" v - 切换检测框可视化(显示/隐藏)")
+ print(" p - 暂停/继续实时检测")
+ print(" f - 保存当前帧截图到screenshots目录")
+ print(" t - 调整人脸匹配阈值(0~1)")
+ print("=" * 50)
+
+ while cap.isOpened():
+ # 暂停状态下只处理按键,不读取帧
+ if not is_paused:
+ ret, frame = cap.read()
+ if not ret:
+ print("❌ 无法读取摄像头画面,程序退出")
+ break
+
+ # 执行检测
+ results = engine.detect(frame)
+ largest_human_bbox = engine.get_largest_human(results)
+ if largest_human_bbox:
+ print(f"🔍 检测到最大人体:{largest_human_bbox}", end="\r") # 实时打印(覆盖行)
+
+ # 绘制检测框(根据可视化状态)
+ if show_visual:
+ frame_display = engine.match_faces(frame, results, face_db)
+ else:
+ frame_display = frame.copy()
+ else:
+ frame_display = frame.copy() # 暂停时保持最后一帧
+
+ # 显示画面
+ cv2.imshow("🤖 人体/人脸检测系统", frame_display)
+
+ # 按键响应(非阻塞,等待1ms)
+ key = cv2.waitKey(1) & 0xFF
+ if key == ord('q'):
+ # 退出程序
+ print("\n👋 程序正常退出")
+ break
+ elif key == ord('p'):
+ # 暂停/继续
+ is_paused = not is_paused
+ status = "暂停" if is_paused else "继续"
+ print(f"\n⏸️ 检测已{status}")
+ elif key == ord('v'):
+ # 切换可视化
+ show_visual = not show_visual
+ status = "显示" if show_visual else "隐藏"
+ print(f"\n🎨 检测框已{status}")
+ elif key == ord('f'):
+ # 保存截图
+ screenshot_path = os.path.join(screenshot_dir, f"screenshot_{np.datetime64('now').astype(str)}.png")
+ cv2.imwrite(screenshot_path, frame_display)
+ print(f"\n📸 截图已保存:{screenshot_path}")
+ elif key == ord('l'):
+ # 重新加载人脸
+ face_db.load_all_faces()
+ elif key == ord('d'):
+ # 删除人脸(控制台输入姓名)
+ del_name = input("\n🗑️ 请输入要删除的人脸姓名:")
+ face_db.delete_face(del_name)
+ elif key == ord('t'):
+ # 调整阈值
+ try:
+ new_thresh = float(input("\n🎛️ 请输入新的人脸匹配阈值(0~1):"))
+ if 0 <= new_thresh <= 1:
+ face_db.threshold = new_thresh
+ print(f"✅ 阈值已更新为:{new_thresh}")
+ else:
+ print("❌ 阈值需在0~1之间")
+ except ValueError:
+ print("❌ 输入无效,请输入数字")
+ elif key == ord('s'):
+ # 保存当前最大人脸(优先人脸检测,无则取人体框内的人脸区域)
+ save_name = input("\n💾 请输入要保存的人脸姓名:")
+ save_remark = input("📝 请输入备注(可选):")
+
+ # 尝试获取人脸ROI(优先检测到的人脸,无则取最大人体的中间区域)
+ face_roi = None
+ results = engine.detect(frame)
+ # 方式1:如果用了人脸模型,提取第一个检测到的人脸
+ if engine.face_class_id is not None:
+ for r in results:
+ if hasattr(r, 'boxes') and r.boxes is not None:
+ for box in r.boxes:
+ if int(box.cls[0]) == engine.face_class_id:
+ x1, y1, x2, y2 = map(int, box.xyxy[0])
+ face_roi = frame[y1:y2, x1:x2]
+ break
+ # 方式2:无检测人脸时,取最大人体的上半部分(人脸区域)
+ if face_roi is None:
+ largest_human = engine.get_largest_human(results)
+ if largest_human:
+ x1, y1, x2, y2 = largest_human
+ # 截取人体上半部分作为人脸ROI(需手动调整比例)
+ face_h = int((y2 - y1) * 0.3)
+ face_roi = frame[y1:y1 + face_h, x1:x2]
+
+ if face_roi is not None and face_roi.size > 0:
+ face_db.save_face(save_name, face_roi, save_remark, overwrite=True)
+ else:
+ print("❌ 未检测到有效人脸区域,保存失败")
+
+ # 释放资源
+ cap.release()
+ cv2.destroyAllWindows()
\ No newline at end of file
diff --git a/src/Active-Lane-Keeping-Assistant/world.py b/src/Active-Lane-Keeping-Assistant/world.py
index 7f106f10ca..13348785e4 100644
--- a/src/Active-Lane-Keeping-Assistant/world.py
+++ b/src/Active-Lane-Keeping-Assistant/world.py
@@ -47,12 +47,23 @@ def __init__(self, server_ip: str = '127.0.0.1', port: int = 2000,
# Use Tesla Model 3 as Car
self.bp = self.blueprint_library.filter('model3')[0]
- # === 修复 2: 随机选择出生点,防止车位被占导致无法生成 ===
- spawn_points = self.world.get_map().get_spawn_points()
- if spawn_points:
- self.spawn_point = random.choice(spawn_points)
- else:
- self.spawn_point = carla.Transform() # 防止没有生成点的情况
+ # === 修改: 将出生点设置为当前上帝视角相机的位置 ===
+ # 1. 获取上帝视角的观察者对象
+ spectator = self.world.get_spectator()
+
+ # 2. 获取观察者现在的坐标和朝向
+ transform = spectator.get_transform()
+
+ # 3.稍微把位置抬高一点(+2米),防止相机贴地导致车卡在地里
+ transform.location.z += 2.0
+
+ # 4. 稍微把位置往前挪一点(+5米),让你能直接看到车出现在眼前
+ # (利用朝向向量计算前方的位置)
+ forward_vector = transform.get_forward_vector()
+ transform.location.x += forward_vector.x * 5.0
+ transform.location.y += forward_vector.y * 5.0
+
+ self.spawn_point = transform
# ====================================================
self.vehicle = None
diff --git a/src/AutonomousDriving_RL/carla_env_multi_obs.py b/src/AutonomousDriving_RL/carla_env_multi_obs.py
index 51784501e2..1d97813b7f 100644
--- a/src/AutonomousDriving_RL/carla_env_multi_obs.py
+++ b/src/AutonomousDriving_RL/carla_env_multi_obs.py
@@ -1,25 +1,64 @@
# carla_env/carla_env_multi_obs.py
-import carla
-import numpy as np
-import random
-import time
-from gymnasium import Env, spaces
+# 本文件定义了一个基于 CARLA 仿真器的自定义 Gymnasium 环境,
+# 用于训练强化学习智能体(如 PPO)控制自动驾驶车辆。
+# 观测空间为 4 维连续状态(位置 + 速度),动作空间为 3 维连续控制(油门、转向、刹车)。
+
+import carla # CARLA 仿真器 Python API
+import numpy as np # 数值计算库
+import random # 随机数生成
+import time # 时间控制(用于延迟)
+import os # 操作系统接口(文件操作)
+import json # JSON 文件读写(用于保存/加载车辆ID)
+from gymnasium import Env, spaces # Gymnasium 标准环境接口
+
+# 定义临时文件路径,用于记录上一次运行生成的车辆ID(与脚本同目录)
+VEHICLE_ID_FILE = ".last_vehicle_id.json"
class CarlaEnvMultiObs(Env):
- def __init__(self):
+ """
+ 自定义 CARLA 强化学习环境类,继承自 gymnasium.Env。
+ 支持自动清理历史车辆、多地图适配、第三人称视角跟随、安全spawn等特性。
+ """
+
+ def __init__(self, keep_alive_after_exit=True):
+ """
+ 初始化环境。
+ :param keep_alive_after_exit: 若为 True,close() 时不销毁车辆,便于人工观察或录屏。
+ """
super(CarlaEnvMultiObs, self).__init__()
+
+ # CARLA 客户端与世界对象
self.client = None
self.world = None
- self.vehicle = None
- self.actor_list = []
- self.frame_count = 0
- self.max_frames = 1000
- # 观测空间:[x, y, speed_x, speed_y]
+ # 车辆相关
+ self.vehicle = None # 当前控制的车辆 Actor
+ self._current_vehicle_id = None # 本次生成的车辆 ID(用于下次清理)
+
+ # 训练控制
+ self.frame_count = 0 # 已执行的仿真步数
+ self.max_frames = 1000 # 最大允许步数(用于 truncated 判定)
+ self.prev_x = 0.0 # 上一帧的 x 坐标(用于计算位移奖励)
+
+ # 视角控制
+ self.spectator = None # CARLA 观察者(摄像头)
+
+ # 行为标志
+ self.keep_alive = keep_alive_after_exit
+
+ # 定义观测空间:[x, y, vx, vy] —— 位置 (m) + 速度 (m/s)
self.observation_space = spaces.Box(
- low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32
+ low=-np.inf,
+ high=np.inf,
+ shape=(4,),
+ dtype=np.float32
)
+
+ # 定义动作空间:[throttle, steer, brake]
+ # - throttle: [0.0, 1.0] 油门(0=松开,1=全踩)
+ # - steer: [-1.0, 1.0] 转向(-1=左打满,1=右打满)
+ # - brake: [0.0, 1.0] 刹车(0=松开,1=全刹)
self.action_space = spaces.Box(
low=np.array([0.0, -1.0, 0.0]),
high=np.array([1.0, 1.0, 1.0]),
@@ -27,47 +66,217 @@ def __init__(self):
)
def reset(self, seed=None, options=None):
+ """
+ 重置环境到初始状态。
+ :param seed: 随机种子(用于可复现性)
+ :param options: 额外选项(本实现未使用)
+ :return: 初始观测值 (obs), info 字典
+ """
super().reset(seed=seed)
- if self.client is None:
- self.client = carla.Client('localhost', 2000)
- self.client.set_timeout(60.0)
- print("🔄 连接 CARLA...")
- self.world = self.client.get_world() # 不 reload_world!
+ # 设置随机种子(确保行为可复现)
+ if seed is not None:
+ random.seed(seed)
+ np.random.seed(seed)
+
+ try:
+ # 连接 CARLA 服务器(localhost:2000)
+ if self.client is None:
+ print("🔄 尝试连接 CARLA 服务器...")
+ self.client = carla.Client('localhost', 2000)
+ self.client.set_timeout(20.0) # 超时20秒
+ self.world = self.client.get_world()
+ if self.world is None:
+ raise RuntimeError("❌ 无法获取 CARLA 世界!")
+ print(f"✅ 成功连接到 CARLA!地图: {self.world.get_map().name}")
+
+ # 清理上一次运行残留的车辆(通过ID文件)
+ self._destroy_last_run_vehicle()
+
+ # 生成新车
+ self.spawn_vehicle()
+
+ # 同步几帧,确保车辆稳定
+ for _ in range(5):
+ self.world.tick()
+ time.sleep(0.05)
+
+ # 获取观察者并设置第三人称视角
+ self.spectator = self.world.get_spectator()
+ self._update_spectator_view()
+ print("🎥 第三人称视角已激活(完整车身 + 前方道路可见)")
+
+ # 重置计数器
+ self.frame_count = 0
+ obs = self.get_observation()
+ self.prev_x = obs[0] # 记录初始x位置
+ return obs, {}
+
+ except Exception as e:
+ print(f"❌ 初始化失败: {e}")
+ raise
+
+ def _destroy_last_run_vehicle(self):
+ """
+ 安全销毁上一次运行留下的车辆。
+ 即使 .last_vehicle_id.json 文件损坏、为空或不存在,也能优雅处理,不抛出异常。
+ """
+ # 若无记录文件,直接跳过
+ if not os.path.exists(VEHICLE_ID_FILE):
+ print("ℹ️ 无历史车辆记录,跳过清理")
+ return
+
+ last_id = None
+ try:
+ # 安全读取 JSON 文件
+ with open(VEHICLE_ID_FILE, 'r') as f:
+ content = f.read().strip()
+ if not content:
+ print("⚠️ 车辆ID文件为空")
+ return
+ data = json.loads(content)
+ last_id = data.get("vehicle_id")
+ if last_id is None:
+ print("⚠️ 车辆ID字段缺失")
+ return
+ except (json.JSONDecodeError, OSError, ValueError) as e:
+ # 文件损坏时,尝试删除它
+ print(f"⚠️ 读取车辆ID文件失败(文件可能损坏): {e}")
+ try:
+ os.remove(VEHICLE_ID_FILE)
+ except OSError:
+ pass
+ return
+
+ # 验证ID类型
+ if not isinstance(last_id, int):
+ print(f"⚠️ 车辆ID类型无效: {type(last_id)}")
+ return
+
+ # 发送销毁命令
+ print(f"🧹 正在销毁上一次运行的车辆 (ID: {last_id})...")
+ batch = [carla.command.DestroyActor(last_id)]
+ responses = self.client.apply_batch_sync(batch, do_tick=True)
+
+ if responses[0].error:
+ print(f" - 销毁失败: {responses[0].error}")
else:
- self._destroy_actors()
+ print("✅ 上次车辆已成功清理")
- self.spawn_vehicle()
- self.frame_count = 0
- return self.get_observation(), {}
+ # 清理后删除ID文件
+ try:
+ os.remove(VEHICLE_ID_FILE)
+ except OSError as e:
+ print(f"⚠️ 删除车辆ID文件失败: {e}")
def spawn_vehicle(self):
+ """
+ 在当前地图的安全位置生成一辆特斯拉 Model 3(若不可用则随机选车)。
+ 支持 Town01/03/05/10 的预设 spawn 点,其他地图自动选择最低 z 的点。
+ """
blueprint_library = self.world.get_blueprint_library()
vehicle_bp = blueprint_library.find('vehicle.tesla.model3')
if not vehicle_bp:
vehicle_bp = random.choice(blueprint_library.filter('vehicle.*'))
- transform = random.choice(self.world.get_map().get_spawn_points())
- self.vehicle = self.world.spawn_actor(vehicle_bp, transform)
- self.actor_list.append(self.vehicle)
- print(f"🚗 车辆生成: {self.vehicle.type_id}")
-
- def _destroy_actors(self):
- for actor in self.actor_list:
- if actor and actor.is_alive:
- actor.destroy()
- self.actor_list.clear()
- for _ in range(3):
- self.world.tick()
- time.sleep(0.1)
+
+ # 根据地图名称选择 spawn 位置
+ map_name = self.world.get_map().name.lower()
+ if 'town01' in map_name:
+ spawn_transform = carla.Transform(
+ carla.Location(x=-60.0, y=20.0, z=0.3),
+ carla.Rotation(yaw=90.0)
+ )
+ elif 'town03' in map_name:
+ spawn_transform = carla.Transform(
+ carla.Location(x=70.0, y=-10.0, z=0.3),
+ carla.Rotation(yaw=180.0)
+ )
+ elif 'town05' in map_name:
+ spawn_transform = carla.Transform(
+ carla.Location(x=-75.0, y=16.0, z=0.3),
+ carla.Rotation(yaw=90.0)
+ )
+ elif 'town10' in map_name:
+ spawn_transform = carla.Transform(
+ carla.Location(x=100.0, y=130.0, z=0.3),
+ carla.Rotation(yaw=180.0)
+ )
+ else:
+ # 通用 fallback:选择 z 最低的 spawn 点(更平坦安全)
+ spawn_points = self.world.get_map().get_spawn_points()
+ if not spawn_points:
+ raise RuntimeError("❌ 地图中没有可用的 spawn points!")
+ spawn_transform = min(spawn_points, key=lambda t: t.location.z)
+
+ # 尝试生成车辆
+ self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform)
+ if self.vehicle is None:
+ # 若失败,遍历所有 spawn 点,增加 z 安全余量
+ spawn_points = self.world.get_map().get_spawn_points()
+ for transform in spawn_points:
+ safe_z = max(transform.location.z, 0.0) + 0.3
+ safe_transform = carla.Transform(
+ carla.Location(x=transform.location.x, y=transform.location.y, z=safe_z),
+ transform.rotation
+ )
+ self.vehicle = self.world.try_spawn_actor(vehicle_bp, safe_transform)
+ if self.vehicle is not None:
+ break
+
+ if self.vehicle is None:
+ raise RuntimeError("❌ 无法生成车辆!")
+
+ # 记录车辆信息
+ self._current_vehicle_id = self.vehicle.id
+ loc = self.vehicle.get_location()
+ print(
+ f"✅ 车辆生成成功: {self.vehicle.type_id} | ID={self._current_vehicle_id} | 位置: ({loc.x:.1f}, {loc.y:.1f}, {loc.z:.1f})")
+
+ # 原子写入车辆ID文件(防止并发写入损坏)
+ temp_file = VEHICLE_ID_FILE + ".tmp"
+ try:
+ with open(temp_file, 'w') as f:
+ json.dump({"vehicle_id": self._current_vehicle_id}, f)
+ os.replace(temp_file, VEHICLE_ID_FILE) # 原子操作
+ except Exception as e:
+ print(f"⚠️ 保存车辆ID失败(不影响运行): {e}")
+
+ def _update_spectator_view(self):
+ """
+ 更新 CARLA 观察者视角,使其跟随车辆(第三人称)。
+ 相机位于车辆后上方,俯视前方道路。
+ """
+ if not (self.vehicle and self.spectator):
+ return
+ v_transform = self.vehicle.get_transform()
+ # 相对偏移:后方8米,上方4米
+ offset = carla.Location(x=-8.0, y=0.0, z=4.0)
+ spectator_loc = v_transform.transform(offset)
+ spectator_rot = carla.Rotation(
+ pitch=-20.0, # 俯视角
+ yaw=v_transform.rotation.yaw, # 跟随车辆朝向
+ roll=0.0
+ )
+ self.spectator.set_transform(carla.Transform(spectator_loc, spectator_rot))
def get_observation(self):
+ """
+ 获取当前环境观测值。
+ :return: np.array([x, y, vx, vy], dtype=np.float32)
+ """
if not self.vehicle or not self.vehicle.is_alive:
+ # 车辆不存在时返回零向量(避免崩溃)
return np.zeros(4, dtype=np.float32)
loc = self.vehicle.get_location()
vel = self.vehicle.get_velocity()
return np.array([loc.x, loc.y, vel.x, vel.y], dtype=np.float32)
def step(self, action):
+ """
+ 执行一步环境交互。
+ :param action: [throttle, steer, brake]
+ :return: obs, reward, terminated, truncated, info
+ """
throttle, steer, brake = action
control = carla.VehicleControl(
throttle=float(throttle),
@@ -75,14 +284,56 @@ def step(self, action):
brake=float(brake)
)
self.vehicle.apply_control(control)
- self.world.tick()
+ self.world.tick() # 推进仿真
self.frame_count += 1
+ self._update_spectator_view() # 更新视角
+
+ # 检查车辆是否被销毁
+ if not self.vehicle or not self.vehicle.is_alive:
+ return np.zeros(4, dtype=np.float32), -10.0, True, False, {}
+ # 获取观测
obs = self.get_observation()
- reward = float(np.linalg.norm([obs[2], obs[3]])) # 速度奖励
- terminated = False
- truncated = self.frame_count >= self.max_frames
+ x, y, vx, vy = obs
+ speed = np.linalg.norm([vx, vy])
+
+ # ========================
+ # ✅【强力推荐】使用车辆朝向速度作为主奖励
+ # ========================
+ vehicle_transform = self.vehicle.get_transform()
+ forward_vector = vehicle_transform.get_forward_vector() # 车头方向单位向量
+
+ # 计算速度在车头方向的投影(鼓励向前行驶)
+ forward_speed = vx * forward_vector.x + vy * forward_vector.y
+
+ # 主奖励:只奖励正向前进(倒车不奖励)
+ reward = 1.0 * max(forward_speed, 0.0)
+
+ # 额外惩罚:如果几乎静止,施加较大惩罚(促进行动)
+ if speed < 0.1:
+ reward -= 0.5
+
+ self.prev_x = x
+ terminated = False # 暂无终止条件(如碰撞)
+ truncated = self.frame_count >= self.max_frames # 超过最大步数则截断
return obs, reward, terminated, truncated, {}
def close(self):
- self._destroy_actors()
\ No newline at end of file
+ """
+ 关闭环境,释放资源。
+ 若 keep_alive=True,则保留车辆供人工观察;否则销毁。
+ """
+ if self.keep_alive:
+ print("ℹ️ 车辆已保留(ID已记录,下次运行时将自动清理)")
+ if self.vehicle:
+ # 松开所有控制,让车自然停下
+ self.vehicle.apply_control(carla.VehicleControl())
+ for i in range(30): # 同步30帧确保停止
+ self.world.tick()
+ self._update_spectator_view()
+ time.sleep(0.1)
+ print("✅ 现在你可以自由操作 CARLA 视角(按 F1~F4)!")
+ else:
+ # 彻底清理
+ if self.vehicle and self.vehicle.is_alive:
+ self.vehicle.destroy()
diff --git a/src/AutonomousDriving_RL/eval_agent.py b/src/AutonomousDriving_RL/eval_agent.py
new file mode 100644
index 0000000000..d7cb9b9eb7
--- /dev/null
+++ b/src/AutonomousDriving_RL/eval_agent.py
@@ -0,0 +1,64 @@
+# eval_agent.py
+import os
+import sys
+import numpy as np
+
+# 添加当前目录到路径
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+
+from stable_baselines3 import PPO
+from carla_env.carla_env_multi_obs import CarlaEnvMultiObs
+
+
+def main():
+ model_path = "final_model.zip"
+ if not os.path.exists(model_path):
+ print(f"❌ 模型文件不存在: {model_path}")
+ return
+
+ print("🔄 加载模型...")
+ model = PPO.load(model_path)
+ print("✅ 模型加载成功!")
+
+ # ✅ 关键:启用 keep_alive_after_exit=True(默认已为 True)
+ env = CarlaEnvMultiObs(keep_alive_after_exit=True)
+
+ try:
+ obs, _ = env.reset()
+ print("▶️ 开始驾驶演示(运行 200 步)...")
+
+ for step in range(200):
+ action, _states = model.predict(obs, deterministic=True)
+ obs, reward, terminated, truncated, info = env.step(action)
+
+ if step % 50 == 0:
+ x, y, vx, vy = obs
+ speed = np.linalg.norm([vx, vy])
+ print(f" Step {step}: 位置=({x:.1f}, {y:.1f}), 速度={speed:.2f} m/s")
+
+ print("✅ 演示完成!")
+
+ except KeyboardInterrupt:
+ print("\n🛑 用户中断")
+ except Exception as e:
+ print(f"💥 运行时错误: {e}")
+ raise
+ finally:
+ # 注意:env.close() 会松开控制,但不会销毁车辆(因为 keep_alive=True)
+ env.close()
+
+ # ✅✅✅ 关键新增:阻塞进程,防止退出 → 车辆保留在 CARLA 中
+ print("\n" + "="*60)
+ print("🚗 车辆已保留在 CARLA 中!")
+ print("💡 操作指南:")
+ print(" 1. 切换到 CARLA 窗口")
+ print(" 2. 按 F5 键进入第三人称跟随视角(推荐)")
+ print(" 3. 可自由旋转/平移视角观察车辆")
+ print(" 4. 录制 GIF 或截图")
+ print("\n🛑 准备好后,请回到本窗口按 Enter 键退出...")
+ input() # ⬅️ 阻塞:只要不按回车,Python 进程就不退出,车就不会消失
+ print("👋 再见!")
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/AutonomousDriving_RL/train_agent.py b/src/AutonomousDriving_RL/train_agent.py
index 1756d7de02..8469a358cc 100644
--- a/src/AutonomousDriving_RL/train_agent.py
+++ b/src/AutonomousDriving_RL/train_agent.py
@@ -1,48 +1,102 @@
# train_agent.py
-"""CARLA 强化学习训练脚本,使用 CarlaEnvMultiObs 环境 + PPO 算法"""
+# 本脚本用于训练基于 PPO 算法的强化学习智能体(Agent),环境为 CARLA 自动驾驶仿真平台。
+# 使用 Stable Baselines3 库实现,支持断点续训、自动保存检查点、安全中断等功能。
import os
import sys
+
+# 将当前脚本所在目录添加到 Python 模块搜索路径,确保能正确导入本地模块(如 carla_env)
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+# 导入 Stable Baselines3 中的 PPO(Proximal Policy Optimization)算法
from stable_baselines3 import PPO
+
+# 导入环境检查工具,用于验证自定义环境是否符合 Gym 接口规范
from stable_baselines3.common.env_checker import check_env
+
+# 导入回调函数:CheckpointCallback,用于定期保存模型检查点
+from stable_baselines3.common.callbacks import CheckpointCallback
+
+# 导入自定义的 CARLA 多观测空间环境(包含图像、速度、位置等多种状态信息)
from carla_env.carla_env_multi_obs import CarlaEnvMultiObs
+
def main():
- print("🚀 正在创建 CARLA 环境...")
+ """
+ 主函数:初始化环境、加载/创建模型、启动训练流程。
+ """
+ print("🔄 初始化 CARLA 环境...")
+
+ # 创建自定义 CARLA 环境实例
env = CarlaEnvMultiObs()
- print("🔍 检查环境...")
- check_env(env, warn=True)
-
- print("🧠 初始化 PPO 模型...")
- model = PPO(
- "MlpPolicy",
- env,
- verbose=1,
- learning_rate=3e-4,
- n_steps=2048,
- batch_size=64,
- n_epochs=10,
- gamma=0.99,
- gae_lambda=0.95,
- clip_range=0.2,
- ent_coef=0.01,
- tensorboard_log="./logs/"
+ try:
+ # 使用 Stable Baselines3 提供的 check_env 工具验证环境是否符合 Gym 标准
+ # 若不符合,会抛出警告或异常,帮助开发者快速定位问题
+ check_env(env, warn=True)
+ print("✅ 环境检查通过!")
+ except Exception as e:
+ # 如果环境检查失败,打印错误信息并安全关闭环境,退出程序
+ print(f"❌ 环境检查失败: {e}")
+ env.close()
+ return
+
+ # 设置回调函数:每训练 10,000 步自动保存一次模型检查点
+ checkpoint_callback = CheckpointCallback(
+ save_freq=10000, # 保存频率(以环境步数为单位)
+ save_path="./checkpoints/", # 检查点保存目录
+ name_prefix="ppo_carla" # 检查点文件名前缀,如 ppo_carla_10000_steps.zip
)
- print("▶️ 开始训练...")
+ # 定义最新模型的路径,用于判断是否需要继续训练
+ model_path = "./checkpoints/ppo_carla_latest.zip"
+
+ # 检查是否存在已保存的模型(用于断点续训)
+ if os.path.exists(model_path):
+ print(f"🔁 加载已有模型: {model_path}")
+ # 从指定路径加载预训练模型,并绑定当前环境
+ model = PPO.load(model_path, env=env)
+ total_timesteps = 100000 # 总训练步数目标(累计)
+ reset_num_timesteps = False # 不重置步数计数器,继续之前的训练进度
+ else:
+ print("🆕 训练新模型")
+ # 创建全新的 PPO 模型
+ model = PPO(
+ "MlpPolicy", # 使用全连接神经网络策略(适用于非图像输入)
+ env, # 绑定训练环境
+ verbose=1, # 输出训练日志(1 表示基本信息)
+ learning_rate=3e-4, # 学习率,常用值,平衡收敛速度与稳定性
+ n_steps=2048, # 每次更新策略前收集的环境交互步数(影响样本效率)
+ batch_size=64, # 每次梯度更新使用的样本批次大小
+ n_epochs=10, # 每批数据重复训练的轮数(提升数据利用率)
+ tensorboard_log="./logs/" # TensorBoard 日志目录,用于可视化训练过程
+ )
+ total_timesteps = 100000 # 总训练步数
+ reset_num_timesteps = True # 重置步数计数器(因为是新训练)
+
+ print("▶️ 开始训练(按 Ctrl+C 可安全中断)...")
+
try:
- # 注意:progress_bar=False 避免 tqdm/rich 依赖错误
- model.learn(total_timesteps=50000, progress_bar=False)
- print("✅ 训练完成!保存模型...")
- model.save("carla_ppo_agent")
+ # 启动模型训练
+ model.learn(
+ total_timesteps=total_timesteps, # 总训练步数
+ callback=checkpoint_callback, # 注册回调函数(自动保存)
+ reset_num_timesteps=reset_num_timesteps, # 是否重置内部步数计数
+ progress_bar=False # 不显示进度条(可设为 True 查看进度)
+ )
+ # 训练正常完成后,保存最终模型
+ model.save("final_model")
+ print("🎉 训练完成!模型已保存为 final_model.zip")
except KeyboardInterrupt:
- print("⚠️ 训练被用户中断")
+ # 捕获用户中断信号(Ctrl+C),安全保存当前模型
+ print("⚠️ 训练被用户中断,正在保存最新模型...")
+ model.save("./checkpoints/ppo_carla_latest")
+ print("💾 已保存至 ./checkpoints/ppo_carla_latest.zip")
finally:
+ # 无论训练成功与否,都确保关闭 CARLA 环境,释放资源
env.close()
- print("CloseOperation: 环境已关闭")
+
+# 程序入口:确保只有直接运行本脚本时才执行 main()
if __name__ == "__main__":
- main()
\ No newline at end of file
+ main()
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/README.md b/src/Autonomous_vehicle_navigation_using_deep_learning_master/README.md
index 3a90822a45..4940205df8 100644
--- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/README.md
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/README.md
@@ -26,20 +26,54 @@ pip install -r requirements.txt
3. **运行主程序**:
```bash
+cd main
python main.py
```
+4. **运行测试程序**:
+```bash
+cd test
+python test_driving.py
+```
## 项目结构
```
-├── main.py # 主程序入口
-├── config.py # 配置文件
-├── get_location.py # 位置坐标获取工具
-├── test_braking.py # 刹车模型测试
-├── test_driving.py # 驾驶模型测试
-├── pedestrians_1.py # 行人生成器1
-├── pedestrians_2.py # 行人生成器2
-└── requirements.txt # 依赖包列表
+│ README.md
+│ requirements.txt
+├─agents
+│ │ __init__.py
+│ ├─navigation
+│ │ basic_agent.py
+│ │ behavior_agent.py
+│ │ behavior_types.py
+│ │ controller.py
+│ │ global_route_planner.py
+│ │ local_planner.py
+│ │ __init__.py
+│ └─tools
+│ misc.py
+│ __init__.py
+├─main
+│ car_env.py
+│ config.py
+│ config_manager.py
+│ get_location.py
+│ main.py
+│ model_manager.py
+│ route_visualizer.py
+│ traffic_manager.py
+│ trajectory_manager.py
+│ vehicle_tracker.py
+├─models
+│ Braking___282.00max__282.00avg__282.00min__1679121006.model
+│ Driving__6030.00max_6030.00avg_6030.00min__1679109656.model
+└─test
+ braking_dqn.py
+ driving_dqn.py
+ pedestrians_1.py
+ pedestrians_2.py
+ test_braking.py
+ test_driving.py
```
## 核心功能
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main.py
deleted file mode 100644
index 69975cea17..0000000000
--- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main.py
+++ /dev/null
@@ -1,279 +0,0 @@
-"""
-主程序入口 - 协调所有模块
-"""
-import os
-os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
-import time
-import numpy as np
-from collections import deque
-
-from car_env import CarEnv
-from route_visualizer import RouteVisualizer
-from vehicle_tracker import VehicleTracker
-from model_manager import ModelManager
-from trajectory_manager import TrajectoryManager
-from traffic_manager import TrafficManager
-from config_manager import ConfigManager
-import config as cfg
-
-def setup_environment():
- """设置整体环境"""
- print("=" * 60)
- print("CARLA自动驾驶系统启动")
- print("=" * 60)
-
- # 获取配置
- trajectory = cfg.get_current_trajectory()
- if trajectory is None:
- print("❌ 无法获取轨迹配置")
- return None, None, None, None, None, None, None
-
- print(f"📌 使用轨迹: {trajectory['description']}")
-
- # 创建CARLA环境
- try:
- env = CarEnv(trajectory['start'], trajectory['end'])
- print("✅ CARLA环境创建成功")
- except Exception as e:
- print(f"❌ 创建CARLA环境失败: {e}")
- return None, None, None, None, None, None, None
-
- # 创建配置管理器
- config_mgr = ConfigManager(client=env.client)
-
- # 应用默认设置
- config_mgr.apply_default_settings()
-
- # 设置仿真参数(提高性能)
- settings = env.world.get_settings()
- settings.fixed_delta_seconds = cfg.FIXED_DELTA_SECONDS
- settings.synchronous_mode = cfg.SYNCHRONOUS_MODE
- settings.no_rendering_mode = cfg.NO_RENDERING_MODE
- env.world.apply_settings(settings)
-
- print(f"📊 设置时间步长: {cfg.FIXED_DELTA_SECONDS}s ({1/cfg.FIXED_DELTA_SECONDS:.1f} FPS)")
-
- # 检查模拟器状态
- config_mgr.inspect_simulation()
-
- # 创建交通管理器
- traffic_mgr = TrafficManager(client=env.client)
-
- # 生成交通流
- if cfg.ENABLE_TRAFFIC:
- print("\n🚦 生成交通流...")
- traffic_mgr.generate_traffic(
- num_vehicles=cfg.TRAFFIC_VEHICLES,
- num_walkers=cfg.TRAFFIC_WALKERS,
- safe_mode=cfg.TRAFFIC_SAFE_MODE,
- hybrid_mode=cfg.TRAFFIC_HYBRID_MODE,
- sync_mode=cfg.TRAFFIC_SYNC_MODE,
- respawn_vehicles=cfg.TRAFFIC_RESPAWN
- )
-
- # 创建路线可视化器
- visualizer = RouteVisualizer(env.world)
-
- # 创建车辆跟踪器(控制视角)
- tracker = VehicleTracker(env.world)
-
- # 创建模型管理器
- model_mgr = ModelManager()
-
- # 创建轨迹管理器
- traj_mgr = TrajectoryManager(env)
-
- return env, config_mgr, traffic_mgr, visualizer, tracker, model_mgr, traj_mgr
-
-def run_episode(env, config_mgr, traffic_mgr, visualizer, tracker, model_mgr, traj_mgr, episode_num):
- """运行单个episode"""
- print(f"\n{'='*60}")
- print(f"Episode {episode_num}")
- print(f"{'='*60}")
-
- # 重置环境
- try:
- current_state = env.reset()
- print(f"✅ 环境重置成功")
- except Exception as e:
- print(f"❌ 环境重置失败: {e}")
- return False
-
- # 获取车辆
- ego_vehicle = env.vehicle
- if ego_vehicle is None:
- print("❌ 未找到车辆")
- return False
-
- # 重置跟踪器
- tracker.reset()
-
- # 设置初始视角(俯视)
- tracker.set_top_down_view(ego_vehicle, height=cfg.TOP_DOWN_HEIGHT)
-
- # 获取并绘制规划路线
- route_points = traj_mgr.get_route_points()
- visualizer.draw_planned_route(route_points)
-
- # 重置可视化器历史
- visualizer.reset_history()
-
- done = False
- step_count = 0
- frame_skip_count = 0
- fps_counter = deque(maxlen=120) # 增加历史长度
- last_frame_time = time.time()
-
- while not done and step_count < cfg.MAX_STEPS_PER_EPISODE:
- step_count += 1
- step_start = time.time()
- current_time = time.time()
-
- # 计算帧间隔
- frame_interval = current_time - last_frame_time
- last_frame_time = current_time
-
- # 自适应跳帧逻辑
- should_skip_frame = False
- if cfg.MAX_FRAME_SKIP > 0 and frame_interval > cfg.FIXED_DELTA_SECONDS * 1.5:
- frame_skip_count += 1
- if frame_skip_count <= cfg.MAX_FRAME_SKIP:
- should_skip_frame = True
- if cfg.DEBUG_MODE and step_count % 50 == 0:
- print(f"[跳过] 帧间隔过大: {frame_interval:.3f}s,跳过更新")
- else:
- frame_skip_count = 0
-
- # 更新交通管理器(如果是同步模式)
- if traffic_mgr and cfg.TRAFFIC_SYNC_MODE and not should_skip_frame:
- traffic_mgr.update()
-
- # 获取车辆状态
- vehicle_state = tracker.get_vehicle_state(ego_vehicle)
-
- # 更新视角(跳过某些帧时也更新,但减少频率)
- if not should_skip_frame or step_count % 2 == 0:
- tracker.smooth_follow_vehicle(ego_vehicle, height=cfg.TOP_DOWN_HEIGHT)
-
- # 更新车辆可视化(可以适当降低频率)
- if vehicle_state and (step_count % 2 == 0 or not should_skip_frame):
- visualizer.update_vehicle_display(
- vehicle_state['x'],
- vehicle_state['y'],
- vehicle_state['heading']
- )
-
- # 模型预测动作
- action = model_mgr.predict_action(current_state, vehicle_state)
-
- # 执行动作
- try:
- new_state, reward, done, _ = env.step(action, current_state)
- current_state = new_state
- except Exception as e:
- print(f"❌ 执行动作失败: {e}")
- done = True
-
- # 显示进度
- if step_count % 100 == 0:
- progress_info = tracker.calculate_progress(
- vehicle_state['x'] if vehicle_state else 0,
- vehicle_state['y'] if vehicle_state else 0,
- route_points
- )
- print(f"步骤 {step_count}, 奖励: {reward:.2f}, {progress_info}")
-
- # 计算FPS
- frame_time = time.time() - step_start
- fps_counter.append(frame_time)
-
- # 计算平滑FPS
- if len(fps_counter) >= 30:
- avg_frame_time = np.mean(list(fps_counter)[-30:])
- current_fps = 1.0 / avg_frame_time if avg_frame_time > 0 else 0
- else:
- current_fps = len(fps_counter) / sum(fps_counter) if fps_counter else 0
-
- # 显示调试信息
- if cfg.DEBUG_MODE and step_count % 100 == 0:
- if vehicle_state:
- print(f"[{step_count:4d}] FPS: {current_fps:5.1f} | "
- f"动作: {cfg.ACTION_NAMES[action]} | "
- f"位置: ({vehicle_state['x']:.1f}, {vehicle_state['y']:.1f}) | "
- f"速度: {vehicle_state['speed_2d']:.1f}m/s")
-
- if done:
- print(f"✅ Episode {episode_num} 完成,步数: {step_count}")
- break
-
- # 限制帧率(如果需要)
- if cfg.FPS_LIMIT > 0:
- target_frame_time = 1.0 / cfg.FPS_LIMIT
- actual_frame_time = time.time() - step_start
- if actual_frame_time < target_frame_time:
- time.sleep(target_frame_time - actual_frame_time)
-
- if step_count >= cfg.MAX_STEPS_PER_EPISODE:
- print(f"⏰ Episode {episode_num} 达到最大步数限制")
-
- # 显示统计信息
- path_length = visualizer.calculate_path_length()
- avg_fps = 1.0 / (sum(fps_counter) / len(fps_counter)) if fps_counter else 0
- print(f"📊 行驶距离: {path_length:.1f}m, 平均FPS: {avg_fps:.1f}")
-
- return True
-
-def main():
- """主函数"""
- # 初始化各模块
- result = setup_environment()
- if result[0] is None:
- return
-
- env, config_mgr, traffic_mgr, visualizer, tracker, model_mgr, traj_mgr = result
-
- # 加载模型
- if not model_mgr.load_models():
- print("❌ 模型加载失败")
- return
-
- print("\n🚗 开始自动驾驶...")
-
- # 运行多个episode
- for episode in range(cfg.TOTAL_EPISODES):
- success = run_episode(
- env, config_mgr, traffic_mgr, visualizer, tracker, model_mgr, traj_mgr, episode + 1
- )
-
- if not success:
- print(f"❌ Episode {episode + 1} 运行失败")
-
- # 等待片刻再开始下一个episode
- if episode < cfg.TOTAL_EPISODES - 1:
- print(f"\n等待 {cfg.EPISODE_INTERVAL} 秒开始下一个episode...")
- time.sleep(cfg.EPISODE_INTERVAL)
-
- # 清理
- print("\n" + "=" * 60)
- print("所有episode完成!")
- print("=" * 60)
-
- # 清理交通流
- if traffic_mgr:
- traffic_mgr.cleanup()
-
- # 显示最终统计
- print("\n📈 最终统计:")
- print(f"总episodes: {cfg.TOTAL_EPISODES}")
- print(f"每episode最大步数: {cfg.MAX_STEPS_PER_EPISODE}")
- print(f"交通模式: {'启用' if cfg.ENABLE_TRAFFIC else '禁用'}")
-
- if cfg.ENABLE_TRAFFIC and traffic_mgr:
- traffic_info = traffic_mgr.get_traffic_info()
- print(f"交通车辆数: {traffic_info['num_vehicles']}")
- print(f"交通行人数: {traffic_info['num_walkers']}")
-
- print("程序结束")
-
-if __name__ == '__main__':
- main()
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/car_env.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/car_env.py
similarity index 98%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/car_env.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/main/car_env.py
index 0c5caf86d9..13bdf2eb7a 100644
--- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/car_env.py
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/car_env.py
@@ -13,6 +13,12 @@
except IndexError:
pass
+# 获取当前文件的目录
+current_dir = os.path.dirname(os.path.abspath(__file__))
+# 获取父目录
+parent_dir = os.path.dirname(current_dir)
+# 将父目录添加到系统路径
+sys.path.append(parent_dir)
from agents.navigation.global_route_planner import GlobalRoutePlanner
import carla
@@ -420,7 +426,7 @@ def step(self, action, current_state):
self.vehicle.apply_control(carla.VehicleControl(throttle=0, brake=1.0))
print("执行动作: 刹车")
elif action == 1:
- self.vehicle.apply_control(carla.VehicleControl(throttle=0.5, steer= 0*self.STEER_AMT))
+ self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer= 0*self.STEER_AMT))
print("执行动作: 直行")
elif action == 2:
self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=-0.3*self.STEER_AMT))
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/config.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/config.py
similarity index 84%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/config.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/main/config.py
index 195518bacc..565a3cf51c 100644
--- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/config.py
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/config.py
@@ -1,7 +1,7 @@
"""
配置文件 - 存储所有配置参数
"""
-
+import os
# ==================== 轨迹配置 ====================
TRAJECTORIES = {
"custom_trajectory": {
@@ -27,10 +27,17 @@ def get_current_trajectory():
print(f"❌ 轨迹 '{CURRENT_TRAJECTORY}' 不存在")
return None
+
# ==================== 模型配置 ====================
+# 设置基础路径
+current_dir = os.path.dirname(os.path.abspath(__file__))
+base_dir = os.path.dirname(current_dir) # 上一级目录
+MODELS_DIR = os.path.join(base_dir, 'models')
+braking = os.path.join(MODELS_DIR, 'Braking___282.model')
+driving = os.path.join(MODELS_DIR, 'Driving__6030.model')
MODEL_PATHS = {
- 'braking': "models/Braking___282.model",
- 'driving': "models/Driving__6030.model"
+ 'braking': braking ,
+ 'driving': driving
}
# ==================== 动作配置 ====================
@@ -43,8 +50,8 @@ def get_current_trajectory():
# ==================== 交通配置 ====================
ENABLE_TRAFFIC = True # 是否启用交通流(暂时关闭,避免干扰)
-TRAFFIC_VEHICLES = 15 # 交通车辆数量
-TRAFFIC_WALKERS = 20 # 交通行人数量
+TRAFFIC_VEHICLES = 30 # 交通车辆数量
+TRAFFIC_WALKERS = 50 # 交通行人数量
TRAFFIC_SAFE_MODE = True # 交通安全模式
TRAFFIC_HYBRID_MODE = True # 混合物理模式
TRAFFIC_SYNC_MODE = False # 交通同步模式
@@ -53,7 +60,7 @@ def get_current_trajectory():
# ==================== 可视化配置 ====================
ROUTE_COLOR = (255, 0, 0) # 路线颜色 (红色)
PATH_COLOR = (0, 100, 255) # 路径颜色 (蓝色)
-VEHICLE_COLOR = (0, 255, 0) # 车辆颜色 (绿色)
+VEHICLE_COLOR = (0, 0, 0) # 车辆颜色 (绿色)
ROUTE_HEIGHT = 0.3 # 路线显示高度
PATH_HEIGHT = 0.2 # 路径显示高度
@@ -62,16 +69,16 @@ def get_current_trajectory():
# ==================== 视角配置 ====================
TOP_DOWN_HEIGHT = 30.0 # 俯视视角高度(提高)
TOP_DOWN_PITCH = -85.0 # 俯视角 (几乎垂直向下,-90是完全垂直)
-SMOOTH_FOLLOW_FACTOR = 0.001 # 视角平滑系数 (0-1,越小越平滑)
-MIN_SMOOTH_FACTOR = 0 # 最小平滑系数
+SMOOTH_FOLLOW_FACTOR = 0.01 # 视角平滑系数 (0-1,越小越平滑)
+MIN_SMOOTH_FACTOR = 0.001 # 最小平滑系数
MAX_SMOOTH_FACTOR = 0.3 # 最大平滑系数
SMOOTH_FACTOR_ADAPTIVE = True # 是否自适应平滑系数
DISTANCE_THRESHOLD = 5.0 # 距离阈值,超过此值加大平滑系数
# ==================== 性能配置 ====================
DEBUG_MODE = True # 调试模式
-FPS_LIMIT = 0 # FPS限制 (0为无限制,提高性能)
-UPDATE_RATE = 1.0 # 更新率 (1.0 = 每步更新)
+FPS_LIMIT = 120 # FPS限制 (0为无限制,提高性能)
+UPDATE_RATE = 0.5 # 更新率 (1.0 = 每步更新)
USE_SMOOTH_INTERPOLATION = True # 使用平滑插值
INTERPOLATION_STEPS = 5 # 插值步数
MAX_FRAME_SKIP = 2 # 最大跳帧数
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/config_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/config_manager.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/config_manager.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/main/config_manager.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/get_location.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/get_location.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/get_location.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/main/get_location.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/main.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/main.py
new file mode 100644
index 0000000000..1307269b1b
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/main.py
@@ -0,0 +1,217 @@
+
+#!/usr/bin/env python3
+"""
+主程序 - 简化版
+"""
+
+import os
+os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
+import time
+import sys
+import glob
+
+# 导入CARLA
+try:
+ sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
+ sys.version_info.major,
+ sys.version_info.minor,
+ 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+except IndexError:
+ pass
+
+import carla
+import numpy as np
+from collections import deque
+
+from car_env import CarEnv
+from route_visualizer import RouteVisualizer
+from vehicle_tracker import VehicleTracker
+from traffic_manager import TrafficManager
+import config as cfg
+
+def setup_environment():
+ """设置环境"""
+ print("启动CARLA自动驾驶系统")
+
+ # 获取轨迹
+ trajectory = cfg.get_current_trajectory()
+ if not trajectory:
+ print("轨迹配置错误")
+ return None, None, None, None, None
+
+ print(f"使用轨迹: {trajectory['description']}")
+
+ # 创建环境
+ try:
+ env = CarEnv(trajectory['start'], trajectory['end'])
+ print("CARLA环境创建成功")
+ except Exception as e:
+ print(f"创建环境失败: {e}")
+ return None, None, None, None, None
+
+ # 设置仿真参数
+ settings = env.world.get_settings()
+ settings.fixed_delta_seconds = cfg.FIXED_DELTA_SECONDS
+ settings.synchronous_mode = False
+ settings.no_rendering_mode = False
+ env.world.apply_settings(settings)
+
+ # 设置天气
+ env.world.set_weather(carla.WeatherParameters.ClearNoon)
+
+ # 创建交通管理器
+ traffic_mgr = TrafficManager(env.client, env.world)
+
+ # 生成交通
+ if cfg.ENABLE_TRAFFIC:
+ print("生成交通流...")
+ traffic_mgr.generate_traffic(
+ num_vehicles=cfg.TRAFFIC_VEHICLES,
+ num_walkers=cfg.TRAFFIC_WALKERS,
+ safe_mode=cfg.TRAFFIC_SAFE_MODE
+ )
+
+ # 创建其他管理器
+ visualizer = RouteVisualizer(env.world)
+ tracker = VehicleTracker(env.world)
+
+ return env, traffic_mgr, visualizer, tracker, trajectory
+
+def load_models():
+ """加载模型"""
+ try:
+ from tensorflow.keras.models import load_model
+ import tensorflow as tf
+
+ braking_model = load_model(cfg.MODEL_PATHS['braking'])
+ driving_model = load_model(cfg.MODEL_PATHS['driving'])
+
+ print("模型加载成功")
+ return braking_model, driving_model
+ except Exception as e:
+ print(f"模型加载失败: {e}")
+ return None, None
+
+def predict_action(braking_model, driving_model, current_state, vehicle_state=None):
+ """预测动作"""
+ if not braking_model or not driving_model:
+ return 0
+
+ # 刹车模型预测
+ braking_state = np.array(current_state[:2]).reshape(1, -1)
+ braking_qs = braking_model.predict(braking_state, verbose=0)[0]
+ braking_action = np.argmax(braking_qs)
+
+ # 如果安全,使用驾驶模型
+ if braking_action == 1:
+ driving_state = np.array(current_state[2:]).reshape(1, -1)
+ driving_qs = driving_model.predict(driving_state, verbose=0)[0]
+ return np.argmax(driving_qs) + 1
+
+ return 0
+
+def run_episode(env, traffic_mgr, visualizer, tracker, episode_num):
+ """运行一个episode"""
+ print(f"\nEpisode {episode_num}")
+
+ # 加载模型
+ braking_model, driving_model = load_models()
+ if not braking_model or not driving_model:
+ print("无法加载模型,退出")
+ return False
+
+ # 重置环境
+ try:
+ current_state = env.reset()
+ print("环境重置成功")
+ except Exception as e:
+ print(f"环境重置失败: {e}")
+ return False
+
+ # 获取车辆
+ ego_vehicle = env.vehicle
+ if not ego_vehicle:
+ print("未找到车辆")
+ return False
+
+ # 设置俯视视角
+ tracker.set_top_down_view(ego_vehicle)
+
+ # 绘制路线
+ if hasattr(env, 'path') and env.path:
+ route_points = []
+ for waypoint in env.path:
+ location = waypoint.transform.location
+ route_points.append((location.x, location.y, location.z))
+ visualizer.draw_planned_route(route_points)
+
+ # 运行循环
+ step_count = 0
+ done = False
+ fps_counter = deque(maxlen=30)
+
+ while not done and step_count < cfg.MAX_STEPS_PER_EPISODE:
+ step_count += 1
+ step_start = time.time()
+
+ # 每步都更新视角
+ tracker.smooth_follow_vehicle(ego_vehicle)
+
+ # 更新车辆显示
+ vehicle_state = tracker.get_vehicle_state(ego_vehicle)
+ if vehicle_state:
+ visualizer.update_vehicle_display(
+ vehicle_state['x'],
+ vehicle_state['y'],
+ vehicle_state['heading']
+ )
+
+ # 预测并执行动作
+ action = predict_action(braking_model, driving_model, current_state, vehicle_state)
+
+ try:
+ new_state, reward, done, _ = env.step(action, current_state)
+ current_state = new_state
+ except Exception as e:
+ print(f"执行动作失败: {e}")
+ done = True
+
+ # 计算FPS
+ frame_time = time.time() - step_start
+ fps_counter.append(frame_time)
+
+ if cfg.DEBUG_MODE and step_count % 50 == 0:
+ fps = len(fps_counter) / sum(fps_counter) if fps_counter else 0
+ print(f"步骤 {step_count}, FPS: {fps:.1f}, 动作: {cfg.ACTION_NAMES[action]}")
+
+ if done:
+ print(f"Episode {episode_num} 完成,步数: {step_count}")
+ break
+
+ return True
+
+def main():
+ """主函数"""
+ # 设置环境
+ result = setup_environment()
+ if not result[0]:
+ return
+
+ env, traffic_mgr, visualizer, tracker, trajectory = result
+
+ # 运行episode
+ for episode in range(cfg.TOTAL_EPISODES):
+ success = run_episode(env, traffic_mgr, visualizer, tracker, episode + 1)
+
+ if episode < cfg.TOTAL_EPISODES - 1:
+ print(f"等待 {cfg.EPISODE_INTERVAL} 秒...")
+ time.sleep(cfg.EPISODE_INTERVAL)
+
+ # 清理
+ if traffic_mgr:
+ traffic_mgr.cleanup()
+
+ print("\n程序结束")
+
+if __name__ == '__main__':
+ main()
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/model_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/model_manager.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/model_manager.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/main/model_manager.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/route_visualizer.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/route_visualizer.py
similarity index 99%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/route_visualizer.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/main/route_visualizer.py
index add06ba397..d2dd0ff3fb 100644
--- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/route_visualizer.py
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/route_visualizer.py
@@ -183,7 +183,7 @@ def _draw_vehicle_outline(self, x, y, heading):
for i in range(len(points) - 1):
self.world.debug.draw_line(
points[i], points[i+1],
- thickness=0.15,
+ thickness=0.01,
color=carla.Color(50, 255, 50), # 亮绿色轮廓
life_time=0.5,
persistent_lines=False
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/traffic_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/traffic_manager.py
new file mode 100644
index 0000000000..584670f20f
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/traffic_manager.py
@@ -0,0 +1,184 @@
+
+"""
+交通管理器 - 简化版
+"""
+
+import sys
+import os
+import glob
+
+try:
+ sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
+ sys.version_info.major,
+ sys.version_info.minor,
+ 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+except IndexError:
+ pass
+
+import carla
+from numpy import random
+
+class TrafficManager:
+ """交通管理器"""
+
+ def __init__(self, client, world):
+ self.client = client
+ self.world = world
+ self.tm_port = 8000
+ self.vehicles_list = []
+ self.walkers_list = []
+ self.all_id = []
+
+ def generate_traffic(self, num_vehicles=15, num_walkers=20, safe_mode=True):
+ """生成交通流"""
+ print(f"生成交通: {num_vehicles}辆车, {num_walkers}个行人")
+
+ # 获取交通管理器
+ traffic_manager = self.client.get_trafficmanager(self.tm_port)
+ traffic_manager.set_global_distance_to_leading_vehicle(2.5)
+
+ # 生成车辆
+ self._spawn_vehicles(traffic_manager, num_vehicles, safe_mode)
+
+ # 生成行人
+ if num_walkers > 0:
+ self._spawn_walkers_simple(num_walkers)
+
+ print(f"完成: {len(self.vehicles_list)}辆车, {len(self.walkers_list)}个行人")
+ return True
+
+ def _spawn_vehicles(self, traffic_manager, num_vehicles, safe_mode):
+ """生成车辆"""
+ blueprints = self.world.get_blueprint_library().filter('vehicle.*')
+
+ if safe_mode:
+ # 过滤危险车辆
+ safe_vehicles = [
+ 'vehicle.audi.a2',
+ 'vehicle.audi.tt',
+ 'vehicle.bmw.grandtourer',
+ 'vehicle.dodge.charger_police',
+ 'vehicle.jeep.wrangler_rubicon',
+ 'vehicle.lincoln.mkz2017',
+ 'vehicle.mercedes-benz.coupe',
+ 'vehicle.mini.cooperst',
+ 'vehicle.nissan.micra',
+ 'vehicle.nissan.patrol',
+ 'vehicle.seat.leon',
+ 'vehicle.tesla.model3',
+ 'vehicle.toyota.prius',
+ 'vehicle.volkswagen.t2'
+ ]
+ blueprints = [bp for bp in blueprints if any(safe in bp.id for safe in safe_vehicles)]
+
+ spawn_points = self.world.get_map().get_spawn_points()
+ random.shuffle(spawn_points)
+
+ batch = []
+ for i in range(min(num_vehicles, len(spawn_points))):
+ blueprint = random.choice(blueprints)
+ batch.append(
+ carla.command.SpawnActor(blueprint, spawn_points[i])
+ .then(carla.command.SetAutopilot(carla.command.FutureActor, True, traffic_manager.get_port()))
+ )
+
+ # 批量生成
+ for response in self.client.apply_batch_sync(batch):
+ if not response.error:
+ self.vehicles_list.append(response.actor_id)
+
+ def _spawn_walkers_simple(self, num_walkers):
+ """简化的行人生成"""
+ walker_bps = self.world.get_blueprint_library().filter('walker.pedestrian.*')
+
+ # 生成行人
+ batch = []
+ walker_speeds = []
+
+ for i in range(num_walkers):
+ spawn_location = self.world.get_random_location_from_navigation()
+ if spawn_location and spawn_location.z < 5: # 确保位置合理
+ walker_bp = random.choice(walker_bps)
+
+ # 设置为非无敌
+ if walker_bp.has_attribute('is_invincible'):
+ walker_bp.set_attribute('is_invincible', 'false')
+
+ # 设置速度
+ speed = 1.0 # 默认速度
+ if walker_bp.has_attribute('speed'):
+ speed = walker_bp.get_attribute('speed').recommended_values[1]
+
+ walker_speeds.append(speed)
+ batch.append(carla.command.SpawnActor(walker_bp, carla.Transform(spawn_location)))
+
+ # 生成行人实体
+ walker_ids = []
+ results = self.client.apply_batch_sync(batch)
+
+ for i, response in enumerate(results):
+ if not response.error:
+ walker_id = response.actor_id
+ walker_ids.append(walker_id)
+ self.walkers_list.append({
+ "id": walker_id,
+ "speed": walker_speeds[i] if i < len(walker_speeds) else 1.0
+ })
+
+ # 生成行人控制器
+ batch = []
+ controller_bp = self.world.get_blueprint_library().find('controller.ai.walker')
+
+ for walker_info in self.walkers_list:
+ batch.append(carla.command.SpawnActor(controller_bp, carla.Transform(), walker_info["id"]))
+
+ results = self.client.apply_batch_sync(batch)
+
+ for i, response in enumerate(results):
+ if not response.error and i < len(self.walkers_list):
+ self.walkers_list[i]["con"] = response.actor_id
+ self.all_id.append(response.actor_id)
+ self.all_id.append(self.walkers_list[i]["id"])
+
+ # 初始化控制器
+ all_actors = self.world.get_actors(self.all_id)
+
+ for i in range(0, len(self.all_id), 2):
+ if i < len(all_actors):
+ controller = all_actors[i]
+ walker = all_actors[i+1] if i+1 < len(all_actors) else None
+
+ if controller and walker:
+ try:
+ controller.start()
+
+ # 设置目的地
+ target = self.world.get_random_location_from_navigation()
+ if target:
+ controller.go_to_location(target)
+
+ # 设置速度
+ walker_index = i // 2
+ if walker_index < len(self.walkers_list):
+ controller.set_max_speed(self.walkers_list[walker_index]["speed"])
+ except:
+ pass
+
+ def cleanup(self):
+ """清理交通"""
+ print("清理交通...")
+
+ # 销毁车辆
+ if self.vehicles_list:
+ self.client.apply_batch([carla.command.DestroyActor(x) for x in self.vehicles_list])
+
+ # 销毁行人
+ if self.all_id:
+ # 先停止控制器
+ all_actors = self.world.get_actors(self.all_id)
+ for i in range(0, len(self.all_id), 2):
+ if i < len(all_actors):
+ all_actors[i].stop()
+
+ # 销毁所有行人相关actor
+ self.client.apply_batch([carla.command.DestroyActor(x) for x in self.all_id])
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/trajectory_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/trajectory_manager.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/trajectory_manager.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/main/trajectory_manager.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/vehicle_tracker.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main/vehicle_tracker.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/vehicle_tracker.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/main/vehicle_tracker.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Braking___282.00max__282.00avg__282.00min__1679121006.model b/src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Braking___282.model
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Braking___282.00max__282.00avg__282.00min__1679121006.model
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Braking___282.model
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model b/src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Driving__6030.model
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Driving__6030.model
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/braking_dqn.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test/braking_dqn.py
similarity index 99%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/braking_dqn.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/test/braking_dqn.py
index 2e1714e262..2b318ef4e3 100644
--- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/braking_dqn.py
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test/braking_dqn.py
@@ -11,7 +11,12 @@
'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
pass
-
+# 获取当前文件的目录
+current_dir = os.path.dirname(os.path.abspath(__file__))
+# 获取父目录
+parent_dir = os.path.dirname(current_dir)
+# 将父目录添加到系统路径
+sys.path.append(parent_dir)
from agents.navigation.global_route_planner import GlobalRoutePlanner
import carla
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/driving_dqn.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test/driving_dqn.py
similarity index 99%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/driving_dqn.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/test/driving_dqn.py
index b9f79b123f..5bcaf60774 100644
--- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/driving_dqn.py
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test/driving_dqn.py
@@ -12,6 +12,11 @@
except IndexError:
pass
+current_dir = os.path.dirname(os.path.abspath(__file__))
+# 获取父目录
+parent_dir = os.path.dirname(current_dir)
+# 将父目录添加到系统路径
+sys.path.append(parent_dir)
from agents.navigation.global_route_planner import GlobalRoutePlanner
import carla
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/pedestrians_1.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test/pedestrians_1.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/pedestrians_1.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/test/pedestrians_1.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/pedestrians_2.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test/pedestrians_2.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/pedestrians_2.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/test/pedestrians_2.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_braking.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test/test_braking.py
similarity index 97%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/test_braking.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/test/test_braking.py
index e89771a8bb..e70c63fefc 100644
--- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_braking.py
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test/test_braking.py
@@ -1,4 +1,5 @@
import os
+import sys
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' # 只显示错误信息
import random
from collections import deque
@@ -13,11 +14,15 @@
from carla import Location
from carla import Rotation
+current_dir = os.path.dirname(os.path.abspath(__file__))
+base_dir = os.path.dirname(current_dir)
+MODELS_DIR = os.path.join(base_dir, 'models')
+
town2 = {1: [53.12553405761719,137.06280517578125,1.3652913570404053, 0], 2:[105.81783294677734,97.80741882324219]}
curves = [0, town2]
epsilon = 0
-MODEL_PATH = "models/Braking___282.model"
+MODEL_PATH = os.path.join(MODELS_DIR, 'Braking___282.model')
def setup_tensorflow():
"""设置 TensorFlow 2.x 配置"""
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_driving.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test/test_driving.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/test_driving.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/test/test_driving.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/traffic_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/traffic_manager.py
deleted file mode 100644
index 583e4a9d2d..0000000000
--- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/traffic_manager.py
+++ /dev/null
@@ -1,361 +0,0 @@
-"""
-交通管理器 - 封装generate_traffic.py功能,生成和管理交通流
-"""
-
-import sys
-import os
-import glob
-import time
-import logging
-from numpy import random
-
-# 添加CARLA路径
-try:
- sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
- sys.version_info.major,
- sys.version_info.minor,
- 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
-except IndexError:
- pass
-
-import carla
-from carla import VehicleLightState as vls
-
-class TrafficManager:
- """交通管理器 - 负责生成和控制交通流"""
-
- def __init__(self, client=None, host='localhost', port=2000):
- """
- 初始化交通管理器
-
- Args:
- client: 可选的CARLA客户端对象
- host: CARLA服务器主机
- port: CARLA服务器端口
- """
- if client:
- self.client = client
- self.world = client.get_world()
- else:
- self.client = carla.Client(host, port)
- self.client.set_timeout(10.0)
- self.world = self.client.get_world()
-
- self.tm_port = 8000
- self.traffic_manager = None
- self.vehicles_list = []
- self.walkers_list = []
- self.all_actors = []
- self.all_id = []
-
- self.is_synchronous = False
- self.synchronous_master = False
-
- print("🚦 交通管理器初始化完成")
-
- def generate_traffic(self,
- num_vehicles=20,
- num_walkers=30,
- safe_mode=True,
- hybrid_mode=True,
- sync_mode=False,
- respawn_vehicles=False):
- """
- 生成交通流
-
- Args:
- num_vehicles: 车辆数量
- num_walkers: 行人数量
- safe_mode: 安全模式(避免事故倾向车辆)
- hybrid_mode: 混合物理模式
- sync_mode: 同步模式
- respawn_vehicles: 是否重生休眠车辆
-
- Returns:
- bool: 是否成功
- """
- print("\n" + "="*50)
- print("生成交通流")
- print("="*50)
- print(f"车辆: {num_vehicles}辆")
- print(f"行人: {num_walkers}个")
- print(f"安全模式: {'开启' if safe_mode else '关闭'}")
- print(f"混合模式: {'开启' if hybrid_mode else '关闭'}")
- print(f"同步模式: {'开启' if sync_mode else '关闭'}")
-
- try:
- # 获取交通管理器
- self.traffic_manager = self.client.get_trafficmanager(self.tm_port)
-
- # 配置交通管理器
- self.traffic_manager.set_global_distance_to_leading_vehicle(2.5)
-
- if respawn_vehicles:
- self.traffic_manager.set_respawn_dormant_vehicles(True)
-
- if hybrid_mode:
- self.traffic_manager.set_hybrid_physics_mode(True)
- self.traffic_manager.set_hybrid_physics_radius(70.0)
-
- # 设置仿真模式
- self.is_synchronous = sync_mode
- settings = self.world.get_settings()
-
- if sync_mode:
- self.traffic_manager.set_synchronous_mode(True)
- if not settings.synchronous_mode:
- self.synchronous_master = True
- settings.synchronous_mode = True
- settings.fixed_delta_seconds = 0.05
- else:
- self.synchronous_master = False
-
- self.world.apply_settings(settings)
- print("✅ 同步模式已启用")
-
- # 生成车辆
- self._spawn_vehicles(num_vehicles, safe_mode)
-
- # 生成行人
- self._spawn_walkers(num_walkers)
-
- # 配置交通管理器参数
- self.traffic_manager.global_percentage_speed_difference(30.0)
-
- print(f"\n✅ 交通流生成完成!")
- print(f" 生成车辆: {len(self.vehicles_list)}辆")
- print(f" 生成行人: {len(self.walkers_list)}个")
-
- return True
-
- except Exception as e:
- print(f"❌ 生成交通流失败: {e}")
- return False
-
- def _spawn_vehicles(self, num_vehicles, safe_mode):
- """生成车辆"""
- print("🚗 生成车辆...")
-
- # 获取车辆蓝图
- blueprints = self.world.get_blueprint_library().filter('vehicle.*')
-
- if safe_mode:
- # 过滤掉不安全或特殊车辆
- blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4]
- blueprints = [x for x in blueprints if not x.id.endswith('microlino')]
- blueprints = [x for x in blueprints if not x.id.endswith('carlacola')]
- blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')]
- blueprints = [x for x in blueprints if not x.id.endswith('t2')]
- blueprints = [x for x in blueprints if not x.id.endswith('sprinter')]
- blueprints = [x for x in blueprints if not x.id.endswith('firetruck')]
- blueprints = [x for x in blueprints if not x.id.endswith('ambulance')]
-
- blueprints = sorted(blueprints, key=lambda bp: bp.id)
-
- # 获取生成点
- spawn_points = self.world.get_map().get_spawn_points()
- random.shuffle(spawn_points)
-
- if num_vehicles > len(spawn_points):
- print(f"⚠️ 请求的车辆数({num_vehicles})超过生成点数({len(spawn_points)})")
- num_vehicles = len(spawn_points)
-
- # 批量生成车辆
- batch = []
- for n, transform in enumerate(spawn_points):
- if n >= num_vehicles:
- break
-
- blueprint = random.choice(blueprints)
-
- # 设置随机颜色
- if blueprint.has_attribute('color'):
- color = random.choice(blueprint.get_attribute('color').recommended_values)
- blueprint.set_attribute('color', color)
-
- # 设置驾驶员ID
- if blueprint.has_attribute('driver_id'):
- driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values)
- blueprint.set_attribute('driver_id', driver_id)
-
- blueprint.set_attribute('role_name', 'autopilot')
-
- # 添加到批量命令
- batch.append(carla.command.SpawnActor(blueprint, transform)
- .then(carla.command.SetAutopilot(
- carla.command.FutureActor,
- True,
- self.traffic_manager.get_port())))
-
- # 执行批量命令
- for response in self.client.apply_batch_sync(batch, self.synchronous_master):
- if response.error:
- logging.error(f"生成车辆失败: {response.error}")
- else:
- self.vehicles_list.append(response.actor_id)
-
- print(f"✅ 生成 {len(self.vehicles_list)} 辆车辆")
-
- def _spawn_walkers(self, num_walkers):
- """生成行人"""
- print("🚶 生成行人...")
-
- if num_walkers <= 0:
- return
-
- # 获取行人蓝图
- walker_bps = self.world.get_blueprint_library().filter('walker.pedestrian.*')
-
- # 获取随机位置
- spawn_points = []
- for i in range(num_walkers):
- spawn_point = carla.Transform()
- loc = self.world.get_random_location_from_navigation()
- if loc:
- spawn_point.location = loc
- spawn_points.append(spawn_point)
-
- # 生成行人
- batch = []
- walker_speeds = []
- for spawn_point in spawn_points:
- walker_bp = random.choice(walker_bps)
-
- # 设置为非无敌
- if walker_bp.has_attribute('is_invincible'):
- walker_bp.set_attribute('is_invincible', 'false')
-
- # 设置速度
- speed = 0.0
- if walker_bp.has_attribute('speed'):
- speed = walker_bp.get_attribute('speed').recommended_values[1] # 正常行走速度
-
- walker_speeds.append(speed)
- batch.append(carla.command.SpawnActor(walker_bp, spawn_point))
-
- # 执行批量命令
- results = self.client.apply_batch_sync(batch, True)
-
- for i in range(len(results)):
- if results[i].error:
- logging.error(f"生成行人失败: {results[i].error}")
- else:
- self.walkers_list.append({"id": results[i].actor_id})
-
- # 生成行人控制器
- batch = []
- walker_controller_bp = self.world.get_blueprint_library().find('controller.ai.walker')
-
- for i in range(len(self.walkers_list)):
- batch.append(carla.command.SpawnActor(
- walker_controller_bp,
- carla.Transform(),
- self.walkers_list[i]["id"]))
-
- results = self.client.apply_batch_sync(batch, True)
-
- for i in range(len(results)):
- if results[i].error:
- logging.error(f"生成行人控制器失败: {results[i].error}")
- else:
- self.walkers_list[i]["con"] = results[i].actor_id
- self.all_id.append(results[i].actor_id)
- self.all_id.append(self.walkers_list[i]["id"])
-
- # 获取所有行人actor
- all_actors = self.world.get_actors(self.all_id)
-
- # 初始化控制器
- for i in range(0, len(self.all_id), 2):
- # 启动控制器
- all_actors[i].start()
- # 设置随机目标
- all_actors[i].go_to_location(self.world.get_random_location_from_navigation())
- # 设置最大速度
- all_actors[i].set_max_speed(float(walker_speeds[int(i/2)]))
-
- print(f"✅ 生成 {len(self.walkers_list)} 个行人")
-
- def update(self):
- """更新交通管理器(用于同步模式)"""
- if self.is_synchronous and self.synchronous_master:
- self.world.tick()
- elif self.is_synchronous:
- self.world.wait_for_tick()
-
- def set_vehicle_lights(self, enabled=True):
- """设置车辆灯光"""
- if not self.vehicles_list:
- return
-
- try:
- all_vehicle_actors = self.world.get_actors(self.vehicles_list)
- for actor in all_vehicle_actors:
- self.traffic_manager.update_vehicle_lights(actor, enabled)
-
- print(f"✅ 车辆灯光 {'开启' if enabled else '关闭'}")
- except Exception as e:
- print(f"⚠️ 设置车辆灯光失败: {e}")
-
- def set_global_speed_limit(self, percentage=30.0):
- """设置全局速度限制百分比"""
- if self.traffic_manager:
- self.traffic_manager.global_percentage_speed_difference(percentage)
- print(f"✅ 设置全局速度限制: {percentage}%")
-
- def cleanup(self):
- """清理所有生成的交通"""
- print("\n🧹 清理交通流...")
-
- try:
- # 停止同步模式
- if self.is_synchronous and self.synchronous_master:
- settings = self.world.get_settings()
- settings.synchronous_mode = False
- settings.fixed_delta_seconds = None
- self.world.apply_settings(settings)
-
- # 销毁车辆
- if self.vehicles_list:
- print(f"销毁 {len(self.vehicles_list)} 辆车辆...")
- self.client.apply_batch([
- carla.command.DestroyActor(x) for x in self.vehicles_list
- ])
-
- # 销毁行人
- if self.all_id:
- print(f"销毁 {len(self.walkers_list)} 个行人...")
-
- # 先停止控制器
- all_actors = self.world.get_actors(self.all_id)
- for i in range(0, len(self.all_id), 2):
- all_actors[i].stop()
-
- # 销毁所有actor
- self.client.apply_batch([
- carla.command.DestroyActor(x) for x in self.all_id
- ])
-
- # 清空列表
- self.vehicles_list = []
- self.walkers_list = []
- self.all_id = []
-
- print("✅ 交通流清理完成")
-
- except Exception as e:
- print(f"❌ 清理交通流失败: {e}")
-
- def get_traffic_info(self):
- """获取交通信息"""
- return {
- 'num_vehicles': len(self.vehicles_list),
- 'num_walkers': len(self.walkers_list),
- 'is_synchronous': self.is_synchronous,
- 'tm_port': self.tm_port
- }
-
- def __del__(self):
- """析构函数,确保清理"""
- if self.vehicles_list or self.walkers_list:
- self.cleanup()
diff --git a/src/Car-Racing/compare_models.py b/src/Car-Racing/compare_models.py
new file mode 100644
index 0000000000..80db6d3f63
--- /dev/null
+++ b/src/Car-Racing/compare_models.py
@@ -0,0 +1,110 @@
+import os
+import sys
+
+# Adjust sys.path to include the project root
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__))))
+
+# Import modules with their full file names
+import dqn_model.DQN_model as DQN
+import double_dqn_model.double_dqn as DDQN
+
+# Load and evaluate DQN
+try:
+ dqn_driver = DQN.Agent(
+ state_space_shape=(4, 84, 84),
+ action_n=5,
+ load_state=True,
+ load_model='DQN.pt'
+ )
+except Exception as e:
+ print(f"Error loading DQN model: {e}. Initializing with default parameters.")
+ dqn_driver = DQN.Agent(
+ state_space_shape=(4, 84, 84),
+ action_n=5,
+ config_path='configs/dqn.yaml'
+ )
+
+# Load and evaluate Double DQN - Add this block
+try:
+ ddqn_driver = DDQN.DoubleDQNAgent(
+ state_space_shape=(4, 84, 84),
+ action_n=5,
+ load_state=True,
+ load_model='DoubleDQN.pt'
+ )
+except Exception as e:
+ print(f"Error loading Double DQN model: {e}. Initializing with default parameters.")
+ ddqn_driver = DDQN.DoubleDQNAgent(
+ state_space_shape=(4, 84, 84),
+ action_n=5,
+ config_path='configs/double_dqn.yaml'
+ )
+
+def evaluate_dqn(agent, num_episodes=5): # Adjust num_episodes as needed
+ import gymnasium as gym
+ from gymnasium.wrappers import GrayscaleObservation, ResizeObservation, FrameStackObservation, RecordVideo
+ from dqn_model.DQN_model import SkipFrame # Import SkipFrame from DQN_model
+ env = gym.make("CarRacing-v3", continuous=False, render_mode="rgb_array")
+ env = RecordVideo(env, video_folder='videos\\DQN')
+ env = SkipFrame(env, skip=4)
+ env = GrayscaleObservation(env)
+ env = ResizeObservation(env, (84, 84))
+ env = FrameStackObservation(env, stack_size=4)
+ agent.epsilon = 0
+ scores = []
+ steps = []
+ for episode in range(num_episodes):
+ state, info = env.reset(seed=episode)
+ score = 0
+ step_count = 0
+ while True:
+ action = agent.take_action(state)
+ state, reward, terminated, truncated, info = env.step(action)
+ score += reward
+ step_count += 1
+ if terminated or truncated:
+ break
+ scores.append(score)
+ steps.append(step_count)
+ print(f"Evaluation Episode {episode+1}/{num_episodes} | Seed: {episode} | Score: {score:.1f}")
+ env.close()
+ return sum(scores) / len(scores), sum(1 for s in scores if s > 0) / len(scores), sum(steps) / len(steps)
+
+def evaluate_ddqn(agent, num_episodes=5): # Adjust num_episodes as needed
+ import gymnasium as gym
+ from gymnasium.wrappers import GrayscaleObservation, ResizeObservation, FrameStackObservation, RecordVideo
+ from dqn_model.DQN_model import SkipFrame # Import SkipFrame from DQN_model
+ env = gym.make("CarRacing-v3", continuous=False, render_mode="rgb_array")
+ env = RecordVideo(env, video_folder='C:\\Users\\25012\\Desktop\\rl-car-ddqn-main\\videos\\DoubleDQN')
+ env = SkipFrame(env, skip=4)
+ env = GrayscaleObservation(env)
+ env = ResizeObservation(env, (84, 84))
+ env = FrameStackObservation(env, stack_size=4)
+ agent.epsilon = 0
+ scores = []
+ steps = []
+ for episode in range(num_episodes):
+ state, info = env.reset(seed=episode)
+ score = 0
+ step_count = 0
+ while True:
+ action = agent.take_action(state)
+ state, reward, terminated, truncated, info = env.step(action)
+ score += reward
+ step_count += 1
+ if terminated or truncated:
+ break
+ scores.append(score)
+ steps.append(step_count)
+ print(f"Evaluation Episode {episode+1}/{num_episodes} | Seed: {episode} | Score: {score:.1f}")
+ env.close()
+ return sum(scores) / len(scores), sum(1 for s in scores if s > 0) / len(scores), sum(steps) / len(steps)
+
+# Run evaluations
+dqn_avg_score, dqn_success_rate, dqn_avg_steps = evaluate_dqn(dqn_driver)
+ddqn_avg_score, ddqn_success_rate, ddqn_avg_steps = evaluate_ddqn(ddqn_driver)
+
+# Print comparison
+print("Model Comparison:")
+print(f"DQN - Avg Score: {dqn_avg_score:.1f}, Success Rate: {dqn_success_rate:.2%}, Avg Steps: {dqn_avg_steps:.1f}")
+print(f"Double DQN - Avg Score: {ddqn_avg_score:.1f}, Success Rate: {ddqn_success_rate:.2%}, Avg Steps: {ddqn_avg_steps:.1f}")
\ No newline at end of file
diff --git a/src/Car-Racing/requirements.txt b/src/Car-Racing/requirements.txt
index 60186dabf5..ce08bc89ab 100644
Binary files a/src/Car-Racing/requirements.txt and b/src/Car-Racing/requirements.txt differ
diff --git a/src/Content_Creation_Map/README.md b/src/Content_Creation_Map/README.md
index 70d8c947c0..7fe03a9986 100644
--- a/src/Content_Creation_Map/README.md
+++ b/src/Content_Creation_Map/README.md
@@ -579,3 +579,89 @@ QGIS内置了多语言支持,切换为中文无需安装额外语言包。
最后的应用界面如下图说明你的QGIS已经成功配置可以使用了

+
+
+# 天地图数据获取与RoadRunner场景构建全流程教程
+
+本教程旨在指导你如何从天地图获取卫星影像数据,并将其作为精确参考底图导入专业道路场景建模软件RoadRunner,用于创建高真实度的仿真场景。
+
+## 第一步:前置准备 —— 获取天地图访问密钥
+
+在开始任何操作前,你必须拥有一个有效的天地图服务密钥。
+
+1. **访问官网**:前往 [天地图官网](http://lbs.tianditu.gov.cn/home.html) 注册并登录。
+2. **创建应用**:进入控制台的 **“应用管理”**,点击 **“创建新应用”**。
+3. **关键设置**:
+ * **应用类型**:**务必选择 “浏览器端”**。
+ * **IP地址**:为保持使用灵活性,建议**留空不填**。若需填写,格式必须为 `http://你的IP或域名`。
+4. **获取密钥**:应用创建成功后,系统会生成一个唯一的密钥(一串字符串)。请妥善保存,后续所有步骤都将用到它。
+
+
+
+*注意:这是我自己的密钥,请使用你自己自己创建应用成功后的密钥并且妥善保管*
+
+## 第二步:核心操作 —— 在QGIS中获取并导出底图
+
+本部分使用QGIS作为桥梁,将在线天地图服务“固化”为本地地理图像文件。
+
+### 2.1 在QGIS中添加天地图服务
+1. **安装并打开QGIS**。
+2. **新建连接**:在左侧“浏览器”面板,右键点击 `XYZ Tiles`,选择 **新建连接**。
+3. **填写连接信息**:
+ * **名称**:可自定义,如 `天地图影像`。
+ * **URL**:粘贴以下地址,并务必将 `你的密钥` 替换为你在第一章获取的真实密钥。
+ ```
+ https://t0.tianditu.gov.cn/img_w/wmts?SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={z}&TILEROW={y}&TILECOL={x}&tk=你的密钥
+ ```
+
+ 
+5. **加载图层**:点击“确定”后,双击此新建连接,天地图卫星影像将加载到主地图窗口。
+ 
+> **可选:添加注记图层**
+> 如需显示道路名、地名等文字信息,请按上述步骤再添加一个连接,URL如下:
+> ```
+> https://t0.tianditu.gov.cn/cia_w/wmts?SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=cia&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={z}&TILEROW={y}&TILECOL={x}&tk=你的密钥
+> ```
+
+### 2.2 定位并设置清晰度
+1. **导航到目标区域**:使用鼠标滚轮缩放和平移,找到你要创建场景的具体位置。
+2. **选择合适缩放级别**:**缩放级别直接决定最终图像的清晰度**。状态栏会显示当前级别,建议参考:
+ * **城市/区域范围**:13-15级
+ * **街区/园区范围**:16-17级
+ * **路口/建筑细节**:18-19级
+
+ 
+
+### 2.3 导出为地理配准图像(关键步骤)
+此步骤将在线地图转换为RoadRunner可读的本地文件。
+
+1. 点击菜单栏:**项目 → 导入/导出 → 导出地图为图片...**。
+2. 在保存对话框中:
+ * **选择保存位置和文件名**。
+ * **格式选择**:在下拉菜单中选择 **`TIFF`**。该格式即是带地理信息的 **GeoTIFF**。
+3. **设置参数(可选但重要)**:
+ * **分辨率 (DPI)**:建议设置为 **300** 或更高,以获得清晰图像。
+ * **范围**:选择“使用地图画布范围”以导出当前视图,或点击“选择在地图上”手动框选。
+4. 点击 **保存**。QGIS将开始渲染并生成最终的 `.tiff` 文件。
+
+## 第三步:场景构建 —— 在RoadRunner中导入与配准
+
+### 3.1 导入底图
+1. 启动RoadRunner并打开你的项目。
+2. 将上一步导出的 `.tiff` 文件直接从文件资源管理器**拖拽**到RoadRunner的 **Library Browser** 或主视图窗口。
+ 
+ 
+
+### 3.3 开始场景创作
+配准完成后,天地图底图将作为**静态参考背景**。后续所有三维创作均需基于此背景**手动完成**:
+
+* **绘制路网**:使用道路工具,**严格沿底图上的道路影像边缘描绘**,生成车道、路口。
+* **摆放资产**:参照底图,将建筑、树木、交通标志等三维模型放置在正确位置。
+* **地形塑造**:根据影像中的地貌起伏,调整地形高度。
+
+## 第四章:进阶技巧
+
+### 提高效率:使用矢量数据辅助路网创建
+完全手动描路效率较低。可以寻找道路矢量数据导入RoadRunner作为辅助参考线。
+* **推荐数据源**:**开放街道地图 (OpenStreetMap, OSM)** 提供免费、全球覆盖的道路矢量数据,可通过 [OSM官网](https://www.openstreetmap.org) 或QGIS的 **QuickOSM** 插件方便地导出所需区域数据。
+
diff --git a/src/Driving_Accident_Video_Recognition/config.py b/src/Driving_Accident_Video_Recognition/config.py
new file mode 100644
index 0000000000..78f37f238b
--- /dev/null
+++ b/src/Driving_Accident_Video_Recognition/config.py
@@ -0,0 +1,29 @@
+"""
+全局配置文件:所有可配置参数集中管理,新手只需修改这里
+"""
+# YOLOv8模型配置
+YOLO_MODEL_PATH = "yolov8n.pt" # 轻量化模型(自动下载)
+CONFIDENCE_THRESHOLD = 0.5 # 目标检测置信度阈值(0-1)
+
+# 检测源配置
+DETECTION_SOURCE = 0 # 0=电脑摄像头,可改为视频路径如"test_accident.mp4"
+
+# 事故识别配置
+ACCIDENT_CLASSES = [0, 2, 7] # YOLOv8类别:0=人,2=汽车,7=卡车
+MIN_VEHICLE_COUNT = 2 # 至少2辆车判定为事故
+PERSON_VEHICLE_CONTACT = True # 行人和车辆同时出现判定为事故
+
+# 帧处理优化配置(低配电脑推荐)
+RESIZE_WIDTH = 640
+RESIZE_HEIGHT = 480
+
+# 依赖包配置
+REQUIRED_PACKAGES = [
+ "ultralytics>=8.0.0",
+ "opencv-python>=4.8.0",
+ "numpy>=1.24.0",
+ "torch>=2.0.0"
+]
+
+# 清华镜像源(加速依赖下载)
+PYPI_MIRROR = "https://pypi.tuna.tsinghua.edu.cn/simple"
\ No newline at end of file
diff --git a/src/Driving_Accident_Video_Recognition/core/detector.py b/src/Driving_Accident_Video_Recognition/core/detector.py
new file mode 100644
index 0000000000..4155840ab8
--- /dev/null
+++ b/src/Driving_Accident_Video_Recognition/core/detector.py
@@ -0,0 +1,121 @@
+"""
+事故检测器核心类:负责模型加载、检测流程执行
+"""
+import sys
+import cv2
+from ultralytics import YOLO
+from config import (
+ YOLO_MODEL_PATH, CONFIDENCE_THRESHOLD, ACCIDENT_CLASSES,
+ MIN_VEHICLE_COUNT, PERSON_VEHICLE_CONTACT,
+ RESIZE_WIDTH, RESIZE_HEIGHT, DETECTION_SOURCE
+)
+from core.process import process_box_coords, draw_annotations
+
+class AccidentDetector:
+ def __init__(self):
+ """初始化检测器,加载YOLOv8模型"""
+ self.model = None
+ self.accident_detected = False
+ self._load_model()
+
+ def _load_model(self):
+ """私有方法:加载模型,包含重试逻辑"""
+ try:
+ print("🔄 正在加载YOLOv8模型(首次运行会自动下载)...")
+ self.model = YOLO(YOLO_MODEL_PATH)
+ print("✅ YOLOv8模型加载成功")
+ except Exception as e:
+ print(f"❌ 模型加载失败:{e}")
+ # 重试加载模型
+ try:
+ print("🔄 尝试重新下载模型...")
+ self.model = YOLO("yolov8n.pt")
+ print("✅ 模型重新加载成功")
+ except Exception as e2:
+ print(f"❌ 模型重新加载失败:{e2}")
+ sys.exit(1)
+
+ def detect_frame(self, frame):
+ """处理单帧,返回标注后的帧和是否检测到事故"""
+ detected_objects = []
+ try:
+ # 缩放帧提升速度
+ frame_resized = cv2.resize(frame, (RESIZE_WIDTH, RESIZE_HEIGHT))
+ # YOLOv8推理
+ results = self.model(frame_resized, conf=CONFIDENCE_THRESHOLD)
+
+ # 解析检测结果
+ for r in results:
+ if hasattr(r, 'boxes') and r.boxes is not None:
+ for box in r.boxes:
+ if not hasattr(box, 'cls') or box.cls is None:
+ continue
+ cls_idx = int(box.cls[0])
+ if cls_idx in ACCIDENT_CLASSES:
+ cls_name = self.model.names[cls_idx]
+ # 处理坐标
+ scale_x = frame.shape[1] / RESIZE_WIDTH
+ scale_y = frame.shape[0] / RESIZE_HEIGHT
+ x1, y1, x2, y2 = process_box_coords(box, scale_x, scale_y)
+ detected_objects.append((cls_name, x1, y1, x2, y2))
+
+ # 判断事故
+ person_count = sum(1 for obj in detected_objects if obj[0] == "person")
+ vehicle_count = sum(1 for obj in detected_objects if obj[0] in ["car", "truck"])
+ is_accident = (vehicle_count >= MIN_VEHICLE_COUNT) or (person_count >= 1 and vehicle_count >= 1 and PERSON_VEHICLE_CONTACT)
+ self.accident_detected = is_accident
+
+ # 绘制标注
+ frame = draw_annotations(frame, detected_objects, is_accident)
+
+ except Exception as e:
+ print(f"⚠️ 帧处理出现小错误:{e},继续运行...")
+
+ return frame, self.accident_detected
+
+ def run_detection(self):
+ """启动检测流程,包含完善的容错逻辑"""
+ # 多次尝试打开检测源
+ cap = None
+ for i in range(3):
+ cap = cv2.VideoCapture(DETECTION_SOURCE)
+ if cap.isOpened():
+ break
+ print(f"⚠️ 第{i+1}次打开检测源失败,重试中...")
+ cv2.waitKey(1000)
+
+ if not cap or not cap.isOpened():
+ print(f"❌ 无法打开检测源:{DETECTION_SOURCE}")
+ # 强制切换为摄像头
+ print("🔄 强制切换为电脑摄像头...")
+ cap = cv2.VideoCapture(0)
+ if not cap.isOpened():
+ print("❌ 摄像头也无法打开,请检查设备")
+ sys.exit(1)
+
+ print("✅ 检测源打开成功,开始实时检测(按Q/ESC键退出)")
+ print("💡 提示:检测到2辆车或行人和车辆同时出现时,显示红色警告")
+
+ # 逐帧处理
+ while True:
+ ret, frame = cap.read()
+ if not ret:
+ print("🔚 视频/摄像头流结束")
+ break
+
+ frame, _ = self.detect_frame(frame)
+ cv2.imshow("驾驶事故检测(按Q退出)", frame)
+
+ # 退出逻辑
+ key = cv2.waitKey(1) & 0xFF
+ if key == ord('q') or key == 27:
+ print("🛑 用户手动退出")
+ break
+
+ # 释放资源
+ cap.release()
+ cv2.destroyAllWindows()
+ print(f"\n📊 检测总结:是否检测到事故 → {'✅ 是' if self.accident_detected else '❌ 否'}")
+
+# 供外部导入的类
+__all__ = ["AccidentDetector"]
\ No newline at end of file
diff --git a/src/Driving_Accident_Video_Recognition/core/process.py b/src/Driving_Accident_Video_Recognition/core/process.py
new file mode 100644
index 0000000000..87df8b1bbd
--- /dev/null
+++ b/src/Driving_Accident_Video_Recognition/core/process.py
@@ -0,0 +1,46 @@
+"""
+辅助处理工具:负责坐标转换、帧处理等通用逻辑
+"""
+import numpy as np
+import torch
+import cv2
+
+def process_box_coords(box, scale_x, scale_y):
+ """
+ 安全处理YOLOv8检测框坐标,解决张量类型错误
+ :param box: YOLOv8的检测框对象
+ :param scale_x: 宽度缩放比例
+ :param scale_y: 高度缩放比例
+ :return: 转换后的坐标(x1, y1, x2, y2)
+ """
+ # 兼容张量和numpy数组
+ if isinstance(box.xyxy[0], torch.Tensor):
+ box_xyxy = box.xyxy[0].cpu().numpy()
+ else:
+ box_xyxy = np.array(box.xyxy[0])
+ # 缩放坐标
+ scaled_box = box_xyxy * [scale_x, scale_y, scale_x, scale_y]
+ # 转换为整数
+ return map(int, scaled_box)
+
+def draw_annotations(frame, detected_objects, is_accident):
+ """
+ 在帧上绘制检测标注和事故警告
+ :param frame: 原始视频帧
+ :param detected_objects: 检测到的目标列表
+ :param is_accident: 是否检测到事故
+ :return: 标注后的帧
+ """
+ # 绘制目标检测框
+ for (cls_name, x1, y1, x2, y2) in detected_objects:
+ cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
+ cv2.putText(frame, cls_name, (x1, y1 - 10),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
+ # 绘制事故警告
+ if is_accident:
+ cv2.putText(frame, "⚠️ 检测到事故!", (50, 50),
+ cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 0, 255), 3)
+ return frame
+
+# 供外部导入的函数
+__all__ = ["process_box_coords", "draw_annotations"]
\ No newline at end of file
diff --git a/src/Driving_Accident_Video_Recognition/main.py b/src/Driving_Accident_Video_Recognition/main.py
new file mode 100644
index 0000000000..cf778e433b
--- /dev/null
+++ b/src/Driving_Accident_Video_Recognition/main.py
@@ -0,0 +1,35 @@
+"""
+主程序入口:整合所有模块,启动驾驶事故检测工具
+"""
+import sys
+import os
+
+# 关键:将当前脚本所在的目录(code目录)加入Python模块搜索路径
+# 确保Python能找到core、utils等子模块
+current_dir = os.path.dirname(os.path.abspath(__file__))
+sys.path.append(current_dir)
+
+# 现在可以正常导入模块了
+from config import REQUIRED_PACKAGES, PYPI_MIRROR
+from utils.dependencies import install_dependencies
+from core.detector import AccidentDetector
+
+def main():
+ """主函数:执行依赖安装 → 初始化检测器 → 启动检测"""
+ try:
+ print("🚀 启动驾驶事故视频识别工具...")
+ # 第一步:自动安装依赖
+ install_dependencies(REQUIRED_PACKAGES, PYPI_MIRROR)
+ # 第二步:初始化检测器
+ detector = AccidentDetector()
+ # 第三步:启动检测
+ detector.run_detection()
+ except KeyboardInterrupt:
+ print("\n🛑 用户强制中断程序")
+ except Exception as e:
+ print(f"\n❌ 程序运行出错:{e}")
+ finally:
+ print("👋 程序正常退出")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/Driving_Car/README.md b/src/Driving_Car/README.md
index 8c27ba2124..aea3eb6553 100644
--- a/src/Driving_Car/README.md
+++ b/src/Driving_Car/README.md
@@ -1 +1,39 @@
-无人车
\ No newline at end of file
+
+---
+
+# 无人驾驶汽车项目
+
+## 项目简介
+
+本项目是一个基于Python和PyCharm Community Edition开发的无人驾驶汽车仿真系统。通过计算机视觉、机器学习和传感器数据融合技术,实现车辆在模拟环境中的自主导航和避障功能。
+
+## 核心功能
+
+**路径规划**: 基于A*算法和RRT算法的智能路径规划
+
+**障碍物检测**: 使用YOLO和OpenCV进行实时目标检测与识别
+
+**车辆控制**: PID控制器实现精确的转向和速度控制
+
+**传感器模拟**: 模拟激光雷达、摄像头和超声波传感器数据
+
+**3D可视化**: 基于PyGame的实时场景渲染
+
+## 技术栈
+**开发环境**: PyCharm Community Edition 2024+
+
+**核心语言**: Python 3.8+
+
+**计算机视觉**: OpenCV, PIL, NumPy
+
+**机器学习**: TensorFlow, scikit-learn
+
+**3D图形**: PyGame, Pyglet.
+
+**数据处理**: Pandas, Matplotlib
+
+## 快速开始
+
+1. 克隆项目到PyCharm
+2. 安装依赖: `pip install -r requirements.txt`
+3. 运行主程序: `python main.py`
diff --git a/src/Drone_hand_gesture_project/README.md b/src/Drone_hand_gesture_project/README.md
index cb2911c8f7..6a7429a331 100644
--- a/src/Drone_hand_gesture_project/README.md
+++ b/src/Drone_hand_gesture_project/README.md
@@ -6,8 +6,12 @@
├── main.py # 主程序
├── drone_controller.py # 无人机控制
├── simulation_3d.py # 3D仿真
+ ├── physics_engine.py # 物理仿真引擎
├── gesture_detector.py # 基础手势检测器
├── gesture_data_collector.py # 手势图像数据收集
+ ├── gesture_detector_enhanced.py # 增强手势检测器
+ ├── gesture_classifier.py # 手势识别分类器
+ ├── train_gesture_model.py # 训练识别模型
├── dataset/ # 数据集目录
│ ├── raw/ # 原始数据
│ ├── processed/ # 处理后的数据
@@ -16,6 +20,7 @@
### 运行步骤(在ubuntu或是xubuntu下)
1. 进入项目目录:
+
```bash
cd /mnt/hgfs/nn/src/Drone_hand_gesture_project
@@ -24,7 +29,17 @@
```bash
source gesture_env/bin/activate
-3. 运行主程序:
+3. 收集手势图像:
+
+ ```bash
+ python gesture_data_collector.py
+
+4. 训练学习模型:
+
+ ```bash
+ python train_gesture_model.py --model_type ensemble
+
+5. 运行主程序:
```bash
python main.py
diff --git a/src/Drone_hand_gesture_project/gesture_classifier.py b/src/Drone_hand_gesture_project/gesture_classifier.py
new file mode 100644
index 0000000000..ccab2284dd
--- /dev/null
+++ b/src/Drone_hand_gesture_project/gesture_classifier.py
@@ -0,0 +1,249 @@
+import numpy as np
+import pickle
+import os
+from sklearn.model_selection import train_test_split
+from sklearn.preprocessing import StandardScaler
+from sklearn.svm import SVC
+from sklearn.ensemble import RandomForestClassifier
+from sklearn.neural_network import MLPClassifier
+from sklearn.metrics import classification_report, accuracy_score
+import joblib
+
+
+class GestureClassifier:
+ """机器学习手势分类器"""
+
+ def __init__(self, model_type='svm', model_path=None):
+ self.model_type = model_type
+ self.model_path = model_path
+ self.scaler = StandardScaler()
+ self.model = None
+ self.gesture_classes = None
+
+ # 如果提供了模型路径,加载模型
+ if model_path and os.path.exists(model_path):
+ self.load_model(model_path)
+
+ def create_model(self):
+ """创建分类模型"""
+ if self.model_type == 'svm':
+ self.model = SVC(
+ kernel='rbf',
+ C=10.0,
+ gamma='scale',
+ probability=True,
+ random_state=42
+ )
+ elif self.model_type == 'random_forest':
+ self.model = RandomForestClassifier(
+ n_estimators=100,
+ max_depth=10,
+ random_state=42,
+ n_jobs=-1
+ )
+ elif self.model_type == 'mlp':
+ self.model = MLPClassifier(
+ hidden_layer_sizes=(128, 64),
+ activation='relu',
+ solver='adam',
+ max_iter=1000,
+ random_state=42
+ )
+ else:
+ raise ValueError(f"未知的模型类型: {self.model_type}")
+
+ def load_dataset(self, dataset_path):
+ """加载数据集"""
+ with open(dataset_path, 'rb') as f:
+ dataset = pickle.load(f)
+
+ X = dataset['features']
+ y = dataset['labels']
+ self.gesture_classes = dataset['gesture_classes']
+
+ return X, y
+
+ def preprocess_data(self, X_train, X_test):
+ """数据预处理"""
+ # 标准化特征
+ X_train_scaled = self.scaler.fit_transform(X_train)
+ X_test_scaled = self.scaler.transform(X_test)
+
+ return X_train_scaled, X_test_scaled
+
+ def train(self, dataset_path, test_size=0.2, save_path=None):
+ """训练模型"""
+ # 加载数据
+ X, y = self.load_dataset(dataset_path)
+
+ # 检查数据
+ print(f"数据集信息:")
+ print(f" 总样本数: {len(X)}")
+ print(f" 特征维度: {X.shape[1]}")
+
+ unique_labels, counts = np.unique(y, return_counts=True)
+ print(" 各类样本数:")
+ for label, count in zip(unique_labels, counts):
+ class_name = self.gesture_classes[label]
+ print(f" {class_name}: {count}")
+
+ if len(unique_labels) < 2:
+ raise ValueError(f"需要至少2个类别才能训练,当前只有 {len(unique_labels)} 个类别")
+
+ # 划分训练集和测试集
+ X_train, X_test, y_train, y_test = train_test_split(
+ X, y, test_size=test_size, random_state=42, stratify=y
+ )
+
+ # 数据预处理
+ X_train_scaled, X_test_scaled = self.preprocess_data(X_train, X_test)
+
+ # 创建并训练模型
+ self.create_model()
+ print(f"训练 {self.model_type} 模型...")
+ self.model.fit(X_train_scaled, y_train)
+
+ # 评估模型
+ train_acc = self.model.score(X_train_scaled, y_train)
+ test_acc = self.model.score(X_test_scaled, y_test)
+
+ print(f"训练准确率: {train_acc:.4f}")
+ print(f"测试准确率: {test_acc:.4f}")
+
+ # 详细分类报告
+ y_pred = self.model.predict(X_test_scaled)
+ print("\n分类报告:")
+ print(classification_report(y_test, y_pred,
+ target_names=self.gesture_classes))
+
+ # 保存模型
+ if save_path:
+ self.save_model(save_path)
+
+ return test_acc
+
+ def predict(self, landmarks):
+ """预测手势类别"""
+ if self.model is None:
+ raise ValueError("模型未训练或未加载")
+
+ # 检查landmarks是否有效
+ if len(landmarks) != 63: # 21个关键点 * 3坐标
+ return "none", 0.0
+
+ # 预处理
+ landmarks_scaled = self.scaler.transform([landmarks])
+
+ # 预测
+ probabilities = self.model.predict_proba(landmarks_scaled)[0]
+ predicted_class_idx = np.argmax(probabilities)
+ confidence = probabilities[predicted_class_idx]
+
+ predicted_class = self.gesture_classes[predicted_class_idx]
+
+ return predicted_class, confidence
+
+ def predict_batch(self, landmarks_list):
+ """批量预测"""
+ if not landmarks_list:
+ return [], []
+
+ # 预处理
+ landmarks_array = np.array(landmarks_list)
+ landmarks_scaled = self.scaler.transform(landmarks_array)
+
+ # 预测
+ probabilities = self.model.predict_proba(landmarks_scaled)
+ predicted_indices = np.argmax(probabilities, axis=1)
+ confidences = np.max(probabilities, axis=1)
+
+ predicted_classes = [self.gesture_classes[idx] for idx in predicted_indices]
+
+ return predicted_classes, confidences
+
+ def save_model(self, save_path):
+ """保存模型"""
+ model_data = {
+ 'model_type': self.model_type,
+ 'model': self.model,
+ 'scaler': self.scaler,
+ 'gesture_classes': self.gesture_classes
+ }
+
+ joblib.dump(model_data, save_path)
+ print(f"模型已保存到: {save_path}")
+
+ def load_model(self, model_path):
+ """加载模型"""
+ model_data = joblib.load(model_path)
+
+ self.model_type = model_data['model_type']
+ self.model = model_data['model']
+ self.scaler = model_data['scaler']
+ self.gesture_classes = model_data['gesture_classes']
+
+ print(f"模型已从 {model_path} 加载")
+ print(f"模型类型: {self.model_type}")
+ print(f"手势类别: {self.gesture_classes}")
+
+
+class EnsembleGestureClassifier:
+ """集成学习手势分类器"""
+
+ def __init__(self, model_paths=None):
+ self.models = []
+ self.gesture_classes = None
+
+ if model_paths:
+ self.load_models(model_paths)
+
+ def load_models(self, model_paths):
+ """加载多个模型"""
+ for path in model_paths:
+ classifier = GestureClassifier(model_path=path)
+ self.models.append(classifier)
+
+ # 使用第一个模型的类别
+ if self.models:
+ self.gesture_classes = self.models[0].gesture_classes
+
+ def predict(self, landmarks, voting='soft'):
+ """集成预测"""
+ if not self.models:
+ raise ValueError("没有加载任何模型")
+
+ if len(landmarks) != 63:
+ return "none", 0.0
+
+ predictions = []
+ confidences = []
+
+ for model in self.models:
+ pred_class, confidence = model.predict(landmarks)
+ predictions.append(pred_class)
+ confidences.append(confidence)
+
+ if voting == 'hard':
+ # 硬投票
+ from collections import Counter
+ most_common = Counter(predictions).most_common(1)[0]
+ final_prediction = most_common[0]
+ final_confidence = most_common[1] / len(predictions)
+ else:
+ # 软投票(平均置信度)
+ class_confidences = {}
+ for model, pred, conf in zip(self.models, predictions, confidences):
+ if pred not in class_confidences:
+ class_confidences[pred] = []
+ class_confidences[pred].append(conf)
+
+ # 平均每个类别的置信度
+ avg_confidences = {
+ cls: np.mean(confs) for cls, confs in class_confidences.items()
+ }
+
+ # 选择平均置信度最高的类别
+ final_prediction = max(avg_confidences.items(), key=lambda x: x[1])[0]
+ final_confidence = avg_confidences[final_prediction]
+
+ return final_prediction, final_confidence
\ No newline at end of file
diff --git a/src/Drone_hand_gesture_project/gesture_data_collector.py b/src/Drone_hand_gesture_project/gesture_data_collector.py
index 8db9e2863f..4d1b5630ae 100644
--- a/src/Drone_hand_gesture_project/gesture_data_collector.py
+++ b/src/Drone_hand_gesture_project/gesture_data_collector.py
@@ -7,8 +7,6 @@
import mediapipe as mp
import pickle
from PIL import Image, ImageDraw, ImageFont
-import sys
-
class GestureDataCollector:
"""手势数据收集器"""
@@ -27,30 +25,16 @@ def __init__(self, data_dir="dataset"):
min_tracking_confidence=0.5
)
- # 定义手势类别
+ # 定义手势类别(8个手势,去掉no_hand)
self.gesture_classes = [
- "张开手掌", # open_palm
- "握拳", # closed_fist
- "胜利手势", # victory
- "大拇指向上", # thumb_up
- "大拇指向下", # thumb_down
- "食指上指", # pointing_up
- "食指向下", # pointing_down
- "OK手势", # ok_sign
- "无手" # no_hand
- ]
-
- # 手势类别的英文标识(用于保存文件)
- self.gesture_english_names = [
- "open_palm",
- "closed_fist",
- "victory",
- "thumb_up",
- "thumb_down",
- "pointing_up",
- "pointing_down",
- "ok_sign",
- "no_hand"
+ "open_palm", # 张开手掌
+ "closed_fist", # 握拳
+ "victory", # 胜利手势
+ "thumb_up", # 大拇指
+ "thumb_down", # 大拇指向下
+ "pointing_up", # 食指上指
+ "pointing_down", # 食指向下
+ "ok_sign", # OK手势
]
# 数据存储
@@ -58,31 +42,6 @@ def __init__(self, data_dir="dataset"):
self.current_gesture = None
self.collection_active = False
- # 尝试加载中文字体
- self.font = None
- try:
- # Windows系统字体路径
- if sys.platform == 'win32':
- font_path = 'C:/Windows/Fonts/simhei.ttf' # 黑体
- # Linux系统字体路径
- elif sys.platform == 'linux':
- font_path = '/usr/share/fonts/truetype/wqy/wqy-microhei.ttc' # 文泉驿微米黑
- # macOS系统字体路径
- elif sys.platform == 'darwin':
- font_path = '/System/Library/Fonts/PingFang.ttc' # 苹方字体
- else:
- font_path = None
-
- if font_path and os.path.exists(font_path):
- self.font = ImageFont.truetype(font_path, 30)
- print(f"已加载中文字体: {font_path}")
- else:
- print("未找到中文字体,将使用默认字体")
- self.font = ImageFont.load_default()
- except Exception as e:
- print(f"加载字体失败: {e}")
- self.font = ImageFont.load_default()
-
def create_directories(self):
"""创建必要的目录"""
directories = [
@@ -115,26 +74,31 @@ def collect_sample(self, gesture_class, image, landmarks):
"""收集一个样本"""
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f")
- # 获取对应的英文手势名称用于保存文件
- if gesture_class in self.gesture_classes:
- english_name = self.gesture_english_names[self.gesture_classes.index(gesture_class)]
- else:
- english_name = gesture_class
-
# 保存图像
- img_filename = f"{english_name}_{timestamp}.jpg"
+ img_filename = f"{gesture_class}_{timestamp}.jpg"
img_path = os.path.join(self.data_dir, "raw", "images", img_filename)
cv2.imwrite(img_path, image)
- # 保存关键点数据
+ # 只保存有手部关键点的数据
+ if landmarks and len(landmarks) > 0:
+ landmarks_data = landmarks[0] # 取第一个手
+ # 确保是63维
+ if len(landmarks_data) < 63:
+ landmarks_data.extend([0.0] * (63 - len(landmarks_data)))
+ elif len(landmarks_data) > 63:
+ landmarks_data = landmarks_data[:63]
+ else:
+ # 如果没有检测到手,跳过这个样本(不收集无手数据)
+ print(f"警告: {gesture_class} 未检测到手部,跳过此样本")
+ return None
+
data_entry = {
'timestamp': timestamp,
'gesture_class': gesture_class,
- 'gesture_english': english_name,
'image_path': img_path,
- 'landmarks': landmarks[0] if landmarks else [], # 取第一个手
- 'num_landmarks': 21 if landmarks else 0,
- 'has_hand': len(landmarks) > 0
+ 'landmarks': landmarks_data,
+ 'num_landmarks': 21,
+ 'has_hand': True
}
self.collected_data.append(data_entry)
@@ -156,15 +120,19 @@ def save_dataset(self, filename="gesture_dataset.pkl"):
y = []
for data in self.collected_data:
- if data['has_hand'] and len(data['landmarks']) == 63: # 21个点 * 3坐标
+ if data['has_hand'] and len(data['landmarks']) == 63:
X.append(data['landmarks'])
y.append(self.gesture_classes.index(data['gesture_class']))
+ if len(X) == 0:
+ print("错误: 没有有效的训练数据!")
+ print("请确保每个手势都收集了足够的样本(每个手势至少10个)")
+ return None
+
dataset = {
'features': np.array(X),
'labels': np.array(y),
'gesture_classes': self.gesture_classes,
- 'gesture_english_names': self.gesture_english_names,
'num_samples': len(X)
}
@@ -180,18 +148,6 @@ def save_dataset(self, filename="gesture_dataset.pkl"):
return dataset
- def put_chinese_text(self, img, text, position, font_color=(255, 255, 255)):
- """在图像上绘制中文文本"""
- # 将OpenCV图像转换为PIL图像
- img_pil = Image.fromarray(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
- draw = ImageDraw.Draw(img_pil)
-
- # 绘制文本
- draw.text(position, text, font=self.font, fill=font_color)
-
- # 将PIL图像转换回OpenCV图像
- return cv2.cvtColor(np.array(img_pil), cv2.COLOR_RGB2BGR)
-
def run_collection_app(self):
"""运行数据收集应用程序"""
cap = cv2.VideoCapture(1)
@@ -201,7 +157,7 @@ def run_collection_app(self):
print("手势数据收集工具")
print("=" * 50)
print("命令:")
- print(" 1-9: 选择手势类别")
+ print(" 1-8: 选择手势类别(共8个手势)")
print(" c: 收集当前样本")
print(" s: 停止收集当前手势")
print(" w: 保存数据集")
@@ -211,18 +167,17 @@ def run_collection_app(self):
for i, gesture in enumerate(self.gesture_classes):
print(f" {i + 1}: {gesture}")
print("=" * 50)
+ print("提示: 每个手势建议收集20-30个样本")
+ print("确保手在摄像头中清晰可见")
+ print("=" * 50)
while True:
ret, frame = cap.read()
if not ret:
- print("无法读取摄像头画面")
break
frame = cv2.flip(frame, 1)
- # 创建显示帧的副本用于绘制
- display_frame = frame.copy()
-
# 提取关键点
landmarks, hand_landmarks = self.extract_landmarks(frame)
@@ -230,70 +185,72 @@ def run_collection_app(self):
if hand_landmarks:
for hand_landmarks in hand_landmarks:
self.mp_drawing.draw_landmarks(
- display_frame, hand_landmarks, self.mp_hands.HAND_CONNECTIONS)
+ frame, hand_landmarks, self.mp_hands.HAND_CONNECTIONS)
- # 在图像上添加中文文本
- try:
- # 第一行:当前手势
- if collecting_for:
- status_text = f"当前手势: {collecting_for}"
- else:
- status_text = "当前手势: 无"
- display_frame = self.put_chinese_text(display_frame, status_text, (10, 30), (0, 255, 0))
-
- # 第二行:当前手势收集样本数
- display_frame = self.put_chinese_text(display_frame, f"当前收集样本数: {sample_count}",
- (10, 70), (0, 255, 0))
+ # 将OpenCV图像转换为PIL图像以支持中文
+ pil_img = Image.fromarray(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB))
+ draw = ImageDraw.Draw(pil_img)
- # 第三行:总样本数
- display_frame = self.put_chinese_text(display_frame, f"总样本数: {len(self.collected_data)}",
- (10, 110), (0, 255, 0))
+ try:
+ # 尝试加载中文字体(Windows系统路径)
+ font_path = "C:/Windows/Fonts/msyh.ttc" # 微软雅黑
+ font = ImageFont.truetype(font_path, 30)
+ except:
+ # 如果找不到字体,使用默认字体
+ font = ImageFont.load_default()
+ print("使用默认字体,中文可能显示为方框")
+
+ # 显示中文
+ if collecting_for:
+ status_text = f"当前手势: {collecting_for}"
+ color = (0, 255, 0)
+ else:
+ status_text = "请选择手势 (1-8)"
+ color = (0, 255, 255)
- # 第四行:操作提示
- display_frame = self.put_chinese_text(display_frame, "按1-9选择手势,c收集样本", (10, 150),
- (255, 255, 255))
+ # 绘制文字
+ draw.text((10, 10), status_text, font=font, fill=color)
+ draw.text((10, 50), f"已收集样本: {sample_count}", font=font, fill=(0, 255, 0))
+ draw.text((10, 90), "按1-8选择手势,c收集样本", font=font, fill=(255, 255, 255))
- # 第五行:操作提示
- display_frame = self.put_chinese_text(display_frame, "按w保存,q退出", (10, 190),
- (255, 255, 255))
+ # 显示手势类别提示
+ y_pos = 130
+ for i, gesture in enumerate(self.gesture_classes):
+ text = f"{i + 1}: {gesture}"
+ draw.text((10, y_pos), text, font=font, fill=(200, 200, 0))
+ y_pos += 35
- except Exception as e:
- print(f"绘制文字时出错: {e}")
- # 如果无法绘制中文,使用英文
- status_text = f"Gesture: {collecting_for if collecting_for else 'None'}"
- cv2.putText(display_frame, status_text, (10, 30),
- cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
+ # 将PIL图像转回OpenCV格式
+ frame = cv2.cvtColor(np.array(pil_img), cv2.COLOR_RGB2BGR)
- cv2.imshow('手势数据收集', display_frame)
+ cv2.imshow('手势数据收集', frame)
key = cv2.waitKey(1) & 0xFF
# 处理按键
if key == ord('q'):
break
- elif ord('1') <= key <= ord('9'):
+ elif ord('1') <= key <= ord('8'): # 只有1-8
gesture_idx = key - ord('1')
if gesture_idx < len(self.gesture_classes):
collecting_for = self.gesture_classes[gesture_idx]
- sample_count = 0
print(f"开始收集手势: {collecting_for}")
elif key == ord('c') and collecting_for:
if landmarks:
- self.collect_sample(collecting_for, frame, landmarks)
- sample_count += 1
- print(f"已收集 {collecting_for} 样本 #{sample_count}")
+ result = self.collect_sample(collecting_for, frame, landmarks)
+ if result: # 只有成功收集才计数
+ sample_count += 1
+ print(f"已收集 {collecting_for} 样本 #{sample_count}")
else:
- print("未检测到手部!")
+ print("未检测到手部!请确保手在摄像头中清晰可见")
elif key == ord('s'):
collecting_for = None
- sample_count = 0
print("停止收集")
elif key == ord('w'):
- if self.collected_data:
- dataset = self.save_dataset()
- print(f"数据集已保存,总样本数: {dataset['num_samples']}")
- else:
- print("没有数据可保存")
+ dataset = self.save_dataset()
+ if dataset:
+ sample_count = dataset['num_samples']
+ print(f"数据集已保存,总样本数: {sample_count}")
cap.release()
cv2.destroyAllWindows()
diff --git a/src/Drone_hand_gesture_project/gesture_detector_enhanced.py b/src/Drone_hand_gesture_project/gesture_detector_enhanced.py
new file mode 100644
index 0000000000..c6c734c53c
--- /dev/null
+++ b/src/Drone_hand_gesture_project/gesture_detector_enhanced.py
@@ -0,0 +1,240 @@
+import cv2
+import os
+import numpy as np
+import mediapipe as mp
+from gesture_classifier import GestureClassifier, EnsembleGestureClassifier
+import PIL
+
+try:
+ from PIL import Image, ImageDraw, ImageFont
+ import numpy as np
+ HAS_PIL = True
+except ImportError:
+ HAS_PIL = False
+ print("⚠️ PIL未安装,中文显示不可用")
+
+class EnhancedGestureDetector:
+ """手势检测器(集成机器学习)"""
+
+ def __init__(self, ml_model_path=None, use_ml=True):
+ # 基础检测器
+ self.mp_hands = mp.solutions.hands
+ self.mp_drawing = mp.solutions.drawing_utils
+ self.mp_drawing_styles = mp.solutions.drawing_styles
+
+ self.hands = self.mp_hands.Hands(
+ static_image_mode=False,
+ max_num_hands=1,
+ min_detection_confidence=0.5,
+ min_tracking_confidence=0.5
+ )
+
+ # 机器学习模型
+ self.use_ml = use_ml
+ self.ml_classifier = None
+
+ if use_ml and ml_model_path:
+ # 检查是否是 ensemble 信息文件(很小的文件)
+ if os.path.exists(ml_model_path):
+ file_size = os.path.getsize(ml_model_path)
+ if file_size < 1024: # 小于1KB,可能是信息文件
+ print(f"⚠️ 检测到小文件 ({file_size}字节),可能是集成模型信息文件")
+ print(f" 尝试加载单个模型文件...")
+
+ # 尝试加载对应的单个模型
+ base_name = os.path.basename(ml_model_path)
+ if "ensemble" in base_name:
+ # 尝试加载 svm 模型
+ svm_path = ml_model_path.replace("ensemble", "svm")
+ if os.path.exists(svm_path):
+ print(f" 尝试加载: {svm_path}")
+ success = self.load_ml_model(svm_path)
+ else:
+ print(f" 未找到替代模型,使用规则检测")
+ self.use_ml = False
+ else:
+ # 正常加载
+ success = self.load_ml_model(ml_model_path)
+ else:
+ self.use_ml = False
+
+ # 手势到控制指令的映射(8个手势)
+ self.gesture_commands = {
+ "open_palm": "takeoff",
+ "closed_fist": "land",
+ "pointing_up": "up",
+ "pointing_down": "down",
+ "victory": "forward",
+ "thumb_up": "backward",
+ "thumb_down": "stop",
+ "ok_sign": "hover",
+ }
+
+ # 中文手势名称映射
+ self.chinese_gesture_names = {
+ "open_palm": "张开手掌",
+ "closed_fist": "握拳",
+ "victory": "胜利手势",
+ "thumb_up": "大拇指",
+ "thumb_down": "大拇指向下",
+ "pointing_up": "食指上指",
+ "pointing_down": "食指向下",
+ "ok_sign": "OK手势",
+ "none": "未检测到手"
+ }
+
+ # 中文指令名称映射
+ self.chinese_command_names = {
+ "takeoff": "起飞",
+ "land": "降落",
+ "up": "上升",
+ "down": "下降",
+ "forward": "前进",
+ "backward": "后退",
+ "stop": "停止",
+ "hover": "悬停",
+ "none": "无指令"
+ }
+
+ # 中文字体
+ self.chinese_font = None
+ if HAS_PIL:
+ try:
+ # Windows字体路径
+ font_paths = [
+ "C:/Windows/Fonts/msyh.ttc", # 微软雅黑
+ "C:/Windows/Fonts/simhei.ttf", # 黑体
+ "C:/Windows/Fonts/simsun.ttc", # 宋体
+ ]
+
+ for font_path in font_paths:
+ try:
+ if os.path.exists(font_path):
+ self.chinese_font = ImageFont.truetype(font_path, 30)
+ print(f"✅ 加载中文字体: {os.path.basename(font_path)}")
+ break
+ except:
+ continue
+
+ if self.chinese_font is None:
+ print("⚠️ 无法加载系统字体,将使用英文显示")
+ except Exception as e:
+ print(f"⚠️ 字体初始化失败: {e}")
+
+
+ # 历史记录(用于平滑预测)
+ self.prediction_history = []
+ self.max_history = 5
+
+ def load_ml_model(self, model_path):
+ """加载机器学习模型"""
+ try:
+ self.ml_classifier = GestureClassifier(model_path=model_path)
+ print(f"机器学习模型已加载: {model_path}")
+ return True
+ except Exception as e:
+ print(f"加载机器学习模型失败: {e}")
+ print("将使用规则检测器")
+ self.use_ml = False
+ return False
+
+ def extract_landmarks_for_ml(self, hand_landmarks):
+ """提取关键点用于机器学习"""
+ landmarks = []
+ for landmark in hand_landmarks.landmark:
+ landmarks.extend([landmark.x, landmark.y, landmark.z])
+
+ # 确保是63维
+ if len(landmarks) < 63:
+ landmarks.extend([0.0] * (63 - len(landmarks)))
+ elif len(landmarks) > 63:
+ landmarks = landmarks[:63]
+
+ return landmarks
+
+ def detect_gestures(self, image, simulation_mode=False):
+ """检测手势"""
+ image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+ results = self.hands.process(image_rgb)
+
+ gesture = "none" # 默认none
+ confidence = 0.0
+ landmarks_data = None
+
+ if results.multi_hand_landmarks:
+ for hand_landmarks in results.multi_hand_landmarks:
+ # 绘制手部关键点
+ self.mp_drawing.draw_landmarks(
+ image,
+ hand_landmarks,
+ self.mp_hands.HAND_CONNECTIONS,
+ self.mp_drawing_styles.get_default_hand_landmarks_style(),
+ self.mp_drawing_styles.get_default_hand_connections_style()
+ )
+
+ # 提取关键点
+ landmarks = self.extract_landmarks_for_ml(hand_landmarks)
+
+ # 使用机器学习或规则检测
+ if self.use_ml and self.ml_classifier and len(landmarks) == 63:
+ # 机器学习预测
+ gesture, confidence = self.ml_classifier.predict(landmarks)
+
+ # 应用平滑滤波
+ self.prediction_history.append((gesture, confidence))
+ if len(self.prediction_history) > self.max_history:
+ self.prediction_history.pop(0)
+
+ # 使用历史中的多数投票
+ if len(self.prediction_history) >= 3:
+ from collections import Counter
+ recent_gestures = [g for g, _ in self.prediction_history[-3:]]
+ most_common = Counter(recent_gestures).most_common(1)[0]
+ if most_common[1] >= 2: # 至少2/3一致
+ gesture = most_common[0]
+ # 计算平均置信度
+ matching_confs = [c for g, c in self.prediction_history if g == gesture]
+ if matching_confs:
+ confidence = np.mean(matching_confs)
+ else:
+ # 如果未使用ML,使用简单的规则检测
+ gesture, confidence = self._classify_by_rules(hand_landmarks)
+
+ # 显示信息
+ ml_info = " (ML)" if self.use_ml else " (规则)"
+ cv2.putText(image, f"Gesture: {gesture}{ml_info}", (10, 30),
+ cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
+ cv2.putText(image, f"Confidence: {confidence:.2f}", (10, 70),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
+
+ command = self.gesture_commands.get(gesture, "none")
+ cv2.putText(image, f"Command: {command}", (10, 110),
+ cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2)
+
+ else:
+ # 无手部检测
+ cv2.putText(image, "未检测到手部", (10, 30),
+ cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
+ gesture = "none"
+ confidence = 0.0
+
+ return image, gesture, confidence, landmarks_data
+
+ def _classify_by_rules(self, hand_landmarks):
+ """简单的规则分类器(备用)"""
+ # 获取关键点
+ landmarks = hand_landmarks.landmark
+
+ # 这里实现简单的规则检测逻辑
+ # 例如:计算手指是否伸直等
+
+ # 暂时返回一个默认值
+ return "open_palm", 0.5
+
+ def get_command(self, gesture):
+ """获取控制指令"""
+ return self.gesture_commands.get(gesture, "none")
+
+ def release(self):
+ """释放资源"""
+ self.hands.close()
\ No newline at end of file
diff --git a/src/Drone_hand_gesture_project/main.py b/src/Drone_hand_gesture_project/main.py
index f42ab48f6e..aad098e0e7 100644
--- a/src/Drone_hand_gesture_project/main.py
+++ b/src/Drone_hand_gesture_project/main.py
@@ -1,8 +1,3 @@
-#!/usr/bin/env python3
-"""
-手势控制无人机仿真系统 - 主程序(取消自动起飞版本)
-集成:手势识别 + 无人机控制 + 3D仿真
-"""
import cv2
import numpy as np
import time
@@ -15,7 +10,15 @@
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
# 导入自定义模块
-from gesture_detector import GestureDetector
+try:
+ from gesture_detector_enhanced import EnhancedGestureDetector
+ print("✅ 导入增强版手势检测器 (机器学习)")
+ HAS_ENHANCED_DETECTOR = True
+except ImportError:
+ print("⚠️ 未找到增强版检测器,使用原始手势检测器")
+ from gesture_detector import GestureDetector
+ HAS_ENHANCED_DETECTOR = False
+
from drone_controller import DroneController
from simulation_3d import Drone3DViewer
@@ -42,7 +45,61 @@ def __init__(self, config=None):
# 初始化模块
print("正在初始化手势检测器...")
- self.gesture_detector = GestureDetector()
+
+ # 检查可用的模型文件(按优先级排序)
+ model_candidates = [
+ ("dataset/models/gesture_svm.pkl", "SVM模型"),
+ ("dataset/models/gesture_random_forest.pkl", "随机森林模型"),
+ ("dataset/models/gesture_mlp.pkl", "神经网络模型"),
+ ]
+
+ selected_model = None
+ selected_model_name = None
+
+ for model_path, model_name in model_candidates:
+ if os.path.exists(model_path):
+ file_size = os.path.getsize(model_path)
+ print(f"📁 找到 {model_name}: {file_size / 1024:.1f} KB")
+
+ # 检查文件大小是否合理
+ if file_size > 10 * 1024:
+ selected_model = model_path
+ selected_model_name = model_name
+ print(f"✅ 选择: {model_name}")
+ break
+
+ if selected_model:
+ print(f"🎯 使用模型: {selected_model_name}")
+
+ try:
+ from gesture_detector_enhanced import EnhancedGestureDetector
+ print("✅ 导入增强版手势检测器")
+
+ # 使用实际的模型文件
+ self.gesture_detector = EnhancedGestureDetector(
+ ml_model_path=selected_model,
+ use_ml=True
+ )
+
+ # 验证模型是否真正加载成功
+ if hasattr(self.gesture_detector, 'ml_classifier') and self.gesture_detector.ml_classifier:
+ print(f"✅ 机器学习模型加载成功 ({selected_model_name})")
+ print(f" 可识别手势: {self.gesture_detector.ml_classifier.gesture_classes}")
+ else:
+ print("⚠️ 机器学习模型未加载,回退到规则检测")
+ self.gesture_detector = EnhancedGestureDetector(use_ml=False)
+
+ except ImportError as e:
+ print(f"⚠️ 无法导入增强版检测器: {e}")
+ print("✅ 使用原始手势检测器")
+ from gesture_detector import GestureDetector
+ self.gesture_detector = GestureDetector()
+
+ else:
+ print("⚠️ 未找到可用的机器学习模型文件")
+ print("✅ 使用原始手势检测器")
+ from gesture_detector import GestureDetector
+ self.gesture_detector = GestureDetector()
print("正在初始化无人机控制器...")
self.drone_controller = DroneController(simulation_mode=True)
@@ -79,16 +136,23 @@ def __init__(self, config=None):
self.command_cooldown = 1.5 # 命令冷却时间(秒),从2.0降低到1.5
# 手势识别阈值(降低以提高灵敏度)
+ # 如果是机器学习模式,阈值可以进一步降低
+ if HAS_ENHANCED_DETECTOR and hasattr(self.gesture_detector, 'use_ml') and self.gesture_detector.use_ml:
+ print("✅ 使用机器学习模式,置信度阈值更低")
+ base_threshold = 0.55 # 机器学习可以更低
+ else:
+ base_threshold = 0.6 # 规则检测需要高一点
+
self.gesture_thresholds = {
- 'open_palm': 0.6, # 降低到0.6
- 'closed_fist': 0.65, # 降低到0.65
- 'victory': 0.65, # 降低到0.65
- 'thumb_up': 0.65, # 降低到0.65
- 'thumb_down': 0.65, # 降低到0.65
- 'pointing_up': 0.6, # 降低到0.6
- 'pointing_down': 0.6, # 降低到0.6
- 'ok_sign': 0.7, # 稍微降低
- 'default': 0.6 # 默认阈值
+ 'open_palm': base_threshold,
+ 'closed_fist': base_threshold + 0.05,
+ 'victory': base_threshold + 0.05,
+ 'thumb_up': base_threshold + 0.05,
+ 'thumb_down': base_threshold + 0.05,
+ 'pointing_up': base_threshold,
+ 'pointing_down': base_threshold,
+ 'ok_sign': base_threshold + 0.1,
+ 'default': base_threshold
}
# 初始化摄像头
@@ -98,11 +162,16 @@ def __init__(self, config=None):
self.data_log = []
self.log_file = "flight_log.json"
- # 不再自动起飞 - 完全由手势控制
print("无人机初始化完成,等待手势指令...")
print("无人机仿真系统初始化完成 ✓")
+ if HAS_ENHANCED_DETECTOR and hasattr(self.gesture_detector, 'use_ml'):
+ if self.gesture_detector.use_ml:
+ print("📊 当前模式: 机器学习手势识别")
+ else:
+ print("📊 当前模式: 规则手势识别")
+
def _initialize_camera(self):
"""初始化摄像头"""
# 尝试多个摄像头ID,优先使用1,如果失败则尝试0
@@ -141,6 +210,15 @@ def _gesture_recognition_loop(self):
"""手势识别循环"""
print("手势识别线程启动...")
+ # 显示当前检测模式
+ if HAS_ENHANCED_DETECTOR and hasattr(self.gesture_detector, 'use_ml'):
+ if self.gesture_detector.use_ml:
+ mode_text = "机器学习模式"
+ else:
+ mode_text = "规则检测模式"
+ else:
+ mode_text = "规则检测模式"
+
# 显示虚拟模式提示(如果摄像头未连接)
if self.cap is None:
print("⚠️ 使用虚拟摄像头模式,请连接摄像头进行真实手势识别")
@@ -160,25 +238,27 @@ def _gesture_recognition_loop(self):
frame = np.ones((480, 640, 3), dtype=np.uint8) * 255
cv2.putText(frame, "Camera Error - Virtual Mode", (50, 50),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
- cv2.putText(frame, "Connect camera for real gesture detection", (50, 100),
+ cv2.putText(frame, f"Connect camera for real gesture detection", (50, 100),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 0), 1)
+ cv2.putText(frame, f"Mode: {mode_text}", (50, 140),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 100, 0), 1)
else:
# 虚拟模式
frame = np.ones((480, 640, 3), dtype=np.uint8) * 255
cv2.putText(frame, "Virtual Camera Mode", (50, 50),
- cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
- cv2.putText(frame, "Gesture Commands:", (50, 100),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 100, 0), 2)
+ cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
+ cv2.putText(frame, f"Gesture Commands ({mode_text}):", (50, 100),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 100, 0), 2)
cv2.putText(frame, "Open Palm - Takeoff", (50, 140),
- cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
cv2.putText(frame, "Closed Fist - Land", (50, 170),
- cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
cv2.putText(frame, "Victory - Forward", (50, 200),
- cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
cv2.putText(frame, "Thumb Up - Backward", (50, 230),
- cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1)
cv2.putText(frame, "Press 'q' to quit", (50, 280),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 0), 2)
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 0), 2)
# 手势检测
try:
@@ -195,14 +275,14 @@ def _gesture_recognition_loop(self):
self._process_gesture_command(gesture, confidence)
# 显示手势识别窗口
- cv2.imshow('手势控制 - Gesture Control', processed_frame)
+ cv2.imshow('Gesture Control', processed_frame)
except Exception as e:
print(f"手势检测错误: {e}")
self.current_frame = frame
self.current_gesture = None
- # 检查退出
+ # 检查退出
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
print("收到退出指令...")
@@ -213,9 +293,48 @@ def _gesture_recognition_loop(self):
self._switch_camera()
elif key == ord('d'): # 调试模式
self._debug_gesture_detection()
+ elif key == ord('m'): # 切换模式(如果有多个模型)
+ self._switch_detection_mode()
print("手势识别线程结束")
+ def _switch_detection_mode(self):
+ """切换检测模式(如果有多个可用模型)"""
+ if not HAS_ENHANCED_DETECTOR:
+ print("当前只有规则检测器可用")
+ return
+
+ # 检查可用的模型
+ model_files = [
+ ("dataset/models/gesture_ensemble.pkl", "集成模型"),
+ ("dataset/models/gesture_svm.pkl", "SVM模型"),
+ ("dataset/models/gesture_random_forest.pkl", "随机森林模型"),
+ ("dataset/models/gesture_mlp.pkl", "神经网络模型"),
+ ]
+
+ available_models = []
+ for path, name in model_files:
+ if os.path.exists(path):
+ available_models.append((path, name))
+
+ if len(available_models) == 0:
+ print("未找到任何机器学习模型")
+ return
+ elif len(available_models) == 1:
+ print(f"只有 {available_models[0][1]} 可用")
+ return
+
+ # 显示可用模型
+ print("\n可用的手势识别模型:")
+ for i, (path, name) in enumerate(available_models, 1):
+ print(f" {i}. {name}")
+
+ print("按数字键选择模型,或按其他键取消")
+
+ # 这里简化处理,实际需要更复杂的交互
+ # 暂时只记录一下
+ print("注意: 需要重启程序切换模型")
+
def _switch_camera(self):
"""切换摄像头"""
if self.cap:
@@ -464,8 +583,20 @@ def _save_log(self):
def run(self):
"""运行主程序"""
print("=" * 60)
- print(" 手势控制无人机仿真系统(完全手势控制版)")
+ print(" 手势控制无人机仿真系统(机器学习增强版)")
print("=" * 60)
+
+ # 显示当前检测模式
+ if HAS_ENHANCED_DETECTOR and hasattr(self.gesture_detector, 'use_ml'):
+ if self.gesture_detector.use_ml:
+ mode_info = "机器学习模式 (更高精度)"
+ else:
+ mode_info = "规则检测模式 (基础)"
+ else:
+ mode_info = "规则检测模式"
+
+ print(f"检测模式: {mode_info}")
+
print("系统功能:")
print(" 1. 实时手势识别 (8种手势)")
print(" 2. 无人机控制仿真")
@@ -561,7 +692,7 @@ def load_config():
if __name__ == "__main__":
- print("手势控制无人机仿真系统 - 启动(完全手势控制版)")
+ print("手势控制无人机仿真系统")
print("=" * 60)
# 检查必要的模块
diff --git a/src/Drone_hand_gesture_project/train_gesture_model.py b/src/Drone_hand_gesture_project/train_gesture_model.py
new file mode 100644
index 0000000000..c0f4be82c2
--- /dev/null
+++ b/src/Drone_hand_gesture_project/train_gesture_model.py
@@ -0,0 +1,78 @@
+import argparse
+import os
+from gesture_classifier import GestureClassifier, EnsembleGestureClassifier
+import numpy as np
+
+
+def main():
+ parser = argparse.ArgumentParser(description='训练手势识别模型')
+ parser.add_argument('--dataset', type=str, default='dataset/processed/gesture_dataset.pkl',
+ help='数据集路径')
+ parser.add_argument('--model_type', type=str, default='ensemble',
+ choices=['svm', 'random_forest', 'mlp', 'ensemble'],
+ help='模型类型')
+ parser.add_argument('--output_dir', type=str, default='dataset/models',
+ help='输出目录')
+ parser.add_argument('--test_size', type=float, default=0.2,
+ help='测试集比例')
+ parser.add_argument('--collect_data', action='store_true',
+ help='是否先收集数据')
+
+ args = parser.parse_args()
+
+ # 创建输出目录
+ os.makedirs(args.output_dir, exist_ok=True)
+
+ # 如果需要收集数据
+ if args.collect_data:
+ from gesture_data_collector import GestureDataCollector
+ print("启动数据收集...")
+ collector = GestureDataCollector()
+ collector.run_collection_app()
+
+ if args.model_type == 'ensemble':
+ # 训练多个模型并集成
+ models = []
+ model_types = ['svm', 'random_forest', 'mlp']
+
+ for model_type in model_types:
+ print(f"\n训练 {model_type} 模型...")
+ classifier = GestureClassifier(model_type=model_type)
+
+ model_path = os.path.join(args.output_dir, f'gesture_{model_type}.pkl')
+ accuracy = classifier.train(args.dataset, args.test_size, model_path)
+
+ models.append(model_path)
+ print(f"{model_type} 准确率: {accuracy:.4f}")
+
+ # 创建集成模型
+ print("\n创建集成模型...")
+ ensemble = EnsembleGestureClassifier(models)
+
+ # 保存集成模型信息
+ ensemble_info = {
+ 'model_paths': models,
+ 'model_types': model_types
+ }
+
+ import joblib
+ ensemble_path = os.path.join(args.output_dir, 'gesture_ensemble.pkl')
+ joblib.dump(ensemble_info, ensemble_path)
+ print(f"集成模型信息已保存到: {ensemble_path}")
+
+ else:
+ # 训练单个模型
+ print(f"训练 {args.model_type} 模型...")
+ classifier = GestureClassifier(model_type=args.model_type)
+
+ model_path = os.path.join(args.output_dir, f'gesture_{args.model_type}.pkl')
+ accuracy = classifier.train(args.dataset, args.test_size, model_path)
+
+ print(f"\n训练完成!")
+ print(f"模型类型: {args.model_type}")
+ print(f"准确率: {accuracy:.4f}")
+ print(f"模型已保存到: {model_path}")
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/Embodied_Human_Arm_Modeling/arm_model.xml b/src/Embodied_Human_Arm_Modeling/arm_model.xml
new file mode 100644
index 0000000000..3fa35a4a42
--- /dev/null
+++ b/src/Embodied_Human_Arm_Modeling/arm_model.xml
@@ -0,0 +1,43 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/Embodied_Human_Arm_Modeling/simulate_arm.py b/src/Embodied_Human_Arm_Modeling/simulate_arm.py
new file mode 100644
index 0000000000..b907272946
--- /dev/null
+++ b/src/Embodied_Human_Arm_Modeling/simulate_arm.py
@@ -0,0 +1,59 @@
+import os
+import sys
+os.environ['MUJOCO_GL'] = 'egl'
+# 关键设置:获取当前脚本文件的绝对目录,并设置为工作目录
+SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
+os.chdir(SCRIPT_DIR)
+# simulate_arm.py
+import mujoco
+import mujoco.viewer
+import numpy as np
+import time
+
+def main():
+ # 1. 加载模型
+ model = mujoco.MjModel.from_xml_path('arm_model.xml')
+ data = mujoco.MjData(model)
+
+ # 2. 设置初始关节位置(例如,让肘部稍微弯曲)
+ data.qpos[model.joint('elbow').id] = -np.radians(45) # -45度
+
+ # 3. 使用 viewer 进行交互式仿真
+ print("正在启动仿真查看器...")
+ print("提示:")
+ print(" - 在查看器窗口按 [Space] 暂停/继续物理仿真。")
+ print(" - 在左侧‘关节’面板,可以拖拽滑块实时驱动电机,观察手臂运动。")
+
+ with mujoco.viewer.launch(model, data) as viewer:
+ # 设置一个简单的摆动目标
+ target_positions = np.array([0.3, 0.1, -0.5, 0.0]) # 对应四个电机的目标位置
+ time_per_cycle = 5.0 # 摆动周期(秒)
+ start_time = time.time()
+
+ # 保持查看器窗口开启
+ while viewer.is_running():
+ step_start = time.time()
+
+ # 计算基于时间的周期性目标
+ t = time.time() - start_time
+ phase = 2 * np.pi * t / time_per_cycle
+ current_target = target_positions * np.sin(phase)
+
+ # 将目标位置设置为电机的控制信号(简单的位置控制)
+ data.ctrl[:] = current_target
+
+ # 执行一步物理仿真
+ mujoco.mj_step(model, data)
+
+ # 同步查看器
+ viewer.sync()
+
+ # 粗略的时间同步,避免占用过多CPU
+ time_until_next_step = model.opt.timestep - (time.time() - step_start)
+ if time_until_next_step > 0:
+ time.sleep(min(time_until_next_step, model.opt.timestep))
+
+ print("仿真结束。")
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/HCL_interaction_task/mobl_arms_index_pointing/README.md b/src/HCL_interaction_task/README.md
similarity index 96%
rename from src/HCL_interaction_task/mobl_arms_index_pointing/README.md
rename to src/HCL_interaction_task/README.md
index b0d81733fa..2eef56715f 100644
--- a/src/HCL_interaction_task/mobl_arms_index_pointing/README.md
+++ b/src/HCL_interaction_task/README.md
@@ -85,8 +85,9 @@ pip install -e .
### 4. 模型文件准备
-完整模型文件集(包含40+手部精细模型文件)可通过以下链接下载:
-[完整模型文件集网盘链接](https://pan.baidu.com/s/1SN5SWpyfKR7KYDbE8lzvBw?pwd=9e9u)
+完整模型文件集(精细模型stl文件和图片)可通过以下链接下载:
+[完整模型文件集网盘链接](通过网盘分享的文件:
+链接: https://pan.baidu.com/s/1sA0BgEPRgxXTqe6ZdEm7Sg?pwd=rq8e 提取码: rq8e)
下载后解压到 `mujoco_ros_demo/config/assets/` 目录
diff --git a/src/HCL_interaction_task/mobl_arms_index_choice_reaction/config.yaml b/src/HCL_interaction_task/mobl_arms_index_choice_reaction/config.yaml
new file mode 100644
index 0000000000..74b0cdcb77
--- /dev/null
+++ b/src/HCL_interaction_task/mobl_arms_index_choice_reaction/config.yaml
@@ -0,0 +1,2 @@
+dt: 0.01 # 仿真步长(调慢一点,看得清)
+render_mode: "human" # 开启可视化!
\ No newline at end of file
diff --git a/src/HCL_interaction_task/mobl_arms_index_choice_reaction/simulation.xml b/src/HCL_interaction_task/mobl_arms_index_choice_reaction/simulation.xml
new file mode 100644
index 0000000000..8f8f98e537
--- /dev/null
+++ b/src/HCL_interaction_task/mobl_arms_index_choice_reaction/simulation.xml
@@ -0,0 +1,456 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/InterconVis_PedSafety_Enh/check_data.py b/src/InterconVis_PedSafety_Enh/check_data.py
new file mode 100644
index 0000000000..f0c6416a8c
--- /dev/null
+++ b/src/InterconVis_PedSafety_Enh/check_data.py
@@ -0,0 +1,151 @@
+import cv2
+import json
+import numpy as np
+import os
+import glob
+import cvips_utils as utils
+
+# ================= 配置 =================
+# 这里的路径要和你 collector 生成的保持一致
+# 比如你刚跑了 Town05_rainy_night,这里就改对应的文件夹名
+DATASET_ROOT = "_out_dataset_final"
+SCENE_NAME = "Town05_rainy_night" # 如果你跑的是默认参数,可能是 Town01_clear_day
+# =======================================
+
+def draw_3d_box(img, target, w2c, K):
+ """
+ 核心绘画函数:将 3D 目标画在 2D 图上
+ """
+ # 1. 解析目标参数
+ loc = target['location'] # [x, y, z]
+ rot = target['rotation'] # [pitch, yaw, roll]
+ extent = target['extent'] # [ex, ey, ez]
+ offset = target['center_offset'] # [ox, oy, oz]
+
+ # 2. 恢复目标的位姿矩阵 (Local -> World)
+ # 注意:这里我们临时借用 carla 的 transform 对象来算矩阵,
+ # 如果不想依赖 carla 库,也可以手动写旋转矩阵公式,但用 carla 最方便
+ import carla
+ t_loc = carla.Location(x=loc[0], y=loc[1], z=loc[2])
+ t_rot = carla.Rotation(pitch=rot[0], yaw=rot[1], roll=rot[2])
+ obj_transform = carla.Transform(t_loc, t_rot)
+ obj_matrix = utils.get_matrix(obj_transform)
+
+ # 3. 定义包围盒的 8 个顶点 (相对于物体中心)
+ # CARLA extent 是半长,所以坐标是 +/- extent
+ dx, dy, dz = extent[0], extent[1], extent[2]
+
+ # 8个顶点的局部坐标 (x, y, z, 1)
+ corners_local = np.array([
+ [dx, dy, dz, 1], [dx, -dy, dz, 1], [dx, -dy, -dz, 1], [dx, dy, -dz, 1], # 前面4个点
+ [-dx, dy, dz, 1], [-dx, -dy, dz, 1], [-dx, -dy, -dz, 1], [-dx, dy, -dz, 1] # 后面4个点
+ ]).T # 转置成 4x8
+
+ # 加上中心偏移量 (如果有的话)
+ corners_local[0, :] += offset[0]
+ corners_local[1, :] += offset[1]
+ corners_local[2, :] += offset[2]
+
+ # 4. 坐标变换流水线
+ # 4.1 Local -> World
+ corners_world = np.dot(obj_matrix, corners_local) # 4x8
+
+ # 4.2 World -> Pixel
+ img_points = []
+ for i in range(8):
+ # 取出单个点的世界坐标
+ p_world_vec = corners_world[:, i]
+ # 使用 utils 投影到像素
+ # 注意 utils.get_image_point 需要 carla.Location 类型,我们手动构造一下
+ p_loc = carla.Location(x=p_world_vec[0], y=p_world_vec[1], z=p_world_vec[2])
+
+ pixel = utils.get_image_point(p_loc, K, w2c)
+ img_points.append(pixel)
+
+ # 5. 连线绘画 (12条棱边)
+ # 定义连接关系
+ edges = [
+ (0, 1), (1, 2), (2, 3), (3, 0), # 前面
+ (4, 5), (5, 6), (6, 7), (7, 4), # 后面
+ (0, 4), (1, 5), (2, 6), (3, 7) # 前后连接
+ ]
+
+ color = (0, 255, 0) # 绿色代表车辆
+ if target['type'] == 'walker':
+ color = (0, 0, 255) # 红色代表行人
+
+ for p1_idx, p2_idx in edges:
+ p1 = tuple(img_points[p1_idx])
+ p2 = tuple(img_points[p2_idx])
+ # 画线
+ cv2.line(img, p1, p2, color, 2)
+
+ return img
+
+def main():
+ base_path = os.path.join(DATASET_ROOT, SCENE_NAME)
+ label_dir = os.path.join(base_path, "label")
+
+ if not os.path.exists(label_dir):
+ print(f"❌ 错误: 找不到路径 {label_dir}")
+ print("请先运行 collector 脚本生成数据,或者检查 check_data.py 里的 SCENE_NAME 配置。")
+ return
+
+ # 获取所有 json 文件并排序
+ json_files = sorted(glob.glob(os.path.join(label_dir, "*.json")))
+ print(f"📂 发现 {len(json_files)} 帧数据,开始回放验证...")
+ print("⌨️ 按任意键下一帧,按 'q' 退出")
+
+ for j_path in json_files:
+ with open(j_path, 'r') as f:
+ label_data = json.load(f)
+
+ fid = label_data['frame_id']
+
+ # 读取图片
+ ego_path = os.path.join(base_path, "ego_rgb", f"{fid:08d}.jpg")
+ rsu_path = os.path.join(base_path, "rsu_rgb", f"{fid:08d}.jpg")
+
+ if not os.path.exists(ego_path) or not os.path.exists(rsu_path):
+ print(f"跳过缺失图片的帧: {fid}")
+ continue
+
+ img_ego = cv2.imread(ego_path)
+ img_rsu = cv2.imread(rsu_path)
+
+ # 获取矩阵参数
+ # 注意: JSON 里存的是 list,转回 numpy array
+ ego_w2c = np.array(label_data['matrices']['ego_w2c'])
+ rsu_w2c = np.array(label_data['matrices']['rsu_w2c'])
+
+ # 内参 (从 JSON 读取或者用 utils 重新生成都可以,这里用 utils 生成)
+ h, w = img_ego.shape[:2]
+ K = utils.build_projection_matrix(w, h, label_data['camera_params']['fov'])
+
+ # 遍历所有目标并画框
+ targets = label_data['targets']
+ for tgt in targets:
+ # 在主车视角画
+ img_ego = draw_3d_box(img_ego, tgt, ego_w2c, K)
+ # 在路侧视角画
+ img_rsu = draw_3d_box(img_rsu, tgt, rsu_w2c, K)
+
+ # 拼接显示
+ # 缩小一点方便看
+ img_ego_s = cv2.resize(img_ego, (960, 540))
+ img_rsu_s = cv2.resize(img_rsu, (960, 540))
+
+ # 上下拼接
+ combined = np.vstack((img_ego_s, img_rsu_s))
+
+ cv2.imshow(f"Validation - {SCENE_NAME}", combined)
+
+ # 按键控制
+ key = cv2.waitKey(0) # 0 表示无限等待,按键才继续;改成 30 可以自动播放
+ if key == ord('q'):
+ break
+
+ cv2.destroyAllWindows()
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/InterconVis_PedSafety_Enh/cvips_collector.py b/src/InterconVis_PedSafety_Enh/cvips_collector.py
index 6e3372d150..7b18ed01b0 100644
--- a/src/InterconVis_PedSafety_Enh/cvips_collector.py
+++ b/src/InterconVis_PedSafety_Enh/cvips_collector.py
@@ -1,3 +1,5 @@
+# --- START OF FILE cvips_collector_final.py ---
+
import carla
import random
import queue
@@ -5,236 +7,394 @@
import argparse
import time
import threading
+import json
+import numpy as np
+import sys
+import cvips_utils as utils # 必须确保同目录下有 cvips_utils.py
# ================= 配置区域 =================
-# 输出文件夹名称
-OUTPUT_FOLDER = "_out_cvips_final"
-
-# 是否开启保存 (调试视角时可设为 False)
-ENABLE_SAVING = True
+OUTPUT_FOLDER = "_out_dataset_final" # 修改输出目录名以示区别
+ENABLE_SAVING = True
+SAVE_INTERVAL = 10 # 每 10 帧存一次
+TARGET_FPS = 30
+FIXED_DELTA_SECONDS = 1.0 / TARGET_FPS
-# 采集间隔:每 15 帧保存一次 (平衡 1080P 的存储压力)
-SAVE_INTERVAL = 15
+# 相机参数 (必须与 spawn_actor 时设置的一致)
+IMAGE_W = 1920
+IMAGE_H = 1080
+FOV = 90.0
# ===========================================
-# 全局标志位,控制后台线程何时停止
+# 全局控制标志
writing_thread_running = True
+def configure_weather(world, weather_type, time_of_day):
+ """设置天气和光照"""
+ weather_presets = {
+ 'clear': carla.WeatherParameters.ClearNoon,
+ 'cloudy': carla.WeatherParameters.CloudyNoon,
+ 'rainy': carla.WeatherParameters.HardRainNoon,
+ 'wet': carla.WeatherParameters.WetNoon,
+ }
+ weather = weather_presets.get(weather_type, carla.WeatherParameters.ClearNoon)
+
+ if time_of_day == 'day':
+ weather.sun_altitude_angle = 75.0
+ elif time_of_day == 'sunset':
+ weather.sun_altitude_angle = 10.0
+ elif time_of_day == 'night':
+ weather.sun_altitude_angle = -90.0
+
+ world.set_weather(weather)
+
+
+def cleanup_previous_hero(world):
+ """启动前清理可能残留的主车"""
+ actors = world.get_actors()
+ potential_heroes = [x for x in actors if
+ x.type_id.startswith('vehicle') and x.attributes.get('role_name') == 'hero']
+ if potential_heroes:
+ print(f"🧹 发现 {len(potential_heroes)} 辆残留的主车,正在清理...")
+ for h in potential_heroes:
+ h.destroy()
+
+
+class pygame_clock:
+ def __init__(self):
+ self.start_time = time.time()
+ self.frame_count = 0
+ self.fps = 0
+
+ def tick(self):
+ self.frame_count += 1
+ elapsed = time.time() - self.start_time
+ if elapsed > 1.0:
+ self.fps = self.frame_count / elapsed
+ self.start_time = time.time()
+ self.frame_count = 0
+ return self.fps
+
+
+def save_worker(q):
+ """
+ 后台保存线程:同时保存图片和 JSON 标签
+ """
+ while writing_thread_running or not q.empty():
+ try:
+ # 获取任务包
+ task = q.get(timeout=0.1)
+ path = task['scene_path']
+
+ # 1. 保存图片
+ images = task['image_data']
+ ego_img = images['ego_rgb']
+ rsu_img = images['rsu_rgb']
+
+ # 这里的 frame 编号统一使用 task 里传过来的,保证对齐
+ fid = task['frame_id']
+
+ ego_img.save_to_disk(f"{path}/ego_rgb/{fid:08d}.jpg")
+ rsu_img.save_to_disk(f"{path}/rsu_rgb/{fid:08d}.jpg")
+
+ # 2. 保存 JSON 标签
+ label_data = task['label_data']
+ with open(f"{path}/label/{fid:08d}.json", 'w') as f:
+ json.dump(label_data, f, indent=2)
+
+ q.task_done()
+ except queue.Empty:
+ pass
+ except Exception as e:
+ print(f"写入错误: {e}")
+
+
+def get_environment_objects(world, ego_id):
+ """
+ 获取环境中的车辆和行人信息
+ """
+ objects = []
+ ego_actor = world.get_actor(ego_id)
+ ego_loc = ego_actor.get_transform().location
+
+ for actor in world.get_actors():
+ # 跳过主车自己(如果你想把主车也算作 RSU 的检测目标,可以注释掉下面两行)
+ if actor.id == ego_id:
+ continue
+
+ if actor.type_id.startswith('vehicle') or actor.type_id.startswith('walker'):
+ # 距离过滤:只记录主车周围 100 米内的物体
+ dist = actor.get_transform().location.distance(ego_loc)
+ if dist < 100:
+ bb = actor.bounding_box
+ t_loc = actor.get_transform().location
+ t_rot = actor.get_transform().rotation
+
+ # 构建单个目标的数据字典
+ obj_data = {
+ "id": actor.id,
+ "type": "vehicle" if actor.type_id.startswith('vehicle') else "walker",
+ "dist_to_ego": dist,
+ # 保存位置 (x, y, z)
+ "location": [t_loc.x, t_loc.y, t_loc.z],
+ # 保存旋转 (pitch, yaw, roll)
+ "rotation": [t_rot.pitch, t_rot.yaw, t_rot.roll],
+ # 保存包围盒半长宽高 (extent)
+ "extent": [bb.extent.x, bb.extent.y, bb.extent.z],
+ # 保存包围盒中心相对于物体原点的偏移
+ "center_offset": [bb.location.x, bb.location.y, bb.location.z]
+ }
+ objects.append(obj_data)
+ return objects
+
+
def main():
- argparser = argparse.ArgumentParser(description="CVIPS 最终版数据采集脚本")
+ argparser = argparse.ArgumentParser(description="CVIPS Final - 带标签采集版")
argparser.add_argument('--town', default='Town01', help='地图名称')
- argparser.add_argument('--num_vehicles', default=25, type=int, help='背景车辆数')
+ argparser.add_argument('--num_vehicles', default=40, type=int, help='车辆数')
argparser.add_argument('--num_walkers', default=40, type=int, help='行人数')
+ argparser.add_argument('--weather', default='clear', choices=['clear', 'rainy', 'cloudy', 'wet'], help='天气')
+ argparser.add_argument('--time_of_day', default='day', choices=['day', 'sunset', 'night'], help='时间')
+ argparser.add_argument('--max_frames', default=1000, type=int, help='采集多少帧后自动停止')
+
args = argparser.parse_args()
- # 1. 创建保存目录
+ # 路径准备
+ scene_name = f"{args.town}_{args.weather}_{args.time_of_day}"
+ scene_output_path = os.path.join(OUTPUT_FOLDER, scene_name)
+
if ENABLE_SAVING:
- os.makedirs(f"{OUTPUT_FOLDER}/ego_rgb", exist_ok=True)
- os.makedirs(f"{OUTPUT_FOLDER}/rsu_rgb", exist_ok=True)
- print(f"📁 数据保存路径: {os.path.abspath(OUTPUT_FOLDER)}")
+ os.makedirs(f"{scene_output_path}/ego_rgb", exist_ok=True)
+ os.makedirs(f"{scene_output_path}/rsu_rgb", exist_ok=True)
+ os.makedirs(f"{scene_output_path}/label", exist_ok=True)
+ print(f"📂 数据将保存在: {scene_output_path}")
- # 2. 连接 CARLA (使用 127.0.0.1 避免 Windows 防火墙问题)
- try:
- client = carla.Client('127.0.0.1', 2000)
- client.set_timeout(60.0)
- print(f"正在加载地图 {args.town} ...")
- world = client.load_world(args.town)
- except RuntimeError as e:
- print(f"❌ 连接失败: {e}")
- print("请确保 CARLA 模拟器已启动!")
- return
-
- # 3. 设置高画质天气 (正午晴天)
- world.set_weather(carla.WeatherParameters.ClearNoon)
-
- # 4. 设置同步模式
- settings = world.get_settings()
- settings.synchronous_mode = True
- settings.fixed_delta_seconds = 0.05 # 固定 20 FPS
- world.apply_settings(settings)
-
- traffic_manager = client.get_trafficmanager(8000)
- traffic_manager.set_synchronous_mode(True)
-
- # 5. 初始化队列和列表
- sensor_queue = queue.Queue() # 接收原始数据
- save_queue = queue.Queue() # 后台保存队列
- actor_list = []
-
- # 6. 启动后台保存线程
+ client = None
+ world = None
+ actor_list = []
+
+ # 传感器数据队列 (只用于暂时接收 Carla 回调数据)
+ sensor_queue = queue.Queue()
+ # 存盘队列 (用于发送给 save_worker)
+ save_queue = queue.Queue()
+
global writing_thread_running
- writing_thread_running = True
- save_thread = threading.Thread(target=save_worker, args=(save_queue,))
- save_thread.start()
- print("✅ 后台保存服务已启动")
try:
- # --- 生成环境 ---
- print("正在构建交通场景...")
+ client = carla.Client('127.0.0.1', 2000)
+ client.set_timeout(10.0)
+
+ world = client.get_world()
+ if world.get_map().name.split('/')[-1] != args.town:
+ print(f"🗺️ 正在切换地图至 {args.town} ...")
+ world = client.load_world(args.town)
+ else:
+ print(f"🗺️ 当前已是 {args.town},准备就绪。")
+
+ cleanup_previous_hero(world)
+ configure_weather(world, args.weather, args.time_of_day)
+
+ # 设置同步模式
+ settings = world.get_settings()
+ settings.synchronous_mode = True
+ settings.fixed_delta_seconds = FIXED_DELTA_SECONDS
+ world.apply_settings(settings)
+
+ traffic_manager = client.get_trafficmanager(8000)
+ traffic_manager.set_synchronous_mode(True)
+ traffic_manager.set_global_distance_to_leading_vehicle(2.5)
+
+ # 启动保存线程
+ writing_thread_running = True
+ save_thread = threading.Thread(target=save_worker, args=(save_queue,))
+ save_thread.daemon = True
+ save_thread.start()
+
+ # --- 1. 生成交通流 (NPC) ---
+ print("🚗 生成交通流...")
blueprint_library = world.get_blueprint_library()
spawn_points = world.get_map().get_spawn_points()
+ random.shuffle(spawn_points)
- # 分离主车点和NPC点,防止碰撞
ego_spawn_point = spawn_points[0]
npc_spawn_points = spawn_points[1:]
- # 生成背景车辆
- bg_vehicle_bp = blueprint_library.filter('vehicle.*')[0]
- for _ in range(args.num_vehicles):
- t = random.choice(npc_spawn_points)
- actor = world.try_spawn_actor(bg_vehicle_bp, t)
- if actor:
- actor.set_autopilot(True)
- actor_list.append(actor)
-
+ # 生成车辆
+ bg_vehicle_bp = blueprint_library.filter('vehicle.*')
+ bg_vehicle_bp = [x for x in bg_vehicle_bp if int(x.get_attribute('number_of_wheels')) == 4]
+
+ batch = []
+ for n, transform in enumerate(npc_spawn_points):
+ if n >= args.num_vehicles: break
+ bp = random.choice(bg_vehicle_bp)
+ if bp.has_attribute('color'):
+ bp.set_attribute('color', random.choice(bp.get_attribute('color').recommended_values))
+ batch.append(carla.command.SpawnActor(bp, transform).then(
+ carla.command.SetAutopilot(carla.command.FutureActor, True, traffic_manager.get_port())))
+
+ for response in client.apply_batch_sync(batch, True):
+ if not response.error: actor_list.append(response.actor_id)
+
# 生成行人
walker_bp = blueprint_library.filter('walker.pedestrian.*')[0]
for _ in range(args.num_walkers):
loc = world.get_random_location_from_navigation()
if loc:
w = world.try_spawn_actor(walker_bp, carla.Transform(loc))
- if w: actor_list.append(w)
+ if w: actor_list.append(w.id)
+
+ # (可选) 给行人加上控制器,让他们动起来,这里简化处理,如有需要需添加 WalkerController
- # --- 生成主车 (Ego) ---
- print("生成主车...")
+ # --- 2. 生成主车 (Ego) ---
+ print("🚘 生成主车...")
ego_bp = blueprint_library.find('vehicle.tesla.model3')
ego_bp.set_attribute('role_name', 'hero')
ego_vehicle = world.spawn_actor(ego_bp, ego_spawn_point)
ego_vehicle.set_autopilot(True)
- actor_list.append(ego_vehicle)
+ actor_list.append(ego_vehicle.id)
- # --- 生成 RSU (路侧单元) ---
+ # --- 3. 生成传感器 (Ego & RSU) ---
+ # 3.1 确定位置
+ # RSU: 主车生成位置上方 10米,稍微偏一点
rsu_loc = ego_spawn_point.location
- rsu_loc.z += 12.0 # 12米高空
- rsu_loc.x += 5.0
- rsu_transform = carla.Transform(rsu_loc, carla.Rotation(pitch=-70, yaw=ego_spawn_point.rotation.yaw))
+ rsu_loc.z += 10.0
+ rsu_loc.x += 8.0
+ rsu_transform = carla.Transform(rsu_loc, carla.Rotation(pitch=-60, yaw=ego_spawn_point.rotation.yaw))
+
+ # Ego Cam: 挂载在主车上
+ cam_transform = carla.Transform(carla.Location(x=-1.5, z=2.4), carla.Rotation(pitch=-15))
- # --- 传感器设置 (1080P 高画质) ---
+ # 3.2 配置蓝图
camera_bp = blueprint_library.find('sensor.camera.rgb')
- camera_bp.set_attribute('image_size_x', '1920')
- camera_bp.set_attribute('image_size_y', '1080')
- camera_bp.set_attribute('fov', '90')
- # 优化画质属性
- camera_bp.set_attribute('exposure_mode', 'histogram')
- camera_bp.set_attribute('motion_blur_intensity', '0.2')
-
- # 主车相机:第三人称 (车后6米,高3米),防遮挡
- cam_transform = carla.Transform(carla.Location(x=-6.0, z=3.0), carla.Rotation(pitch=-20))
-
+ camera_bp.set_attribute('image_size_x', str(IMAGE_W))
+ camera_bp.set_attribute('image_size_y', str(IMAGE_H))
+ camera_bp.set_attribute('fov', str(FOV))
+ camera_bp.set_attribute('sensor_tick', str(FIXED_DELTA_SECONDS))
+
+ # 3.3 Spawn
ego_cam = world.spawn_actor(camera_bp, cam_transform, attach_to=ego_vehicle)
- actor_list.append(ego_cam)
-
- rsu_cam = world.spawn_actor(camera_bp, rsu_transform)
- actor_list.append(rsu_cam)
+ rsu_cam = world.spawn_actor(camera_bp, rsu_transform) # RSU 是固定的(static),不 attach
+ actor_list.append(ego_cam.id)
+ actor_list.append(rsu_cam.id)
- # 监听数据
+ # 3.4 监听数据
ego_cam.listen(lambda image: sensor_queue.put((image.frame, 'ego_rgb', image)))
rsu_cam.listen(lambda image: sensor_queue.put((image.frame, 'rsu_rgb', image)))
- print("\n🔥 正在预热 (Warm Up)... 请保持 CARLA 窗口在前台!")
- for _ in range(50):
- world.tick()
- try:
- # 清空预热期的垃圾数据
- for _ in range(2): sensor_queue.get(timeout=1.0)
- except: pass
+ print(f"\n🚀 采集开始! 目标: {args.max_frames} 帧. 按 Ctrl+C 停止")
- print("🚀 采集开始!按 Ctrl+C 优雅退出...")
-
frame_number = 0
- spectator = world.get_spectator()
+ spectator = world.get_spectator()
+ clock = pygame_clock()
+ # --- 4. 主循环 ---
while True:
- # 1. 物理计算一帧
+ # 仿真步进
world.tick()
w_frame = world.get_snapshot().frame
+ fps = clock.tick()
- # 2. 移动观众视角跟随主车 (方便你观察)
+ # 更新观察者视角跟随主车
spectator.set_transform(ego_cam.get_transform())
+ if args.max_frames > 0 and frame_number >= args.max_frames:
+ print("\n✅ 已达到目标帧数,自动停止。")
+ break
+
try:
- # 3. 获取数据
- current_frame_data = {}
- timeout_counter = 0
- # 尝试凑齐两个相机的数据
- while len(current_frame_data) < 2 and timeout_counter < 10:
+ # 获取 RGB 图片数据
+ current_frame_images = {}
+ timeout = 0
+ # 等待两个相机的图片都到齐
+ while len(current_frame_images) < 2 and timeout < 10:
data = sensor_queue.get(timeout=1.0)
- frame_id, s_type, img_obj = data
- # 允许 1 帧的误差
- if abs(frame_id - w_frame) <= 1:
- current_frame_data[s_type] = img_obj
- timeout_counter += 1
-
- # 4. 放入后台队列
- if ENABLE_SAVING and (frame_number % SAVE_INTERVAL == 0):
- if len(current_frame_data) == 2:
- print(f"Frame: {w_frame} | 待存队列: {save_queue.qsize()}", end='\r')
- save_queue.put(current_frame_data)
-
+ fid, stype, img = data
+ # 只收当前帧的数据,防止错位
+ if abs(fid - w_frame) <= 2:
+ current_frame_images[stype] = img
+ timeout += 1
+
+ # 只有当两个相机数据都齐了,并且满足保存间隔时,才进行标签计算和保存
+ if len(current_frame_images) == 2 and ENABLE_SAVING and (frame_number % SAVE_INTERVAL == 0):
+
+ # --- 核心新增逻辑: 计算矩阵和标签 ---
+
+ # 1. 获取相机位姿
+ ego_trans = ego_cam.get_transform()
+ rsu_trans = rsu_cam.get_transform()
+
+ # 2. 计算 外参矩阵 (World -> Camera)
+ # 这一步需要 cvips_utils.py 的支持
+ ego_w2c = utils.build_world_to_camera_matrix(ego_trans)
+ rsu_w2c = utils.build_world_to_camera_matrix(rsu_trans)
+
+ # 3. 获取所有目标物体 (3D框)
+ targets = get_environment_objects(world, ego_vehicle.id)
+
+ # 4. 组装 JSON 标签数据
+ frame_label_data = {
+ "frame_id": frame_number,
+ "timestamp": world.get_snapshot().timestamp.elapsed_seconds,
+ "camera_params": {
+ "fov": FOV,
+ "width": IMAGE_W,
+ "height": IMAGE_H
+ },
+ "matrices": {
+ "ego_w2c": ego_w2c.tolist(),
+ "rsu_w2c": rsu_w2c.tolist()
+ },
+ "targets": targets
+ }
+
+ # 5. 打包发送给后台线程
+ save_task = {
+ "scene_path": scene_output_path,
+ "frame_id": frame_number,
+ "image_data": current_frame_images,
+ "label_data": frame_label_data
+ }
+ save_queue.put(save_task)
+
+ print(f"FPS: {fps:.1f} | Frame: {frame_number} | Saved: ✅ | Queue: {save_queue.qsize()} ", end='\r')
+ else:
+ # 不保存的帧,仅打印进度
+ print(f"FPS: {fps:.1f} | Frame: {frame_number} | Saved: ⏭️ | Queue: {save_queue.qsize()} ", end='\r')
+
except queue.Empty:
- continue
+ pass
frame_number += 1
except KeyboardInterrupt:
- print("\n🛑 用户请求停止")
-
+ print("\n\n🛑 用户中断")
+ except Exception as e:
+ print(f"\n❌ 错误: {e}")
+ import traceback
+ traceback.print_exc()
finally:
- print("\n🧹 正在执行清理程序...")
-
- # 1. 停止后台线程
+ print("\n♻️ 正在退出...")
+
+ # 停止写入
writing_thread_running = False
-
- # 2. 等待剩余照片保存完毕 (解决报错的关键)
- if not save_queue.empty():
- print(f"⏳ 正在保存剩余的 {save_queue.qsize()} 张照片,请不要关闭窗口...", end='', flush=True)
- save_thread.join()
- print(" 保存完毕!")
- else:
- save_thread.join()
+ if 'save_thread' in locals() and save_thread.is_alive():
+ print("⏳ 等待后台写入完成...", end='')
+ save_thread.join(timeout=5)
+ print("Done")
- # 3. 恢复 CARLA 设置 (防止下次启动变卡)
- try:
- if world:
- settings = world.get_settings()
- settings.synchronous_mode = False
- settings.fixed_delta_seconds = None
- world.apply_settings(settings)
- except:
- pass
+ # 销毁对象
+ if client and actor_list:
+ print("💥 销毁 Actor...")
+ client.apply_batch([carla.command.DestroyActor(x) for x in actor_list])
- # 4. 安全销毁所有对象
- print("🗑️ 销毁车辆和传感器...")
- for actor in actor_list:
- try:
- if actor.is_alive:
- actor.destroy()
- except:
- pass # 忽略销毁时的错误
-
- print("✅ 全部完成,程序安全退出。")
+ # 恢复异步
+ if world:
+ settings = world.get_settings()
+ settings.synchronous_mode = False
+ settings.fixed_delta_seconds = None
+ world.apply_settings(settings)
-# --- 后台工作线程 ---
-def save_worker(q):
- while writing_thread_running or not q.empty():
- try:
- data_dict = q.get(timeout=1.0)
- ego_img = data_dict['ego_rgb']
- rsu_img = data_dict['rsu_rgb']
-
- # 保存 Ego
- path_ego = f"{OUTPUT_FOLDER}/ego_rgb/{ego_img.frame:06d}.png"
- ego_img.save_to_disk(path_ego)
-
- # 保存 RSU
- path_rsu = f"{OUTPUT_FOLDER}/rsu_rgb/{rsu_img.frame:06d}.png"
- rsu_img.save_to_disk(path_rsu)
-
- q.task_done()
- except queue.Empty:
- continue
- except Exception as e:
- print(f"保存错误: {e}")
+ print("✅ 资源已释放,强制返回终端。")
+ os._exit(0)
if __name__ == '__main__':
main()
\ No newline at end of file
diff --git a/src/InterconVis_PedSafety_Enh/cvips_utils.py b/src/InterconVis_PedSafety_Enh/cvips_utils.py
new file mode 100644
index 0000000000..b5ce442a94
--- /dev/null
+++ b/src/InterconVis_PedSafety_Enh/cvips_utils.py
@@ -0,0 +1,89 @@
+import numpy as np
+import math
+import carla
+
+def build_projection_matrix(w, h, fov):
+ """
+ 构建相机的内参矩阵 (Intrinsic Matrix)
+ """
+ focal = w / (2.0 * np.tan(fov * np.pi / 360.0))
+ K = np.identity(3)
+ K[0, 0] = K[1, 1] = focal
+ K[0, 2] = w / 2.0
+ K[1, 2] = h / 2.0
+ return K
+
+def get_image_point(loc, K, w2c):
+ """
+ 将 3D 世界坐标点 (loc) 投影到 2D 像素坐标 (u, v)
+ :param loc: carla.Location / carla.Vector3D
+ :param K: 内参矩阵 (3x3)
+ :param w2c: 世界到相机的变换矩阵 (4x4)
+ """
+ # 1. 构建齐次坐标 [x, y, z, 1]
+ point = np.array([loc.x, loc.y, loc.z, 1])
+
+ # 2. 变换到相机坐标系 (Camera Coordinate)
+ point_camera = np.dot(w2c, point)
+
+ # 3. 归一化相机平面 (除去深度 Z) [y, -z, x] -> standard [x, y, z] conversion happening inside w2c logic usually
+ # 但在 CARLA helper 中,我们直接用投影公式:
+ # 这里的 w2c 实际上已经包含了坐标轴的旋转 (UE4 -> OpenCV)
+
+ # [x,y,z] -> [X_img, Y_img, 1] * depth
+ point_img = np.dot(K, point_camera[:3])
+
+ # 4. 归一化获取像素坐标
+ pixel = [int(point_img[0] / point_img[2]), int(point_img[1] / point_img[2])]
+ return pixel
+
+def get_matrix(transform):
+ """
+ 将 CARLA 的 transform 对象转换为 4x4 变换矩阵
+ """
+ yaw = math.radians(transform.rotation.yaw)
+ pitch = math.radians(transform.rotation.pitch)
+ roll = math.radians(transform.rotation.roll)
+
+ c_y = math.cos(yaw)
+ s_y = math.sin(yaw)
+ c_p = math.cos(pitch)
+ s_p = math.sin(pitch)
+ c_r = math.cos(roll)
+ s_r = math.sin(roll)
+
+ matrix = np.identity(4)
+ matrix[0, 0] = c_p * c_y
+ matrix[0, 1] = c_y * s_p * s_r - s_y * c_r
+ matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r
+ matrix[1, 0] = c_p * s_y
+ matrix[1, 1] = s_y * s_p * s_r + c_y * c_r
+ matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r
+ matrix[2, 0] = s_p
+ matrix[2, 1] = -c_p * s_r
+ matrix[2, 2] = c_p * c_r
+ matrix[0, 3] = transform.location.x
+ matrix[1, 3] = transform.location.y
+ matrix[2, 3] = transform.location.z
+ return matrix
+
+def build_world_to_camera_matrix(camera_transform):
+ """
+ 构建 世界坐标系 -> 相机像素坐标系 的外参矩阵 (Extrinsic Matrix)
+ 关键:包含坐标轴置换 (UE4 -> Standard CV)
+ """
+ # 1. 获取相机在世界坐标系的位姿矩阵 M
+ cam_M = get_matrix(camera_transform)
+
+ # 2. 求逆,得到 世界->相机 的变换
+ w2c = np.linalg.inv(cam_M)
+
+ # 3. 坐标轴修正矩阵 (UE4 x-forward, y-right, z-up -> Camera z-forward, x-right, y-down)
+ calibration = np.identity(4)
+ calibration[0, 2] = 1 # x -> z
+ calibration[1, 0] = -1 # y -> -x
+ calibration[2, 1] = -1 # z -> -y
+
+ # 组合
+ w2c = np.dot(calibration, w2c)
+ return w2c
\ No newline at end of file
diff --git a/src/LocoMuJoCo/double_action.py b/src/LocoMuJoCo/double_action.py
index 9bd089bd3a..d74a98356f 100644
--- a/src/LocoMuJoCo/double_action.py
+++ b/src/LocoMuJoCo/double_action.py
@@ -5,77 +5,82 @@
from loco_mujoco.trajectory import Trajectory, TrajectoryInfo, TrajectoryModel, TrajectoryData
from loco_mujoco.core.utils.mujoco import mj_jntname2qposid
-# 配置参数
-FPS = int(1 / 0.002) # 根据环境实际dt计算(假设dt=0.002)
-WALK_DURATION = 5
-STAY_DURATION = 2
-SQUAT_REPEATS = 5
-WALK_STEPS = WALK_DURATION * FPS
-STAY_STEPS = STAY_DURATION * FPS
+# ===================== 1. 初始化环境+动态获取真实帧率(核心) =====================
+env = ImitationFactory.make(
+ "UnitreeH1",
+ n_substeps=20,
+ default_dataset_conf=DefaultDatasetConf(["walk", "squat"])
+)
+env.reset(jax.random.PRNGKey(0))
+ENV_DT = env.dt # 环境真实时间步长(默认0.02s,对应50FPS)
+FPS = int(1 / ENV_DT)
+print(f"环境真实参数:dt={ENV_DT}s | FPS={FPS}")
+
+# ===================== 2. 精准配置各阶段时长(按需修改) =====================
+WALK_DURATION = 5 # 行走5秒(可改)
+STAY_DURATION = 1 # 停留1秒(严格)
+SQUAT_DURATION = 10 # 下蹲总时长10秒(严格)
+# 计算各阶段精准步数
+WALK_STEPS = int(WALK_DURATION * FPS)
+STAY_STEPS = int(STAY_DURATION * FPS)
+SQUAT_STEPS = int(SQUAT_DURATION * FPS)
-def get_trajectory_segment(env, dataset_name, num_steps):
- """从数据集提取指定长度的轨迹片段"""
+def get_trajectory_segment(env, dataset_name, target_steps):
+ """按目标步数提取轨迹(精准控制时长,避免冗余)"""
temp_env = ImitationFactory.make(
"UnitreeH1",
default_dataset_conf=DefaultDatasetConf([dataset_name]),
n_substeps=env._n_substeps
)
temp_env.reset(jax.random.PRNGKey(0))
- # 加载轨迹(使用 th.traj 访问轨迹数据)
temp_env.load_trajectory(temp_env.th.traj)
- # 从轨迹数据中获取关节位置和速度
- full_qpos = temp_env.th.traj.data.qpos
- full_qvel = temp_env.th.traj.data.qvel
+ raw_qpos = np.array(temp_env.th.traj.data.qpos)
+ raw_qvel = np.array(temp_env.th.traj.data.qvel)
- qpos = []
- qvel = []
- for i in range(num_steps):
- idx = i % len(full_qpos)
- qpos.append(full_qpos[idx])
- qvel.append(full_qvel[idx])
- return np.array(qpos), np.array(qvel)
+ # 鲁棒性校验:空轨迹报错
+ if len(raw_qpos) == 0:
+ raise ValueError(f"数据集 {dataset_name} 无有效轨迹!")
+
+ # 循环填充到目标步数(保证动作流畅)
+ traj_qpos = []
+ traj_qvel = []
+ for i in range(target_steps):
+ idx = i % len(raw_qpos) # 循环取原始轨迹帧
+ traj_qpos.append(raw_qpos[idx])
+ traj_qvel.append(raw_qvel[idx])
+ return np.array(traj_qpos), np.array(traj_qvel)
-# 创建主环境
-env = ImitationFactory.make(
- "UnitreeH1",
- n_substeps=20,
- default_dataset_conf=DefaultDatasetConf(["walk", "squat"])
-)
-env.reset(jax.random.PRNGKey(0))
+# ===================== 3. 生成各阶段轨迹(顺序:走→停→蹲) =====================
model = env.get_model()
data = env.get_data()
-# 提取各阶段轨迹
+# 3.1 行走5秒轨迹
walk_qpos, walk_qvel = get_trajectory_segment(env, "walk", WALK_STEPS)
+print(f"行走阶段:{WALK_DURATION}秒 | {WALK_STEPS}步")
-# 停留阶段轨迹
-last_walk_qpos = walk_qpos[-1]
+# 3.2 停留1秒轨迹(复用行走最后姿态,速度归零)
+last_walk_qpos = walk_qpos[-1].copy()
last_walk_qvel = np.zeros_like(walk_qvel[0])
stay_qpos = np.tile(last_walk_qpos, (STAY_STEPS, 1))
stay_qvel = np.tile(last_walk_qvel, (STAY_STEPS, 1))
+print(f"停留阶段:{STAY_DURATION}秒 | {STAY_STEPS}步")
-# 下蹲阶段轨迹
-squat_temp_env = ImitationFactory.make(
- "UnitreeH1",
- default_dataset_conf=DefaultDatasetConf(["squat"]),
- n_substeps=env._n_substeps
-)
-squat_temp_env.reset(jax.random.PRNGKey(0))
-squat_cycle_steps = len(squat_temp_env.th.traj.data.qpos) # 修正:使用 th.traj 访问
-squat_qpos, squat_qvel = get_trajectory_segment(
- env, "squat", squat_cycle_steps * SQUAT_REPEATS
-)
-
-# 固定下蹲时的根位置
+# 3.3 下蹲10秒轨迹(精准10秒,原地固定)
+squat_qpos, squat_qvel = get_trajectory_segment(env, "squat", SQUAT_STEPS)
+# 固定下蹲根位置(行走结束位置,仅x/y平面)
root_joint_ind = mj_jntname2qposid("root", model)
root_pos_walk_end = walk_qpos[-1, root_joint_ind[:2]]
squat_qpos[:, root_joint_ind[:2]] = root_pos_walk_end
+print(f"下蹲阶段:{SQUAT_DURATION}秒 | {SQUAT_STEPS}步")
-# 拼接轨迹
+# ===================== 4. 拼接轨迹(强制顺序:走→停→蹲) =====================
full_qpos = np.concatenate([walk_qpos, stay_qpos, squat_qpos], axis=0)
full_qvel = np.concatenate([walk_qvel, stay_qvel, squat_qvel], axis=0)
+total_duration = len(full_qpos) / FPS
+print(f"总轨迹:{total_duration:.1f}秒 | {len(full_qpos)}步(5+1+10={16}秒,验证匹配)")
-# 创建并加载轨迹
+# ===================== 5. 加载轨迹并播放 =====================
+# 生成轨迹信息(频率匹配环境真实FPS)
jnt_names = [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) for i in range(model.njnt)]
traj_info = TrajectoryInfo(
jnt_names,
@@ -90,5 +95,5 @@ def get_trajectory_segment(env, dataset_name, num_steps):
traj = Trajectory(traj_info, traj_data)
env.load_trajectory(traj)
-# 播放轨迹
+# 播放轨迹(确保顺序和速度正确)
env.play_trajectory(n_steps_per_episode=len(full_qpos), render=True)
\ No newline at end of file
diff --git a/src/LocoMuJoCo/double_action2.py b/src/LocoMuJoCo/double_action2.py
new file mode 100644
index 0000000000..77528fa95b
--- /dev/null
+++ b/src/LocoMuJoCo/double_action2.py
@@ -0,0 +1,131 @@
+import numpy as np
+import jax
+import mujoco
+from loco_mujoco.task_factories import ImitationFactory, DefaultDatasetConf, LAFAN1DatasetConf
+from loco_mujoco.trajectory import Trajectory, TrajectoryInfo, TrajectoryModel, TrajectoryData
+from loco_mujoco.core.utils.mujoco import mj_jntname2qposid
+
+# ===================== 1. 初始化环境+动态获取真实帧率 =====================
+# 关键修复:使用正确的数据集配置加载不同类型轨迹
+env = ImitationFactory.make(
+ "UnitreeH1",
+ n_substeps=20,
+ default_dataset_conf=DefaultDatasetConf(["squat"]), # 下蹲属于默认数据集
+ lafan1_dataset_conf=LAFAN1DatasetConf(["dance2_subject4"]) # 舞蹈属于LAFAN1数据集
+)
+env.reset(jax.random.PRNGKey(0))
+ENV_DT = env.dt # 环境时间步长(默认0.02s,对应50FPS)
+FPS = int(1 / ENV_DT)
+print(f"环境参数:dt={ENV_DT}s | FPS={FPS}")
+
+# ===================== 2. 配置各阶段时长(修改停留为1秒) =====================
+SQUAT_DURATION = 10 # 下蹲时长10秒
+STAY_DURATION = 1 # 停留时长1秒(修改此处)
+DANCE_DURATION = 20 # 跳舞时长20秒
+CYCLE_TIMES = 3 # 循环次数(可按需调整)
+
+# 计算各阶段步数
+SQUAT_STEPS = int(SQUAT_DURATION * FPS)
+STAY_STEPS = int(STAY_DURATION * FPS)
+DANCE_STEPS = int(DANCE_DURATION * FPS)
+SINGLE_CYCLE_STEPS = SQUAT_STEPS + STAY_STEPS + DANCE_STEPS
+TOTAL_STEPS = SINGLE_CYCLE_STEPS * CYCLE_TIMES
+
+# ===================== 3. 定义轨迹片段生成函数 =====================
+def get_trajectory_segment(env, dataset_type, dataset_name, target_steps):
+ """按目标步数提取指定数据集的轨迹片段"""
+ # 根据数据集类型创建临时环境
+ if dataset_type == "default":
+ temp_env = ImitationFactory.make(
+ "UnitreeH1",
+ default_dataset_conf=DefaultDatasetConf([dataset_name]),
+ n_substeps=env._n_substeps
+ )
+ elif dataset_type == "lafan1":
+ temp_env = ImitationFactory.make(
+ "UnitreeH1",
+ lafan1_dataset_conf=LAFAN1DatasetConf([dataset_name]),
+ n_substeps=env._n_substeps
+ )
+ else:
+ raise ValueError(f"不支持的数据集类型: {dataset_type}")
+
+ temp_env.reset(jax.random.PRNGKey(0))
+ raw_qpos = np.array(temp_env.th.traj.data.qpos)
+ raw_qvel = np.array(temp_env.th.traj.data.qvel)
+
+ # 校验轨迹有效性
+ if len(raw_qpos) == 0:
+ raise ValueError(f"数据集 {dataset_name} 无有效轨迹数据!")
+
+ # 循环填充到目标步数(保证动作流畅循环)
+ traj_qpos = []
+ traj_qvel = []
+ for i in range(target_steps):
+ idx = i % len(raw_qpos) # 循环索引避免越界
+ traj_qpos.append(raw_qpos[idx])
+ traj_qvel.append(raw_qvel[idx])
+ return np.array(traj_qpos), np.array(traj_qvel)
+
+# ===================== 4. 预生成单周期轨迹片段 =====================
+model = env.get_model()
+
+# 4.1 下蹲轨迹(默认数据集)
+squat_qpos, squat_qvel = get_trajectory_segment(
+ env, "default", "squat", SQUAT_STEPS
+)
+print(f"下蹲阶段:{SQUAT_DURATION}秒 | {SQUAT_STEPS}步")
+
+# 4.2 停留轨迹(复用下蹲结束姿态,速度归零)
+last_squat_qpos = squat_qpos[-1].copy()
+last_squat_qvel = np.zeros_like(squat_qvel[0])
+stay_qpos = np.tile(last_squat_qpos, (STAY_STEPS, 1))
+stay_qvel = np.tile(last_squat_qvel, (STAY_STEPS, 1))
+print(f"停留阶段:{STAY_DURATION}秒 | {STAY_STEPS}步")
+
+# 4.3 跳舞轨迹(LAFAN1数据集)
+dance_qpos, dance_qvel = get_trajectory_segment(
+ env, "lafan1", "dance2_subject4", DANCE_STEPS
+)
+# 固定根位置(保持在蹲下结束时的位置,实现原地跳舞)
+root_joint_ind = mj_jntname2qposid("root", model)
+root_pos_squat_end = squat_qpos[-1, root_joint_ind[:2]] # 只固定x/y平面位置
+dance_qpos[:, root_joint_ind[:2]] = root_pos_squat_end
+print(f"跳舞阶段:{DANCE_DURATION}秒 | {DANCE_STEPS}步")
+
+# ===================== 5. 生成多循环完整轨迹 =====================
+full_qpos = []
+full_qvel = []
+
+for cycle in range(CYCLE_TIMES):
+ print(f"生成第 {cycle+1}/{CYCLE_TIMES} 个循环轨迹...")
+ # 拼接单循环的三个阶段
+ full_qpos.extend([squat_qpos, stay_qpos, dance_qpos])
+ full_qvel.extend([squat_qvel, stay_qvel, dance_qvel])
+
+# 合并为numpy数组
+full_qpos = np.concatenate(full_qpos, axis=0)
+full_qvel = np.concatenate(full_qvel, axis=0)
+
+# 验证总时长
+total_duration = len(full_qpos) / FPS
+print(f"总轨迹:{CYCLE_TIMES}次循环 | {total_duration:.1f}秒 | {len(full_qpos)}步")
+
+# ===================== 6. 加载轨迹并播放 =====================
+# 生成轨迹元信息(确保频率匹配环境)
+jnt_names = [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) for i in range(model.njnt)]
+traj_info = TrajectoryInfo(
+ jnt_names,
+ model=TrajectoryModel(model.njnt, jax.numpy.array(model.jnt_type)),
+ frequency=FPS
+)
+traj_data = TrajectoryData(
+ jax.numpy.array(full_qpos),
+ jax.numpy.array(full_qvel),
+ split_points=jax.numpy.array([0, len(full_qpos)])
+)
+traj = Trajectory(traj_info, traj_data)
+env.load_trajectory(traj)
+
+# 播放完整循环轨迹
+env.play_trajectory(n_steps_per_episode=len(full_qpos), render=True)
\ No newline at end of file
diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py
index a61fec162d..3420f3b0bf 100644
--- a/src/MMAP-DRL-Nav/carla_environment.py
+++ b/src/MMAP-DRL-Nav/carla_environment.py
@@ -94,30 +94,55 @@ def is_port_used(port):
self.collision_pedestrian_penalty = -100.0 # 碰撞行人惩罚
self.collision_static_penalty = -15.0 # 碰撞静态物体惩罚
- # 基础配置
+ # ========== 天气系统:仅修改此处(新增+调整) ==========
+ self.weather_mode = "dynamic" # 动态模式:每次reset必切换不同天气
+ self.preset_weathers = [
+ carla.WeatherParameters.ClearNoon,
+ carla.WeatherParameters.CloudyNoon,
+ carla.WeatherParameters.WetNoon,
+ carla.WeatherParameters.WetCloudyNoon,
+ carla.WeatherParameters.MidRainyNoon,
+ carla.WeatherParameters.HardRainNoon,
+ carla.WeatherParameters.SoftRainNoon
+ ]
+ # 初始化:启动就随机选天气(不是固定晴天)
+ self.current_weather = random.choice(self.preset_weathers)
+ self.world.set_weather(self.current_weather)
+ print(f"天气系统初始化完成,模式:{self.weather_mode},当前天气:{self.current_weather}")
+
+ # 基础配置(完全保留)
self.spawn_retry_times = 20
self.spawn_safe_radius = 2.0
- # 动作/观测空间
+ # 动作/观测空间(完全保留)
self.action_space = gym.spaces.Discrete(4)
self.observation_space = gym.spaces.Box(
low=0, high=255, shape=(128, 128, 3), dtype=np.uint8
)
- # 核心对象
+ # 核心对象(完全保留)
self.vehicle = None
self.camera = None
self.collision_sensor = None
self.image_data = None
self.has_collision = False
- # 视角参数
+ # 视角参数(完全保留)
self.view_height = 6.0
self.view_pitch = -15.0
self.view_distance = 8.0
self.z_offset = 0.5
- # ========== 红绿灯判定(移除特殊符号) ==========
+ # ========== 天气切换:仅新增此函数(保证不重复) ==========
+ def switch_weather(self):
+ # 过滤当前天气,确保每次切换都换不同的
+ available_weathers = [w for w in self.preset_weathers if w != self.current_weather]
+ new_weather = random.choice(available_weathers)
+ self.world.set_weather(new_weather)
+ self.current_weather = new_weather
+ print(f"天气已切换:{self.current_weather}")
+
+ # ========== 红绿灯判定(完全保留原始代码) ==========
def _check_traffic_light(self):
current_time = time.time()
if current_time - self.last_traffic_light_time < self.traffic_light_cooldown:
@@ -171,7 +196,7 @@ def _check_traffic_light(self):
return reward
- # ========== 超速检测(无日志) ==========
+ # ========== 超速检测(完全保留原始代码) ==========
def _check_over_speed(self):
current_time = time.time()
if current_time - self.last_over_speed_time < self.over_speed_cooldown:
@@ -197,7 +222,7 @@ def _check_over_speed(self):
return reward
- # ========== 车道偏离检测(3秒检测 + 10秒日志 + 无特殊符号) ==========
+ # ========== 车道偏离检测(完全保留原始代码) ==========
def _check_lane_offset(self):
current_time = time.time()
# 仅每3秒执行一次检测
@@ -254,7 +279,7 @@ def _check_lane_offset(self):
return reward
- # ========== 安全生成车辆(移除特殊符号) ==========
+ # ========== 安全生成车辆(完全保留原始代码) ==========
def _spawn_vehicle_safely(self, vehicle_bp):
spawn_points = self.world.get_map().get_spawn_points()
if not spawn_points:
@@ -278,7 +303,7 @@ def _spawn_vehicle_safely(self, vehicle_bp):
raise RuntimeError("所有出生点都有碰撞,无法生成车辆!")
- # ========== 优化NPC生成 ==========
+ # ========== 优化NPC生成(完全保留原始代码) ==========
def _spawn_small_npc(self):
for v in self.npc_vehicle_list:
if v.is_alive:
@@ -337,7 +362,7 @@ def _spawn_small_npc(self):
except:
continue
- # ========== 初始化碰撞传感器 ==========
+ # ========== 初始化碰撞传感器(完全保留原始代码) ==========
def _init_collision_sensor(self):
collision_bp = self.blueprint_library.find('sensor.other.collision')
collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0))
@@ -346,7 +371,7 @@ def _init_collision_sensor(self):
)
self.collision_sensor.listen(lambda event: self._collision_callback(event))
- # ========== 碰撞回调(仅标记状态) ==========
+ # ========== 碰撞回调(完全保留原始代码) ==========
def _collision_callback(self, event):
self.has_collision = True
other_actor = event.other_actor
@@ -358,7 +383,7 @@ def _collision_callback(self, event):
elif 'static' in other_actor_type or 'building' in other_actor_type or 'guardrail' in other_actor_type:
self.hit_static = True
- # ========== 初始化相机 ==========
+ # ========== 初始化相机(完全保留原始代码) ==========
def _init_camera(self):
camera_bp = self.blueprint_library.find('sensor.camera.rgb')
camera_bp.set_attribute('image_size_x', '128')
@@ -369,9 +394,9 @@ def _init_camera(self):
self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle)
self.camera.listen(lambda img: self._camera_callback(img))
- # ========== 重置环境(重置所有标记) ==========
+ # ========== 重置环境(仅修改天气逻辑:每次reset必切换) ==========
def reset(self):
- # 清理旧资源
+ # 清理旧资源(完全保留)
if self.vehicle is not None and self.vehicle.is_alive:
self.vehicle.destroy()
if self.camera is not None and self.camera.is_alive:
@@ -380,49 +405,52 @@ def reset(self):
self.collision_sensor.destroy()
self.image_data = None
- # 重置所有碰撞标记
+ # 重置所有碰撞标记(完全保留)
self.has_collision = False
self.hit_vehicle = False
self.hit_pedestrian = False
self.hit_static = False
self.collision_penalty_applied = False
- # 重置红绿灯标记
+ # 重置红绿灯标记(完全保留)
self.last_traffic_light_time = 0
self.has_triggered_red = False
self.has_triggered_green = False
- # 重置超速标记
+ # 重置超速标记(完全保留)
self.last_over_speed_time = 0
- # 重置车道检测/日志标记
+ # 重置车道检测/日志标记(完全保留)
self.last_lane_check_time = 0
self.last_lane_log_time = 0
- # 生成车辆
+ # ========== 仅修改此处:强制切换天气(不管什么模式都换) ==========
+ self.switch_weather()
+
+ # 生成车辆(完全保留)
vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0]
self.vehicle = self._spawn_vehicle_safely(vehicle_bp)
- # 初始化传感器
+ # 初始化传感器(完全保留)
self._init_camera()
self._init_collision_sensor()
- # 生成NPC
+ # 生成NPC(完全保留)
self._spawn_small_npc()
- # 等待传感器就绪
+ # 等待传感器就绪(完全保留)
timeout = 0
while self.image_data is None and timeout < 30:
self.world.tick()
time.sleep(0.001)
timeout += 1
- # 绑定视角
+ # 绑定视角(完全保留)
self.follow_vehicle()
self.world.tick()
return self.image_data.copy() if self.image_data is not None else np.zeros((128,128,3), dtype=np.uint8)
- # ========== 视角跟随 ==========
+ # ========== 视角跟随(完全保留原始代码) ==========
def follow_vehicle(self):
spectator = self.world.get_spectator()
if not spectator or not self.vehicle or not self.vehicle.is_alive:
@@ -441,21 +469,21 @@ def follow_vehicle(self):
carla.Rotation(pitch=self.view_pitch, yaw=vehicle_tf.rotation.yaw, roll=0.0)
))
- # ========== 获取观测 ==========
+ # ========== 获取观测(完全保留原始代码) ==========
def get_observation(self):
return self.image_data.copy() if self.image_data is not None else np.zeros((128, 128, 3), dtype=np.uint8)
- # ========== 相机回调 ==========
+ # ========== 相机回调(完全保留原始代码) ==========
def _camera_callback(self, image):
array = np.frombuffer(image.raw_data, dtype=np.uint8)
self.image_data = array.reshape((image.height, image.width, 4))[:, :, :3]
- # ========== 核心:奖励函数(碰撞仅扣一次分 + 移除特殊符号) ==========
+ # ========== 核心:奖励函数(完全保留原始代码) ==========
def step(self, action):
if self.vehicle is None or not self.vehicle.is_alive:
raise RuntimeError("车辆未初始化/已销毁,请先调用reset()")
- # 车辆控制逻辑
+ # 车辆控制逻辑(完全保留)
throttle = 0.0
steer = 0.0
if action == 0: # 前进
@@ -481,19 +509,19 @@ def step(self, action):
self.world.tick()
self.follow_vehicle()
- # ========== 1. 基础行驶奖励 ==========
+ # ========== 1. 基础行驶奖励(完全保留) ==========
base_reward = 0.1 if throttle > 0 else (-0.1 if throttle < 0 else 0.0)
- # ========== 2. 红绿灯奖惩 ==========
+ # ========== 2. 红绿灯奖惩(完全保留) ==========
traffic_light_reward = self._check_traffic_light()
- # ========== 3. 超速奖惩 ==========
+ # ========== 3. 超速奖惩(完全保留) ==========
over_speed_reward = self._check_over_speed()
- # ========== 4. 车道偏离奖惩(3秒检测 + 10秒日志) ==========
+ # ========== 4. 车道偏离奖惩(3秒检测 + 10秒日志)(完全保留) ==========
lane_reward = self._check_lane_offset()
- # ========== 5. 碰撞奖惩(仅扣一次分 + 移除特殊符号) ==========
+ # ========== 5. 碰撞奖惩(仅扣一次分 + 移除特殊符号)(完全保留) ==========
collision_reward = 0.0
done = False
if self.has_collision and not self.collision_penalty_applied:
@@ -514,7 +542,7 @@ def step(self, action):
# 标记惩罚已执行,避免重复扣分
self.collision_penalty_applied = True
- # ========== 总奖励计算 ==========
+ # ========== 总奖励计算(完全保留) ==========
total_reward = (
base_reward # 基础行驶
+ traffic_light_reward # 红绿灯
@@ -534,7 +562,7 @@ def step(self, action):
"total_reward": total_reward
}
- # ========== 关闭环境(移除特殊符号) ==========
+ # ========== 关闭环境(完全保留原始代码) ==========
def close(self):
# 清理NPC
for v in self.npc_vehicle_list:
diff --git a/src/Mujoco_manrun/main.py b/src/Mujoco_manrun/main.py
index 29db331ce3..3e74d0510f 100644
--- a/src/Mujoco_manrun/main.py
+++ b/src/Mujoco_manrun/main.py
@@ -5,11 +5,11 @@
import os
-class G1Stabilizer:
- """修复踮脚后仰的G1机器人稳定站立控制器(新增行走功能)"""
+class HumanoidStabilizer:
+ """适配humanoid.xml的高稳定性行走控制器(修复com_target属性错误)"""
def __init__(self, model_path):
- # 类型检查与模型加载
+ # 基础初始化(保留原有核心逻辑)
if not isinstance(model_path, str):
raise TypeError(f"模型路径必须是字符串,当前是 {type(model_path)} 类型")
@@ -19,147 +19,164 @@ def __init__(self, model_path):
except Exception as e:
raise RuntimeError(f"模型加载失败:{e}\n请检查路径和文件完整性")
- # 仿真核心参数(强化稳定性)
+ # 1. 仿真参数优化:固定时间步+更保守的初始稳定
self.sim_duration = 120.0
- self.dt = 0.001
+ self.dt = 0.005 # 固定0.005s步长,提升仿真稳定性
self.model.opt.timestep = self.dt
- self.init_wait_time = 4.0 # 延长初始稳定时间
+ self.init_wait_time = 5.0 # 延长初始稳定时间至5s
self.model.opt.gravity[2] = -9.81
- self.model.opt.iterations = 200
- self.model.opt.tolerance = 1e-8
+ self.model.opt.iterations = 300 # 增加迭代次数提升接触求解精度
+ self.model.opt.tolerance = 1e-9 # 降低容差,提升计算精度
- # 关节名称映射
+ # 2. 关节名称映射(保留适配humanoid.xml的命名)
self.joint_names = [
- # 左腿关节
- "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint",
- "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint",
- # 右腿关节
- "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint",
- "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint",
- # 腰部关节
- "waist_yaw_joint",
- # 左臂关节
- "left_shoulder_pitch_joint", "left_shoulder_roll_joint",
- "left_shoulder_yaw_joint", "left_elbow_joint", "left_wrist_roll_joint",
- # 右臂关节
- "right_shoulder_pitch_joint", "right_shoulder_roll_joint",
- "right_shoulder_yaw_joint", "right_elbow_joint", "right_wrist_roll_joint"
+ "abdomen_z", "abdomen_y", "abdomen_x",
+ "hip_x_right", "hip_z_right", "hip_y_right",
+ "knee_right", "ankle_y_right", "ankle_x_right",
+ "hip_x_left", "hip_z_left", "hip_y_left",
+ "knee_left", "ankle_y_left", "ankle_x_left",
+ "shoulder1_right", "shoulder2_right", "elbow_right",
+ "shoulder1_left", "shoulder2_left", "elbow_left"
]
-
self.joint_name_to_idx = {name: i for i, name in enumerate(self.joint_names)}
self.num_joints = len(self.joint_names)
- # 关键修复:PD增益(抑制踮脚/后仰)
- self.kp_roll = 120.0 # 降低侧倾增益,避免过度修正
- self.kd_roll = 40.0 # 提高侧倾阻尼
- self.kp_pitch = 100.0 # 大幅降低俯仰增益(核心:防止后仰)
- self.kd_pitch = 35.0 # 提高俯仰阻尼
- self.kp_yaw = 30.0
- self.kd_yaw = 15.0
-
- # 腿部关节增益(重点优化踝关节)
- self.kp_hip = 250.0 # 【增大】髋关节增益,确保有足够力矩迈步
- self.kd_hip = 45.0 # 提高髋关节阻尼
- self.kp_knee = 280.0 # 【增大】膝关节增益
- self.kd_knee = 50.0
- self.kp_ankle = 200.0 # 提高踝关节比例增益(防止踮脚)
- self.kd_ankle = 60.0 # 大幅提高踝关节阻尼(核心:抑制踮脚)
-
- # 腰部/手臂增益(几乎固定)
- self.kp_waist = 40.0
- self.kd_waist = 20.0
- self.kp_arm = 20.0
- self.kd_arm = 20.0
-
- # 重心目标(核心修复:前移+降低重心)
- self.com_target = np.array([0.05, 0.0, 0.78]) # 重心前移5cm,高度降至0.78m
- self.kp_com = 50.0 # 降低重心补偿,避免过度修正
- self.foot_contact_threshold = 1.5 # 降低接触阈值,提高检测灵敏度
-
- # 状态变量
+ # 3. PD增益优化:分阶段动态增益(核心)
+ # 基础增益(支撑腿)
+ self.kp_roll = 150.0 # 提高侧倾增益,增强横向稳定
+ self.kd_roll = 50.0
+ self.kp_pitch = 130.0 # 提高俯仰增益,抑制后仰
+ self.kd_pitch = 45.0
+ self.kp_yaw = 40.0
+ self.kd_yaw = 20.0
+
+ # 腿部增益(分支撑/摆动)
+ self.kp_hip_support = 300.0 # 支撑腿髋关节高增益
+ self.kd_hip_support = 60.0
+ self.kp_hip_swing = 150.0 # 摆动腿髋关节低增益,避免过度摆动
+ self.kd_hip_swing = 30.0
+
+ self.kp_knee_support = 320.0 # 支撑腿膝关节高增益
+ self.kd_knee_support = 70.0
+ self.kp_knee_swing = 180.0 # 摆动腿膝关节低增益
+ self.kd_knee_swing = 40.0
+
+ self.kp_ankle_support = 250.0 # 支撑腿踝关节高增益(核心防踮脚)
+ self.kd_ankle_support = 80.0
+ self.kp_ankle_swing = 120.0 # 摆动腿踝关节低增益
+ self.kd_ankle_swing = 40.0
+
+ # 腰部/手臂增益(小幅提升)
+ self.kp_waist = 50.0
+ self.kd_waist = 25.0
+ self.kp_arm = 25.0
+ self.kd_arm = 25.0
+
+ # 4. 重心控制优化:动态目标+速度反馈
+ self.com_base_target = np.array([0.0, 0.0, 0.85]) # 基础重心高度提升,降低跌倒风险
+ # ========== 关键修复1:初始化com_target,避免属性不存在 ==========
+ self.com_target = self.com_base_target.copy() # 初始赋值,与基础重心一致
+ self.com_gait_offset = np.array([0.08, 0.0, 0.02]) # 步态相位对应的重心偏移
+ self.kp_com = 80.0 # 提高重心比例增益
+ self.kd_com = 30.0 # 新增重心速度阻尼
+ self.foot_contact_threshold = 2.0 # 提高接触阈值,减少误判
+
+ # 5. 状态变量优化:新增滤波和平滑参数
self.joint_targets = np.zeros(self.num_joints)
+ self.joint_targets_prev = np.zeros(self.num_joints) # 上一帧关节目标(滤波用)
self.prev_com = np.zeros(3)
- self.foot_contact = np.zeros(2)
+ self.com_vel = np.zeros(3) # 重心速度
+ self.foot_contact = np.zeros(2) # [right, left]
+ self.foot_contact_history = np.zeros((2, 5)) # 接触状态历史(5帧滤波)
self.integral_roll = 0.0
self.integral_pitch = 0.0
- self.integral_limit = 0.15 # 进一步收紧积分限幅
-
- # 【行走功能新增】步态参数(核心:增大偏移量确保动作明显)
- self.gait_phase = 0.0 # 步态相位(0-1循环)
- self.gait_cycle = 2.0 # 步态周期(秒,放慢周期让动作更明显)
- self.step_offset_hip = 0.8 # 髋关节俯仰偏移量(【大幅增大】原0.1→0.8)
- self.step_offset_knee = 1.2 # 膝关节偏移量(【大幅增大】原0.4→1.2)
- self.step_offset_ankle = 0.5 # 踝关节偏移量
- self.walk_start_time = None # 行走开始时间标记
-
- # 初始化稳定姿态(核心修复:关节角度)
+ self.integral_limit = 0.1 # 进一步收紧积分限幅
+ self.integral_deadband = 0.01 # 积分死区,小误差不积分
+
+ # 6. 步态规划优化:平滑插值+慢周期
+ self.gait_phase = 0.0
+ self.gait_cycle = 2.5 # 延长步态周期至2.5s,动作更平缓
+ self.step_offset_hip = 0.35 # 降低髋关节偏移,减少重心波动
+ self.step_offset_knee = 0.5 # 降低膝关节偏移
+ self.step_offset_ankle = 0.25
+ self.walk_start_time = None
+ self.filter_alpha = 0.2 # 关节目标低通滤波系数
+
+ # 7. 力矩输出滤波:减少力矩突变
+ self.torque_prev = np.zeros(self.num_joints)
+ self.torque_filter_alpha = 0.15
+
+ # 初始化稳定姿态
self._init_stable_pose()
def _init_stable_pose(self):
- """修复:调整关节角度,让脚掌完全落地,重心前移"""
+ """优化初始姿态:更贴近自然站立,降低初始冲击"""
mujoco.mj_resetData(self.model, self.data)
- # 躯干初始位置(更低+更稳)
- self.data.qpos[2] = 0.78 # 初始高度匹配重心目标
+ # 躯干初始位置:略高+零偏航,减少初始调整
+ self.data.qpos[2] = 1.3 # 初始高度略高,落地更平稳
self.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
self.data.qvel[:] = 0.0
self.data.xfrc_applied[:] = 0.0
- # 核心修复:腿部关节角度(避免踮脚/后仰)
- # 左腿关节(脚掌完全落地,无背屈)
- self.joint_targets[self.joint_name_to_idx["left_hip_pitch_joint"]] = 0.1 # 降低髋俯仰,避免重心后移
- self.joint_targets[self.joint_name_to_idx["left_hip_roll_joint"]] = 0.02
- self.joint_targets[self.joint_name_to_idx["left_hip_yaw_joint"]] = 0.0
- self.joint_targets[self.joint_name_to_idx["left_knee_joint"]] = -0.4 # 膝盖稍弯,增加缓冲
- self.joint_targets[self.joint_name_to_idx["left_ankle_pitch_joint"]] = 0.0 # 踝关节0度,脚掌完全落地(核心)
- self.joint_targets[self.joint_name_to_idx["left_ankle_roll_joint"]] = 0.0
-
- # 右腿关节(镜像左腿)
- self.joint_targets[self.joint_name_to_idx["right_hip_pitch_joint"]] = 0.1
- self.joint_targets[self.joint_name_to_idx["right_hip_roll_joint"]] = -0.02
- self.joint_targets[self.joint_name_to_idx["right_hip_yaw_joint"]] = 0.0
- self.joint_targets[self.joint_name_to_idx["right_knee_joint"]] = -0.4
- self.joint_targets[self.joint_name_to_idx["right_ankle_pitch_joint"]] = 0.0 # 踝关节0度(核心)
- self.joint_targets[self.joint_name_to_idx["right_ankle_roll_joint"]] = 0.0
-
- # 腰部/手臂(固定)
- self.joint_targets[self.joint_name_to_idx["waist_yaw_joint"]] = 0.0
- self.joint_targets[self.joint_name_to_idx["left_shoulder_pitch_joint"]] = 0.6
- self.joint_targets[self.joint_name_to_idx["left_elbow_joint"]] = 1.8
- self.joint_targets[self.joint_name_to_idx["right_shoulder_pitch_joint"]] = 0.6
- self.joint_targets[self.joint_name_to_idx["right_elbow_joint"]] = 1.8
+ # 腰部关节:微前倾,匹配自然站立
+ self.joint_targets[self.joint_name_to_idx["abdomen_z"]] = 0.0
+ self.joint_targets[self.joint_name_to_idx["abdomen_y"]] = 0.05 # 微前倾,防止后仰
+ self.joint_targets[self.joint_name_to_idx["abdomen_x"]] = 0.0
+
+ # 右腿关节:微弯,增加缓冲
+ self.joint_targets[self.joint_name_to_idx["hip_x_right"]] = 0.0
+ self.joint_targets[self.joint_name_to_idx["hip_z_right"]] = 0.0
+ self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.05
+ self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.35 # 膝盖微弯,减少落地冲击
+ self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = 0.0
+ self.joint_targets[self.joint_name_to_idx["ankle_x_right"]] = 0.0
+
+ # 左腿关节:镜像右腿,保证对称
+ self.joint_targets[self.joint_name_to_idx["hip_x_left"]] = 0.0
+ self.joint_targets[self.joint_name_to_idx["hip_z_left"]] = 0.0
+ self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.05
+ self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.35
+ self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = 0.0
+ self.joint_targets[self.joint_name_to_idx["ankle_x_left"]] = 0.0
+
+ # 手臂关节:自然下垂,减少干扰
+ self.joint_targets[self.joint_name_to_idx["shoulder1_right"]] = 0.0
+ self.joint_targets[self.joint_name_to_idx["shoulder2_right"]] = 0.2
+ self.joint_targets[self.joint_name_to_idx["elbow_right"]] = 1.6
+ self.joint_targets[self.joint_name_to_idx["shoulder1_left"]] = 0.0
+ self.joint_targets[self.joint_name_to_idx["shoulder2_left"]] = 0.2
+ self.joint_targets[self.joint_name_to_idx["elbow_left"]] = 1.6
mujoco.mj_forward(self.model, self.data)
def _quat_to_euler_xyz(self, quat):
- """纯Numpy四元数转欧拉角"""
+ """四元数转欧拉角(保留)"""
w, x, y, z = quat
- # Roll (X)
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = np.arctan2(sinr_cosp, cosr_cosp)
- # Pitch (Y)
sinp = 2 * (w * y - z * x)
pitch = np.where(np.abs(sinp) >= 1, np.copysign(np.pi / 2, sinp), np.arcsin(sinp))
- # Yaw (Z)
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = np.arctan2(siny_cosp, cosy_cosp)
return np.array([roll, pitch, yaw])
def _get_root_euler(self):
- """提取躯干欧拉角"""
+ """提取躯干欧拉角(保留,增加限幅)"""
quat = self.data.qpos[3:7].astype(np.float64).copy()
euler = self._quat_to_euler_xyz(quat)
euler = np.mod(euler + np.pi, 2 * np.pi) - np.pi
- return np.clip(euler, -0.3, 0.3) # 进一步限制最大倾角
+ return np.clip(euler, -0.2, 0.2) # 更严格的倾角限制,防止过度倾斜
def _detect_foot_contact(self):
- """优化接触检测,确保脚掌落地判定准确"""
+ """优化接触检测:5帧历史滤波+迟滞,避免频繁切换"""
try:
- left_foot_geoms = ["left_foot_1_col", "left_foot_2_col", "left_foot_3_col", "left_foot_4_col"]
- right_foot_geoms = ["right_foot_1_col", "right_foot_2_col", "right_foot_3_col", "right_foot_4_col"]
+ left_foot_geoms = ["foot1_left", "foot2_left"]
+ right_foot_geoms = ["foot1_right", "foot2_right"]
+ # 计算当前接触力
left_force = 0.0
for geom_name in left_foot_geoms:
geom_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_GEOM, geom_name)
@@ -174,244 +191,270 @@ def _detect_foot_contact(self):
mujoco.mj_contactForce(self.model, self.data, geom_id, force)
right_force += np.linalg.norm(force[:3])
- # 迟滞优化:防止接触状态频繁切换
- self.foot_contact[1] = 1 if left_force > self.foot_contact_threshold else 0
- self.foot_contact[0] = 1 if right_force > self.foot_contact_threshold else 0
+ # 1. 更新接触历史(滑动窗口)
+ self.foot_contact_history[0] = np.roll(self.foot_contact_history[0], -1)
+ self.foot_contact_history[1] = np.roll(self.foot_contact_history[1], -1)
+ self.foot_contact_history[0, -1] = 1 if right_force > self.foot_contact_threshold else 0
+ self.foot_contact_history[1, -1] = 1 if left_force > self.foot_contact_threshold else 0
+
+ # 2. 多数表决:5帧中≥3帧接触则判定为支撑
+ self.foot_contact[0] = 1 if np.sum(self.foot_contact_history[0]) >= 3 else 0
+ self.foot_contact[1] = 1 if np.sum(self.foot_contact_history[1]) >= 3 else 0
except Exception as e:
print(f"接触检测警告: {e}")
self.foot_contact = np.ones(2)
+ self.foot_contact_history = np.ones((2, 5))
- # 【行走功能新增】更新步态相位
def _update_gait_phase(self):
- """更新步态相位,确保初始稳定后触发行走"""
+ """步态相位更新(保留)"""
if self.walk_start_time is None:
- self.walk_start_time = self.data.time # 标记行走开始时间
- # 计算归一化步态相位(0-1循环)
+ self.walk_start_time = self.data.time
self.gait_phase = (self.data.time - self.walk_start_time) % self.gait_cycle / self.gait_cycle
- # 【行走功能新增】生成行走的关节目标角度
def _update_walk_joint_targets(self):
- """基于步态相位更新关节目标角度(核心行走逻辑)"""
- # 仅在初始稳定期结束后启动行走
+ """优化步态规划:余弦插值+动态重心,动作更平滑"""
if self.data.time < self.init_wait_time:
- return # 初始稳定期不修改目标角度
+ # 初始稳定阶段:渐进式提升目标,避免突变
+ alpha = min(1.0, (self.data.time) / self.init_wait_time)
+ self.joint_targets = self.joint_targets * alpha + self.joint_targets_prev * (1 - alpha)
+ # ========== 关键修复2:初始阶段保持com_target为基础值 ==========
+ self.com_target = self.com_base_target.copy()
+ return
self._update_gait_phase()
- phase = self.gait_phase # 0-1
+ phase = self.gait_phase
+
+ # 核心优化:使用余弦插值代替正弦,加速度更平滑(减少冲击)
+ phase_rad = phase * 2 * np.pi
+ cos_phase = (1 - np.cos(phase_rad)) / 2 # 0→1→0,无加速度突变
- # ========== 左腿/右腿交替摆动逻辑 ==========
+ # 动态重心目标:随步态相位前移/后移,匹配迈步节奏
if phase < 0.5:
- # 0-0.5周期:右腿摆动(向前迈),左腿支撑
- # 右腿髋关节:向前摆(正弦曲线,0→最大值→0)
- right_hip_pitch = 0.1 + self.step_offset_hip * np.sin(phase * 2 * np.pi)
- # 右腿膝关节:弯曲抬腿(正弦曲线,0→最小值→0)
- right_knee = -0.4 - self.step_offset_knee * np.sin(phase * 2 * np.pi)
- # 右腿踝关节:协调(随膝关节联动)
- right_ankle = 0.0 + self.step_offset_ankle * np.sin(phase * 2 * np.pi)
-
- # 更新右腿目标角度
- self.joint_targets[self.joint_name_to_idx["right_hip_pitch_joint"]] = right_hip_pitch
- self.joint_targets[self.joint_name_to_idx["right_knee_joint"]] = right_knee
- self.joint_targets[self.joint_name_to_idx["right_ankle_pitch_joint"]] = right_ankle
-
- # 左腿保持支撑姿态(小幅调整平衡)
- self.joint_targets[self.joint_name_to_idx["left_hip_pitch_joint"]] = 0.1 - self.step_offset_hip * 0.2
- self.joint_targets[self.joint_name_to_idx["left_knee_joint"]] = -0.4 + self.step_offset_knee * 0.1
+ # 右腿摆动:重心小幅右移+前移
+ self.com_target = self.com_base_target + np.array([0.05, 0.03, 0.0])
+ # 右腿关节目标:余弦插值,动作更平滑
+ right_hip_yaw = 0.05 + self.step_offset_hip * cos_phase
+ right_knee = -0.35 - self.step_offset_knee * cos_phase
+ right_ankle = 0.0 + self.step_offset_ankle * cos_phase
+
+ self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = right_hip_yaw
+ self.joint_targets[self.joint_name_to_idx["knee_right"]] = right_knee
+ self.joint_targets[self.joint_name_to_idx["ankle_y_right"]] = right_ankle
+
+ # 左腿支撑:微调整,保持平衡
+ self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = 0.05 - self.step_offset_hip * 0.1
+ self.joint_targets[self.joint_name_to_idx["knee_left"]] = -0.35 + self.step_offset_knee * 0.05
else:
- # 0.5-1.0周期:左腿摆动(向前迈),右腿支撑
- # 左腿髋关节:向前摆
- left_hip_pitch = 0.1 + self.step_offset_hip * np.sin((phase - 0.5) * 2 * np.pi)
- # 左腿膝关节:弯曲抬腿
- left_knee = -0.4 - self.step_offset_knee * np.sin((phase - 0.5) * 2 * np.pi)
- # 左腿踝关节:协调
- left_ankle = 0.0 + self.step_offset_ankle * np.sin((phase - 0.5) * 2 * np.pi)
-
- # 更新左腿目标角度
- self.joint_targets[self.joint_name_to_idx["left_hip_pitch_joint"]] = left_hip_pitch
- self.joint_targets[self.joint_name_to_idx["left_knee_joint"]] = left_knee
- self.joint_targets[self.joint_name_to_idx["left_ankle_pitch_joint"]] = left_ankle
-
- # 右腿保持支撑姿态
- self.joint_targets[self.joint_name_to_idx["right_hip_pitch_joint"]] = 0.1 - self.step_offset_hip * 0.2
- self.joint_targets[self.joint_name_to_idx["right_knee_joint"]] = -0.4 + self.step_offset_knee * 0.1
+ # 左腿摆动:重心小幅左移+前移
+ self.com_target = self.com_base_target + np.array([0.05, -0.03, 0.0])
+ # 左腿关节目标:余弦插值
+ left_phase_rad = (phase - 0.5) * 2 * np.pi
+ left_cos_phase = (1 - np.cos(left_phase_rad)) / 2
+ left_hip_yaw = 0.05 + self.step_offset_hip * left_cos_phase
+ left_knee = -0.35 - self.step_offset_knee * left_cos_phase
+ left_ankle = 0.0 + self.step_offset_ankle * left_cos_phase
+
+ self.joint_targets[self.joint_name_to_idx["hip_y_left"]] = left_hip_yaw
+ self.joint_targets[self.joint_name_to_idx["knee_left"]] = left_knee
+ self.joint_targets[self.joint_name_to_idx["ankle_y_left"]] = left_ankle
+
+ # 右腿支撑:微调整
+ self.joint_targets[self.joint_name_to_idx["hip_y_right"]] = 0.05 - self.step_offset_hip * 0.1
+ self.joint_targets[self.joint_name_to_idx["knee_right"]] = -0.35 + self.step_offset_knee * 0.05
+
+ # 关节目标低通滤波:平滑目标值,减少抖动
+ self.joint_targets = self.filter_alpha * self.joint_targets + (1 - self.filter_alpha) * self.joint_targets_prev
+ self.joint_targets_prev = self.joint_targets.copy()
def _calculate_stabilizing_torques(self):
- """优化力矩计算,抑制踮脚/后仰 + 集成行走控制"""
- # 【新增】先更新行走的关节目标角度
+ """优化力矩计算:动态增益+积分分离+速度反馈"""
self._update_walk_joint_targets()
-
torques = np.zeros(self.num_joints, dtype=np.float64)
- # 1. 躯干姿态控制(更保守)
+ # 1. 躯干姿态控制:积分分离(核心防积分饱和)
root_euler = self._get_root_euler()
root_vel = self.data.qvel[3:6].astype(np.float64).copy()
- root_vel = np.clip(root_vel, -3.0, 3.0) # 严格限制躯干角速度
+ root_vel = np.clip(root_vel, -2.0, 2.0) # 更严格的速度限制
+ # 积分分离:误差超过死区时不积分
roll_error = -root_euler[0]
- self.integral_roll += roll_error * self.dt
+ if abs(roll_error) > self.integral_deadband:
+ self.integral_roll = 0.0
+ else:
+ self.integral_roll += roll_error * self.dt
self.integral_roll = np.clip(self.integral_roll, -self.integral_limit, self.integral_limit)
- roll_torque = self.kp_roll * roll_error + self.kd_roll * (-root_vel[0]) + 5.0 * self.integral_roll
+ roll_torque = self.kp_roll * roll_error + self.kd_roll * (-root_vel[0]) + 3.0 * self.integral_roll
pitch_error = -root_euler[1]
- self.integral_pitch += pitch_error * self.dt
+ if abs(pitch_error) > self.integral_deadband:
+ self.integral_pitch = 0.0
+ else:
+ self.integral_pitch += pitch_error * self.dt
self.integral_pitch = np.clip(self.integral_pitch, -self.integral_limit, self.integral_limit)
- pitch_torque = self.kp_pitch * pitch_error + self.kd_pitch * (-root_vel[1]) + 4.0 * self.integral_pitch
+ pitch_torque = self.kp_pitch * pitch_error + self.kd_pitch * (-root_vel[1]) + 2.0 * self.integral_pitch
yaw_error = -root_euler[2]
yaw_torque = self.kp_yaw * yaw_error + self.kd_yaw * (-root_vel[2])
torso_torque = np.array([roll_torque, pitch_torque, yaw_torque])
- torso_torque = np.clip(torso_torque, -30.0, 30.0) # 降低躯干修正力矩上限
+ torso_torque = np.clip(torso_torque, -25.0, 25.0) # 降低躯干力矩上限,减少抖动
- # 2. 重心补偿(更柔和)
+ # 2. 重心补偿:新增速度反馈,抑制重心波动
com = self.data.subtree_com[0].astype(np.float64).copy()
+ self.com_vel = (com - self.prev_com) / self.dt # 计算重心速度
+ self.com_vel = np.clip(self.com_vel, -0.5, 0.5) # 速度限幅
com_error = self.com_target - com
- com_error = np.clip(com_error, -0.03, 0.03)
- com_compensation = self.kp_com * com_error
+ com_error = np.clip(com_error, -0.05, 0.05)
+ # 比例+微分补偿,减少重心震荡
+ com_compensation = self.kp_com * com_error + self.kd_com * (-self.com_vel)
- # 3. 关节控制(重点优化踝关节)
+ # 3. 关节控制:动态增益(支撑/摆动腿区分)
self._detect_foot_contact()
current_joints = self.data.qpos[7:7 + self.num_joints].astype(np.float64)
current_vel = self.data.qvel[6:6 + self.num_joints].astype(np.float64)
- current_vel = np.clip(current_vel, -8.0, 8.0) # 限制关节速度
+ current_vel = np.clip(current_vel, -5.0, 5.0) # 降低关节速度上限
- # 腿部关节控制
+ # 腰部关节控制(小幅调整)
+ waist_joints = ["abdomen_z", "abdomen_y", "abdomen_x"]
+ for joint_name in waist_joints:
+ idx = self.joint_name_to_idx[joint_name]
+ joint_error = self.joint_targets[idx] - current_joints[idx]
+ joint_error = np.clip(joint_error, -0.2, 0.2)
+ torques[idx] = self.kp_waist * joint_error - self.kd_waist * current_vel[idx]
+
+ # 腿部关节控制:动态增益(核心优化)
leg_joints = [
- "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint",
- "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint",
- "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint",
- "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint"
+ "hip_x_right", "hip_z_right", "hip_y_right",
+ "knee_right", "ankle_y_right", "ankle_x_right",
+ "hip_x_left", "hip_z_left", "hip_y_left",
+ "knee_left", "ankle_y_left", "ankle_x_left"
]
for joint_name in leg_joints:
idx = self.joint_name_to_idx[joint_name]
joint_error = self.joint_targets[idx] - current_joints[idx]
- joint_error = np.clip(joint_error, -0.3, 0.3) # 【放宽】关节误差限制,允许更大动作
+ joint_error = np.clip(joint_error, -0.2, 0.2)
- # 关节增益匹配
- if "hip" in joint_name:
- kp = self.kp_hip
- kd = self.kd_hip
- # 核心:降低髋关节俯仰的姿态补偿,避免后仰
- if "pitch" in joint_name:
- if "left" in joint_name:
- joint_error += torso_torque[1] * 0.02 # 大幅降低补偿系数
- else:
- joint_error += torso_torque[1] * 0.02
+ # 判断是支撑腿还是摆动腿,分配不同增益
+ is_support = False
+ if "right" in joint_name and self.foot_contact[0] == 1:
+ is_support = True
+ elif "left" in joint_name and self.foot_contact[1] == 1:
+ is_support = True
+ # 动态分配增益
+ if "hip" in joint_name:
+ kp = self.kp_hip_support if is_support else self.kp_hip_swing
+ kd = self.kd_hip_support if is_support else self.kd_hip_swing
+ if "y" in joint_name:
+ joint_error += torso_torque[1] * 0.01 # 降低姿态补偿,减少后仰
elif "knee" in joint_name:
- kp = self.kp_knee
- kd = self.kd_knee
- joint_error += com_compensation[2] * 0.05
-
+ kp = self.kp_knee_support if is_support else self.kp_knee_swing
+ kd = self.kd_knee_support if is_support else self.kd_knee_swing
+ joint_error += com_compensation[2] * 0.03 # 降低重心补偿,减少抖动
elif "ankle" in joint_name:
- kp = self.kp_ankle
- kd = self.kd_ankle
- # 核心:踝关节仅保留极小的姿态补偿,防止踮脚
- if "pitch" in joint_name:
- joint_error += torso_torque[1] * 0.01 # 几乎无补偿
-
- # 非支撑腿增益调整(【修改】摆动腿增益仅小幅降低,确保有力气摆动)
- if ("left" in joint_name and self.foot_contact[1] == 0) or \
- ("right" in joint_name and self.foot_contact[0] == 0):
- kp *= 0.8 # 原0.1→0.8,大幅提高摆动腿增益
- kd *= 0.9
+ kp = self.kp_ankle_support if is_support else self.kp_ankle_swing
+ kd = self.kd_ankle_support if is_support else self.kd_ankle_swing
+ if "y" in joint_name:
+ joint_error += torso_torque[1] * 0.005 # 极小的姿态补偿,防踮脚
torques[idx] = kp * joint_error - kd * current_vel[idx]
- # 腰部/手臂控制(几乎无动作)
- waist_joint = "waist_yaw_joint"
- idx = self.joint_name_to_idx[waist_joint]
- joint_error = self.joint_targets[idx] - current_joints[idx]
- torques[idx] = self.kp_waist * joint_error - self.kd_waist * current_vel[idx]
-
+ # 手臂关节控制(保留,小幅调整)
arm_joints = [
- "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint",
- "left_elbow_joint", "left_wrist_roll_joint", "right_shoulder_pitch_joint",
- "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint",
- "right_wrist_roll_joint"
+ "shoulder1_right", "shoulder2_right", "elbow_right",
+ "shoulder1_left", "shoulder2_left", "elbow_left"
]
for joint_name in arm_joints:
idx = self.joint_name_to_idx[joint_name]
joint_error = self.joint_targets[idx] - current_joints[idx]
torques[idx] = self.kp_arm * joint_error - self.kd_arm * current_vel[idx]
- # 力矩限幅(【放宽】腿部力矩限制,确保能驱动行走)
+ # 力矩限幅:根据支撑/摆动调整,支撑腿更高
torque_limits = {
- "left_hip_pitch_joint": 150, "left_hip_roll_joint": 150, "left_hip_yaw_joint": 150, # 原70→150
- "left_knee_joint": 200, "left_ankle_pitch_joint": 120, "left_ankle_roll_joint": 100, # 原100→200
- "right_hip_pitch_joint": 150, "right_hip_roll_joint": 150, "right_hip_yaw_joint": 150,
- "right_knee_joint": 200, "right_ankle_pitch_joint": 120, "right_ankle_roll_joint": 100,
- "waist_yaw_joint": 50,
- "left_shoulder_pitch_joint": 10, "left_shoulder_roll_joint": 10,
- "left_shoulder_yaw_joint": 10, "left_elbow_joint": 10, "left_wrist_roll_joint": 10,
- "right_shoulder_pitch_joint": 10, "right_shoulder_roll_joint": 10,
- "right_shoulder_yaw_joint": 10, "right_elbow_joint": 10, "right_wrist_roll_joint": 10
+ "abdomen_z": 60, "abdomen_y": 60, "abdomen_x": 60,
+ "hip_x_right": 180 if self.foot_contact[0] == 1 else 100,
+ "hip_z_right": 180 if self.foot_contact[0] == 1 else 100,
+ "hip_y_right": 180 if self.foot_contact[0] == 1 else 100,
+ "knee_right": 220 if self.foot_contact[0] == 1 else 120,
+ "ankle_y_right": 150 if self.foot_contact[0] == 1 else 80,
+ "ankle_x_right": 120 if self.foot_contact[0] == 1 else 70,
+ "hip_x_left": 180 if self.foot_contact[1] == 1 else 100,
+ "hip_z_left": 180 if self.foot_contact[1] == 1 else 100,
+ "hip_y_left": 180 if self.foot_contact[1] == 1 else 100,
+ "knee_left": 220 if self.foot_contact[1] == 1 else 120,
+ "ankle_y_left": 150 if self.foot_contact[1] == 1 else 80,
+ "ankle_x_left": 120 if self.foot_contact[1] == 1 else 70,
+ "shoulder1_right": 25, "shoulder2_right": 25, "elbow_right": 25,
+ "shoulder1_left": 25, "shoulder2_left": 25, "elbow_left": 25
}
for joint_name, limit in torque_limits.items():
idx = self.joint_name_to_idx[joint_name]
torques[idx] = np.clip(torques[idx], -limit, limit)
- # 【新增】调试输出:查看关键参数(确认行走逻辑在运行)
- if self.data.time % 1 < 0.1 and self.data.time > self.init_wait_time:
+ # 力矩输出低通滤波:减少力矩突变,提升平滑性
+ torques = self.torque_filter_alpha * torques + (1 - self.torque_filter_alpha) * self.torque_prev
+ self.torque_prev = torques.copy()
+
+ # 调试输出(保留)
+ if self.data.time % 2 < 0.1 and self.data.time > self.init_wait_time:
print(f"=== 行走调试 ===")
- print(f"步态相位: {self.gait_phase:.2f}")
- print(f"右腿髋俯仰目标: {self.joint_targets[self.joint_name_to_idx['right_hip_pitch_joint']]:.2f}")
- print(f"左腿髋俯仰目标: {self.joint_targets[self.joint_name_to_idx['left_hip_pitch_joint']]:.2f}")
+ print(f"步态相位: {self.gait_phase:.2f} | 重心: {com[0]:.3f}/{com[2]:.3f}m")
print(f"右脚接触: {self.foot_contact[0]}, 左脚接触: {self.foot_contact[1]}")
+ print(f"躯干倾角: roll={root_euler[0]:.3f}, pitch={root_euler[1]:.3f}")
self.prev_com = com
return torques
def simulate_stable_standing(self):
- """运行稳定站立+行走仿真(原方法保留,新增行走逻辑)"""
+ """优化仿真循环:渐进式启动+更严格的跌倒判定"""
with viewer.launch_passive(self.model, self.data) as v:
- # 优化相机视角
- v.cam.distance = 3.0 # 拉远相机,方便看行走
- v.cam.azimuth = 90
- v.cam.elevation = -25
- v.cam.lookat = [0, 0, 0.6]
+ # 优化相机视角:更清晰观察行走
+ v.cam.distance = 4.0
+ v.cam.azimuth = 75 # 调整视角,避免遮挡
+ v.cam.elevation = -20
+ v.cam.lookat = [0, 0, 0.8]
- print("G1机器人稳定站立+行走仿真启动...")
+ print("人形机器人高稳定性行走仿真启动...")
print(f"初始稳定{self.init_wait_time}秒后自动开始行走")
- # 初始落地阶段(更强阻尼)
+ # 初始稳定阶段:渐进式提升控制力度,避免冲击
start_time = time.time()
while time.time() - start_time < self.init_wait_time:
alpha = min(1.0, (time.time() - start_time) / self.init_wait_time)
torques = self._calculate_stabilizing_torques() * alpha
self.data.ctrl[:] = torques
mujoco.mj_step(self.model, self.data)
- self.data.qvel[:] *= 0.97 # 更强的速度衰减,防止初始后仰
+ self.data.qvel[:] *= 0.98 # 稍弱的速度衰减,避免过度制动
v.sync()
- time.sleep(self.dt)
+ time.sleep(self.dt * 0.8) # 稍快的仿真,提升流畅度
- # 主仿真循环(行走阶段)
- print("=== 开始行走 ===")
+ # 主仿真循环:更严格的跌倒判定
+ print("=== 开始稳定行走 ===")
while self.data.time < self.sim_duration:
torques = self._calculate_stabilizing_torques()
self.data.ctrl[:] = torques
mujoco.mj_step(self.model, self.data)
- # 状态监测
+ # 状态监测:增加重心速度输出
if self.data.time % 2 < 0.1:
com = self.data.subtree_com[0]
euler = self._get_root_euler()
print(
- f"时间:{self.data.time:.1f}s | 重心(x/z):{com[0]:.3f}/{com[2]:.3f}m | "
+ f"时间:{self.data.time:.1f}s | 重心(x/z/vel):{com[0]:.3f}/{com[2]:.3f}/{self.com_vel[0]:.3f}m/s | "
f"姿态(roll/pitch):{euler[0]:.3f}/{euler[1]:.3f}rad | 脚接触:{self.foot_contact}"
)
v.sync()
- time.sleep(self.dt * 0.5)
+ time.sleep(self.dt * 0.6)
- # 跌倒判定
+ # 优化跌倒判定:更严格的阈值,提前预警
com = self.data.subtree_com[0]
euler = self._get_root_euler()
- if com[2] < 0.4 or abs(euler[0]) > 0.6 or abs(euler[1]) > 0.6:
+ if com[2] < 0.6 or abs(euler[0]) > 0.4 or abs(euler[1]) > 0.4:
print(
- f"跌倒!时间:{self.data.time:.1f}s | 重心(z):{com[2]:.3f}m | "
+ f"⚠️ 跌倒预警/跌倒!时间:{self.data.time:.1f}s | 重心(z):{com[2]:.3f}m | "
f"最大倾角:{max(abs(euler[0]), abs(euler[1])):.3f}rad"
)
break
@@ -421,17 +464,16 @@ def simulate_stable_standing(self):
if __name__ == "__main__":
current_directory = os.path.dirname(os.path.abspath(__file__))
- model_file_path = os.path.join(current_directory, "g1_23dof.xml")
+ model_file_path = os.path.join(current_directory, "humanoid.xml")
print(f"模型路径:{model_file_path}")
if not os.path.exists(model_file_path):
raise FileNotFoundError(f"模型文件不存在:{model_file_path}")
try:
- stabilizer = G1Stabilizer(model_file_path)
- stabilizer.simulate_stable_standing() # 原方法不变,内部已集成行走
+ stabilizer = HumanoidStabilizer(model_file_path)
+ stabilizer.simulate_stable_standing()
except Exception as e:
print(f"错误:{e}")
import traceback
-
traceback.print_exc()
\ No newline at end of file
diff --git a/src/Neuro_Mujoco/main.py b/src/Neuro_Mujoco/main.py
index 762bee10cc..9238ae5337 100644
--- a/src/Neuro_Mujoco/main.py
+++ b/src/Neuro_Mujoco/main.py
@@ -136,22 +136,16 @@ def test_speed(
logger.error("线程数必须为正数")
return
- # 生成控制噪声(处理nu=0的情况)
- if model.nu == 0:
- ctrl = None
- logger.warning("模型无控制输入(nu=0),将跳过控制噪声")
- else:
- ctrl = ctrlnoise * np.random.randn(nstep, model.nu)
-
logger.info(f"开始速度测试: 线程数={nthread}, 每线程步数={nstep}")
def simulate_thread(thread_id: int) -> float:
- """单线程模拟函数"""
+ """单线程模拟函数(优化:线程内逐步生成噪声,降低内存占用)"""
mj_data = mujoco.MjData(model)
start = time.perf_counter()
for i in range(nstep):
- if ctrl is not None:
- mj_data.ctrl[:] = ctrl[i]
+ # 优化点:线程内逐步生成控制噪声,避免主线程预生成大数组
+ if model.nu > 0:
+ mj_data.ctrl[:] = ctrlnoise * np.random.randn(model.nu)
mujoco.mj_step(model, mj_data)
end = time.perf_counter()
duration = end - start
@@ -180,7 +174,7 @@ def simulate_thread(thread_id: int) -> float:
def visualize(model_path: str, use_ros: bool = False, policy_path: Optional[str] = None) -> None:
"""
可视化模型并运行模拟(支持ROS/策略控制)
- :param model_path: 模型文件/目录路径
+ :param model_path: 模型文件路径/目录路径
:param use_ros: 是否启用ROS模式
:param policy_path: 预训练策略模型路径(.pth)
"""
diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py
new file mode 100644
index 0000000000..285fd97222
--- /dev/null
+++ b/src/Object_Tracking_Planning/object_detection.py
@@ -0,0 +1,1842 @@
+"""Example of automatic vehicle control from client side."""
+
+from __future__ import print_function
+
+import argparse
+import collections
+import datetime
+import glob
+import logging
+import math
+import os
+import cv2
+import random
+import re
+import sys
+import weakref
+from enum import Enum
+from collections import deque
+
+
+try:
+ import pygame
+ from pygame.locals import KMOD_CTRL
+ from pygame.locals import K_ESCAPE
+ from pygame.locals import K_q
+except ImportError:
+ raise RuntimeError('cannot import pygame, make sure pygame package is installed')
+
+try:
+ import numpy as np
+except ImportError:
+ raise RuntimeError(
+ 'cannot import numpy, make sure numpy package is installed')
+
+# ==============================================================================
+# -- Find CARLA module ---------------------------------------------------------
+# ==============================================================================
+try:
+ sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
+ sys.version_info.major,
+ sys.version_info.minor,
+ 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+except IndexError:
+ pass
+
+# ==============================================================================
+# -- Add PythonAPI for release mode --------------------------------------------
+# ==============================================================================
+try:
+ sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla')
+except IndexError:
+ pass
+
+import carla
+from carla import ColorConverter as cc
+from torch.utils.tensorboard import SummaryWriter
+import torch
+#import tensorflow as tf
+
+# Create a SummaryWriter object to write the metrics to Tensorboard
+writer = SummaryWriter()
+
+from agents.navigation.global_route_planner import GlobalRoutePlanner
+from agents.navigation.global_route_planner_dao import GlobalRoutePlannerDAO
+from agents.navigation.types_behavior import Cautious, Aggressive, Normal
+
+from agents.tools.misc import get_speed, positive, draw_waypoints, distance_vehicle
+
+
+# ==============================================================================
+# -- Global functions ----------------------------------------------------------
+# ==============================================================================
+
+
+def find_weather_presets():
+ """Method to find weather presets"""
+ rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)')
+ def name(x): return ' '.join(m.group(0) for m in rgx.finditer(x))
+ presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)]
+ return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets]
+
+
+def get_actor_display_name(actor, truncate=250):
+ """Method to get actor display name"""
+ name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:])
+ return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name
+
+
+# ==============================================================================
+# -- World ---------------------------------------------------------------
+# ==============================================================================
+
+class World(object):
+ """ Class representing the surrounding environment """
+
+ def __init__(self, carla_world, hud, args):
+ """Constructor method"""
+ self.world = carla_world
+ try:
+ self.map = self.world.get_map()
+ except RuntimeError as error:
+ print('RuntimeError: {}'.format(error))
+ print(' The server could not send the OpenDRIVE (.xodr) file:')
+ print(' Make sure it exists, has the same name of your town, and is correct.')
+ sys.exit(1)
+ self.hud = hud
+ self.player = None
+ self.collision_sensor = None
+ self.lane_invasion_sensor = None
+ self.gnss_sensor = None
+ self.camera_manager = None
+ self._weather_presets = find_weather_presets()
+ self._weather_index = 0
+ self._actor_filter = args.filter
+ self._gamma = args.gamma
+ self.restart(args)
+ self.world.on_tick(hud.on_world_tick)
+ self.recording_enabled = False
+ self.recording_start = 0
+
+ def restart(self, args):
+ """Restart the world"""
+ # Keep same camera config if the camera manager exists.
+ cam_index = self.camera_manager.index if self.camera_manager is not None else 0
+ cam_pos_id = self.camera_manager.transform_index if self.camera_manager is not None else 0
+ # Set the seed if requested by user
+ if args.seed is not None:
+ random.seed(args.seed)
+
+ # KK Get a vehicle blueprint
+ #blueprint = self.world.get_blueprint_library().filter('vehicle.lincoln.mkz2017')[0] # KK
+ blueprint = self.world.get_blueprint_library().filter('model3')[0]
+ blueprint.set_attribute('role_name', 'hero')
+ if blueprint.has_attribute('color'):
+ color = '236,102,17' # KK Packt logo color
+ blueprint.set_attribute('color', color)
+ # Spawn the player.
+ print("Spawning the player")
+ if self.player is not None:
+ spawn_point = self.player.get_transform()
+ spawn_point.location.z += 2.0
+ spawn_point.rotation.roll = 0.0
+ spawn_point.rotation.pitch = 0.0
+ self.destroy()
+ self.player = self.world.try_spawn_actor(blueprint, spawn_point)
+
+ while self.player is None:
+ if not self.map.get_spawn_points():
+ print('There are no spawn points available in your map/town.')
+ print('Please add some Vehicle Spawn Point to your UE4 scene.')
+ sys.exit(1)
+ spawn_points = self.map.get_spawn_points()
+ spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
+ #spawn_point = spawn_points[0] if spawn_points else carla.Transform() # KK
+ self.player = self.world.try_spawn_actor(blueprint, spawn_point)
+ # Set up the sensors.
+ self.collision_sensor = CollisionSensor(self.player, self.hud)
+ self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud)
+ self.gnss_sensor = GnssSensor(self.player)
+ self.camera_manager = CameraManager(self.player, self.hud, self._gamma)
+ self.camera_manager.transform_index = cam_pos_id
+ self.camera_manager.set_sensor(cam_index, notify=False)
+ actor_type = get_actor_display_name(self.player)
+ self.hud.notification(actor_type)
+
+ def next_weather(self, reverse=False):
+ """Get next weather setting"""
+ self._weather_index += -1 if reverse else 1
+ self._weather_index %= len(self._weather_presets)
+ preset = self._weather_presets[self._weather_index]
+ self.hud.notification('Weather: %s' % preset[1])
+ self.player.get_world().set_weather(preset[0])
+
+ def tick(self, clock):
+ """Method for every tick"""
+ self.hud.tick(self, clock)
+
+ def render(self, display):
+ """Render world"""
+ self.camera_manager.render(display)
+ self.hud.render(display)
+
+ def destroy_sensors(self):
+ """Destroy sensors"""
+ self.camera_manager.sensor.destroy()
+ self.camera_manager.sensor = None
+ self.camera_manager.index = None
+
+ def destroy(self):
+ """Destroys all actors"""
+ actors = [
+ self.camera_manager.sensor,
+ self.collision_sensor.sensor,
+ self.lane_invasion_sensor.sensor,
+ self.gnss_sensor.sensor,
+ self.player]
+ for actor in actors:
+ if actor is not None:
+ actor.destroy()
+
+
+# ==============================================================================
+# -- KeyboardControl -----------------------------------------------------------
+# ==============================================================================
+
+
+class KeyboardControl(object):
+ def __init__(self, world):
+ world.hud.notification("Press 'H' or '?' for help.", seconds=4.0)
+
+ def parse_events(self):
+ for event in pygame.event.get():
+ if event.type == pygame.QUIT:
+ return True
+ if event.type == pygame.KEYUP:
+ if self._is_quit_shortcut(event.key):
+ return True
+
+ @staticmethod
+ def _is_quit_shortcut(key):
+ """Shortcut for quitting"""
+ return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL)
+
+# ==============================================================================
+# -- HUD -----------------------------------------------------------------------
+# ==============================================================================
+
+
+class HUD(object):
+ """Class for HUD text"""
+
+ def __init__(self, width, height):
+ """Constructor method"""
+ self.dim = (width, height)
+ font = pygame.font.Font(pygame.font.get_default_font(), 20)
+ font_name = 'courier' if os.name == 'nt' else 'mono'
+ fonts = [x for x in pygame.font.get_fonts() if font_name in x]
+ default_font = 'ubuntumono'
+ mono = default_font if default_font in fonts else fonts[0]
+ mono = pygame.font.match_font(mono)
+ self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14)
+ self._notifications = FadingText(font, (width, 40), (0, height - 40))
+ self.help = HelpText(pygame.font.Font(mono, 24), width, height)
+ self.server_fps = 0
+ self.frame = 0
+ self.simulation_time = 0
+ self._show_info = True
+ self._info_text = []
+ self._server_clock = pygame.time.Clock()
+
+ def on_world_tick(self, timestamp):
+ """Gets informations from the world at every tick"""
+ self._server_clock.tick()
+ self.server_fps = self._server_clock.get_fps()
+ self.frame = timestamp.frame_count
+ self.simulation_time = timestamp.elapsed_seconds
+
+ def tick(self, world, clock):
+ """HUD method for every tick"""
+ self._notifications.tick(world, clock)
+ if not self._show_info:
+ return
+ transform = world.player.get_transform()
+ vel = world.player.get_velocity()
+ control = world.player.get_control()
+ heading = 'N' if abs(transform.rotation.yaw) < 89.5 else ''
+ heading += 'S' if abs(transform.rotation.yaw) > 90.5 else ''
+ heading += 'E' if 179.5 > transform.rotation.yaw > 0.5 else ''
+ heading += 'W' if -0.5 > transform.rotation.yaw > -179.5 else ''
+ colhist = world.collision_sensor.get_collision_history()
+ collision = [colhist[x + self.frame - 200] for x in range(0, 200)]
+ max_col = max(1.0, max(collision))
+ collision = [x / max_col for x in collision]
+ vehicles = world.world.get_actors().filter('vehicle.*')
+
+ self._info_text = [
+ 'Server: % 16.0f FPS' % self.server_fps,
+ 'Client: % 16.0f FPS' % clock.get_fps(),
+ '',
+ 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20),
+ 'Map: % 20s' % world.map.name,
+ 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)),
+ '',
+ 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)),
+ u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (transform.rotation.yaw, heading),
+ 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (transform.location.x, transform.location.y)),
+ 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)),
+ 'Height: % 18.0f m' % transform.location.z,
+ '']
+ if isinstance(control, carla.VehicleControl):
+ self._info_text += [
+ ('Throttle:', control.throttle, 0.0, 1.0),
+ ('Steer:', control.steer, -1.0, 1.0),
+ ('Brake:', control.brake, 0.0, 1.0),
+ ('Reverse:', control.reverse),
+ ('Hand brake:', control.hand_brake),
+ ('Manual:', control.manual_gear_shift),
+ 'Gear: %s' % {-1: 'R', 0: 'N'}.get(control.gear, control.gear)]
+ elif isinstance(control, carla.WalkerControl):
+ self._info_text += [
+ ('Speed:', control.speed, 0.0, 5.556),
+ ('Jump:', control.jump)]
+ self._info_text += [
+ '',
+ 'Collision:',
+ collision,
+ '',
+ 'Number of vehicles: % 8d' % len(vehicles)]
+
+ if len(vehicles) > 1:
+ self._info_text += ['Nearby vehicles:']
+
+ def dist(l):
+ return math.sqrt((l.x - transform.location.x)**2 + (l.y - transform.location.y)
+ ** 2 + (l.z - transform.location.z)**2)
+ vehicles = [(dist(x.get_location()), x) for x in vehicles if x.id != world.player.id]
+
+ for dist, vehicle in sorted(vehicles):
+ if dist > 200.0:
+ break
+ vehicle_type = get_actor_display_name(vehicle, truncate=22)
+ self._info_text.append('% 4dm %s' % (dist, vehicle_type))
+
+ def toggle_info(self):
+ """Toggle info on or off"""
+ self._show_info = not self._show_info
+
+ def notification(self, text, seconds=2.0):
+ """Notification text"""
+ self._notifications.set_text(text, seconds=seconds)
+
+ def error(self, text):
+ """Error text"""
+ self._notifications.set_text('Error: %s' % text, (255, 0, 0))
+
+ def render(self, display):
+ """Render for HUD class"""
+ if self._show_info:
+ info_surface = pygame.Surface((220, self.dim[1]))
+ info_surface.set_alpha(100)
+ display.blit(info_surface, (0, 0))
+ v_offset = 4
+ bar_h_offset = 100
+ bar_width = 106
+ for item in self._info_text:
+ if v_offset + 18 > self.dim[1]:
+ break
+ if isinstance(item, list):
+ if len(item) > 1:
+ points = [(x + 8, v_offset + 8 + (1 - y) * 30) for x, y in enumerate(item)]
+ pygame.draw.lines(display, (255, 136, 0), False, points, 2)
+ item = None
+ v_offset += 18
+ elif isinstance(item, tuple):
+ if isinstance(item[1], bool):
+ rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6))
+ pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1)
+ else:
+ rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6))
+ pygame.draw.rect(display, (255, 255, 255), rect_border, 1)
+ fig = (item[1] - item[2]) / (item[3] - item[2])
+ if item[2] < 0.0:
+ rect = pygame.Rect(
+ (bar_h_offset + fig * (bar_width - 6), v_offset + 8), (6, 6))
+ else:
+ rect = pygame.Rect((bar_h_offset, v_offset + 8), (fig * bar_width, 6))
+ pygame.draw.rect(display, (255, 255, 255), rect)
+ item = item[0]
+ if item: # At this point has to be a str.
+ surface = self._font_mono.render(item, True, (255, 255, 255))
+ display.blit(surface, (8, v_offset))
+ v_offset += 18
+ self._notifications.render(display)
+ self.help.render(display)
+
+# ==============================================================================
+# -- FadingText ----------------------------------------------------------------
+# ==============================================================================
+
+
+class FadingText(object):
+ """ Class for fading text """
+
+ def __init__(self, font, dim, pos):
+ """Constructor method"""
+ self.font = font
+ self.dim = dim
+ self.pos = pos
+ self.seconds_left = 0
+ self.surface = pygame.Surface(self.dim)
+
+ def set_text(self, text, color=(255, 255, 255), seconds=2.0):
+ """Set fading text"""
+ text_texture = self.font.render(text, True, color)
+ self.surface = pygame.Surface(self.dim)
+ self.seconds_left = seconds
+ self.surface.fill((0, 0, 0, 0))
+ self.surface.blit(text_texture, (10, 11))
+
+ def tick(self, _, clock):
+ """Fading text method for every tick"""
+ delta_seconds = 1e-3 * clock.get_time()
+ self.seconds_left = max(0.0, self.seconds_left - delta_seconds)
+ self.surface.set_alpha(500.0 * self.seconds_left)
+
+ def render(self, display):
+ """Render fading text method"""
+ display.blit(self.surface, self.pos)
+
+# ==============================================================================
+# -- HelpText ------------------------------------------------------------------
+# ==============================================================================
+
+
+class HelpText(object):
+ """ Helper class for text render"""
+
+ def __init__(self, font, width, height):
+ """Constructor method"""
+ lines = __doc__.split('\n')
+ self.font = font
+ self.dim = (680, len(lines) * 22 + 12)
+ self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1])
+ self.seconds_left = 0
+ self.surface = pygame.Surface(self.dim)
+ self.surface.fill((0, 0, 0, 0))
+ for i, line in enumerate(lines):
+ text_texture = self.font.render(line, True, (255, 255, 255))
+ self.surface.blit(text_texture, (22, i * 22))
+ self._render = False
+ self.surface.set_alpha(220)
+
+ def toggle(self):
+ """Toggle on or off the render help"""
+ self._render = not self._render
+
+ def render(self, display):
+ """Render help text method"""
+ if self._render:
+ display.blit(self.surface, self.pos)
+
+# ==============================================================================
+# -- CollisionSensor -----------------------------------------------------------
+# ==============================================================================
+
+
+class CollisionSensor(object):
+ """ Class for collision sensors"""
+
+ def __init__(self, parent_actor, hud):
+ """Constructor method"""
+ self.sensor = None
+ self.history = []
+ self._parent = parent_actor
+ self.hud = hud
+ world = self._parent.get_world()
+ blueprint = world.get_blueprint_library().find('sensor.other.collision')
+ self.sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self._parent)
+ # We need to pass the lambda a weak reference to
+ # self to avoid circular reference.
+ weak_self = weakref.ref(self)
+ self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event))
+
+ def get_collision_history(self):
+ """Gets the history of collisions"""
+ history = collections.defaultdict(int)
+ for frame, intensity in self.history:
+ history[frame] += intensity
+ return history
+
+ @staticmethod
+ def _on_collision(weak_self, event):
+ """On collision method"""
+ self = weak_self()
+ if not self:
+ return
+ actor_type = get_actor_display_name(event.other_actor)
+ self.hud.notification('Collision with %r' % actor_type)
+ impulse = event.normal_impulse
+ intensity = math.sqrt(impulse.x ** 2 + impulse.y ** 2 + impulse.z ** 2)
+ self.history.append((event.frame, intensity))
+ if len(self.history) > 4000:
+ self.history.pop(0)
+
+# ==============================================================================
+# -- LaneInvasionSensor --------------------------------------------------------
+# ==============================================================================
+
+
+class LaneInvasionSensor(object):
+ """Class for lane invasion sensors"""
+
+ def __init__(self, parent_actor, hud):
+ """Constructor method"""
+ self.sensor = None
+ self._parent = parent_actor
+ self.hud = hud
+ world = self._parent.get_world()
+ bp = world.get_blueprint_library().find('sensor.other.lane_invasion')
+ self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent)
+ # We need to pass the lambda a weak reference to self to avoid circular
+ # reference.
+ weak_self = weakref.ref(self)
+ self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event))
+
+ @staticmethod
+ def _on_invasion(weak_self, event):
+ """On invasion method"""
+ self = weak_self()
+ if not self:
+ return
+ lane_types = set(x.type for x in event.crossed_lane_markings)
+ text = ['%r' % str(x).split()[-1] for x in lane_types]
+ self.hud.notification('Crossed line %s' % ' and '.join(text))
+
+# ==============================================================================
+# -- GnssSensor --------------------------------------------------------
+# ==============================================================================
+
+
+class GnssSensor(object):
+ """ Class for GNSS sensors"""
+
+ def __init__(self, parent_actor):
+ """Constructor method"""
+ self.sensor = None
+ self._parent = parent_actor
+ self.lat = 0.0
+ self.lon = 0.0
+ world = self._parent.get_world()
+ blueprint = world.get_blueprint_library().find('sensor.other.gnss')
+ self.sensor = world.spawn_actor(blueprint, carla.Transform(carla.Location(x=1.0, z=2.8)),
+ attach_to=self._parent)
+ # We need to pass the lambda a weak reference to
+ # self to avoid circular reference.
+ weak_self = weakref.ref(self)
+ self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event))
+
+ @staticmethod
+ def _on_gnss_event(weak_self, event):
+ """GNSS method"""
+ self = weak_self()
+ if not self:
+ return
+ self.lat = event.latitude
+ self.lon = event.longitude
+
+# ==============================================================================
+# -- CameraManager -------------------------------------------------------------
+# ==============================================================================
+
+
+class CameraManager(object):
+ """ Class for camera management"""
+
+ def __init__(self, parent_actor, hud, gamma_correction):
+ """Constructor method"""
+ self.sensor = None
+ self.surface = None
+ self._parent = parent_actor
+ self.hud = hud
+ self.recording = False
+ bound_y = 0.5 + self._parent.bounding_box.extent.y
+ attachment = carla.AttachmentType
+ self._camera_transforms = [
+ (carla.Transform(
+ carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), attachment.SpringArm),
+ (carla.Transform(
+ carla.Location(x=1.6, z=1.7)), attachment.Rigid),
+ (carla.Transform(
+ carla.Location(x=5.5, y=1.5, z=1.5)), attachment.SpringArm),
+ (carla.Transform(
+ carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), attachment.SpringArm),
+ (carla.Transform(
+ carla.Location(x=-1, y=-bound_y, z=0.5)), attachment.Rigid)]
+ self.transform_index = 1
+ self.sensors = [
+ ['sensor.camera.rgb', cc.Raw, 'Camera RGB'],
+ ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'],
+ ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'],
+ ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'],
+ ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'],
+ ['sensor.camera.semantic_segmentation', cc.CityScapesPalette,
+ 'Camera Semantic Segmentation (CityScapes Palette)'],
+ ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']]
+ world = self._parent.get_world()
+ bp_library = world.get_blueprint_library()
+ for item in self.sensors:
+ blp = bp_library.find(item[0])
+ if item[0].startswith('sensor.camera'):
+ blp.set_attribute('image_size_x', str(hud.dim[0]))
+ blp.set_attribute('image_size_y', str(hud.dim[1]))
+ if blp.has_attribute('gamma'):
+ blp.set_attribute('gamma', str(gamma_correction))
+ elif item[0].startswith('sensor.lidar'):
+ blp.set_attribute('range', '50')
+ item.append(blp)
+ self.index = None
+
+ def toggle_camera(self):
+ """Activate a camera"""
+ self.transform_index = (self.transform_index + 1) % len(self._camera_transforms)
+ self.set_sensor(self.index, notify=False, force_respawn=True)
+
+ def set_sensor(self, index, notify=True, force_respawn=False):
+ """Set a sensor"""
+ index = index % len(self.sensors)
+ needs_respawn = True if self.index is None else (
+ force_respawn or (self.sensors[index][0] != self.sensors[self.index][0]))
+ if needs_respawn:
+ if self.sensor is not None:
+ self.sensor.destroy()
+ self.surface = None
+ self.sensor = self._parent.get_world().spawn_actor(
+ self.sensors[index][-1],
+ self._camera_transforms[self.transform_index][0],
+ attach_to=self._parent,
+ attachment_type=self._camera_transforms[self.transform_index][1])
+
+ # We need to pass the lambda a weak reference to
+ # self to avoid circular reference.
+ weak_self = weakref.ref(self)
+ self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image))
+ if notify:
+ self.hud.notification(self.sensors[index][2])
+ self.index = index
+
+ def next_sensor(self):
+ """Get the next sensor"""
+ self.set_sensor(self.index + 1)
+
+ def toggle_recording(self):
+ """Toggle recording on or off"""
+ self.recording = not self.recording
+ self.hud.notification('Recording %s' % ('On' if self.recording else 'Off'))
+
+ def render(self, display):
+ """Render method"""
+ if self.surface is not None:
+ display.blit(self.surface, (0, 0))
+
+ @staticmethod
+ def _parse_image(weak_self, image):
+ self = weak_self()
+ if not self:
+ return
+ if self.sensors[self.index][0].startswith('sensor.lidar'):
+ points = np.frombuffer(image.raw_data, dtype=np.dtype('f4'))
+ points = np.reshape(points, (int(points.shape[0] / 3), 3))
+ lidar_data = np.array(points[:, :2])
+ lidar_data *= min(self.hud.dim) / 100.0
+ lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1])
+ lidar_data = np.fabs(lidar_data) # pylint: disable=assignment-from-no-return
+ lidar_data = lidar_data.astype(np.int32)
+ lidar_data = np.reshape(lidar_data, (-1, 2))
+ lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3)
+ lidar_img = np.zeros(lidar_img_size)
+ lidar_img[tuple(lidar_data.T)] = (255, 255, 255)
+ self.surface = pygame.surfarray.make_surface(lidar_img)
+ else:
+ image.convert(self.sensors[self.index][1])
+ array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
+ array = np.reshape(array, (image.height, image.width, 4))
+ array = array[:, :, :3]
+ array = array[:, :, ::-1]
+
+ #########################################################################################################################################
+############################################################ yolov3 ####################################################################
+ def calculate_iou(box1, box2):
+ """
+ Calculate the Intersection over Union (IoU) between two bounding boxes.
+
+ Args:
+ box1 (list): List containing the coordinates of the first bounding box [x, y, w, h].
+ box2 (list): List containing the coordinates of the second bounding box [x, y, w, h].
+
+ Returns:
+ float: The IoU value.
+
+ """
+ # Extract box coordinates
+ x1, y1, w1, h1 = box1
+ x2, y2, w2, h2 = box2
+
+ # Calculate box coordinates
+ x_left = max(x1, x2)
+ y_top = max(y1, y2)
+ x_right = min(x1 + w1, x2 + w2)
+ y_bottom = min(y1 + h1, y2 + h2)
+
+ # Calculate intersection area
+ intersection_area = max(0, x_right - x_left) * max(0, y_bottom - y_top)
+
+ # Calculate union area
+ box1_area = w1 * h1
+ box2_area = w2 * h2
+ union_area = box1_area + box2_area - intersection_area
+
+ # Calculate IoU
+ iou = intersection_area / union_area
+
+ return iou
+
+
+ test_im = np.array(image.raw_data)
+ test_im = test_im.copy()
+ test_im = test_im.reshape((image.height, image.width, 4))
+ test_im = test_im[:, :, :3]
+
+ # Load YOLO
+ model_weights = 'yolov3.cfg'
+ model_cfg = 'yolov3.weights'
+ #frameWidth = 1200
+ #frameHeight = 720
+ frameWidth = 640
+ frameHeight = 480
+ net = cv2.dnn.readNet(model_weights, model_cfg)
+
+ # Define your loss function
+ #loss_fn = tf.keras.losses.BinaryCrossentropy()
+
+ classes = []
+ with open("coco.names", "r") as f:
+ classes = [line.strip() for line in f.readlines()]
+
+ layers_names = net.getLayerNames()
+ #output_layers = [layers_names[i[0] - 1] for i in net.getUnconnectedOutLayers()]
+ output_layers = [layers_names[i - 1] for i in net.getUnconnectedOutLayers()]
+ colors = np.random.uniform(0, 255, size=(len(classes), 3))
+
+ # Loading image
+ img = cv2.resize(test_im, (frameWidth, frameHeight), None)
+ height, width, channels = img.shape
+ img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
+
+ # Detect objects
+ blob = cv2.dnn.blobFromImage(img, 1/255, (416, 416), (0, 0, 0), True, crop=False)
+ net.setInput(blob)
+ outs = net.forward(output_layers)
+
+ # Perform object detection
+ class_ids = []
+ confidences = []
+ boxes = []
+
+ for out in outs:
+ for detection in out:
+ scores = detection[5:]
+ class_id = np.argmax(scores)
+ confidence = scores[class_id]
+
+ if confidence > 0.5:
+ # Object detected
+ center_x = int(detection[0] * width)
+ center_y = int(detection[1] * height)
+ w = int(detection[2] * width)
+ h = int(detection[3] * height)
+
+ # Rectangle coordinates
+ x = int(center_x - w / 2)
+ y = int(center_y - h / 2)
+
+ boxes.append([x, y, w, h])
+ confidences.append(float(confidence))
+ class_ids.append(class_id)
+
+ indexes = cv2.dnn.NMSBoxes(boxes, confidences, 0.5, 0.4)
+ font = cv2.FONT_HERSHEY_SIMPLEX
+ colors = np.random.uniform(0, 255, size=(len(boxes), 3))
+
+ detected_labels = []
+ detected_boxes = []
+ predicted_confidences = []
+ predicted_class_probs = []
+
+ for i in range(len(boxes)):
+ if i in indexes:
+ x, y, w, h = boxes[i]
+ label = str(classes[class_ids[i]])
+ confidence = str(round(confidences[i], 2))
+ color = colors[i]
+ cv2.rectangle(img, (x, y), (x + w, y + h), color, 2)
+ cv2.putText(img, label + " " + confidence, (x + 60, y + 60), font, 0.8, (255, 255, 200), 1)
+ detected_labels.append(label)
+ detected_boxes.append([x, y, w, h])
+ predicted_confidences.append(float(confidence))
+ predicted_class_probs.append(label)
+
+ # Ground truth annotations
+ ground_truth_labels = ['person', 'bicycle', 'car', 'traffic light', 'fire hydrant', 'pottedplant', 'stop sign', 'bench']
+ ground_truth_boxes = [[250, 220, 160, 190], [200, 200, 80, 80], [200, 200, 80, 80], [450, 90, 20, 50], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80], [200, 200, 80, 80]] # [x, y, w, h] format
+
+ #def normalize_boxes(boxes, image_width, image_height):
+ # Convert box coordinates to numpy array
+ # boxes = np.array(boxes)
+
+ # Extract box coordinates
+ #x, y, w, h = np.split(boxes, 4, axis=1)
+
+ # Normalize box coordinates
+ #normalized_x = x / image_width
+ #normalized_y = y / image_height
+ #normalized_w = w / image_width
+ #normalized_h = h / image_height
+
+ # Concatenate normalized coordinates
+ # normalized_boxes = np.concatenate((normalized_x, normalized_y, normalized_w, normalized_h), axis=1)
+
+ #return normalized_boxes
+ '''
+ def preprocess_ground_truth(ground_truth_labels, ground_truth_boxes):
+ # Define a dictionary to map class labels to class IDs
+ label_to_id = {
+ 'person': 0,
+ 'bicycle': 1,
+ 'car': 2,
+ 'traffic light': 3,
+ 'fire hydrant': 4,
+ 'pottedplant': 5,
+ 'stop sign': 6,
+ 'bench': 7
+ }
+ '''
+
+ # Convert class labels to class IDs#
+ '''
+ class_ids = [label_to_id[label] for label in ground_truth_labels]
+
+ # Convert class IDs to one-hot encoding
+ num_classes = len(label_to_id)
+ one_hot_labels = tf.one_hot(class_ids, num_classes)
+
+ # Normalize bounding box coordinates
+ normalized_boxes = normalize_boxes(ground_truth_boxes, width, height) # Implement 'normalize_boxes' function to scale box coordinates between 0 and 1
+
+ print('Normalized Boxes Shape:', normalized_boxes.shape)
+ print('One-Hot Labels Shape:', one_hot_labels.shape)
+ print('Normalized Boxes Data Type:', normalized_boxes.dtype)
+ print('One-Hot Labels Data Type:', one_hot_labels.dtype)
+
+ if normalized_boxes.shape != one_hot_labels.shape:
+ print('Error: Shapes of normalized_boxes and one_hot_labels do not match.')
+ return None
+
+ if normalized_boxes.dtype != one_hot_labels.dtype:
+ print('Error: Data types of normalized_boxes and one_hot_labels do not match.')
+ return None
+
+ # Combine labels and boxes into a single tensor
+ ground_truth_targets = tf.concat([normalized_boxes, one_hot_labels], axis=-1)
+
+ return ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id
+ # Call the function and store the returned values
+ #ground_truth_targets, normalized_boxes, one_hot_labels, label_to_id = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes)
+
+ #num_classes = len(label_to_id)
+ '''
+
+
+ # Perform object matching for ground truth and detected objects
+ TP = 0 # True positives
+ FP = 0 # False positives
+ FN = 0 # False negatives
+ threshold = 0.5
+ print("Detected Labels: ", detected_labels)
+ print("Detected boxes: ", detected_boxes)
+
+ for i, (gt_label, gt_box) in enumerate(zip(ground_truth_labels, ground_truth_boxes)):
+ max_iou = 0
+ max_iou_index = -1
+
+ for j, (det_label, det_box) in enumerate(zip(detected_labels, detected_boxes)):
+ iou = calculate_iou(gt_box, det_box)
+ if iou > max_iou:
+ max_iou = iou
+ max_iou_index = j
+
+ if max_iou >= threshold:
+ # Matched object found (True positive)
+ TP += 1
+ # Remove the matched object from detected objects
+ if max_iou_index != -1:
+ detected_labels.pop(max_iou_index)
+ detected_boxes.pop(max_iou_index)
+ else:
+ # No match found (False negative)
+ FN += 1
+
+ # Any remaining unmatched detected objects are false positives
+ FP = len(detected_labels)
+
+ eps = 1e-7 # Small value to avoid division by zero
+
+ # Calculate performance metrics
+ accuracy = TP / (TP + FP + FN + eps)
+ precision = TP / (TP + FP + eps)
+ recall = TP / (TP + FN + eps)
+ matt_corr_coeff = (TP - (FP * FN)) / math.sqrt((TP + FP + eps) * (TP + FN + eps) * (FP + eps) * (FN + eps))
+
+
+ # Calculate the loss
+ #ground_truth_targets = preprocess_ground_truth(ground_truth_labels, ground_truth_boxes) # Preprocess the ground truth targets
+
+ # localization_loss = loss_fn(detected_boxes, ground_truth_targets) # Calculate the localization loss
+ # confidence_loss = loss_fn(predicted_confidences, ground_truth_targets) # Calculate the confidence loss
+ # class_prob_loss = loss_fn(predicted_class_probs, ground_truth_targets) # Calculate the class probability loss
+
+ # Log the metrics to Tensorboard
+ log_dir = "./logs3" # Specify the log directory her
+ writer = SummaryWriter(log_dir=log_dir)
+ writer.add_scalar('Accuracy', accuracy, image.frame)
+ writer.add_scalar('Precision', precision, image.frame)
+ writer.add_scalar('Recall', recall, image.frame)
+ writer.add_scalar('IOU', max_iou, image.frame)
+ #writer.add_scalar('Matthews Correlation Coefficient', matt_corr_coeff)
+ #writer.add_scalar('Localization Loss', localization_loss, image.frame)
+ #writer.add_scalar('Confidence Loss', confidence_loss, image.frame)
+ #writer.add_scalar('Class Probability Loss', class_prob_loss, image.frame)
+
+ # Show the annotated image
+ #cv2.imshow("Annotated Image", img)
+ #cv2.waitKey(0)
+ #cv2.destroyAllWindows()
+
+ self.surface = pygame.surfarray.make_surface(img.swapaxes(0, 1))
+ if self.recording:
+ image.save_to_disk('output/%08d' % image.frame)
+
+
+# ==============================================================================
+# -- KK PID Controllers ---------------------------------------------------------
+# ==============================================================================
+
+class VehiclePIDController():
+ """
+ VehiclePIDController is the combination of two PID controllers
+ (lateral and longitudinal) to perform the
+ low level control a vehicle from client side
+ """
+
+
+ def __init__(self, vehicle, args_lateral, args_longitudinal, max_throttle=0.75, max_brake=0.3, max_steering=0.8):
+ """
+ Constructor method.
+
+ :param vehicle: actor to apply to local planner logic onto
+ :param args_lateral: dictionary of arguments to set the lateral PID controller
+ using the following semantics:
+ K_P -- Proportional term
+ K_D -- Differential term
+ K_I -- Integral term
+ :param args_longitudinal: dictionary of arguments to set the longitudinal
+ PID controller using the following semantics:
+ K_P -- Proportional term
+ K_D -- Differential term
+ K_I -- Integral term
+ """
+
+ self.max_brake = max_brake
+ self.max_throt = max_throttle
+ self.max_steer = max_steering
+
+ self._vehicle = vehicle
+ self._world = self._vehicle.get_world()
+ self.past_steering = self._vehicle.get_control().steer
+ self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal)
+ self._lat_controller = PIDLateralController(self._vehicle, **args_lateral)
+
+ def run_step(self, target_speed, waypoint):
+ """
+ Execute one step of control invoking both lateral and longitudinal
+ PID controllers to reach a target waypoint
+ at a given target_speed.
+
+ :param target_speed: desired vehicle speed
+ :param waypoint: target location encoded as a waypoint
+ :return: distance (in meters) to the waypoint
+ """
+
+ acceleration = self._lon_controller.run_step(target_speed)
+ current_steering = self._lat_controller.run_step(waypoint)
+ control = carla.VehicleControl()
+ if acceleration >= 0.0:
+ control.throttle = min(acceleration, self.max_throt)
+ control.brake = 0.0
+ else:
+ control.throttle = 0.0
+ control.brake = min(abs(acceleration), self.max_brake)
+
+ # Steering regulation: changes cannot happen abruptly, can't steer too much.
+
+ if current_steering > self.past_steering + 0.1:
+ current_steering = self.past_steering + 0.1
+ elif current_steering < self.past_steering - 0.1:
+ current_steering = self.past_steering - 0.1
+
+ if current_steering >= 0:
+ steering = min(self.max_steer, current_steering)
+ else:
+ steering = max(-self.max_steer, current_steering)
+
+ control.steer = steering
+ control.hand_brake = False
+ control.manual_gear_shift = False
+ self.past_steering = steering
+
+ return control
+
+
+class PIDLongitudinalController():
+ """
+ PIDLongitudinalController implements longitudinal control using a PID.
+ """
+
+
+ def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03):
+ """
+ Constructor method.
+
+ :param vehicle: actor to apply to local planner logic onto
+ :param K_P: Proportional term
+ :param K_D: Differential term
+ :param K_I: Integral term
+ :param dt: time differential in seconds
+ """
+ self._vehicle = vehicle
+ self._k_p = K_P
+ self._k_d = K_D
+ self._k_i = K_I
+ self._dt = dt
+ self._error_buffer = deque(maxlen=10)
+
+ def run_step(self, target_speed, debug=False):
+ """
+ Execute one step of longitudinal control to reach a given target speed.
+
+ :param target_speed: target speed in Km/h
+ :param debug: boolean for debugging
+ :return: throttle control
+ """
+ current_speed = get_speed(self._vehicle) / 2
+ target_speed = target_speed / 2
+ current_speed = get_speed(self._vehicle)
+
+ if debug:
+ print('Current speed = {}'.format(current_speed))
+
+ return self._pid_control(target_speed, current_speed)
+
+ def _pid_control(self, target_speed, current_speed):
+ """
+ Estimate the throttle/brake of the vehicle based on the PID equations
+
+ :param target_speed: target speed in Km/h
+ :param current_speed: current speed of the vehicle in Km/h
+ :return: throttle/brake control
+ """
+
+ error = target_speed - current_speed
+ self._error_buffer.append(error)
+
+ if len(self._error_buffer) >= 2:
+ _de = (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt
+ _ie = sum(self._error_buffer) * self._dt
+ else:
+ _de = 0.0
+ _ie = 0.0
+
+ return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)
+
+
+class PIDLateralController():
+ """
+ PIDLateralController implements lateral control using a PID.
+ """
+
+ def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03):
+ """
+ Constructor method.
+
+ :param vehicle: actor to apply to local planner logic onto
+ :param K_P: Proportional term
+ :param K_D: Differential term
+ :param K_I: Integral term
+ :param dt: time differential in seconds
+ """
+ self._vehicle = vehicle
+ self._k_p = K_P
+ self._k_d = K_D
+ self._k_i = K_I
+ self._dt = dt
+ self._e_buffer = deque(maxlen=10)
+
+ def run_step(self, waypoint):
+ """
+ Execute one step of lateral control to steer
+ the vehicle towards a certain waypoin.
+
+ :param waypoint: target waypoint
+ :return: steering control in the range [-1, 1] where:
+ -1 maximum steering to left
+ +1 maximum steering to right
+ """
+ return self._pid_control(waypoint, self._vehicle.get_transform())
+
+ def _pid_control(self, waypoint, vehicle_transform):
+ """
+ Estimate the steering angle of the vehicle based on the PID equations
+
+ :param waypoint: target waypoint
+ :param vehicle_transform: current transform of the vehicle
+ :return: steering control in the range [-1, 1]
+ """
+ v_begin = vehicle_transform.location
+ v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)),
+ y=math.sin(math.radians(vehicle_transform.rotation.yaw)))
+
+ v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0])
+ w_vec = np.array([waypoint.transform.location.x -
+ v_begin.x, waypoint.transform.location.y -
+ v_begin.y, 0.0])
+ _dot = math.acos(np.clip(np.dot(w_vec, v_vec) /
+ (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0))
+
+ _cross = np.cross(v_vec, w_vec)
+
+ if _cross[2] < 0:
+ _dot *= -1.0
+
+ self._e_buffer.append(_dot)
+ if len(self._e_buffer) >= 2:
+ _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt
+ _ie = sum(self._e_buffer) * self._dt
+ else:
+ _de = 0.0
+ _ie = 0.0
+
+ return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)
+
+
+# ==============================================================================
+# -- KK Local Planner ---------------------------------------------------------
+# ==============================================================================
+
+class RoadOption(Enum):
+ """
+ RoadOption represents the possible topological configurations
+ when moving from a segment of lane to other.
+ """
+ VOID = -1
+ LEFT = 1
+ RIGHT = 2
+ STRAIGHT = 3
+ LANEFOLLOW = 4
+ CHANGELANELEFT = 5
+ CHANGELANERIGHT = 6
+
+
+class LocalPlanner(object):
+ """
+ LocalPlanner implements the basic behavior of following a trajectory
+ of waypoints that is generated on-the-fly.
+ The low-level motion of the vehicle is computed by using two PID controllers,
+ one is used for the lateral control
+ and the other for the longitudinal control (cruise speed).
+
+ When multiple paths are available (intersections)
+ this local planner makes a random choice.
+ """
+
+ # Minimum distance to target waypoint as a percentage
+ # (e.g. within 80% of total distance)
+
+ # FPS used for dt
+ FPS = 20
+
+ def __init__(self, agent):
+ """
+ :param agent: agent that regulates the vehicle
+ :param vehicle: actor to apply to local planner logic onto
+ """
+ self._vehicle = agent.vehicle
+ self._map = agent.vehicle.get_world().get_map()
+
+ self._target_speed = None
+ self.sampling_radius = None
+ self._min_distance = None
+ self._current_waypoint = None
+ self.target_road_option = None
+ self._next_waypoints = None
+ self.target_waypoint = None
+ self._vehicle_controller = None
+ self._global_plan = None
+ self._pid_controller = None
+ self.waypoints_queue = deque(maxlen=20000) # queue with tuples of (waypoint, RoadOption)
+ self._buffer_size = 5
+ self._waypoint_buffer = deque(maxlen=self._buffer_size)
+
+ self._init_controller() # initializing controller
+
+ def get_distance_to_nearest_obstacle(self):
+ ego_vehicle_transform = self._vehicle.get_transform()
+ ego_vehicle_location = ego_vehicle_transform.location
+
+ # Get the bounding box of the ego vehicle
+ ego_vehicle_bbox = self._vehicle.bounding_box
+
+ # Calculate the minimum distance to other vehicles
+ distance_to_nearest_obstacle = None
+
+ for actor in self._vehicle.get_world().get_actors().filter('vehicle.*'):
+ if actor.id != self._vehicle.id:
+ actor_transform = actor.get_transform()
+ actor_location = actor_transform.location
+
+ # Calculate the distance between the ego vehicle and the other vehicle
+ distance = ego_vehicle_location.distance(actor_location)
+
+ # Calculate the minimum distance to the other vehicle's bounding box
+ distance_to_obstacle = distance - ego_vehicle_bbox.extent.x - actor.bounding_box.extent.x
+
+ if distance_to_nearest_obstacle is None or distance_to_obstacle < distance_to_nearest_obstacle:
+ distance_to_nearest_obstacle = distance_to_obstacle
+
+
+ return distance_to_nearest_obstacle
+
+ def reset_vehicle(self):
+ """Reset the ego-vehicle"""
+ self._vehicle = None
+ print("Resetting ego-vehicle!")
+
+ def _init_controller(self):
+ """
+ Controller initialization.
+
+ dt -- time difference between physics control in seconds.
+ This is can be fixed from server side
+ using the arguments -benchmark -fps=F, since dt = 1/F
+
+ target_speed -- desired cruise speed in km/h
+
+ min_distance -- minimum distance to remove waypoint from queue
+
+ lateral_dict -- dictionary of arguments to setup the lateral PID controller
+ {'K_P':, 'K_D':, 'K_I':, 'dt'}
+
+ longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller
+ {'K_P':, 'K_D':, 'K_I':, 'dt'}
+ """
+ # Default parameters
+ self.args_lat_hw_dict = {
+ 'K_P': 0.75,
+ 'K_D': 0.02,
+ 'K_I': 0.4,
+ 'dt': 1.0 / self.FPS}
+ self.args_lat_city_dict = {
+ 'K_P': 0.58,
+ 'K_D': 0.02,
+ 'K_I': 0.5,
+ 'dt': 1.0 / self.FPS}
+ self.args_long_hw_dict = {
+ 'K_P': 0.37,
+ 'K_D': 0.024,
+ 'K_I': 0.032,
+ 'dt': 1.0 / self.FPS}
+ self.args_long_city_dict = {
+ 'K_P': 0.15,
+ 'K_D': 0.05,
+ 'K_I': 0.07,
+ 'dt': 1.0 / self.FPS}
+
+ self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
+
+ self._global_plan = False
+
+ self._target_speed = self._vehicle.get_speed_limit()
+
+ self._min_distance = 3
+
+ def set_speed(self, speed):
+ """
+ Request new target speed.
+
+ :param speed: new target speed in km/h
+ """
+
+ self._target_speed = speed
+
+ def set_global_plan(self, current_plan):
+ """
+ Sets new global plan.
+
+ :param current_plan: list of waypoints in the actual plan
+ """
+ for elem in current_plan:
+ self.waypoints_queue.append(elem)
+ self._global_plan = True
+
+ def get_incoming_waypoint_and_direction(self, steps=3):
+ """
+ Returns direction and waypoint at a distance ahead defined by the user.
+
+ :param steps: number of steps to get the incoming waypoint.
+ """
+ if len(self.waypoints_queue) > steps:
+ return self.waypoints_queue[steps]
+
+ else:
+ try:
+ wpt, direction = self.waypoints_queue[-1]
+ return wpt, direction
+ except IndexError as i:
+ print(i)
+ return None, RoadOption.VOID
+ return None, RoadOption.VOID
+
+ def run_step(self, target_speed=None, debug=False):
+ """
+ Execute one step of local planning which involves
+ running the longitudinal and lateral PID controllers to
+ follow the waypoints trajectory.
+
+ :param target_speed: desired speed
+ :param debug: boolean flag to activate waypoints debugging
+ :return: control
+ """
+
+ if target_speed is not None:
+ self._target_speed = target_speed
+ else:
+ self._target_speed = self._vehicle.get_speed_limit()
+
+ if len(self.waypoints_queue) == 0:
+ control = carla.VehicleControl()
+ control.steer = 0.0
+ control.throttle = 0.0
+ control.brake = 1.0
+ control.hand_brake = False
+ control.manual_gear_shift = False
+ return control
+
+ # Buffering the waypoints
+ if not self._waypoint_buffer:
+ for i in range(self._buffer_size):
+ if self.waypoints_queue:
+ self._waypoint_buffer.append(
+ self.waypoints_queue.popleft())
+ else:
+ break
+
+ # Current vehicle waypoint
+ self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
+
+ # Target waypoint
+ self.target_waypoint, self.target_road_option = self._waypoint_buffer[0]
+
+ if target_speed > 50:
+ args_lat = self.args_lat_hw_dict
+ args_long = self.args_long_hw_dict
+ else:
+ args_lat = self.args_lat_city_dict
+ args_long = self.args_long_city_dict
+
+ self._pid_controller = VehiclePIDController(self._vehicle,
+ args_lateral=args_lat,
+ args_longitudinal=args_long)
+
+ control = self._pid_controller.run_step(self._target_speed, self.target_waypoint)
+
+ # Purge the queue of obsolete waypoints
+ vehicle_transform = self._vehicle.get_transform()
+ max_index = -1
+
+ for i, (waypoint, _) in enumerate(self._waypoint_buffer):
+ if distance_vehicle(
+ waypoint, vehicle_transform) < self._min_distance:
+ max_index = i
+ if max_index >= 0:
+ for i in range(max_index + 1):
+ self._waypoint_buffer.popleft()
+
+ if debug:
+ draw_waypoints(self._vehicle.get_world(),
+ [self.target_waypoint], 1.0)
+ return control
+
+
+# ==============================================================================
+# -- KK Agent Class ---------------------------------------------------------
+# ==============================================================================
+
+class AgentState(Enum):
+ """
+ AGENT_STATE represents the possible states of a roaming agent
+ """
+ NAVIGATING = 1
+ BLOCKED_BY_VEHICLE = 2
+ BLOCKED_RED_LIGHT = 3
+
+
+class Agent(object):
+ """Base class to define agents in CARLA"""
+
+ def __init__(self, vehicle):
+ """
+ Constructor method.
+
+ :param vehicle: actor to apply to local planner logic onto
+ """
+ self._vehicle = vehicle
+ self._proximity_tlight_threshold = 5.0 # meters
+ self._proximity_vehicle_threshold = 10.0 # meters
+ self._local_planner = None
+ self._world = self._vehicle.get_world()
+ try:
+ self._map = self._world.get_map()
+ except RuntimeError as error:
+ print('RuntimeError: {}'.format(error))
+ print(' The server could not send the OpenDRIVE (.xodr) file:')
+ print(' Make sure it exists, has the same name of your town, and is correct.')
+ sys.exit(1)
+ self._last_traffic_light = None
+
+ def get_local_planner(self):
+ """Get method for protected member local planner"""
+ return self._local_planner
+
+ @staticmethod
+ def run_step(debug=False):
+ """
+ Execute one step of navigation.
+
+ :param debug: boolean flag for debugging
+ :return: control
+ """
+ control = carla.VehicleControl()
+
+ if debug:
+ control.steer = 0.0
+ control.throttle = 0.0
+ control.brake = 0.0
+ control.hand_brake = False
+ control.manual_gear_shift = False
+
+ return control
+
+ @staticmethod
+ def emergency_stop():
+ """
+ Send an emergency stop command to the vehicle
+
+ :return: control for braking
+ """
+ control = carla.VehicleControl()
+ control.steer = 0.0
+ control.throttle = 0.0
+ control.brake = 1.0
+ control.hand_brake = False
+
+ return control
+
+
+# ==============================================================================
+# -- KK Behavior Agent Class ---------------------------------------------------------
+# ==============================================================================
+
+class BehaviorAgent(Agent):
+ """
+ BehaviorAgent implements an agent that navigates scenes to reach a given
+ target destination, by computing the shortest possible path to it.
+ This agent can correctly follow traffic signs, speed limitations,
+ traffic lights, while also taking into account nearby vehicles. Lane changing
+ decisions can be taken by analyzing the surrounding environment,
+ such as overtaking or tailgating avoidance. Adding to these are possible
+ behaviors, the agent can also keep safety distance from a car in front of it
+ by tracking the instantaneous time to collision and keeping it in a certain range.
+ Finally, different sets of behaviors are encoded in the agent, from cautious
+ to a more aggressive ones.
+ """
+
+ def __init__(self, vehicle, ignore_traffic_light=False, behavior='normal'):
+ """
+ Constructor method.
+
+ :param vehicle: actor to apply to local planner logic onto
+ :param ignore_traffic_light: boolean to ignore any traffic light
+ :param behavior: type of agent to apply
+ """
+
+ super(BehaviorAgent, self).__init__(vehicle)
+ self.vehicle = vehicle
+ self.ignore_traffic_light = ignore_traffic_light
+ self._local_planner = LocalPlanner(self)
+ self._grp = None
+ self.look_ahead_steps = 0
+
+ # Vehicle information
+ self.speed = 0
+ self.speed_limit = 0
+ self.direction = None
+ self.incoming_direction = None
+ self.incoming_waypoint = None
+ self.start_waypoint = None
+ self.end_waypoint = None
+ self.is_at_traffic_light = 0
+ self.light_state = "Green"
+ self.light_id_to_ignore = -1
+ self.min_speed = 5
+ self.behavior = None
+ self._sampling_resolution = 4.5
+
+ # Parameters for agent behavior
+ if behavior == 'cautious':
+ self.behavior = Cautious()
+
+ elif behavior == 'normal':
+ self.behavior = Normal()
+
+ elif behavior == 'aggressive':
+ self.behavior = Aggressive()
+
+ def update_information(self, world):
+ """
+ This method updates the information regarding the ego
+ vehicle based on the surrounding world.
+
+ :param world: carla.world object
+ """
+ self.speed = get_speed(self.vehicle)
+ self.speed_limit = world.player.get_speed_limit()
+ # self.speed_limit = 100 # KK set speed to 100 km/h
+ self._local_planner.set_speed(self.speed_limit)
+ self.direction = self._local_planner.target_road_option
+ if self.direction is None:
+ self.direction = RoadOption.LANEFOLLOW
+
+ self.look_ahead_steps = int(self.speed_limit / 10)
+
+ self.incoming_waypoint, self.incoming_direction = self._local_planner.get_incoming_waypoint_and_direction(
+ steps=self.look_ahead_steps)
+ if self.incoming_direction is None:
+ self.incoming_direction = RoadOption.LANEFOLLOW
+
+ self.is_at_traffic_light = world.player.is_at_traffic_light()
+ if self.ignore_traffic_light:
+ self.light_state = "Green"
+ else:
+ if self.is_at_traffic_light:
+ traffic_light = world.player.get_traffic_light()
+ self.light_state = str(traffic_light.state)
+ if self.light_id_to_ignore != -1 and traffic_light.id == self.light_id_to_ignore:
+ # Ignore this traffic light for this iteration
+ self.light_state = "Green"
+ # Get distance to the nearest obstacle
+ distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle()
+
+ # Check if the obstacle is closer than the minimum distance
+ if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0:
+ # Apply braking if the distance to the nearest obstacle is below the threshold
+ self._local_planner.brake = 1.0
+ else:
+ self._local_planner.brake = 0.0
+ #else:
+ # This method also includes stop signs and intersections.
+ #self.light_state = str(self.vehicle.get_traffic_light_state())
+
+ def set_destination(self, start_location, end_location, clean=False):
+ """
+ This method creates a list of waypoints from agent's position to destination location
+ based on the route returned by the global router.
+
+ :param start_location: initial position
+ :param end_location: final position
+ :param clean: boolean to clean the waypoint queue
+ """
+ if clean:
+ self._local_planner.waypoints_queue.clear()
+ self.start_waypoint = self._map.get_waypoint(start_location)
+ self.end_waypoint = self._map.get_waypoint(end_location)
+
+ route_trace = self._trace_route(self.start_waypoint, self.end_waypoint)
+
+ self._local_planner.set_global_plan(route_trace)
+
+ def reroute(self, spawn_points):
+ """
+ This method implements re-routing for vehicles approaching its destination.
+ It finds a new target and computes another path to reach it.
+
+ :param spawn_points: list of possible destinations for the agent
+ """
+
+ print("Target almost reached, setting new destination...")
+ random.shuffle(spawn_points)
+ new_start = self._local_planner.waypoints_queue[-1][0].transform.location
+ destination = spawn_points[0].location if spawn_points[0].location != new_start else spawn_points[1].location
+ print("New destination: " + str(destination))
+
+ self.set_destination(new_start, destination)
+
+ def _trace_route(self, start_waypoint, end_waypoint):
+ """
+ This method sets up a global router and returns the
+ optimal route from start_waypoint to end_waypoint.
+
+ :param start_waypoint: initial position
+ :param end_waypoint: final position
+ """
+ # Setting up global router
+ if self._grp is None:
+ wld = self.vehicle.get_world()
+ dao = GlobalRoutePlannerDAO(
+ wld.get_map(), sampling_resolution=self._sampling_resolution)
+ grp = GlobalRoutePlanner(dao)
+ grp.setup()
+ self._grp = grp
+
+ # Obtain route plan
+ route = self._grp.trace_route(
+ start_waypoint.transform.location,
+ end_waypoint.transform.location)
+
+ return route
+
+ def run_step(self, debug=False):
+ """
+ Execute one step of navigation.
+
+ :param debug: boolean for debugging
+ :return control: carla.VehicleControl
+ """
+ control = carla.VehicleControl()
+
+ ego_vehicle_loc = self.vehicle.get_location()
+ ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc)
+
+ if self.is_at_traffic_light and self.light_state != "Green":
+ # Stop at red traffic light
+ control = carla.VehicleControl()
+ control.throttle = 0.0
+ control.brake = 1.0
+ control.steer = 0.0
+ else:
+ # Get distance to the nearest obstacle
+ distance_to_nearest_obstacle = self._local_planner.get_distance_to_nearest_obstacle()
+
+ # Check if the obstacle is closer than the minimum distance
+ if distance_to_nearest_obstacle is not None and distance_to_nearest_obstacle < 3.0:
+ # Apply braking if the distance to the nearest obstacle is below the threshold
+ control.throttle = 0.0
+ control.brake = 1.0
+ control.steer = 0.0
+ else:
+ # Continue following the planned route
+ control = self._local_planner.run_step(
+ target_speed=min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug)
+ self.vehicle.apply_control(control)
+
+ return control
+
+
+# ==============================================================================
+# -- Game Loop ---------------------------------------------------------
+# ==============================================================================
+
+
+def game_loop(args):
+ """ Main loop for agent"""
+
+ pygame.init()
+ pygame.font.init()
+ world = None
+ tot_target_reached = 0
+ num_min_waypoints = 21
+
+ try:
+ client = carla.Client(args.host, args.port)
+ client.set_timeout(70.0)
+
+ display = pygame.display.set_mode(
+ (args.width, args.height),
+ pygame.HWSURFACE | pygame.DOUBLEBUF)
+
+ hud = HUD(args.width, args.height)
+
+ #selected_world = client.load_world("Town01") # KK Changes world to Town04
+ selected_world = client.get_world()
+
+ world = World(client.get_world(), hud, args)
+ controller = KeyboardControl(world)
+
+ agent = BehaviorAgent(world.player, behavior=args.behavior)
+
+ spawn_points = world.map.get_spawn_points()
+ random.shuffle(spawn_points)
+
+ if spawn_points[0].location != agent.vehicle.get_location():
+ destination = spawn_points[0].location
+ else:
+ destination = spawn_points[1].location
+
+ agent.set_destination(agent.vehicle.get_location(), destination, clean=True)
+
+ clock = pygame.time.Clock()
+
+ while True:
+ clock.tick_busy_loop(60)
+ if controller.parse_events():
+ return
+
+ # As soon as the server is ready continue!
+ if not world.world.wait_for_tick(10.0):
+ continue
+
+ if args.agent == "Roaming" or args.agent == "Basic":
+ if controller.parse_events():
+ return
+
+ #agent.update_information(world)
+ # as soon as the server is ready continue!
+ world.world.wait_for_tick(10.0)
+
+ world.tick(clock)
+ world.render(display)
+ pygame.display.flip()
+ control = agent.run_step()
+ control.manual_gear_shift = False
+ world.player.apply_control(control)
+ else:
+ agent.update_information(world)
+
+ world.tick(clock)
+ world.render(display)
+ pygame.display.flip()
+
+
+ # Set new destination when target has been reached
+ if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop:
+ agent.reroute(spawn_points)
+ tot_target_reached += 1
+ world.hud.notification("The target has been reached " +
+ str(tot_target_reached) + " times.", seconds=4.0)
+
+ elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop:
+ print("Target reached, mission accomplished...")
+ break
+
+ speed_limit = world.player.get_speed_limit()
+ agent.get_local_planner().set_speed(speed_limit)
+
+ control = agent.run_step()
+ world.player.apply_control(control)
+
+ finally:
+ if world is not None:
+ world.destroy()
+
+ pygame.quit()
+
+
+# ==============================================================================
+# -- main() --------------------------------------------------------------
+# ==============================================================================
+
+
+def main():
+ """Main method"""
+
+ argparser = argparse.ArgumentParser(
+ description='CARLA Automatic Control Client')
+ argparser.add_argument(
+ '-v', '--verbose',
+ action='store_true',
+ dest='debug',
+ help='Print debug information')
+ argparser.add_argument(
+ '--host',
+ metavar='H',
+ default='127.0.0.1',
+ help='IP of the host server (default: 127.0.0.1)')
+ argparser.add_argument(
+ '-p', '--port',
+ metavar='P',
+ default=2000,
+ type=int,
+ help='TCP port to listen to (default: 2000)')
+ argparser.add_argument(
+ '--res',
+ metavar='WIDTHxHEIGHT',
+ default='1280x720',
+ help='Window resolution (default: 1280x720)')
+ argparser.add_argument(
+ '--filter',
+ metavar='PATTERN',
+ default='vehicle.*',
+ help='Actor filter (default: "vehicle.*")')
+ argparser.add_argument(
+ '--gamma',
+ default=2.2,
+ type=float,
+ help='Gamma correction of the camera (default: 2.2)')
+ argparser.add_argument(
+ '-l', '--loop',
+ action='store_true',
+ dest='loop',
+ help='Sets a new random destination upon reaching the previous one (default: False)')
+ argparser.add_argument(
+ '-b', '--behavior', type=str,
+ choices=["cautious", "normal", "aggressive"],
+ help='Choose one of the possible agent behaviors (default: normal) ',
+ default='normal')
+ argparser.add_argument("-a", "--agent", type=str,
+ choices=["Behavior", "Roaming", "Basic"],
+ help="select which agent to run",
+ default="Behavior")
+ argparser.add_argument(
+ '-s', '--seed',
+ help='Set seed for repeating executions (default: None)',
+ default=None,
+ type=int)
+
+ args = argparser.parse_args()
+
+ args.width, args.height = [int(x) for x in args.res.split('x')]
+
+ log_level = logging.DEBUG if args.debug else logging.INFO
+ logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level)
+
+ logging.info('listening to server %s:%s', args.host, args.port)
+
+ print(__doc__)
+
+ try:
+ game_loop(args)
+
+ except KeyboardInterrupt:
+ print('\nCancelled by user. Bye!')
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md
new file mode 100644
index 0000000000..c7c8ba177a
--- /dev/null
+++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md
@@ -0,0 +1,24 @@
+# 让一台装了激光雷达,IMU、GNSS和轮式底盘的机器人,能够在室外大范围、无明显人工特征的环境中实现:
+# 1.高精度实时建图
+# 2.可靠的全局定位
+# 3.无人工干预的自主路径规划与导航
+
+# 构建目录结构
+
+```text
+outdoor_ws/src/robot_description/
+├── CMakeLists.txt
+├── package.xml
+├── urdf/
+│ ├── robot.xacro # 主机器人描述文件
+│ ├── materials.xacro # 颜色定义
+│ └── sensors/
+│ ├── lidar.xacro # LiDAR模型
+│ └── imu.xacro # IMU模型
+├── launch/
+│ ├── display.launch.py # 显示模型
+│ └── control.launch.py # 控制启动
+├── config/
+│ ├── controllers.yaml # ros2_control配置
+│ └── robot_control.yaml # 控制参数
+└── meshes/ # 可选:3D模型文件
\ No newline at end of file
diff --git a/src/Smart_car/README.md b/src/Smart_car/README.md
index e69de29bb2..6cb0dd221a 100644
--- a/src/Smart_car/README.md
+++ b/src/Smart_car/README.md
@@ -0,0 +1,171 @@
+---
+AIGC:
+ ContentProducer: Minimax Agent AI
+ ContentPropagator: Minimax Agent AI
+ Label: AIGC
+ ProduceID: 77d13f0624762f2de86f1694a087dcde
+ PropagateID: 77d13f0624762f2de86f1694a087dcde
+ ReservedCode1: 30450220450de8ed6ebcc59d795ca07193a8349cdff122af3914c845bc06a5eccd697a86022100801cefd02340cae09cb226bc46bd40a26c0a6a3fa0b44e7f34697b6d73b2d4f2
+ ReservedCode2: 3045022100f50987c5dc34b38c3c06803f29fc5d64f7d0a1d3dc0ea74c7108f5005deb029e02203a74966d98dd7f581473e2263aa5c776873a84476b30fffe04599a50827e0d83
+---
+
+# 无人车项目
+
+一个基于计算机视觉的简单无人车导航系统,支持车道线检测、目标识别和自动控制。
+
+## 功能特性
+
+1.**车道线检测**: 实时识别道路车道线
+2.**目标识别**: 检测车辆、行人和交通标志
+3.**路径规划**: 自动规划安全行驶路径
+4.**智能控制**: PID控制器实现精确转向和速度控制
+5.**实时显示**: 可视化检测结果和控制状态
+
+## 快速开始
+
+### 方式一:一键启动(推荐)
+
+**Linux/Mac:**
+```bash
+chmod +x start.sh
+./start.sh
+```
+
+**Windows:**
+```cmd
+start.bat
+```
+
+### 方式二:手动启动
+
+1. **安装依赖**
+ ```bash
+ pip install -r requirements.txt
+ ```
+
+2. **运行程序**
+ ```bash
+ python main.py
+ ```
+
+## 运行模式
+
+`python main.py` - 完整模式(所有功能)
+`python main.py --mode perception` - 仅感知测试
+`python main.py --debug` - 调试模式(显示详细信息)
+
+## 系统要求
+
+**操作系统**: Windows/Linux/macOS
+**Python**: 3.8+
+**内存**: 4GB+
+**摄像头**: USB摄像头(720p以上)
+
+## 项目结构
+
+```
+无人车项目/
+├── main.py # 主程序
+├── config.yaml # 配置文件
+├── requirements.txt # 依赖包
+├── start.sh / start.bat # 启动脚本
+├── src/ # 源代码
+│ ├── perception/ # 感知模块
+│ ├── planning/ # 规划模块
+│ └── control/ # 控制模块
+└── docs/ # 文档
+```
+
+## 核心模块
+
+### 感知模块 (perception)
+**车道线检测**: 使用OpenCV识别车道线
+**目标检测**: 检测车辆、行人等目标
+
+### 规划模块 (planning)
+**路径生成**: 基于感知结果生成行驶路径
+**障碍物避让**: 智能避障算法
+
+### 控制模块 (control)
+**PID控制**: 精确的转向和速度控制
+**轨迹跟踪**: 沿着规划路径行驶
+
+## 配置说明
+
+主要配置参数在 `config.yaml` 文件中:
+
+```yaml
+# 相机设置
+camera:
+ resolution: [1280, 720] # 分辨率
+ fps: 30 # 帧率
+
+# 检测设置
+lane_detection:
+ min_line_length: 50 # 最小线段长度
+
+control:
+ target_speed: 25 # 目标速度 (km/h)
+ kp: 1.2 # PID参数
+```
+
+## 运行效果
+
+程序运行时会显示:
+**实时视频**: 来自摄像头的画面
+**检测结果**: 车道线和目标识别结果
+**控制信息**: 转向角度和速度控制输出
+**状态日志**: 系统运行状态
+
+示例输出:
+```
+[INFO] 启动无人车导航系统
+[INFO] 相机初始化完成: 1280x720@30fps
+[INFO] 车道线检测: 找到2条车道线
+[INFO] 检测到目标: 车辆(距离12.5m) 行人(距离8.2m)
+[INFO] 路径规划完成,长度: 45个点
+[INFO] 控制输出: 转向=2.3度, 速度=24.8km/h
+```
+
+## 常见问题
+
+**Q: 摄像头无法启动?**
+A: 检查摄像头连接,或尝试 `--camera 1` 指定其他设备
+
+**Q: 程序运行很慢?**
+A: 降低相机分辨率或关闭调试模式
+
+**Q: 车道线检测不准确?**
+A: 检查光线条件,调整配置文件中的参数
+
+## 故障排除
+
+1. **依赖安装失败**
+ ```bash
+ pip install --upgrade pip
+ pip install -r requirements.txt
+ ```
+
+2. **权限问题 (Linux)**
+ ```bash
+ sudo usermod -a -G video $USER
+ # 重新登录或重启
+ ```
+
+3. **调试模式**
+ ```bash
+ python main.py --debug --save-images
+ ```
+
+## 性能调优
+
+提升性能的方法:
+使用更高配置的硬件
+降低图像分辨率
+关闭不必要的可视化
+调整检测参数
+## 技术支持
+
+查看 `docs/usage_guide.md` 了解详细使用说明
+运行 `python tests/test_basic.py` 进行功能测试
+检查日志文件了解运行状态
diff --git a/src/Smart_car/main.py b/src/Smart_car/main.py
new file mode 100644
index 0000000000..8ab006469e
--- /dev/null
+++ b/src/Smart_car/main.py
@@ -0,0 +1,143 @@
+import numpy as np
+import pandas as pd
+import matplotlib.pyplot as plt
+from sklearn.preprocessing import MinMaxScaler
+from sklearn.model_selection import train_test_split
+from sklearn.metrics import mean_squared_error, mean_absolute_error, r2_score
+from tensorflow.keras.models import Sequential
+from tensorflow.keras.layers import LSTM, Dense, Dropout
+from tensorflow.keras.callbacks import EarlyStopping
+
+
+# 1. 生成模拟驾驶数据
+def generate_driving_data(n_samples=10000):
+ time = np.linspace(0, 100, n_samples)
+ speed = np.zeros(n_samples)
+ acceleration = np.zeros(n_samples)
+ steering_angle = np.zeros(n_samples)
+
+ # 模拟驾驶场景:起步→加速→匀速→减速→转弯→加速→停车
+ segment1 = slice(0, 1500)
+ segment2 = slice(1500, 4000)
+ segment3 = slice(4000, 5000)
+ segment4 = slice(5000, 6500)
+ segment5 = slice(6500, 8500)
+ segment6 = slice(8500, 10000)
+
+ # 计算各段长度
+ len1 = segment1.stop - segment1.start
+ len2 = segment2.stop - segment2.start
+ len3 = segment3.stop - segment3.start
+ len4 = segment4.stop - segment4.start
+ len5 = segment5.stop - segment5.start
+ len6 = segment6.stop - segment6.start
+
+ # 填充速度数据
+ speed[segment1] = np.linspace(0, 15, len1) + np.random.normal(0, 0.3, len1)
+ speed[segment2] = 15 + np.random.normal(0, 0.2, len2)
+ speed[segment3] = np.linspace(15, 5, len3) + np.random.normal(0, 0.3, len3)
+ speed[segment4] = 5 + np.random.normal(0, 0.2, len4)
+ speed[segment5] = np.linspace(5, 12, len5) + np.random.normal(0, 0.3, len5)
+ speed[segment6] = np.linspace(12, 0, len6) + np.random.normal(0, 0.3, len6)
+
+ # 填充加速度数据
+ acceleration[segment1] = 0.5 + np.random.normal(0, 0.1, len1)
+ acceleration[segment2] = 0 + np.random.normal(0, 0.05, len2)
+ acceleration[segment3] = -0.3 + np.random.normal(0, 0.1, len3)
+ acceleration[segment4] = 0 + np.random.normal(0, 0.05, len4)
+ acceleration[segment5] = 0.2 + np.random.normal(0, 0.1, len5)
+ acceleration[segment6] = -0.4 + np.random.normal(0, 0.1, len6)
+
+ # 填充转向角数据
+ steering_angle[segment1] = 0 + np.random.normal(0, 0.5, len1)
+ steering_angle[segment2] = 0 + np.random.normal(0, 0.5, len2)
+ steering_angle[segment3] = 0 + np.random.normal(0, 0.5, len3)
+ steering_angle[segment4] = np.linspace(0, 15, len4) + np.random.normal(0, 1, len4)
+ steering_angle[segment5] = 0 + np.random.normal(0, 0.5, len5)
+ steering_angle[segment6] = 0 + np.random.normal(0, 0.5, len6)
+
+ # 组合成DataFrame
+ data = pd.DataFrame({
+ 'time': time,
+ 'speed': speed,
+ 'acceleration': acceleration,
+ 'steering_angle': steering_angle
+ })
+ return data
+
+
+# 2. 数据预处理:构建时间序列
+def create_sequences(data, seq_length=50):
+ X, y = [], []
+ for i in range(len(data) - seq_length):
+ X.append(data[i:i + seq_length])
+ y.append(data[i + seq_length, 0]) # 预测speed(第0列)
+ return np.array(X), np.array(y)
+
+
+# 主流程
+if __name__ == "__main__":
+ # 生成数据
+ df = generate_driving_data()
+ features = df[['speed', 'acceleration', 'steering_angle']].values
+
+ # 归一化
+ scaler = MinMaxScaler(feature_range=(0, 1))
+ scaled_features = scaler.fit_transform(features)
+
+ # 构建时间序列
+ seq_length = 50
+ X, y = create_sequences(scaled_features, seq_length)
+
+ # 划分训练集和测试集
+ X_train, X_test, y_train, y_test = train_test_split(X, y, test_size=0.2, shuffle=False)
+
+ # 3. 构建LSTM模型
+ model = Sequential()
+ model.add(LSTM(64, return_sequences=True, input_shape=(X_train.shape[1], X_train.shape[2])))
+ model.add(Dropout(0.2))
+ model.add(LSTM(32, return_sequences=False))
+ model.add(Dropout(0.2))
+ model.add(Dense(16, activation='relu'))
+ model.add(Dense(1))
+
+ model.compile(optimizer='adam', loss='mean_squared_error')
+
+ # 早停机制
+ early_stopping = EarlyStopping(monitor='val_loss', patience=5, restore_best_weights=True)
+
+ # 训练模型
+ history = model.fit(
+ X_train, y_train,
+ batch_size=32,
+ epochs=50,
+ validation_split=0.1,
+ callbacks=[early_stopping]
+ )
+
+ # 4. 模型评估
+ y_pred = model.predict(X_test)
+
+ # 反归一化(仅针对speed)
+ # 构造反归一化的临时数组
+ y_test_inv = scaler.inverse_transform(np.hstack((y_test.reshape(-1, 1), np.zeros((len(y_test), 2)))))[:, 0]
+ y_pred_inv = scaler.inverse_transform(np.hstack((y_pred, np.zeros((len(y_pred), 2)))))[:, 0]
+
+ # 计算指标
+ mse = mean_squared_error(y_test_inv, y_pred_inv)
+ mae = mean_absolute_error(y_test_inv, y_pred_inv)
+ r2 = r2_score(y_test_inv, y_pred_inv)
+
+ print(f"测试集MSE: {mse:.4f}")
+ print(f"测试集MAE: {mae:.4f}")
+ print(f"测试集R2: {r2:.4f}")
+
+ # 5. 结果可视化
+ plt.figure(figsize=(12, 6))
+ plt.plot(y_test_inv, label='真实速度')
+ plt.plot(y_pred_inv, label='预测速度')
+ plt.title('无人车速度预测结果')
+ plt.xlabel('时间步')
+ plt.ylabel('速度 (m/s)')
+ plt.legend()
+ plt.show()#预测
\ No newline at end of file
diff --git a/src/UAV_navigation_system/cnn_evaluate.py b/src/UAV_navigation_system/cnn_evaluate.py
new file mode 100644
index 0000000000..77031d27f6
--- /dev/null
+++ b/src/UAV_navigation_system/cnn_evaluate.py
@@ -0,0 +1,58 @@
+# cnn_evaluate.py
+
+import os
+import tensorflow as tf
+from tensorflow.keras.preprocessing.image import ImageDataGenerator
+import numpy as np
+from sklearn.metrics import classification_report, confusion_matrix
+import matplotlib.pyplot as plt
+import seaborn as sns
+
+# Paths
+base_dir = os.path.abspath("../data")
+test_dir = os.path.join(base_dir, "test")
+
+# Match image size with training
+img_size = (128, 128)
+batch_size = 32
+
+# Load and preprocess test data
+test_datagen = ImageDataGenerator(rescale=1./255)
+test_generator = test_datagen.flow_from_directory(
+ test_dir,
+ target_size=img_size,
+ batch_size=batch_size,
+ class_mode='categorical',
+ shuffle=False # Required for accurate predictions alignment
+)
+
+# Load trained model
+model_path = os.path.join(base_dir, "best_model.h5")
+model = tf.keras.models.load_model(model_path)
+
+# Evaluate
+loss, accuracy = model.evaluate(test_generator)
+print(f"Test Loss: {loss:.4f}")
+print(f"Test Accuracy: {accuracy:.4f}")
+
+# ---------- 🧠 Add this section below to analyze predictions ----------
+# Predict labels
+Y_pred = model.predict(test_generator)
+y_pred = np.argmax(Y_pred, axis=1)
+
+# Classification report
+print("\nClassification Report:")
+print(classification_report(test_generator.classes, y_pred, target_names=test_generator.class_indices.keys()))
+
+# Confusion matrix
+cm = confusion_matrix(test_generator.classes, y_pred)
+plt.figure(figsize=(8, 6))
+sns.heatmap(cm, annot=True, fmt="d",
+ xticklabels=test_generator.class_indices.keys(),
+ yticklabels=test_generator.class_indices.keys(),
+ cmap="Blues")
+plt.title("Confusion Matrix")
+plt.xlabel("Predicted")
+plt.ylabel("Actual")
+plt.tight_layout()
+plt.show()
\ No newline at end of file
diff --git a/src/UAV_navigation_system/main.py b/src/UAV_navigation_system/main.py
new file mode 100644
index 0000000000..e24185760c
--- /dev/null
+++ b/src/UAV_navigation_system/main.py
@@ -0,0 +1,19 @@
+import torch
+import cv2
+
+print("✅ PyTorch version:", torch.__version__)
+print("✅ OpenCV version:", cv2.__version__)
+print("🎉 All set to build the drone navigation system!")
+print("✅ Virtual environment is working!")
+
+
+# src/main.py
+
+import torch
+
+def main():
+ print("Welcome to Drone Navigation Vision Project!")
+ print("Torch version:", torch.__version__)
+
+if __name__ == "__main__":
+ main()
diff --git a/src/Unmanned_vehicle_AD_DQN/Environment.py b/src/Unmanned_vehicle_AD_DQN/Environment.py
index a9d245b04c..0a8394be95 100644
--- a/src/Unmanned_vehicle_AD_DQN/Environment.py
+++ b/src/Unmanned_vehicle_AD_DQN/Environment.py
@@ -7,6 +7,7 @@
import numpy as np
import cv2
import math
+from collections import deque
import carla
from carla import ColorConverter
@@ -17,8 +18,8 @@
except ImportError:
# 如果在导入Hyperparameters时出错,使用默认值
SHOW_PREVIEW = False
- IM_WIDTH = 640
- IM_HEIGHT = 480
+ IM_WIDTH = 160 # 修改默认值
+ IM_HEIGHT = 120 # 修改默认值
class CarEnv:
@@ -40,6 +41,13 @@ def __init__(self):
self.episode_start_time = None # 每轮开始时间
self.last_ped_distance = float('inf') # 上次最近行人距离
self.current_episode = 1 # 当前episode编号
+ self.obstacle_detected_time = None # 障碍物检测到的时间
+ self.reaction_start_time = None # 反应开始时间
+ self.proactive_action_count = 0 # 主动避障动作计数
+
+ # 新增:多帧缓存用于运动检测
+ self.frame_buffer = deque(maxlen=3)
+ self.motion_detected = False
# 加载世界和蓝图
try:
@@ -255,6 +263,11 @@ def reset(self, episode=1):
self.last_action = 1
self.episode_start_time = time.time()
self.last_ped_distance = float('inf')
+ self.obstacle_detected_time = None
+ self.reaction_start_time = None
+ self.proactive_action_count = 0
+ self.frame_buffer.clear()
+ self.motion_detected = False
# 设置车辆生成点
spawn_point = carla.Transform()
@@ -366,6 +379,31 @@ def process_img(self, image):
processed_image = np.array(image.raw_data)
processed_image = processed_image.reshape((self.im_height, self.im_width, 4))
processed_image = processed_image[:, :, :3] # 移除alpha通道
+
+ # 添加简单的图像增强
+ # 增加对比度以突出障碍物
+ processed_image = cv2.convertScaleAbs(processed_image, alpha=1.2, beta=10)
+
+ # 存储到帧缓冲区用于运动检测
+ if len(self.frame_buffer) == 0:
+ self.frame_buffer.append(processed_image.copy())
+ else:
+ # 简单的运动检测
+ if len(self.frame_buffer) >= 2:
+ prev_frame = self.frame_buffer[-1]
+ gray_prev = cv2.cvtColor(prev_frame, cv2.COLOR_RGB2GRAY)
+ gray_current = cv2.cvtColor(processed_image, cv2.COLOR_RGB2GRAY)
+
+ # 计算帧间差异
+ diff = cv2.absdiff(gray_prev, gray_current)
+ _, diff_thresh = cv2.threshold(diff, 25, 255, cv2.THRESH_BINARY)
+
+ # 如果差异足够大,检测到运动
+ motion_pixels = np.sum(diff_thresh > 0)
+ motion_ratio = motion_pixels / (self.im_width * self.im_height)
+ self.motion_detected = motion_ratio > 0.01 # 1%的像素变化
+
+ self.frame_buffer.append(processed_image.copy())
# 显示预览(如果启用)
if self.SHOW_CAM:
@@ -377,7 +415,7 @@ def process_img(self, image):
print(f"处理图像时出错: {e}")
def reward(self, speed_kmh, current_steer):
- """增强的奖励函数 - 特别强调行人避障"""
+ """增强的奖励函数 - 特别强调行人避障和快速反应"""
reward = 0
done = False
@@ -385,19 +423,19 @@ def reward(self, speed_kmh, current_steer):
vehicle_location = self.vehicle.get_location()
vehicle_rotation = self.vehicle.get_transform().rotation.yaw
- # 1. 道路保持奖励(重要的基础)
+ # 1. 道路保持奖励
heading_error = abs(vehicle_rotation)
if heading_error < 5: # 完美保持方向
- reward += 0.8 # 略微降低权重,给行人避障更多空间
+ reward += 0.5
elif heading_error < 15: # 良好保持
- reward += 0.4
+ reward += 0.2
elif heading_error < 30: # 可接受
- reward += 0.1
+ reward += 0.05
else: # 方向偏差过大
- reward -= 0.3 * (heading_error / 30.0) # 降低惩罚强度
+ reward -= 0.2 * (heading_error / 30.0)
- # 2. 行人避障(最高优先级) - 增强奖励机制
+ # 2. 行人避障(最高优先级) - 增强反应机制
min_ped_distance = float('inf')
closest_pedestrian = None
@@ -422,14 +460,24 @@ def reward(self, speed_kmh, current_steer):
if active_pedestrians == 0:
min_ped_distance = float('inf')
- # 行人距离分级奖励 - 增强避障激励
+ # 行人检测和反应时间记录
+ current_time = time.time()
+
+ # 检测到行人进入危险区域
if min_ped_distance < 100: # 只考虑100米内的行人
+ if self.obstacle_detected_time is None:
+ self.obstacle_detected_time = current_time
+ self.reaction_start_time = current_time
+
+ # 行人距离分级奖励/惩罚 - 强调提前反应
if min_ped_distance < 3.0: # 紧急避让距离
- reward -= 8.0 # 增加惩罚
+ reward -= 15.0 # 大幅增加惩罚
done = True
- print(f"Episode {self.current_episode}: 与行人距离过近 ({min_ped_distance:.1f}m)!")
+ print(f"Episode {self.current_episode}: 与行人碰撞! 距离: {min_ped_distance:.1f}m")
+
elif min_ped_distance < 5.0: # 危险距离
- reward -= 3.0 # 增加惩罚
+ reward -= 5.0 # 增加惩罚
+
# 计算避让方向
if closest_pedestrian:
ped_y = closest_pedestrian.get_location().y
@@ -439,58 +487,93 @@ def reward(self, speed_kmh, current_steer):
self.suggested_action = 4 # 右转
else: # 行人在右侧
self.suggested_action = 3 # 左转
+
+ # 反应时间惩罚
+ if self.reaction_start_time:
+ reaction_time = current_time - self.reaction_start_time
+ if reaction_time > 0.5: # 反应超过0.5秒
+ reward -= 2.0 * (reaction_time - 0.5)
+
elif min_ped_distance < 8.0: # 预警距离
- reward -= 0.8
+ reward -= 1.5
+
+ # 计算避让方向(提前建议)
+ if closest_pedestrian:
+ ped_y = closest_pedestrian.get_location().y
+ veh_y = vehicle_location.y
+ if ped_y < veh_y: # 行人在左侧
+ self.suggested_action = 4 # 右转
+ else: # 行人在右侧
+ self.suggested_action = 3 # 左转
+
+ # 提前反应的奖励
+ if self.last_ped_distance < 12.0 and min_ped_distance > self.last_ped_distance:
+ # 如果上次距离更近,这次距离更远了,说明正在成功避让
+ reward += 1.0
+ self.proactive_action_count += 1
+
elif min_ped_distance < 12.0: # 安全距离
- reward += 0.5 # 增加安全距离奖励
- else: # 非常安全
- reward += 0.2
-
- # 3. 成功避障奖励 - 当成功避开行人后给予额外奖励
- if self.last_ped_distance < 8.0 and min_ped_distance > self.last_ped_distance:
- # 如果上次距离危险,这次距离更远了,说明成功避让
- reward += 0.3
+ # 检测到行人但距离安全,给予轻微奖励
+ reward += 0.3
+
+ # 如果检测到运动,给予额外奖励
+ if self.motion_detected:
+ reward += 0.2
+
+ # 提前规划奖励
+ if self.last_ped_distance < 8.0:
+ # 从危险距离进入安全距离,成功避障
+ reward += 2.0
+
+ else:
+ # 没有检测到行人,重置反应计时
+ self.obstacle_detected_time = None
+ self.reaction_start_time = None
self.last_ped_distance = min_ped_distance # 保存当前距离
- # 4. 速度奖励(平衡避障和前进)
+ # 3. 速度奖励(平衡避障和前进)
if 20 <= speed_kmh <= 35: # 理想速度区间
- reward += 0.4
- elif 10 <= speed_kmh < 20: # 较慢但安全(避障时可能需要减速)
- reward += 0.2
+ reward += 0.5
+ elif 10 <= speed_kmh < 20: # 较慢但安全
+ reward += 0.3
elif 35 < speed_kmh <= 40: # 稍快
reward += 0.1
elif speed_kmh > 40: # 过快,在行人环境中危险
- reward -= 0.5 # 增加惩罚
+ reward -= 1.0 # 增加惩罚
else: # 停车或极慢
- reward -= 0.05 # 轻微惩罚
+ reward -= 0.1 # 轻微惩罚
- # 5. 转向平滑性奖励(避障时可能需要适当转向)
- steer_penalty = abs(current_steer) * 0.3 # 降低惩罚,给避障转向更多空间
+ # 4. 转向平滑性奖励
+ steer_penalty = abs(current_steer) * 0.2 # 降低惩罚,给避障转向更多空间
reward -= steer_penalty
- # 6. 碰撞检测
+ # 5. 碰撞检测
if len(self.collision_history) != 0:
- reward = -10 # 减少碰撞惩罚
+ reward = -20 # 增加碰撞惩罚
done = True
print(f"Episode {self.current_episode}: 发生碰撞!")
- # 7. 进度奖励(次要于避障)
+ # 6. 进度奖励
progress = (vehicle_location.x + 81) / 236.0 # 从-81到155
- reward += progress * 0.2 # 降低进度权重
+ reward += progress * 0.3
- # 8. 边界检查
+ # 7. 边界检查
if vehicle_location.x > 155: # 成功到达终点
- reward += 15 # 减少到达终点的奖励
+ reward += 20 # 增加到达终点的奖励
done = True
print(f"Episode {self.current_episode}: 成功到达终点!")
elif vehicle_location.x < -90 or abs(vehicle_location.y + 195) > 30: # 偏离道路
- reward -= 3 # 降低偏离惩罚
+ reward -= 5 # 增加偏离惩罚
done = True
print(f"Episode {self.current_episode}: 偏离道路!")
+ # 8. 主动避障奖励
+ if self.proactive_action_count > 0:
+ reward += self.proactive_action_count * 0.5
+
# 限制奖励范围
- reward = max(min(reward, 20), -15)
+ reward = max(min(reward, 30), -25)
return reward, done
@@ -510,32 +593,46 @@ def step(self, action):
brake = 0.0
steer = 0.0
- # 速度控制
+ # 速度控制 - 优化动作响应
if action == 0: # 减速
throttle = 0.0
- brake = 0.5
+ brake = 0.7 # 增加制动强度
elif action == 1: # 保持/轻微加速
- throttle = 0.3
+ throttle = 0.4 # 增加油门
brake = 0.0
elif action == 2: # 加速
- throttle = 0.7
+ throttle = 0.8
brake = 0.0
elif action == 3: # 左转
- throttle = 0.4
+ throttle = 0.5 # 增加转向时的油门
brake = 0.0
- steer = -0.2 * speed_factor # 速度相关的转向幅度
+ steer = -0.25 * speed_factor # 增加转向幅度
elif action == 4: # 右转
- throttle = 0.4
+ throttle = 0.5
brake = 0.0
- steer = 0.2 * speed_factor # 速度相关的转向幅度
+ steer = 0.25 * speed_factor # 增加转向幅度
+
+ # 如果有建议的避让动作,调整当前动作
+ if self.suggested_action is not None:
+ # 如果当前动作与建议动作不同,调整转向幅度
+ if self.suggested_action in [3, 4] and action in [3, 4]:
+ # 如果都是转向动作,但方向不同,增加建议方向的转向
+ if self.suggested_action != action:
+ if self.suggested_action == 3: # 建议左转
+ steer = -0.3 * speed_factor
+ else: # 建议右转
+ steer = 0.3 * speed_factor
+ elif self.suggested_action == 0: # 建议减速
+ brake = max(brake, 0.8) # 强制制动
+ throttle = 0.0
# 限制连续同向转向 - 防止过度转向
if (action == 3 and self.last_action == 3) or (action == 4 and self.last_action == 4):
self.same_steer_counter += 1
- if self.same_steer_counter > 3: # 连续3次同向转向后强制回正
- steer *= 0.5 # 减小转向幅度
- throttle *= 0.8 # 减速
- brake = 0.1 # 轻微制动
+ if self.same_steer_counter > 2: # 连续2次同向转向后强制回正
+ steer *= 0.3 # 大幅减小转向幅度
+ throttle *= 0.6 # 减速
+ brake = 0.2 # 轻微制动
else:
self.same_steer_counter = 0
@@ -549,13 +646,13 @@ def step(self, action):
steer=steer
))
- # 等待物理更新
- time.sleep(0.05)
+ # 减少等待时间,加快反应
+ time.sleep(0.03) # 从0.05减少到0.03
# 计算奖励和完成状态
reward, done = self.reward(speed_kmh, steer)
# 限制极端奖励值
- reward = np.clip(reward, -15, 20)
+ reward = np.clip(reward, -25, 30)
return self.front_camera, reward, done, None
\ No newline at end of file
diff --git a/src/Unmanned_vehicle_AD_DQN/Hyperparameters.py b/src/Unmanned_vehicle_AD_DQN/Hyperparameters.py
index bf5be3a954..e701e4507e 100644
--- a/src/Unmanned_vehicle_AD_DQN/Hyperparameters.py
+++ b/src/Unmanned_vehicle_AD_DQN/Hyperparameters.py
@@ -13,14 +13,14 @@
REWARD_OFFSET = -100
# 停止模拟的奖励阈值
-MIN_REPLAY_MEMORY_SIZE = 3_000
-# 开始训练前经验回放缓冲区的最小大小 - 增加以获得更稳定训练
+MIN_REPLAY_MEMORY_SIZE = 1_000 # 减少预热样本数,让训练更快开始
+# 开始训练前经验回放缓冲区的最小大小
-REPLAY_MEMORY_SIZE = 10_000
+REPLAY_MEMORY_SIZE = 15_000 # 增加经验回放容量
# 经验回放缓冲区的最大容量
-MINIBATCH_SIZE = 64
-# 每次训练从经验回放中采样的经验数量 - 增加批次大小
+MINIBATCH_SIZE = 128 # 增加批次大小
+# 每次训练从经验回放中采样的经验数量
PREDICTION_BATCH_SIZE = 1
# 预测阶段使用的批次大小
@@ -28,40 +28,40 @@
TRAINING_BATCH_SIZE = MINIBATCH_SIZE // 4
# 训练阶段使用的批次大小
-EPISODES = 800 # 从1000减少到800,因为减少了无行人阶段
+EPISODES = 500 # 增加训练轮次
# 智能体训练的总轮次数
SECONDS_PER_EPISODE = 60
# 每轮训练的秒数
-MIN_EPSILON = 0.01
+MIN_EPSILON = 0.05 # 增加最小探索率,保持一定的探索
# 最小探索率
EPSILON = 1.0
# 初始探索率
-EPSILON_DECAY = 0.998
-# 探索率的衰减率 - 减缓衰减速度
+EPSILON_DECAY = 0.995 # 减缓衰减速度
+# 探索率的衰减率
-MODEL_NAME = "YY_Optimized"
+MODEL_NAME = "YY_Optimized_v2"
# 训练模型的名称标识
-MIN_REWARD = 5
+MIN_REWARD = -100 # 降低阈值,更容易保存模型
# 被认为是"良好"或"积极"经验的最小奖励值
-UPDATE_TARGET_EVERY = 20
-# 目标网络更新的频率 - 增加以获得更稳定训练
+UPDATE_TARGET_EVERY = 15 # 更频繁地更新目标网络
+# 目标网络更新的频率
-AGGREGATE_STATS_EVERY = 10
+AGGREGATE_STATS_EVERY = 5 # 更频繁地记录统计信息
# 计算和聚合统计信息(如平均得分、奖励)的频率
SHOW_PREVIEW = False
# 是否显示预览窗口 - 测试时设为False以显示CARLA主窗口
-IM_WIDTH = 640
+IM_WIDTH = 160 # 减少图像尺寸,加快处理速度
# 预览或模拟中捕获图像的宽度
-IM_HEIGHT = 480
+IM_HEIGHT = 120 # 减少图像尺寸,加快处理速度
# 预览或模拟中捕获图像的高度
SLOW_COUNTER = 330
@@ -73,17 +73,17 @@
SUCCESSFUL_THRESHOLD = 3
# 成功阈值
-LEARNING_RATE = 0.00005
-# 优化器的学习率 - 降低以获得更稳定训练
+LEARNING_RATE = 0.0001 # 增加学习率,加速收敛
+# 优化器的学习率
# PER (优先经验回放) 参数
-PER_ALPHA = 0.6
+PER_ALPHA = 0.7 # 增加优先级影响
# 优先级程度 (0 = 均匀采样, 1 = 完全优先级)
-PER_BETA_START = 0.4
+PER_BETA_START = 0.5
# 重要性采样权重起始值
-PER_BETA_FRAMES = 100000
+PER_BETA_FRAMES = 50000 # 减少beta增长时间
# beta线性增长的帧数
# 训练策略参数
@@ -97,9 +97,9 @@
# 是否使用模仿学习(预训练)
# 多目标优化权重(这些权重会自动调整)
-SAFETY_WEIGHT = 0.40
-EFFICIENCY_WEIGHT = 0.25
-COMFORT_WEIGHT = 0.20
+SAFETY_WEIGHT = 0.50 # 提高安全权重
+EFFICIENCY_WEIGHT = 0.20
+COMFORT_WEIGHT = 0.15
RULE_FOLLOWING_WEIGHT = 0.15
# 课程学习参数
@@ -107,4 +107,8 @@
# 课程学习阶段数量
CURRICULUM_SUCCESS_THRESHOLDS = [0.3, 0.5, 0.7, 0.85, 0.9]
-# 每个阶段进入下一阶段所需的成功率阈值
\ No newline at end of file
+# 每个阶段进入下一阶段所需的成功率阈值
+
+# 新增:反应时间参数
+REACTION_TIME_PENALTY = 0.1 # 对反应迟钝的惩罚系数
+PROACTIVE_REWARD = 0.5 # 对提前行动的奖励
\ No newline at end of file
diff --git a/src/Unmanned_vehicle_AD_DQN/Model.py b/src/Unmanned_vehicle_AD_DQN/Model.py
index 19daa0e80c..b1c7d7f4ed 100644
--- a/src/Unmanned_vehicle_AD_DQN/Model.py
+++ b/src/Unmanned_vehicle_AD_DQN/Model.py
@@ -10,8 +10,9 @@
import matplotlib.pyplot as plt
from collections import deque
from tensorflow.keras.layers import Dense, GlobalAveragePooling2D, Input, Concatenate, Conv2D, AveragePooling2D, Activation, \
- Flatten, Dropout, BatchNormalization, MaxPooling2D, Multiply, Add, Lambda, Subtract
-from tensorflow.keras.optimizers import Adam
+ Flatten, Dropout, BatchNormalization, MaxPooling2D, Multiply, Add, Lambda, Subtract, Reshape, LayerNormalization, \
+ SpatialDropout2D, SeparableConv2D
+from tensorflow.keras.optimizers import Adam, RMSprop
from tensorflow.keras.models import Sequential, Model
from tensorflow.keras.callbacks import TensorBoard
import tensorflow as tf
@@ -37,7 +38,7 @@
LEARNING_RATE = 0.00005
IM_HEIGHT = 480
IM_WIDTH = 640
- MODEL_NAME = "YY_Optimized"
+ MODEL_NAME = "YY_Optimized_v2"
# 自定义TensorBoard类
@@ -72,6 +73,54 @@ def update_stats(self, **stats):
self.writer.flush()
+# 注意力机制模块
+class AttentionModule:
+ @staticmethod
+ def channel_attention(input_feature, ratio=8):
+ """通道注意力机制"""
+ channel_axis = -1
+ channel = input_feature.shape[channel_axis]
+
+ shared_layer_one = Dense(channel//ratio, activation='relu', kernel_initializer='he_normal',
+ use_bias=True, bias_initializer='zeros')
+ shared_layer_two = Dense(channel, kernel_initializer='he_normal', use_bias=True,
+ bias_initializer='zeros')
+
+ avg_pool = GlobalAveragePooling2D()(input_feature)
+ avg_pool = Reshape((1, 1, channel))(avg_pool)
+ avg_pool = shared_layer_one(avg_pool)
+ avg_pool = shared_layer_two(avg_pool)
+
+ max_pool = tf.reduce_max(input_feature, axis=[1, 2], keepdims=True)
+ max_pool = shared_layer_one(max_pool)
+ max_pool = shared_layer_two(max_pool)
+
+ cbam_feature = Add()([avg_pool, max_pool])
+ cbam_feature = Activation('sigmoid')(cbam_feature)
+
+ return Multiply()([input_feature, cbam_feature])
+
+ @staticmethod
+ def spatial_attention(input_feature):
+ """空间注意力机制"""
+ avg_pool = tf.reduce_mean(input_feature, axis=3, keepdims=True)
+ max_pool = tf.reduce_max(input_feature, axis=3, keepdims=True)
+ concat = Concatenate(axis=3)([avg_pool, max_pool])
+ attention = Conv2D(1, kernel_size=7, padding='same', activation='sigmoid',
+ kernel_initializer='he_normal', use_bias=False)(concat)
+
+ return Multiply()([input_feature, attention])
+
+ @staticmethod
+ def cbam_block(input_feature, ratio=8):
+ """完整的CBAM注意力模块"""
+ # 通道注意力
+ x = AttentionModule.channel_attention(input_feature, ratio)
+ # 空间注意力
+ x = AttentionModule.spatial_attention(x)
+ return x
+
+
# DQN智能体类 - 升级版(整合训练策略)
class DQNAgent:
def __init__(self, use_dueling=True, use_per=True, use_curriculum=True, use_multi_objective=True):
@@ -110,6 +159,10 @@ def __init__(self, use_dueling=True, use_per=True, use_curriculum=True, use_mult
self.multi_objective_optimizer = None
self.imitation_manager = None
+ # 新增:反应时间追踪
+ self.reaction_times = deque(maxlen=100)
+ self.last_obstacle_distance = float('inf')
+
def setup_training_strategies(self, env=None):
"""设置训练策略组件"""
if self.use_curriculum and env:
@@ -127,44 +180,47 @@ def setup_training_strategies(self, env=None):
self.imitation_manager = ImitationLearningManager()
def create_model(self):
- """创建标准深度Q网络模型"""
+ """创建标准深度Q网络模型 - 优化版"""
# 使用函数式API
inputs = Input(shape=(IM_HEIGHT, IM_WIDTH, 3))
- # 第一卷积块 - 使用较小的卷积核
- x = Conv2D(32, (3, 3), strides=(1, 1), padding='same')(inputs)
+ # 第一卷积块 - 使用深度可分离卷积加快计算
+ x = SeparableConv2D(32, (3, 3), strides=(1, 1), padding='same')(inputs)
x = Activation('relu')(x)
x = BatchNormalization()(x)
x = MaxPooling2D(pool_size=(2, 2))(x)
# 第二卷积块
- x = Conv2D(64, (3, 3), padding='same')(x)
+ x = SeparableConv2D(64, (3, 3), padding='same')(x)
x = Activation('relu')(x)
x = BatchNormalization()(x)
x = MaxPooling2D(pool_size=(2, 2))(x)
# 第三卷积块
- x = Conv2D(128, (3, 3), padding='same')(x)
+ x = SeparableConv2D(128, (3, 3), padding='same')(x)
x = Activation('relu')(x)
x = BatchNormalization()(x)
x = MaxPooling2D(pool_size=(2, 2))(x)
- # 空间注意力机制
- attention = Conv2D(1, (1, 1), padding='same', activation='sigmoid')(x)
- x = Multiply()([x, attention])
+ # 注意力机制 - 增强对障碍物的关注
+ x = AttentionModule.cbam_block(x)
+
+ # 第四卷积块 - 增加一层特征提取
+ x = SeparableConv2D(256, (3, 3), padding='same')(x)
+ x = Activation('relu')(x)
+ x = BatchNormalization()(x)
+ x = SpatialDropout2D(0.1)(x)
# 展平层
x = Flatten()(x)
- # 全连接层 - 减少神经元数量
+ # 全连接层 - 优化网络结构
x = Dense(256, activation='relu')(x)
x = Dropout(0.3)(x)
x = Dense(128, activation='relu')(x)
x = Dropout(0.3)(x)
x = Dense(64, activation='relu')(x)
x = Dropout(0.2)(x)
- x = Dense(32, activation='relu')(x)
- x = Dropout(0.1)(x)
# 输出层 - 5个动作
outputs = Dense(5, activation='linear')(x)
@@ -172,36 +228,43 @@ def create_model(self):
# 创建模型
model = Model(inputs=inputs, outputs=outputs)
- # 编译模型
- model.compile(loss="huber", optimizer=Adam(learning_rate=LEARNING_RATE), metrics=["mae"])
+ # 编译模型 - 使用自适应学习率优化器
+ model.compile(loss="huber",
+ optimizer=Adam(learning_rate=LEARNING_RATE, clipnorm=1.0),
+ metrics=["mae"])
return model
def create_dueling_model(self):
- """创建Dueling DQN模型架构"""
+ """创建Dueling DQN模型架构 - 优化版"""
inputs = Input(shape=(IM_HEIGHT, IM_WIDTH, 3))
# 共享的特征提取层
# 第一卷积块
- x = Conv2D(32, (3, 3), strides=(1, 1), padding='same')(inputs)
+ x = SeparableConv2D(32, (3, 3), strides=(1, 1), padding='same')(inputs)
x = Activation('relu')(x)
x = BatchNormalization()(x)
x = MaxPooling2D(pool_size=(2, 2))(x)
# 第二卷积块
- x = Conv2D(64, (3, 3), padding='same')(x)
+ x = SeparableConv2D(64, (3, 3), padding='same')(x)
x = Activation('relu')(x)
x = BatchNormalization()(x)
x = MaxPooling2D(pool_size=(2, 2))(x)
# 第三卷积块
- x = Conv2D(128, (3, 3), padding='same')(x)
+ x = SeparableConv2D(128, (3, 3), padding='same')(x)
x = Activation('relu')(x)
x = BatchNormalization()(x)
x = MaxPooling2D(pool_size=(2, 2))(x)
- # 空间注意力机制
- attention = Conv2D(1, (1, 1), padding='same', activation='sigmoid')(x)
- x = Multiply()([x, attention])
+ # 注意力机制
+ x = AttentionModule.cbam_block(x)
+
+ # 第四卷积块
+ x = SeparableConv2D(256, (3, 3), padding='same')(x)
+ x = Activation('relu')(x)
+ x = BatchNormalization()(x)
+ x = SpatialDropout2D(0.1)(x)
# 展平层
x = Flatten()(x)
@@ -230,13 +293,25 @@ def create_dueling_model(self):
model = Model(inputs=inputs, outputs=q_values)
# 编译模型
- model.compile(loss="huber", optimizer=Adam(learning_rate=LEARNING_RATE), metrics=["mae"])
+ model.compile(loss="huber",
+ optimizer=Adam(learning_rate=LEARNING_RATE, clipnorm=1.0),
+ metrics=["mae"])
return model
- def update_replay_memory(self, transition):
+ def update_replay_memory(self, transition, reaction_time=None):
"""更新经验回放缓冲区"""
# transition = (当前状态, 动作, 奖励, 新状态, 完成标志)
+ if reaction_time is not None:
+ # 记录反应时间
+ self.reaction_times.append(reaction_time)
+
+ # 如果反应时间过长,增加惩罚
+ if reaction_time > 1.0: # 反应时间超过1秒
+ transition = list(transition)
+ transition[2] -= 0.5 # 减少奖励
+ transition = tuple(transition)
+
if self.use_per:
# PER: 初始添加时使用最大优先级
self.replay_buffer.add(transition, error=1.0) # 初始误差设为1.0
@@ -244,7 +319,7 @@ def update_replay_memory(self, transition):
self.replay_memory.append(transition)
def minibatch_chooser(self):
- """改进的经验采样策略"""
+ """改进的经验采样策略 - 优先采样危险情况"""
if self.use_per:
# PER采样
if len(self.replay_buffer) < MIN_REPLAY_MEMORY_SIZE:
@@ -253,34 +328,41 @@ def minibatch_chooser(self):
indices, samples, weights = self.replay_buffer.sample(MINIBATCH_SIZE)
return samples
else:
- # 标准采样
+ # 标准采样 - 增加危险经验的权重
if len(self.replay_memory) < MIN_REPLAY_MEMORY_SIZE:
return random.sample(self.replay_memory, min(len(self.replay_memory), MINIBATCH_SIZE))
# 分类经验样本
- positive_samples = [] # 高奖励经验
- negative_samples = [] # 负奖励/碰撞经验
- neutral_samples = [] # 中性奖励经验
+ danger_samples = [] # 接近碰撞的经验
+ collision_samples = [] # 碰撞经验
+ positive_samples = [] # 成功避障经验
+ neutral_samples = [] # 中性经验
for sample in self.replay_memory:
- _, _, reward, _, done = sample
+ state, action, reward, next_state, done = sample
- if done and reward < -5: # 碰撞或严重错误
- negative_samples.append(sample)
- elif reward > 1: # 积极经验
+ if done and reward < -5: # 碰撞经验
+ collision_samples.append(sample)
+ elif reward < -2 and not done: # 危险但未碰撞
+ danger_samples.append(sample)
+ elif reward > 2: # 积极经验(成功避障)
positive_samples.append(sample)
else: # 中性经验
neutral_samples.append(sample)
- # 平衡采样
+ # 平衡采样 - 增加危险经验的权重
batch = []
- # 采样负经验 (20%)
- num_negative = min(len(negative_samples), MINIBATCH_SIZE // 5)
- batch.extend(random.sample(negative_samples, num_negative))
+ # 采样碰撞经验 (25%)
+ num_collision = min(len(collision_samples), MINIBATCH_SIZE // 4)
+ batch.extend(random.sample(collision_samples, num_collision))
- # 采样正经验 (30%)
- num_positive = min(len(positive_samples), MINIBATCH_SIZE // 3)
+ # 采样危险经验 (30%)
+ num_danger = min(len(danger_samples), MINIBATCH_SIZE * 3 // 10)
+ batch.extend(random.sample(danger_samples, num_danger))
+
+ # 采样成功避障经验 (25%)
+ num_positive = min(len(positive_samples), MINIBATCH_SIZE // 4)
batch.extend(random.sample(positive_samples, num_positive))
# 用中性经验补全批次
@@ -389,4 +471,10 @@ def train_in_loop(self):
def get_qs(self, state):
"""获取状态的Q值"""
- return self.model.predict(np.array(state).reshape(-1, *state.shape) / 255, verbose=0)[0]
\ No newline at end of file
+ return self.model.predict(np.array(state).reshape(-1, *state.shape) / 255, verbose=0)[0]
+
+ def get_average_reaction_time(self):
+ """获取平均反应时间"""
+ if len(self.reaction_times) > 0:
+ return np.mean(self.reaction_times)
+ return 0
\ No newline at end of file
diff --git a/src/Unmanned_vehicle_AD_DQN/Test.py b/src/Unmanned_vehicle_AD_DQN/Test.py
index ccd53b468c..b90d26188f 100644
--- a/src/Unmanned_vehicle_AD_DQN/Test.py
+++ b/src/Unmanned_vehicle_AD_DQN/Test.py
@@ -66,7 +66,7 @@ def select_best_model(model_files, preferred_keywords=None, excluded_keywords=No
return None
if preferred_keywords is None:
- preferred_keywords = ["best", "advanced", "dueling_per"]
+ preferred_keywords = ["best", "advanced", "dueling_per", "v2"]
if excluded_keywords is None:
excluded_keywords = ["min", "avg", "final"] # 排除统计文件
@@ -151,16 +151,22 @@ def get_safe_action_advanced(model, state, env, previous_action, uncertainty_thr
qs[1] *= 1.2 # 提高保持倾向
qs[2] *= 1.3 # 提高加速倾向
- # 2. 行人避障优先级
+ # 2. 行人避障优先级 - 增强版
if hasattr(env, 'suggested_action') and env.suggested_action is not None:
- qs[env.suggested_action] += 3.0 # 大幅提高建议动作的Q值
- print(f"🚨 安全避让: 执行动作 {env.suggested_action}")
+ # 根据距离调整优先级
+ min_ped_distance = getattr(env, 'last_ped_distance', float('inf'))
+ if min_ped_distance < 8.0: # 危险距离
+ qs[env.suggested_action] += 5.0 # 大幅提高建议动作的Q值
+ print(f"🚨 紧急避让: 执行动作 {env.suggested_action}, 距离: {min_ped_distance:.1f}m")
+ elif min_ped_distance < 12.0: # 预警距离
+ qs[env.suggested_action] += 2.0
+ print(f"⚠️ 安全避让: 执行动作 {env.suggested_action}, 距离: {min_ped_distance:.1f}m")
env.suggested_action = None
# 3. 防止过度转向
if hasattr(env, 'same_steer_counter') and env.same_steer_counter > 2:
if previous_action in [3, 4]:
- qs[previous_action] -= 1.5 # 降低连续同向转向的倾向
+ qs[previous_action] -= 2.0 # 降低连续同向转向的倾向
# 4. 动作平滑性
if previous_action in [3, 4]: # 转向动作
@@ -177,18 +183,24 @@ def get_safe_action_advanced(model, state, env, previous_action, uncertainty_thr
elif abs(vehicle_rotation) > 30: # 方向偏差大
# 鼓励向相反方向转向以回正
if vehicle_rotation > 0: # 偏左,鼓励右转
- qs[4] += 1.0
+ qs[4] += 1.5
else: # 偏右,鼓励左转
- qs[3] += 1.0
+ qs[3] += 1.5
- # 6. 紧急情况处理
+ # 6. 紧急情况处理 - 增强版
min_ped_distance = getattr(env, 'last_ped_distance', float('inf'))
if min_ped_distance < 5.0: # 紧急避让距离
# 大幅调整Q值以确保安全
- qs[0] += 2.0 # 紧急制动
+ qs[0] += 3.0 # 紧急制动
if min_ped_distance < 3.0: # 极危险
qs[2] = -float('inf') # 禁止加速
- print("⚠️ 紧急制动!")
+ qs[1] = -float('inf') # 禁止保持
+ print("⚠️ 紧急制动! 危险距离!")
+
+ # 7. 考虑运动检测结果
+ if hasattr(env, 'motion_detected') and env.motion_detected:
+ # 如果检测到运动,提高警惕
+ qs[0] += 0.5 # 鼓励减速观察
# 选择动作
action = np.argmax(qs)
@@ -225,6 +237,9 @@ def run_test_episode(model, env, episode_num, use_advanced_safety=True):
previous_action = 1
fps_counter = deque(maxlen=30)
+ # 反应时间统计
+ reaction_times = []
+
# 运行episode
max_steps = SECONDS_PER_EPISODE * 60
@@ -242,6 +257,12 @@ def run_test_episode(model, env, episode_num, use_advanced_safety=True):
previous_action = action
+ # 记录反应开始时间(如果有障碍物)
+ if hasattr(env, 'obstacle_detected_time') and env.obstacle_detected_time is not None:
+ if hasattr(env, 'reaction_start_time') and env.reaction_start_time is not None:
+ reaction_time = time.time() - env.reaction_start_time
+ reaction_times.append(reaction_time)
+
# 执行动作
new_state, reward, done, _ = env.step(action)
@@ -260,10 +281,14 @@ def run_test_episode(model, env, episode_num, use_advanced_safety=True):
velocity = env.vehicle.get_velocity()
speed_kmh = 3.6 * np.linalg.norm([velocity.x, velocity.y, velocity.z])
+ # 获取最近行人距离
+ min_ped_distance = getattr(env, 'last_ped_distance', float('inf'))
+
status = "✅" if reward > 0 else "⚠️" if reward < -1 else "➡️"
print(f"{status} 步数: {step_count:4d} | FPS: {fps:4.1f} | "
- f"速度: {speed_kmh:5.1f} km/h | 奖励: {reward:6.2f} | 累计: {total_reward:7.2f}")
+ f"速度: {speed_kmh:5.1f} km/h | 行人距离: {min_ped_distance:5.1f}m | "
+ f"奖励: {reward:6.2f} | 累计: {total_reward:7.2f}")
if done:
break
@@ -275,10 +300,14 @@ def run_test_episode(model, env, episode_num, use_advanced_safety=True):
success = total_reward > 5
result = "成功" if success else "失败"
+ # 计算平均反应时间
+ avg_reaction_time = np.mean(reaction_times) if reaction_times else 0
+
print(f"\nEpisode {episode_num} 结果: {result}")
print(f"总步数: {step_count}, 总奖励: {total_reward:.2f}")
+ print(f"平均反应时间: {avg_reaction_time:.2f}秒")
- return success, total_reward, step_count
+ return success, total_reward, step_count, avg_reaction_time
def load_model_with_fallback(model_path):
@@ -374,17 +403,19 @@ def comprehensive_model_evaluation(model_path, num_episodes=5):
'successes': 0,
'total_rewards': [],
'episode_lengths': [],
+ 'reaction_times': [],
'start_time': time.time()
}
try:
for episode in range(1, num_episodes + 1):
- success, reward, length = run_test_episode(model, env, episode, use_advanced_safety=True)
+ success, reward, length, avg_rt = run_test_episode(model, env, episode, use_advanced_safety=True)
if success:
results['successes'] += 1
results['total_rewards'].append(reward)
results['episode_lengths'].append(length)
+ results['reaction_times'].append(avg_rt)
# 短暂暂停
time.sleep(1)
@@ -407,6 +438,7 @@ def comprehensive_model_evaluation(model_path, num_episodes=5):
results['avg_length'] = np.mean(results['episode_lengths'])
results['max_reward'] = max(results['total_rewards'])
results['min_reward'] = min(results['total_rewards'])
+ results['avg_reaction_time'] = np.mean(results['reaction_times']) if results['reaction_times'] else 0
# 显示评估报告
print(f"\n{'='*60}")
@@ -417,6 +449,7 @@ def comprehensive_model_evaluation(model_path, num_episodes=5):
print(f"成功率: {results.get('success_rate', 0):.1f}%")
print(f"平均奖励: {results.get('avg_reward', 0):.2f}")
print(f"平均步数: {results.get('avg_length', 0):.1f}")
+ print(f"平均反应时间: {results.get('avg_reaction_time', 0):.2f}秒")
print(f"最佳表现: {results.get('max_reward', 0):.2f}")
print(f"最差表现: {results.get('min_reward', 0):.2f}")
print(f"总测试时间: {results.get('total_time', 0):.1f}秒")
@@ -500,7 +533,7 @@ def interactive_model_selection(model_files):
def main():
"""主函数 - 自动查找和测试模型"""
print(f"\n{'='*60}")
- print("自动驾驶模型测试系统")
+ print("自动驾驶模型测试系统 - 优化版")
print(f"{'='*60}")
# 显示当前脚本所在目录
diff --git a/src/Unmanned_vehicle_AD_DQN/TrainingStrategies.py b/src/Unmanned_vehicle_AD_DQN/TrainingStrategies.py
index cb5e611506..6f35c41a38 100644
--- a/src/Unmanned_vehicle_AD_DQN/TrainingStrategies.py
+++ b/src/Unmanned_vehicle_AD_DQN/TrainingStrategies.py
@@ -16,82 +16,118 @@ def __init__(self, env):
self.current_stage = 0
self.stage_thresholds = [0.3, 0.5, 0.7, 0.85] # 成功率阈值
self.stage_configs = [
- # 阶段0: 入门
+ # 阶段0: 入门 - 更早引入行人,但数量少速度慢
{
- 'pedestrian_cross': 2, # 十字路口行人数量(减少)
- 'pedestrian_normal': 1, # 普通路段行人数量(减少)
- 'pedestrian_speed_min': 0.5, # 行人最低速度
- 'pedestrian_speed_max': 1.0, # 行人最高速度
- 'max_episode_steps': 1200, # 最大步数 (20秒 * 60FPS)
- 'success_threshold': 0.3 # 进入下一阶段成功率
+ 'pedestrian_cross': 3, # 增加一点难度
+ 'pedestrian_normal': 2,
+ 'pedestrian_speed_min': 0.3, # 降低行人速度
+ 'pedestrian_speed_max': 0.8,
+ 'max_episode_steps': 900, # 减少最大步数,加速训练
+ 'success_threshold': 0.3,
+ 'difficulty_name': '简单'
},
# 阶段1: 初级
{
- 'pedestrian_cross': 4, # 逐步增加
- 'pedestrian_normal': 2,
+ 'pedestrian_cross': 5,
+ 'pedestrian_normal': 3,
+ 'pedestrian_speed_min': 0.5,
+ 'pedestrian_speed_max': 1.0,
+ 'max_episode_steps': 1200,
+ 'success_threshold': 0.5,
+ 'difficulty_name': '初级'
+ },
+ # 阶段2: 中级 - 增加反应时间训练
+ {
+ 'pedestrian_cross': 7,
+ 'pedestrian_normal': 4,
'pedestrian_speed_min': 0.7,
'pedestrian_speed_max': 1.3,
- 'max_episode_steps': 1800, # 30秒
- 'success_threshold': 0.5
+ 'max_episode_steps': 1500,
+ 'success_threshold': 0.6, # 提高阈值
+ 'difficulty_name': '中级'
},
- # 阶段2: 中级
+ # 阶段3: 高级
{
- 'pedestrian_cross': 6,
- 'pedestrian_normal': 3,
- 'pedestrian_speed_min': 0.8,
- 'pedestrian_speed_max': 1.5,
- 'max_episode_steps': 2400, # 40秒
- 'success_threshold': 0.7
+ 'pedestrian_cross': 9,
+ 'pedestrian_normal': 5,
+ 'pedestrian_speed_min': 0.9,
+ 'pedestrian_speed_max': 1.6,
+ 'max_episode_steps': 1800,
+ 'success_threshold': 0.7,
+ 'difficulty_name': '高级'
},
- # 阶段3: 高级 (正常难度)
+ # 阶段4: 专家 (正常难度)
{
- 'pedestrian_cross': 8,
- 'pedestrian_normal': 4,
+ 'pedestrian_cross': 10,
+ 'pedestrian_normal': 6,
'pedestrian_speed_min': 1.0,
'pedestrian_speed_max': 2.0,
- 'max_episode_steps': 3600, # 60秒
- 'success_threshold': 0.85
+ 'max_episode_steps': 2400,
+ 'success_threshold': 0.8,
+ 'difficulty_name': '专家'
},
- # 阶段4: 专家 (挑战)
+ # 阶段5: 大师 (挑战)
{
- 'pedestrian_cross': 10, # 适当减少
- 'pedestrian_normal': 5,
+ 'pedestrian_cross': 12,
+ 'pedestrian_normal': 8,
'pedestrian_speed_min': 1.2,
'pedestrian_speed_max': 2.5,
- 'max_episode_steps': 3600,
- 'success_threshold': 0.9
+ 'max_episode_steps': 3000,
+ 'success_threshold': 0.85,
+ 'difficulty_name': '大师'
}
]
# 训练历史
self.success_history = deque(maxlen=20) # 记录最近20轮的成功情况
self.reward_history = deque(maxlen=50) # 记录最近50轮的奖励
+ self.reaction_time_history = deque(maxlen=50) # 记录反应时间
- def update_stage(self, success, reward):
+ def update_stage(self, success, reward, reaction_time=None):
"""更新训练阶段"""
# 记录历史
self.success_history.append(1 if success else 0)
self.reward_history.append(reward)
+ if reaction_time is not None:
+ self.reaction_time_history.append(reaction_time)
# 计算最近成功率
if len(self.success_history) >= 10:
success_rate = sum(self.success_history) / len(self.success_history)
avg_reward = np.mean(self.reward_history) if self.reward_history else 0
- # 减少打印频率
- if len(self.success_history) % 10 == 0:
- print(f"课程学习 - 当前阶段: {self.current_stage}, 成功率: {success_rate:.2f}, 平均奖励: {avg_reward:.2f}")
+ # 每20轮打印一次
+ if len(self.success_history) % 20 == 0:
+ stage_info = self.get_current_config()
+ print(f"课程学习 - 阶段: {self.current_stage}({stage_info['difficulty_name']}), "
+ f"成功率: {success_rate:.2f}, 平均奖励: {avg_reward:.2f}")
+ if self.reaction_time_history:
+ avg_rt = np.mean(self.reaction_time_history)
+ print(f" 平均反应时间: {avg_rt:.2f}秒")
# 检查是否可以进入下一阶段
if self.current_stage < len(self.stage_configs) - 1:
next_stage_threshold = self.stage_configs[self.current_stage]['success_threshold']
- if success_rate >= next_stage_threshold and avg_reward > 5:
+
+ # 不仅要看成功率,还要看反应时间(如果可用)
+ can_advance = success_rate >= next_stage_threshold and avg_reward > 5
+ if can_advance and self.reaction_time_history:
+ avg_rt = np.mean(self.reaction_time_history)
+ # 要求反应时间小于1秒
+ if avg_rt < 1.0:
+ self.current_stage += 1
+ print(f"🎉 课程学习: 进阶到阶段 {self.current_stage}!")
+ print(f" 新配置: {self.stage_configs[self.current_stage]['difficulty_name']}")
+ return True
+ elif can_advance:
self.current_stage += 1
print(f"🎉 课程学习: 进阶到阶段 {self.current_stage}!")
+ print(f" 新配置: {self.stage_configs[self.current_stage]['difficulty_name']}")
return True
- # 如果表现太差,退回上一阶段
- if self.current_stage > 0 and success_rate < 0.2:
+ # 如果表现太差或反应时间太长,退回上一阶段
+ if self.current_stage > 0 and (success_rate < 0.2 or
+ (self.reaction_time_history and np.mean(self.reaction_time_history) > 2.0)):
self.current_stage -= 1
print(f"⚠️ 课程学习: 退回阶段 {self.current_stage}")
return True
@@ -113,23 +149,28 @@ class MultiObjectiveOptimizer:
def __init__(self):
# 定义优化目标及其权重(可动态调整)
self.objectives = {
+ 'reaction_time': {
+ 'weight': 0.25, # 新增:反应时间权重
+ 'description': '快速反应避障',
+ 'metrics': ['reaction_time', 'proactive_actions']
+ },
'safety': {
- 'weight': 0.35, # 稍微降低安全权重
+ 'weight': 0.30,
'description': '安全避障和避免碰撞',
'metrics': ['collision_avoidance', 'pedestrian_distance']
},
'efficiency': {
- 'weight': 0.30, # 提高效率权重
+ 'weight': 0.25,
'description': '快速到达目的地',
'metrics': ['progress_speed', 'total_time']
},
'comfort': {
- 'weight': 0.20,
+ 'weight': 0.15,
'description': '平稳驾驶体验',
'metrics': ['smoothness', 'steering_changes']
},
'rule_following': {
- 'weight': 0.15,
+ 'weight': 0.05,
'description': '遵守交通规则',
'metrics': ['lane_keeping', 'speed_limit']
}
@@ -137,6 +178,7 @@ def __init__(self):
# 指标跟踪
self.metrics_history = {
+ 'reaction_time': [],
'safety': [],
'efficiency': [],
'comfort': [],
@@ -156,13 +198,25 @@ def compute_composite_reward(self, metrics):
# 记录指标历史
self.metrics_history[obj_name].append(normalized_value)
- # 特殊惩罚项
+ # 特殊奖励/惩罚项
if metrics.get('collision', False):
- composite -= 8 # 减少碰撞惩罚
+ composite -= 10 # 增加碰撞惩罚
if metrics.get('off_road', False):
- composite -= 3 # 减少偏离道路惩罚
+ composite -= 5 # 增加偏离道路惩罚
if metrics.get('dangerous_action', False):
- composite -= 2 # 减少危险动作惩罚
+ composite -= 3 # 增加危险动作惩罚
+
+ # 新增:反应时间相关奖励/惩罚
+ if 'reaction_time' in metrics:
+ rt = metrics['reaction_time']
+ if rt < 0.5: # 快速反应
+ composite += 2
+ elif rt > 1.5: # 反应太慢
+ composite -= 3
+
+ # 新增:主动避障奖励
+ if metrics.get('proactive_action', False):
+ composite += 1.5
return composite
@@ -170,10 +224,11 @@ def _normalize_metric(self, value, metric_name):
"""归一化指标值到[0, 1]范围"""
# 不同指标的归一化方式不同
normalization_rules = {
- 'safety': lambda x: min(max(x / 10, 0), 1), # 假设安全分满分10
- 'efficiency': lambda x: min(max(x / 100, 0), 1), # 效率分满分100
- 'comfort': lambda x: min(max((x + 5) / 10, 0), 1), # 舒适度[-5, 5] -> [0, 1]
- 'rule_following': lambda x: min(max(x, 0), 1) # 规则遵循度[0, 1]
+ 'reaction_time': lambda x: max(0, 1 - x/3), # 反应时间越短越好
+ 'safety': lambda x: min(max(x / 10, 0), 1),
+ 'efficiency': lambda x: min(max(x / 100, 0), 1),
+ 'comfort': lambda x: min(max((x + 5) / 10, 0), 1),
+ 'rule_following': lambda x: min(max(x, 0), 1)
}
if metric_name in normalization_rules:
@@ -196,7 +251,7 @@ def adjust_weights(self, performance_feedback):
# 如果最差目标表现低于阈值,增加其权重
if recent_performance[worst_obj] < 0.3:
- adjustment = 0.02 # 减少调整幅度
+ adjustment = 0.03 # 调整幅度
self.objectives[worst_obj]['weight'] += adjustment
self.objectives[best_obj]['weight'] -= adjustment
@@ -223,7 +278,7 @@ def get_performance_report(self):
return report
-# 模仿学习管理器
+# 模仿学习管理器(保持不变,略作修改)
class ImitationLearningManager:
def __init__(self, expert_data_path=None):
self.expert_data_path = expert_data_path
@@ -244,232 +299,11 @@ def load_expert_data(self, path):
except Exception as e:
print(f"加载专家数据失败: {e}")
return False
-
- def collect_expert_demonstration(self, env, num_episodes=10):
- """收集专家示范数据(可以手动控制或使用规则控制器)"""
- print(f"开始收集专家示范数据 ({num_episodes}个episodes)...")
-
- demonstrations = []
-
- for episode in range(num_episodes):
- print(f"收集专家示范 Episode {episode + 1}/{num_episodes}")
-
- state = env.reset(episode)
- done = False
- episode_data = []
-
- step_count = 0
- max_steps = 60 * 60 # 最大60秒
-
- while not done and step_count < max_steps:
- # 这里可以使用规则控制器或手动控制
- # 示例:简单的规则控制器
- action = self._rule_based_controller(env)
-
- new_state, reward, done, _ = env.step(action)
-
- # 保存示范数据
- episode_data.append({
- 'state': state.copy(),
- 'action': action,
- 'reward': reward,
- 'next_state': new_state.copy(),
- 'done': done
- })
-
- state = new_state
- step_count += 1
-
- demonstrations.extend(episode_data)
- env.cleanup_actors()
-
- # 保存专家数据
- self.expert_data = demonstrations
- timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
- save_path = f"expert_data_{timestamp}.pkl"
-
- with open(save_path, 'wb') as f:
- pickle.dump(demonstrations, f)
-
- print(f"专家示范数据已保存到: {save_path}, 共 {len(demonstrations)} 条记录")
- return True
-
- def _rule_based_controller(self, env):
- """基于规则的控制器(作为专家示范)"""
- # 获取车辆状态
- vehicle_location = env.vehicle.get_location()
- velocity = env.vehicle.get_velocity()
- speed_kmh = 3.6 * math.sqrt(velocity.x**2 + velocity.y**2)
-
- # 简单规则:保持速度在20-35 km/h,避免障碍物
- if speed_kmh < 20:
- return 2 # 加速
- elif speed_kmh > 35:
- return 0 # 减速
- else:
- # 检查前方障碍物
- has_obstacle_ahead = self._check_obstacle_ahead(env)
- if has_obstacle_ahead:
- return 0 # 减速
- else:
- return 1 # 保持
-
- return 1 # 默认保持
-
- def _check_obstacle_ahead(self, env):
- """检查前方是否有障碍物(简化版本)"""
- # 这里可以添加更复杂的障碍物检测逻辑
- # 暂时返回False
- return False
-
- def pretrain_with_behavioral_cloning(self, model, epochs=20):
- """使用行为克隆进行预训练"""
- if not self.expert_data:
- print("没有专家数据可用,跳过预训练")
- return model
-
- print(f"开始行为克隆预训练 ({epochs}个epochs)...")
-
- # 准备训练数据
- states = []
- actions = []
-
- for demo in self.expert_data:
- states.append(demo['state'])
- actions.append(demo['action'])
-
- # 将状态归一化
- states = np.array(states) / 255.0
-
- # 将动作转换为one-hot编码
- actions_onehot = tf.keras.utils.to_categorical(actions, num_classes=5)
-
- # 备份原始编译设置
- original_loss = model.loss
- original_optimizer = model.optimizer
- original_metrics = model.metrics_names
-
- # 重新编译模型用于分类任务
- model.compile(
- optimizer=Adam(learning_rate=0.0001),
- loss='categorical_crossentropy',
- metrics=['accuracy']
- )
-
- # 训练模型模仿专家行为
- history = model.fit(
- states, actions_onehot,
- batch_size=32,
- epochs=epochs,
- validation_split=0.2,
- verbose=1
- )
-
- print(f"预训练完成 - 最终准确率: {history.history['accuracy'][-1]:.3f}")
-
- # 恢复原始编译设置
- model.compile(
- optimizer=original_optimizer,
- loss=original_loss,
- metrics=original_metrics
- )
-
- self.is_pretrained = True
- return model
-
- def train_with_dagger(self, model, env, iterations=5, episodes_per_iter=5):
- """使用DAgger算法进行训练"""
- print(f"开始DAgger训练 ({iterations}次迭代,每次{episodes_per_iter}个episodes)...")
-
- aggregated_data = self.expert_data.copy()
-
- for iteration in range(iterations):
- print(f"\nDAgger 迭代 {iteration + 1}/{iterations}")
-
- # 使用当前策略收集数据
- new_demos = []
-
- for episode in range(episodes_per_iter):
- print(f" 收集数据 Episode {episode + 1}/{episodes_per_iter}")
-
- state = env.reset(episode)
- done = False
- step_count = 0
- max_steps = 60 * 60
-
- while not done and step_count < max_steps:
- # 使用当前策略选择动作
- qs = model.predict(np.array(state).reshape(-1, *state.shape) / 255, verbose=0)[0]
- action = np.argmax(qs)
-
- # 执行动作
- new_state, reward, done, _ = env.step(action)
-
- # 专家纠正(这里可以添加专家纠正逻辑)
- # 如果策略动作与专家建议不同,使用专家动作
- expert_action = self._rule_based_controller(env)
-
- # 保存数据(使用专家纠正后的动作)
- new_demos.append({
- 'state': state.copy(),
- 'action': expert_action, # 使用专家动作
- 'reward': reward,
- 'next_state': new_state.copy(),
- 'done': done
- })
-
- state = new_state
- step_count += 1
-
- env.cleanup_actors()
-
- # 合并数据
- aggregated_data.extend(new_demos)
-
- # 在合并数据上重新训练
- states = [d['state'] for d in aggregated_data]
- actions = [d['action'] for d in aggregated_data]
-
- states = np.array(states) / 255.0
- actions_onehot = tf.keras.utils.to_categorical(actions, num_classes=5)
-
- # 备份原始编译设置
- original_loss = model.loss
- original_optimizer = model.optimizer
- original_metrics = model.metrics_names
-
- # 重新编译用于分类
- model.compile(
- optimizer=Adam(learning_rate=0.0001),
- loss='categorical_crossentropy',
- metrics=['accuracy']
- )
-
- # 训练模型
- history = model.fit(
- states, actions_onehot,
- batch_size=32,
- epochs=10,
- validation_split=0.1,
- verbose=0
- )
-
- # 恢复原始编译设置
- model.compile(
- optimizer=original_optimizer,
- loss=original_loss,
- metrics=original_metrics
- )
-
- print(f" 训练完成 - 准确率: {history.history['accuracy'][-1]:.3f}")
-
- print("DAgger训练完成!")
- return model
-# 优先经验回放缓冲区
+# 优先经验回放缓冲区(增加对危险经验的优先级)
class PrioritizedReplayBuffer:
- def __init__(self, max_size=10000, alpha=0.6, beta_start=0.4, beta_frames=100000):
+ def __init__(self, max_size=15000, alpha=0.7, beta_start=0.5, beta_frames=50000):
self.max_size = max_size
self.alpha = alpha # 优先级程度 (0 = 均匀采样, 1 = 完全优先级)
self.beta_start = beta_start # 重要性采样权重起始值
@@ -494,6 +328,11 @@ def add(self, experience, error=None):
else:
priority = (abs(error) + 1e-5) ** self.alpha
+ # 如果是危险经验(负奖励较大),增加优先级
+ reward = experience[2]
+ if reward < -2: # 危险经验
+ priority *= 1.5
+
self.buffer.append(experience)
self.priorities.append(priority)
diff --git a/src/Unmanned_vehicle_AD_DQN/main.py b/src/Unmanned_vehicle_AD_DQN/main.py
index e627c3006d..f69fb91625 100644
--- a/src/Unmanned_vehicle_AD_DQN/main.py
+++ b/src/Unmanned_vehicle_AD_DQN/main.py
@@ -1,4 +1,4 @@
-# main.py
+# main_fixed.py - 修复模型保存问题的版本
import glob
import os
import sys
@@ -26,6 +26,27 @@
# 从Hyperparameters导入所有参数
from Hyperparameters import *
+def ensure_models_directory():
+ """确保models目录存在"""
+ if not os.path.exists('models'):
+ os.makedirs('models')
+ print("✅ 已创建 models 目录")
+ return 'models'
+
+def save_model_with_retry(model, filepath, max_retries=3):
+ """带重试机制的模型保存"""
+ for attempt in range(max_retries):
+ try:
+ model.save(filepath)
+ print(f"✅ 模型保存成功: {os.path.basename(filepath)}")
+ return True
+ except Exception as e:
+ print(f"⚠️ 保存失败 (尝试 {attempt+1}/{max_retries}): {e}")
+ time.sleep(1)
+
+ print(f"❌ 无法保存模型: {filepath}")
+ return False
+
def extended_reward_calculation(env, action, reward, done, step_info):
"""
扩展的奖励计算函数,用于多目标优化
@@ -38,7 +59,23 @@ def extended_reward_calculation(env, action, reward, done, step_info):
# 计算多目标指标
metrics = {}
- # 1. 安全性指标 - 基于最近行人距离
+ # 1. 反应时间指标
+ reaction_time = 0
+ if hasattr(env, 'obstacle_detected_time') and env.obstacle_detected_time is not None:
+ if hasattr(env, 'reaction_start_time') and env.reaction_start_time is not None:
+ reaction_time = time.time() - env.reaction_start_time
+
+ metrics['reaction_time'] = reaction_time
+
+ # 2. 主动避障指标
+ proactive_action = False
+ if hasattr(env, 'suggested_action') and env.suggested_action is not None:
+ if action == env.suggested_action:
+ proactive_action = True
+
+ metrics['proactive_action'] = proactive_action
+
+ # 3. 安全性指标 - 基于最近行人距离
min_ped_distance = getattr(env, 'last_ped_distance', float('inf'))
safety_score = 0
if min_ped_distance < 100:
@@ -55,12 +92,12 @@ def extended_reward_calculation(env, action, reward, done, step_info):
metrics['safety'] = safety_score
- # 2. 效率指标 - 基于进度
+ # 4. 效率指标 - 基于进度
progress = (vehicle_location.x + 81) / 236.0 # 从-81到155
efficiency_score = progress * 100 # 进度百分比
metrics['efficiency'] = efficiency_score
- # 3. 舒适度指标 - 基于转向平滑性
+ # 5. 舒适度指标 - 基于转向平滑性
comfort_score = 5 # 默认舒适
if hasattr(env, 'last_action') and env.last_action in [3, 4]:
@@ -75,7 +112,7 @@ def extended_reward_calculation(env, action, reward, done, step_info):
metrics['comfort'] = comfort_score
- # 4. 规则遵循指标 - 基于速度
+ # 6. 规则遵循指标 - 基于速度
rule_score = 0.3 # 默认较低分数
if 20 <= speed_kmh <= 35: # 理想速度范围
@@ -89,11 +126,11 @@ def extended_reward_calculation(env, action, reward, done, step_info):
metrics['rule_following'] = rule_score
- # 5. 特殊事件
+ # 7. 特殊事件
metrics['collision'] = len(getattr(env, 'collision_history', [])) > 0
metrics['off_road'] = vehicle_location.x < -90 or abs(vehicle_location.y + 195) > 30
- # 6. 危险动作检测
+ # 8. 危险动作检测
if speed_kmh > 40 and action in [3, 4]: # 高速急转
metrics['dangerous_action'] = True
else:
@@ -105,19 +142,14 @@ def extended_reward_calculation(env, action, reward, done, step_info):
FPS = 60 # 帧率
ep_rewards = [-200] # 存储每轮奖励
+ # 确保models目录存在
+ models_dir = ensure_models_directory()
+
# GPU内存配置
gpu_options = tf.compat.v1.GPUOptions(per_process_gpu_memory_fraction=MEMORY_FRACTION)
tf.compat.v1.keras.backend.set_session(
tf.compat.v1.Session(config=tf.compat.v1.ConfigProto(gpu_options=gpu_options)))
- # 创建模型保存目录
- if not os.path.isdir('models'):
- os.makedirs('models')
-
- # 创建专家数据目录
- if not os.path.isdir('expert_data'):
- os.makedirs('expert_data')
-
# 创建智能体和环境
agent = DQNAgent(
use_dueling=True,
@@ -131,30 +163,6 @@ def extended_reward_calculation(env, action, reward, done, step_info):
# 设置训练策略
agent.setup_training_strategies(env)
- # 可选:使用模仿学习进行预训练
- use_imitation_pretraining = False # 设置为True启用模仿学习预训练
-
- if use_imitation_pretraining:
- print("=" * 60)
- print("开始模仿学习预训练阶段")
- print("=" * 60)
-
- # 检查是否有现有的专家数据
- expert_files = glob.glob("expert_data/*.pkl")
- if expert_files:
- # 使用最新的专家数据
- latest_expert = max(expert_files, key=os.path.getctime)
- agent.imitation_manager.load_expert_data(latest_expert)
- else:
- # 收集新的专家数据
- print("未找到专家数据,开始收集...")
- agent.imitation_manager.collect_expert_demonstration(env, num_episodes=5)
-
- # 使用行为克隆进行预训练
- agent.model = agent.imitation_manager.pretrain_with_behavioral_cloning(agent.model, epochs=15)
- agent.target_model.set_weights(agent.model.get_weights())
- print("模仿学习预训练完成!")
-
# 启动训练线程并等待训练初始化完成
trainer_thread = Thread(target=agent.train_in_loop, daemon=True)
trainer_thread.start()
@@ -178,6 +186,7 @@ def extended_reward_calculation(env, action, reward, done, step_info):
# 多目标统计
multi_obj_stats = {
+ 'reaction_time': [],
'safety': [],
'efficiency': [],
'comfort': [],
@@ -187,6 +196,9 @@ def extended_reward_calculation(env, action, reward, done, step_info):
# 课程学习阶段记录
curriculum_stages = []
+ # 反应时间统计
+ reaction_time_stats = []
+
# 迭代训练轮次
epds = []
for episode in tqdm(range(1, EPISODES + 1), ascii=True, unit='episodes'):
@@ -197,7 +209,7 @@ def extended_reward_calculation(env, action, reward, done, step_info):
if agent.curriculum_manager:
config = agent.curriculum_manager.get_current_config()
if episode % 50 == 0: # 每50轮打印一次
- print(f"课程学习 - 阶段 {agent.curriculum_manager.current_stage}: "
+ print(f"课程学习 - 阶段 {agent.curriculum_manager.current_stage}({config['difficulty_name']}): "
f"行人(十字路口={config['pedestrian_cross']}, 普通={config['pedestrian_normal']})")
curriculum_stages.append(agent.curriculum_manager.current_stage)
@@ -207,6 +219,7 @@ def extended_reward_calculation(env, action, reward, done, step_info):
# 多目标指标记录
episode_metrics = {
+ 'reaction_time': [],
'safety': [],
'efficiency': [],
'comfort': [],
@@ -234,7 +247,18 @@ def extended_reward_calculation(env, action, reward, done, step_info):
# 从Q网络获取动作(利用)
qs = agent.get_qs(current_state)
action = np.argmax(qs)
- if episode % 50 == 0 and step % 30 == 0: # 减少打印频率
+
+ # 如果有建议的避让动作,考虑采纳
+ if hasattr(env, 'suggested_action') and env.suggested_action is not None:
+ # 检查建议动作的Q值
+ suggested_q = qs[env.suggested_action]
+ current_best_q = qs[action]
+
+ # 如果建议动作的Q值接近最佳动作,采纳建议
+ if suggested_q > current_best_q * 0.8:
+ action = env.suggested_action
+
+ if episode % 100 == 0 and step % 100 == 0: # 减少打印频率
print(f'Ep {episode} Step {step}: Q值 [{qs[0]:>5.2f}, {qs[1]:>5.2f}, {qs[2]:>5.2f}, {qs[3]:>5.2f}, {qs[4]:>5.2f}] 动作: {action}')
else:
# 随机选择动作(探索)
@@ -245,6 +269,12 @@ def extended_reward_calculation(env, action, reward, done, step_info):
# 执行动作并获取结果
new_state, reward, done, _ = env.step(action)
+ # 计算反应时间
+ reaction_time = 0
+ if hasattr(env, 'obstacle_detected_time') and env.obstacle_detected_time is not None:
+ if hasattr(env, 'reaction_start_time') and env.reaction_start_time is not None:
+ reaction_time = time.time() - env.reaction_start_time
+
# 计算多目标指标
if agent.multi_objective_optimizer:
step_info = {'step': step, 'action': action}
@@ -261,8 +291,9 @@ def extended_reward_calculation(env, action, reward, done, step_info):
score += reward # 累加奖励
- # 更新经验回放
- agent.update_replay_memory((current_state, action, reward, new_state, done))
+ # 更新经验回放(带反应时间信息)
+ agent.update_replay_memory((current_state, action, reward, new_state, done),
+ reaction_time=reaction_time)
current_state = new_state # 更新当前状态
step += 1
@@ -272,40 +303,68 @@ def extended_reward_calculation(env, action, reward, done, step_info):
# 本轮结束 - 销毁所有actor
env.cleanup_actors()
-
+
+ # 计算平均反应时间
+ if episode_metrics['reaction_time']:
+ avg_reaction_time = np.mean([rt for rt in episode_metrics['reaction_time'] if rt > 0])
+ reaction_time_stats.append(avg_reaction_time)
+
# 计算本轮平均指标
avg_metrics = {}
for key, values in episode_metrics.items():
if values:
- avg_metrics[key] = np.mean(values)
+ # 过滤掉零值(无反应时)
+ if key == 'reaction_time':
+ filtered_values = [v for v in values if v > 0]
+ avg_metrics[key] = np.mean(filtered_values) if filtered_values else 0
+ else:
+ avg_metrics[key] = np.mean(values)
# 记录到统计中
if key in multi_obj_stats:
multi_obj_stats[key].append(avg_metrics[key])
- # 更新课程学习
+ # 更新课程学习(带反应时间)
success = score > 5 # 成功完成的阈值
+ avg_rt = avg_metrics.get('reaction_time', 0)
if agent.curriculum_manager:
- stage_changed = agent.curriculum_manager.update_stage(success, score)
+ stage_changed = agent.curriculum_manager.update_stage(success, score, avg_rt)
if stage_changed:
print(f"课程学习阶段已更新: {agent.curriculum_manager.current_stage}")
+
+ # 阶段变化时保存模型
+ model_path = f'{models_dir}/{MODEL_NAME}_stage_{agent.curriculum_manager.current_stage}_ep_{episode}.model'
+ save_model_with_retry(agent.model, model_path)
# 更新多目标优化器权重
if agent.multi_objective_optimizer and episode % 20 == 0:
agent.multi_objective_optimizer.adjust_weights(avg_metrics)
- if episode % 100 == 0:
- print(agent.multi_objective_optimizer.get_performance_report())
# 更新成功计数
if success:
success_count += 1
- # 动态保存最佳模型
+ # ============================================
+ # 修复:简化模型保存条件
+ # ============================================
+
+ # 1. 定期保存模型(每10轮)
+ if episode % 10 == 0:
+ model_path = f'{models_dir}/{MODEL_NAME}_ep{episode}_score{score:.1f}.model'
+ save_model_with_retry(agent.model, model_path)
+
+ # 2. 保存最佳模型(如果比之前好)
if score > best_score:
best_score = score
- model_suffix = f"advanced_best_{score:.2f}"
- agent.model.save(f'models/{MODEL_NAME}_{model_suffix}.model')
- print(f"新的最佳模型已保存: 得分={score:.2f}")
-
+ model_path = f'{models_dir}/{MODEL_NAME}_best_ep{episode}_score{score:.1f}.model'
+ save_model_with_retry(agent.model, model_path)
+ print(f"🏆 新的最佳模型: Episode {episode}, 得分: {score:.2f}")
+
+ # 3. 保存里程碑模型(每50轮)
+ if episode % 50 == 0:
+ model_path = f'{models_dir}/{MODEL_NAME}_milestone_ep{episode}.model'
+ save_model_with_retry(agent.model, model_path)
+ print(f"🎯 里程碑模型: Episode {episode}")
+
# 记录得分统计
scores.append(score)
avg_scores.append(np.mean(scores[-10:]) if len(scores) >= 10 else np.mean(scores))
@@ -334,23 +393,24 @@ def extended_reward_calculation(env, action, reward, done, step_info):
# 添加多目标指标
if agent.multi_objective_optimizer:
- for obj in ['safety', 'efficiency', 'comfort', 'rule_following']:
+ for obj in ['reaction_time', 'safety', 'efficiency', 'comfort', 'rule_following']:
if multi_obj_stats[obj]:
recent_avg = np.mean(multi_obj_stats[obj][-AGGREGATE_STATS_EVERY:]) if len(multi_obj_stats[obj]) >= AGGREGATE_STATS_EVERY else np.mean(multi_obj_stats[obj])
stats_dict[f'{obj}_score'] = recent_avg
+ # 添加反应时间统计
+ if reaction_time_stats:
+ avg_rt = np.mean(reaction_time_stats[-AGGREGATE_STATS_EVERY:]) if len(reaction_time_stats) >= AGGREGATE_STATS_EVERY else np.mean(reaction_time_stats)
+ stats_dict['reaction_time'] = avg_rt
+
agent.tensorboard.update_stats(**stats_dict)
- # 保存模型,仅当最小奖励达到设定值时
- if min_reward >= MIN_REWARD and (episode not in epds):
- model_suffix = f"advanced_{max_reward:_>7.2f}max_{average_reward:_>7.2f}avg_{min_reward:_>7.2f}min"
- agent.model.save(f'models/{MODEL_NAME}_{model_suffix}__{int(time.time())}.model')
-
epds.append(episode)
# 打印训练信息
if episode % 10 == 0: # 每10轮打印一次
- info_str = f'轮次: {episode:3d}, 得分: {score:6.2f}, 成功: {success_count:3d}'
+ avg_rt = np.mean(reaction_time_stats[-10:]) if len(reaction_time_stats) >= 10 else 0
+ info_str = f'轮次: {episode:3d}, 得分: {score:6.2f}, 成功: {success_count:3d}, 反应时间: {avg_rt:.2f}s'
if agent.curriculum_manager:
info_str += f', 阶段: {agent.curriculum_manager.current_stage}'
print(info_str)
@@ -364,116 +424,57 @@ def extended_reward_calculation(env, action, reward, done, step_info):
agent.terminate = True
trainer_thread.join()
+ # ============================================
+ # 修复:始终保存最终模型
+ # ============================================
+
# 保存最终模型
- if len(scores) > 0:
- final_max_reward = max(scores[-AGGREGATE_STATS_EVERY:] if len(scores) >= AGGREGATE_STATS_EVERY else scores)
- final_avg_reward = np.mean(scores[-AGGREGATE_STATS_EVERY:] if len(scores) >= AGGREGATE_STATS_EVERY else scores)
- final_min_reward = min(scores[-AGGREGATE_STATS_EVERY:] if len(scores) >= AGGREGATE_STATS_EVERY else scores)
-
- model_suffix = "advanced_final"
- agent.model.save(
- f'models/{MODEL_NAME}_{model_suffix}__{final_max_reward:_>7.2f}max_{final_avg_reward:_>7.2f}avg_{final_min_reward:_>7.2f}min__{int(time.time())}.model')
-
- # 保存训练统计数据
- training_stats = {
- 'scores': scores,
- 'avg_scores': avg_scores,
- 'multi_obj_stats': multi_obj_stats,
- 'curriculum_stages': curriculum_stages,
- 'final_scores': {
- 'max': final_max_reward,
- 'avg': final_avg_reward,
- 'min': final_min_reward
- }
+ final_model_path = f'{models_dir}/{MODEL_NAME}_final_ep{EPISODES}_avg{np.mean(scores):.1f}.model'
+ save_model_with_retry(agent.model, final_model_path)
+ print(f"✅ 最终模型已保存: {final_model_path}")
+
+ # 同时保存目标网络
+ final_target_path = f'{models_dir}/{MODEL_NAME}_target_final.model'
+ save_model_with_retry(agent.target_model, final_target_path)
+ print(f"✅ 目标网络已保存: {final_target_path}")
+
+ # 保存训练统计数据
+ training_stats = {
+ 'scores': scores,
+ 'avg_scores': avg_scores,
+ 'multi_obj_stats': multi_obj_stats,
+ 'reaction_time_stats': reaction_time_stats,
+ 'curriculum_stages': curriculum_stages,
+ 'final_scores': {
+ 'max': max(scores) if scores else 0,
+ 'avg': np.mean(scores) if scores else 0,
+ 'min': min(scores) if scores else 0,
}
-
- stats_file = f'training_stats_{int(time.time())}.pkl'
- with open(stats_file, 'wb') as f:
- pickle.dump(training_stats, f)
- print(f"训练统计数据已保存到: {stats_file}")
-
- # 绘制训练曲线
- fig, axes = plt.subplots(3, 2, figsize=(15, 15))
-
- # 1. 得分曲线
- axes[0, 0].plot(scores, label='每轮得分', alpha=0.6, linewidth=1)
- axes[0, 0].plot(avg_scores, label='平均得分(最近10轮)', linewidth=2, color='red')
- axes[0, 0].set_ylabel('得分')
- axes[0, 0].set_xlabel('训练轮次')
- axes[0, 0].set_title('训练进度 - 得分曲线')
- axes[0, 0].legend()
- axes[0, 0].grid(True, alpha=0.3)
-
- # 2. 探索率衰减曲线
- eps_values = [max(MIN_EPSILON, 1.0 * (EPSILON_DECAY ** i)) for i in range(len(scores))]
- axes[0, 1].plot(eps_values, color='red', linewidth=2)
- axes[0, 1].set_ylabel('探索率 (ε)')
- axes[0, 1].set_xlabel('训练轮次')
- axes[0, 1].set_title('探索率衰减曲线')
- axes[0, 1].grid(True, alpha=0.3)
-
- # 3. PER缓冲区大小
- if per_stats['buffer_size']:
- axes[1, 0].plot(per_stats['buffer_size'], color='green', linewidth=2)
- axes[1, 0].axhline(y=REPLAY_MEMORY_SIZE, color='r', linestyle='--', alpha=0.5, label='最大容量')
- axes[1, 0].set_ylabel('缓冲区大小')
- axes[1, 0].set_xlabel('训练轮次')
- axes[1, 0].set_title('PER缓冲区使用情况')
- axes[1, 0].legend()
- axes[1, 0].grid(True, alpha=0.3)
-
- # 4. 课程学习阶段变化
- if curriculum_stages:
- axes[1, 1].plot(curriculum_stages, color='purple', linewidth=2, drawstyle='steps-post')
- axes[1, 1].set_ylabel('课程学习阶段')
- axes[1, 1].set_xlabel('训练轮次')
- axes[1, 1].set_title('课程学习阶段变化')
- axes[1, 1].grid(True, alpha=0.3)
-
- # 5. 多目标指标
- if multi_obj_stats['safety']:
- colors = ['blue', 'green', 'orange', 'purple']
- for i, (key, values) in enumerate(multi_obj_stats.items()):
- if values:
- # 计算滑动平均
- window = 10
- if len(values) >= window:
- smoothed = np.convolve(values, np.ones(window)/window, mode='valid')
- axes[2, 0].plot(range(len(smoothed)), smoothed, label=key, color=colors[i], alpha=0.7)
- else:
- axes[2, 0].plot(values, label=key, color=colors[i], alpha=0.7)
-
- axes[2, 0].set_ylabel('分数')
- axes[2, 0].set_xlabel('训练轮次')
- axes[2, 0].set_title('多目标优化指标')
- axes[2, 0].legend()
- axes[2, 0].grid(True, alpha=0.3)
-
- # 6. 成功率统计
- success_rates = []
- for i in range(len(scores)):
- window = scores[max(0, i-9):i+1]
- success_rate = sum(1 for s in window if s > 5) / len(window) * 100
- success_rates.append(success_rate)
-
- axes[2, 1].plot(success_rates, color='darkred', linewidth=2)
- axes[2, 1].axhline(y=80, color='g', linestyle='--', alpha=0.5, label='目标成功率80%')
- axes[2, 1].set_ylabel('成功率 (%)')
- axes[2, 1].set_xlabel('训练轮次')
- axes[2, 1].set_title('最近10轮成功率')
- axes[2, 1].legend()
- axes[2, 1].grid(True, alpha=0.3)
-
- plt.suptitle('高级训练策略 - 综合训练报告', fontsize=16, fontweight='bold')
- plt.tight_layout()
- plt.show()
+ }
+ stats_file = f'training_stats_{int(time.time())}.pkl'
+ with open(stats_file, 'wb') as f:
+ pickle.dump(training_stats, f)
+ print(f"📊 训练统计数据已保存到: {stats_file}")
+
+ # 打印最终统计
print("\n" + "="*60)
print("训练完成!")
print("="*60)
print(f"最终统计:")
print(f" 总轮次: {EPISODES}")
- print(f" 最佳得分: {best_score:.2f}")
- print(f" 平均得分: {np.mean(scores):.2f}")
+ print(f" 最佳得分: {max(scores) if scores else 0:.2f}")
+ print(f" 平均得分: {np.mean(scores) if scores else 0:.2f}")
print(f" 成功率: {(success_count/EPISODES)*100:.1f}%")
- print(f" 最终探索率: {Hyperparameters.EPSILON:.4f}")
\ No newline at end of file
+ print(f" 平均反应时间: {np.mean(reaction_time_stats) if reaction_time_stats else 0:.2f}秒")
+ print(f" 最终探索率: {Hyperparameters.EPSILON:.4f}")
+
+ # 显示保存的模型文件
+ print(f"\n已保存的模型文件:")
+ model_files = glob.glob(f'{models_dir}/*.model')
+ if model_files:
+ for model_file in sorted(model_files, key=os.path.getmtime):
+ file_size = os.path.getsize(model_file) / (1024 * 1024) # MB
+ print(f" 📁 {os.path.basename(model_file)} ({file_size:.1f} MB)")
+ else:
+ print(" ⚠️ 没有找到模型文件,请检查保存路径和权限")
\ No newline at end of file
diff --git a/src/Unmanned_vehicle_AD_DQN/requirements.txt b/src/Unmanned_vehicle_AD_DQN/requirements.txt
index 33f0018cf2..44823773cd 100644
--- a/src/Unmanned_vehicle_AD_DQN/requirements.txt
+++ b/src/Unmanned_vehicle_AD_DQN/requirements.txt
@@ -3,4 +3,4 @@ numpy==1.19.5
tensorflow==2.6.0
tqdm==4.67.1
matplotlib==3.5.3
-opencv-python==4.12.0.88
\ No newline at end of file
+opencv-python==4.12.0.88
diff --git a/src/Yolov4_Vehicle_inspection/README.md b/src/Yolov4_Vehicle_inspection/README.md
new file mode 100644
index 0000000000..d1a82789a8
--- /dev/null
+++ b/src/Yolov4_Vehicle_inspection/README.md
@@ -0,0 +1,62 @@
+## Yolov4: 基于Pytorch的Yolov4算法实现车辆检测以及跟踪(深度学习)
+
+---
+
+本项目是基于**[yolov4-pytorch](https://github.com/bubbliiiing/yolov4-pytorch)**实现的
+
+## 文件下载
+
+### 预训练权重下载
+链接:https://pan.baidu.com/s/1Wt7_yn6m8B9Uud80ScLkPQ 提取码:6666
+其中yolov4_voc_weights.pth是voc数据集的预训练权重,yolov4_coco_weights.pth是coco数据集的预训练权重
+
+### UA-DATRAC数据集下载
+
+链接:https://pan.baidu.com/s/14G_LLbilhe7adrLDVq1k-w 提取码:6666
+
+### 训练完成模型文件下载
+链接:https://pan.baidu.com/s/1Ipw7KXqdUIjmUTQ6i0clhw 提取码:6666
+该文件为以voc数据集预训练权重使用UA-DATRAC数据集训练了23个epoch后的模型文件
+
+## 数据处理
+
+其中`transfer.py`和`move_pic.py`是针对UA-DATRAC数据集进行处理的两个文件。
+
+## 训练步骤
+
+1. 将VOC格式文件放至data的文件夹
+2. JPEGImages文件夹中存放数据图片
+3. Annotations文件夹中存放与图片一一对应的标签文件
+4. 调整voc2yolo4.py文件中的`trainval_percent` 、`train_percent`的值,控制训练集、测试集、验证集的比例;调整完成后运行该文件,在`data/ImageSets/Main`文件夹中生成对应数据集的图片文件名
+5. 运行`voc_annotation.py`文件,运行前将文件中的`classes`列表替换为自己数据集的class的名字
+6. 运行后自动生成`yolo_train.txt`文件,文件中每一行的格式为图片路径和图片中标注框的位置以及分类,如下所示
+ `data/JPEGImages/MVI_20011__img00001.jpg 592,378,752,540,3 557,120,604,163,2 545,88,580,118,2 508,67,536,92,3 553,70,582,89,7 731,114,783,153,2 902,250,960,357,3`
+7. 修改`train.py`中的路径参数与自己电脑上的路径相匹配
+ + classes_path:`classes.txt`文件路径,文件格式如下:
+ ```
+ Truck-Flatbed
+ MiniVan
+ Sedan
+ Suv
+ Truck-Pickup
+ Police
+ Truck-Box-Med
+ Taxi
+ ...
+ ```
+ + model_path:预训练权重文件路径或者中断后继续训练时前一个保存的模型文件的路径
+ + annotation_path:第6步生成的`yolo_train.txt`文件
+8. 调整`train.py`中的参数后运行文件进行训练
+
+
+## 预测
+1. 修改`model_path`的参数,选择合适的模型用于预测,并根据自己电脑情况选择是否启用cuda
+2. 修改`predict.py`中的`mode`参数,进入不同的预测模式:`predict`表示单张图片预测,`video`表示视频检测
+3. 在predict模式下运行`predict.py`文件,输入预测图片的路径即可进行前向推理,最后通过PIL库输出处理后的图片与处理时间
+4. 在video模式下,需要预先修改video_name参数的值,将路径指向需要检测的视频,修改video_fps参数可以控制输出视频的fps;调整好后运行`predict.py`即可对视频进行预测
+
+## 效果
+
+
+
+
\ No newline at end of file
diff --git a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/Annotations/README.md b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/Annotations/README.md
new file mode 100644
index 0000000000..d73a1916c9
--- /dev/null
+++ b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/Annotations/README.md
@@ -0,0 +1 @@
+存放标签文件
\ No newline at end of file
diff --git a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/ImageSets/README.md b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/ImageSets/README.md
new file mode 100644
index 0000000000..2f64921067
--- /dev/null
+++ b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/ImageSets/README.md
@@ -0,0 +1 @@
+存放训练索引文件
\ No newline at end of file
diff --git a/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/JPEGImages/README.md b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/JPEGImages/README.md
new file mode 100644
index 0000000000..58f32ea1aa
--- /dev/null
+++ b/src/Yolov4_Vehicle_inspection/VOCdevkit/VOC2007/JPEGImages/README.md
@@ -0,0 +1 @@
+存放图片文件
\ No newline at end of file
diff --git a/src/Yolov4_Vehicle_inspection/VOCdevkit/voc2yolo4.py b/src/Yolov4_Vehicle_inspection/VOCdevkit/voc2yolo4.py
new file mode 100644
index 0000000000..8a28e399d0
--- /dev/null
+++ b/src/Yolov4_Vehicle_inspection/VOCdevkit/voc2yolo4.py
@@ -0,0 +1,62 @@
+#----------------------------------------------------------------------#
+# 验证集的划分在train.py代码里面进行
+# test.txt和val.txt里面没有内容是正常的。训练不会使用到。
+#----------------------------------------------------------------------#
+'''
+#--------------------------------注意----------------------------------#
+如果在pycharm中运行时提示:
+FileNotFoundError: [WinError 3] 系统找不到指定的路径。: './VOCdevkit/VOC2007/Annotations'
+这是pycharm运行目录的问题,最简单的方法是将该文件复制到根目录后运行。
+可以查询一下相对目录和根目录的概念。在VSCODE中没有这个问题。
+#--------------------------------注意----------------------------------#
+'''
+import os
+import random
+random.seed(0)
+
+xmlfilepath=r'VOCdevkit/VOC2007/Annotations'
+saveBasePath=r"VOCdevkit/VOC2007/ImageSets/Main/"
+
+#----------------------------------------------------------------------#
+# 想要增加测试集修改trainval_percent
+# train_percent不需要修改
+#----------------------------------------------------------------------#
+trainval_percent=1
+train_percent=1
+
+temp_xml = os.listdir(xmlfilepath)
+total_xml = []
+for xml in temp_xml:
+ if xml.endswith(".xml"):
+ total_xml.append(xml)
+
+num=len(total_xml)
+list=range(num)
+tv=int(num*trainval_percent)
+tr=int(tv*train_percent)
+trainval= random.sample(list,tv)
+train=random.sample(trainval,tr)
+
+print("train and val size",tv)
+print("traub suze",tr)
+ftrainval = open(os.path.join(saveBasePath,'trainval.txt'), 'w')
+ftest = open(os.path.join(saveBasePath,'test.txt'), 'w')
+ftrain = open(os.path.join(saveBasePath,'train.txt'), 'w')
+fval = open(os.path.join(saveBasePath,'val.txt'), 'w')
+
+from tqdm import tqdm
+for i in tqdm(list):
+ name=total_xml[i][:-4]+'\n'
+ if i in trainval:
+ ftrainval.write(name)
+ if i in train:
+ ftrain.write(name)
+ else:
+ fval.write(name)
+ else:
+ ftest.write(name)
+
+ftrainval.close()
+ftrain.close()
+fval.close()
+ftest .close()
diff --git a/src/Yolov4_Vehicle_inspection/logs/README.md b/src/Yolov4_Vehicle_inspection/logs/README.md
new file mode 100644
index 0000000000..984825a33f
--- /dev/null
+++ b/src/Yolov4_Vehicle_inspection/logs/README.md
@@ -0,0 +1 @@
+用于存放训练好的文件
\ No newline at end of file
diff --git a/src/Yolov4_Vehicle_inspection/train.py b/src/Yolov4_Vehicle_inspection/train.py
new file mode 100644
index 0000000000..b2965c59cf
--- /dev/null
+++ b/src/Yolov4_Vehicle_inspection/train.py
@@ -0,0 +1,332 @@
+#-------------------------------------#
+# 对数据集进行训练
+#-------------------------------------#
+import numpy as np
+import torch
+import torch.backends.cudnn as cudnn
+import torch.optim as optim
+from torch.utils.data import DataLoader
+from tqdm import tqdm
+
+from nets.yolo4 import YoloBody
+from nets.yolo_training import LossHistory, YOLOLoss, weights_init
+from utils.dataloader import YoloDataset, yolo_dataset_collate
+
+
+#---------------------------------------------------#
+# 获得类和先验框
+#---------------------------------------------------#
+def get_classes(classes_path):
+ '''loads the classes'''
+ with open(classes_path) as f:
+ class_names = f.readlines()
+ class_names = [c.strip() for c in class_names]
+ return class_names
+
+def get_anchors(anchors_path):
+ '''loads the anchors from a file'''
+ with open(anchors_path) as f:
+ anchors = f.readline()
+ anchors = [float(x) for x in anchors.split(',')]
+ return np.array(anchors).reshape([-1,3,2])[::-1,:,:]
+
+def get_lr(optimizer):
+ for param_group in optimizer.param_groups:
+ return param_group['lr']
+
+
+def fit_one_epoch(net,yolo_loss,epoch,epoch_size,epoch_size_val,gen,genval,Epoch,cuda):
+ if Tensorboard:
+ global train_tensorboard_step, val_tensorboard_step
+ total_loss = 0
+ val_loss = 0
+
+ net.train()
+ print('Start Train')
+ with tqdm(total=epoch_size,desc=f'Epoch {epoch + 1}/{Epoch}',postfix=dict,mininterval=0.3) as pbar:
+ for iteration, batch in enumerate(gen):
+ if iteration >= epoch_size:
+ break
+ images, targets = batch[0], batch[1]
+ with torch.no_grad():
+ if cuda:
+ images = torch.from_numpy(images).type(torch.FloatTensor).cuda()
+ targets = [torch.from_numpy(ann).type(torch.FloatTensor) for ann in targets]
+ else:
+ images = torch.from_numpy(images).type(torch.FloatTensor)
+ targets = [torch.from_numpy(ann).type(torch.FloatTensor) for ann in targets]
+ #----------------------#
+ # 清零梯度
+ #----------------------#
+ optimizer.zero_grad()
+ #----------------------#
+ # 前向传播
+ #----------------------#
+ outputs = net(images)
+ losses = []
+ num_pos_all = 0
+ #----------------------#
+ # 计算损失
+ #----------------------#
+ for i in range(3):
+ loss_item, num_pos = yolo_loss(outputs[i], targets)
+ losses.append(loss_item)
+ num_pos_all += num_pos
+
+ loss = sum(losses) / num_pos_all
+ total_loss += loss.item()
+
+ #----------------------#
+ # 反向传播
+ #----------------------#
+ loss.backward()
+ optimizer.step()
+
+ if Tensorboard:
+ # 将loss写入tensorboard,每一步都写
+ writer.add_scalar('Train_loss', loss, train_tensorboard_step)
+ train_tensorboard_step += 1
+
+ pbar.set_postfix(**{'total_loss': total_loss / (iteration + 1),
+ 'lr' : get_lr(optimizer)})
+ pbar.update(1)
+
+ # 将loss写入tensorboard,下面注释的是每个世代保存一次
+ # if Tensorboard:
+ # writer.add_scalar('Train_loss', total_loss/(iteration+1), epoch)
+ net.eval()
+ print('Start Validation')
+ with tqdm(total=epoch_size_val, desc=f'Epoch {epoch + 1}/{Epoch}',postfix=dict,mininterval=0.3) as pbar:
+ for iteration, batch in enumerate(genval):
+ if iteration >= epoch_size_val:
+ break
+ images_val, targets_val = batch[0], batch[1]
+
+ with torch.no_grad():
+ if cuda:
+ images_val = torch.from_numpy(images_val).type(torch.FloatTensor).cuda()
+ targets_val = [torch.from_numpy(ann).type(torch.FloatTensor) for ann in targets_val]
+ else:
+ images_val = torch.from_numpy(images_val).type(torch.FloatTensor)
+ targets_val = [torch.from_numpy(ann).type(torch.FloatTensor) for ann in targets_val]
+ optimizer.zero_grad()
+
+ outputs = net(images_val)
+ losses = []
+ num_pos_all = 0
+ for i in range(3):
+ loss_item, num_pos = yolo_loss(outputs[i], targets_val)
+ losses.append(loss_item)
+ num_pos_all += num_pos
+ loss = sum(losses) / num_pos_all
+ val_loss += loss.item()
+
+ # 将loss写入tensorboard, 下面注释的是每一步都写
+ # if Tensorboard:
+ # writer.add_scalar('Val_loss', loss, val_tensorboard_step)
+ # val_tensorboard_step += 1
+ pbar.set_postfix(**{'total_loss': val_loss / (iteration + 1)})
+ pbar.update(1)
+
+ # 将loss写入tensorboard,每个世代保存一次
+ if Tensorboard:
+ writer.add_scalar('Val_loss',val_loss / (epoch_size_val+1), epoch)
+ loss_history.append_loss(total_loss/(epoch_size+1), val_loss/(epoch_size_val+1))
+ print('Finish Validation')
+ print('Epoch:'+ str(epoch+1) + '/' + str(Epoch))
+ print('Total Loss: %.4f || Val Loss: %.4f ' % (total_loss/(epoch_size+1),val_loss/(epoch_size_val+1)))
+ print('Saving state, iter:', str(epoch+1))
+ torch.save(model.state_dict(), 'logs/Epoch%d-Total_Loss%.4f-Val_Loss%.4f.pth'%((epoch+1),total_loss/(epoch_size+1),val_loss/(epoch_size_val+1)))
+
+#----------------------------------------------------#
+# 检测精度mAP和pr曲线计算参考视频
+# https://www.bilibili.com/video/BV1zE411u7Vw
+#----------------------------------------------------#
+if __name__ == "__main__":
+ #-------------------------------#
+ # 是否使用Tensorboard
+ #-------------------------------#
+ Tensorboard = False
+ #-------------------------------#
+ # 是否使用Cuda
+ # 没有GPU可以设置成False
+ #-------------------------------#
+ Cuda = True
+ #------------------------------------------------------#
+ # 是否对损失进行归一化,用于改变loss的大小
+ # 用于决定计算最终loss是除上batch_size还是除上正样本数量
+ #------------------------------------------------------#
+ normalize = False
+ #-------------------------------#
+ # 输入的shape大小
+ # 显存比较小可以使用416x416
+ # 显存比较大可以使用608x608
+ #-------------------------------#
+ input_shape = (416,416)
+ #----------------------------------------------------#
+ # classes和anchor的路径,非常重要
+ # 训练前一定要修改classes_path,使其对应自己的数据集
+ #----------------------------------------------------#
+ anchors_path = 'model_data/yolo_anchors.txt'
+ classes_path = 'model_data/voc_classes.txt'
+ #------------------------------------------------------#
+ # Yolov4的tricks应用
+ # mosaic 马赛克数据增强 True or False
+ # 实际测试时mosaic数据增强并不稳定,所以默认为False
+ # Cosine_scheduler 余弦退火学习率 True or False
+ # label_smoothing 标签平滑 0.01以下一般 如0.01、0.005
+ #------------------------------------------------------#
+ mosaic = False
+ Cosine_lr = False
+ smoooth_label = 0
+
+ #----------------------------------------------------#
+ # 获取classes和anchor
+ #----------------------------------------------------#
+ class_names = get_classes(classes_path)
+ anchors = get_anchors(anchors_path)
+ num_classes = len(class_names)
+
+ #------------------------------------------------------#
+ # 创建yolo模型
+ # 训练前一定要修改classes_path和对应的txt文件
+ #------------------------------------------------------#
+ model = YoloBody(len(anchors[0]), num_classes)
+ weights_init(model)
+
+ #------------------------------------------------------#
+ # 权值文件请看README,百度网盘下载
+ #------------------------------------------------------#
+ model_path = "model_data/yolo4_weights.pth"
+ print('Loading weights into state dict...')
+ device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
+ model_dict = model.state_dict()
+ pretrained_dict = torch.load(model_path, map_location=device)
+ pretrained_dict = {k: v for k, v in pretrained_dict.items() if np.shape(model_dict[k]) == np.shape(v)}
+ model_dict.update(pretrained_dict)
+ model.load_state_dict(model_dict)
+ print('Finished!')
+ # torch.save(model.state_dict(),"logs/test.pth",_use_new_zipfile_serialization=False )
+
+ net = model.train()
+
+ if Cuda:
+ net = torch.nn.DataParallel(model)
+ cudnn.benchmark = True
+ net = net.cuda()
+
+ yolo_loss = YOLOLoss(np.reshape(anchors,[-1,2]), num_classes, (input_shape[1], input_shape[0]), smoooth_label, Cuda, normalize)
+ loss_history = LossHistory("logs/")
+
+ #----------------------------------------------------#
+ # 获得图片路径和标签
+ #----------------------------------------------------#
+ annotation_path = '2007_train.txt'
+ #----------------------------------------------------------------------#
+ # 验证集的划分在train.py代码里面进行
+ # 2007_test.txt和2007_val.txt里面没有内容是正常的。训练不会使用到。
+ # 当前划分方式下,验证集和训练集的比例为1:9
+ #----------------------------------------------------------------------#
+ val_split = 0.1
+ with open(annotation_path) as f:
+ lines = f.readlines()
+ np.random.seed(10101)
+ np.random.shuffle(lines)
+ np.random.seed(None)
+ num_val = int(len(lines)*val_split)
+ num_train = len(lines) - num_val
+
+ if Tensorboard:
+ from tensorboardX import SummaryWriter
+ writer = SummaryWriter(log_dir='logs',flush_secs=60)
+ if Cuda:
+ graph_inputs = torch.randn(1,3,input_shape[0],input_shape[1]).type(torch.FloatTensor).cuda()
+ else:
+ graph_inputs = torch.randn(1,3,input_shape[0],input_shape[1]).type(torch.FloatTensor)
+ writer.add_graph(model, graph_inputs)
+ train_tensorboard_step = 1
+ val_tensorboard_step = 1
+
+ #------------------------------------------------------#
+ # 主干特征提取网络特征通用,冻结训练可以加快训练速度
+ # 也可以在训练初期防止权值被破坏。
+ # Init_Epoch为起始世代
+ # Freeze_Epoch为冻结训练的世代
+ # Epoch总训练世代
+ # 提示OOM或者显存不足请调小Batch_size
+ #------------------------------------------------------#
+ if True:
+ lr = 1e-3
+ Batch_size = 4
+ Init_Epoch = 0
+ Freeze_Epoch = 50
+
+ #----------------------------------------------------------------------------#
+ # 我在实际测试时,发现optimizer的weight_decay起到了反作用,
+ # 所以去除掉了weight_decay,大家也可以开起来试试,一般是weight_decay=5e-4
+ #----------------------------------------------------------------------------#
+ optimizer = optim.Adam(net.parameters(),lr)
+ if Cosine_lr:
+ lr_scheduler = optim.lr_scheduler.CosineAnnealingLR(optimizer, T_max=5, eta_min=1e-5)
+ else:
+ lr_scheduler = optim.lr_scheduler.StepLR(optimizer, step_size=1, gamma=0.92)
+
+ train_dataset = YoloDataset(lines[:num_train], (input_shape[0], input_shape[1]), mosaic=mosaic, is_train=True)
+ val_dataset = YoloDataset(lines[num_train:], (input_shape[0], input_shape[1]), mosaic=False, is_train=False)
+ gen = DataLoader(train_dataset, shuffle=True, batch_size=Batch_size, num_workers=4, pin_memory=True,
+ drop_last=True, collate_fn=yolo_dataset_collate)
+ gen_val = DataLoader(val_dataset, shuffle=True, batch_size=Batch_size, num_workers=4,pin_memory=True,
+ drop_last=True, collate_fn=yolo_dataset_collate)
+
+ epoch_size = num_train // Batch_size
+ epoch_size_val = num_val // Batch_size
+
+ if epoch_size == 0 or epoch_size_val == 0:
+ raise ValueError("数据集过小,无法进行训练,请扩充数据集。")
+ #------------------------------------#
+ # 冻结一定部分训练
+ #------------------------------------#
+ for param in model.backbone.parameters():
+ param.requires_grad = False
+
+ for epoch in range(Init_Epoch,Freeze_Epoch):
+ fit_one_epoch(net,yolo_loss,epoch,epoch_size,epoch_size_val,gen,gen_val,Freeze_Epoch,Cuda)
+ lr_scheduler.step()
+
+ if True:
+ lr = 1e-4
+ Batch_size = 2
+ Freeze_Epoch = 50
+ Unfreeze_Epoch = 100
+
+ #----------------------------------------------------------------------------#
+ # 我在实际测试时,发现optimizer的weight_decay起到了反作用,
+ # 所以去除掉了weight_decay,大家也可以开起来试试,一般是weight_decay=5e-4
+ #----------------------------------------------------------------------------#
+ optimizer = optim.Adam(net.parameters(),lr)
+ if Cosine_lr:
+ lr_scheduler = optim.lr_scheduler.CosineAnnealingLR(optimizer, T_max=5, eta_min=1e-5)
+ else:
+ lr_scheduler = optim.lr_scheduler.StepLR(optimizer, step_size=1, gamma=0.92)
+
+ train_dataset = YoloDataset(lines[:num_train], (input_shape[0], input_shape[1]), mosaic=mosaic, is_train=True)
+ val_dataset = YoloDataset(lines[num_train:], (input_shape[0], input_shape[1]), mosaic=False, is_train=False)
+ gen = DataLoader(train_dataset, shuffle=True, batch_size=Batch_size, num_workers=4, pin_memory=True,
+ drop_last=True, collate_fn=yolo_dataset_collate)
+ gen_val = DataLoader(val_dataset, shuffle=True, batch_size=Batch_size, num_workers=4,pin_memory=True,
+ drop_last=True, collate_fn=yolo_dataset_collate)
+
+ epoch_size = num_train // Batch_size
+ epoch_size_val = num_val // Batch_size
+
+ if epoch_size == 0 or epoch_size_val == 0:
+ raise ValueError("数据集过小,无法进行训练,请扩充数据集。")
+ #------------------------------------#
+ # 解冻后训练
+ #------------------------------------#
+ for param in model.backbone.parameters():
+ param.requires_grad = True
+
+ for epoch in range(Freeze_Epoch,Unfreeze_Epoch):
+ fit_one_epoch(net,yolo_loss,epoch,epoch_size,epoch_size_val,gen,gen_val,Unfreeze_Epoch,Cuda)
+ lr_scheduler.step()
diff --git a/src/Yolov4_Vehicle_inspection/voc_annotation.py b/src/Yolov4_Vehicle_inspection/voc_annotation.py
new file mode 100644
index 0000000000..703be73ab8
--- /dev/null
+++ b/src/Yolov4_Vehicle_inspection/voc_annotation.py
@@ -0,0 +1,41 @@
+#---------------------------------------------#
+# 运行前一定要修改classes
+# 如果生成的2007_train.txt里面没有目标信息
+# 那么就是因为classes没有设定正确
+#---------------------------------------------#
+import xml.etree.ElementTree as ET
+from os import getcwd
+
+sets=[('2007', 'train'), ('2007', 'val'), ('2007', 'test')]
+#-----------------------------------------------------#
+# 这里设定的classes顺序要和model_data里的txt一样
+#-----------------------------------------------------#
+classes = ['Truck-Flatbed', 'MiniVan', 'Sedan', 'Suv', 'Truck-Pickup', 'Police', 'Truck-Box-Med', 'Taxi', 'Truck-Box-Large', 'Van', 'Hatchback', 'Bus', 'Truck-Util']
+
+def convert_annotation(year, image_id, list_file):
+ in_file = open('VOCdevkit/VOC%s/Annotations/%s.xml'%(year, image_id), encoding='utf-8')
+ tree=ET.parse(in_file)
+ root = tree.getroot()
+
+ for obj in root.iter('object'):
+ difficult = 0
+ if obj.find('difficult')!=None:
+ difficult = obj.find('difficult').text
+ cls = obj.find('name').text
+ if cls not in classes or int(difficult)==1:
+ continue
+ cls_id = classes.index(cls)
+ xmlbox = obj.find('bndbox')
+ b = (int(float(xmlbox.find('xmin').text)), int(float(xmlbox.find('ymin').text)), int(float(xmlbox.find('xmax').text)), int(float(xmlbox.find('ymax').text)))
+ list_file.write(" " + ",".join([str(a) for a in b]) + ',' + str(cls_id))
+
+wd = getcwd()
+
+for year, image_set in sets:
+ image_ids = open('VOCdevkit/VOC%s/ImageSets/Main/%s.txt'%(year, image_set), encoding='utf-8').read().strip().split()
+ list_file = open('%s_%s.txt'%(year, image_set), 'w', encoding='utf-8')
+ for image_id in image_ids:
+ list_file.write('%s/VOCdevkit/VOC%s/JPEGImages/%s.jpg'%( wd, year, image_id))
+ convert_annotation(year, image_id, list_file)
+ list_file.write('\n')
+ list_file.close()
diff --git a/src/Yolov4_Vehicle_inspection/yolo.py b/src/Yolov4_Vehicle_inspection/yolo.py
new file mode 100644
index 0000000000..fce0fbd1a6
--- /dev/null
+++ b/src/Yolov4_Vehicle_inspection/yolo.py
@@ -0,0 +1,361 @@
+#-------------------------------------#
+# 创建YOLO类
+#-------------------------------------#
+import colorsys
+import os
+import time
+
+import numpy as np
+import torch
+import torch.nn as nn
+from PIL import Image, ImageDraw, ImageFont
+
+from nets.yolo4 import YoloBody
+from utils.utils import (DecodeBox, letterbox_image, non_max_suppression,
+ yolo_correct_boxes)
+
+
+#--------------------------------------------#
+# 使用自己训练好的模型预测需要修改2个参数
+# model_path和classes_path都需要修改!
+# 如果出现shape不匹配,一定要注意
+# 训练时的model_path和classes_path参数的修改
+#--------------------------------------------#
+class YOLO(object):
+ _defaults = {
+ # "model_path" : 'model_data/Epoch23-Total_Loss0.7718-Val_Loss0.6001.pth',
+ "model_path" : 'model_data/yolo4_coco_weights.pth',
+ # "model_path" : 'model_data/Epoch25-Total_Loss0.7681-Val_Loss0.5913.pth',
+ "anchors_path" : 'model_data/yolo_anchors.txt',
+ "classes_path" : 'model_data/coco_classes.txt',
+ "model_image_size" : (416, 416, 3),
+ "confidence" : 0.7,
+ "iou" : 0.3,
+ "cuda" : True,
+ #---------------------------------------------------------------------#
+ # 该变量用于控制是否使用letterbox_image对输入图像进行不失真的resize,
+ # 在多次测试后,发现关闭letterbox_image直接resize的效果更好
+ #---------------------------------------------------------------------#
+ "letterbox_image" : False,
+ }
+
+ @classmethod
+ def get_defaults(cls, n):
+ if n in cls._defaults:
+ return cls._defaults[n]
+ else:
+ return "Unrecognized attribute name '" + n + "'"
+
+ #---------------------------------------------------#
+ # 初始化YOLO
+ #---------------------------------------------------#
+ def __init__(self, **kwargs):
+ self.__dict__.update(self._defaults)
+ self.class_names = self._get_class()
+ self.anchors = self._get_anchors()
+ self.generate()
+
+ #---------------------------------------------------#
+ # 获得所有的分类
+ #---------------------------------------------------#
+ def _get_class(self):
+ classes_path = os.path.expanduser(self.classes_path)
+ with open(classes_path) as f:
+ class_names = f.readlines()
+ class_names = [c.strip() for c in class_names]
+ return class_names
+
+ #---------------------------------------------------#
+ # 获得所有的先验框
+ #---------------------------------------------------#
+ def _get_anchors(self):
+ anchors_path = os.path.expanduser(self.anchors_path)
+ with open(anchors_path) as f:
+ anchors = f.readline()
+ anchors = [float(x) for x in anchors.split(',')]
+ return np.array(anchors).reshape([-1, 3, 2])[::-1,:,:]
+
+ #---------------------------------------------------#
+ # 生成模型
+ #---------------------------------------------------#
+ def generate(self):
+ #---------------------------------------------------#
+ # 建立yolov4模型
+ #---------------------------------------------------#
+ self.net = YoloBody(len(self.anchors[0]), len(self.class_names)).eval()
+
+ #---------------------------------------------------#
+ # 载入yolov4模型的权重
+ #---------------------------------------------------#
+ print('Loading weights into state dict...')
+ device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
+ state_dict = torch.load(self.model_path, map_location=device)
+ self.net.load_state_dict(state_dict)
+ print('Finished!')
+
+ if self.cuda:
+ self.net = nn.DataParallel(self.net)
+ self.net = self.net.cuda()
+
+ #---------------------------------------------------#
+ # 建立三个特征层解码用的工具
+ #---------------------------------------------------#
+ self.yolo_decodes = []
+ for i in range(3):
+ self.yolo_decodes.append(DecodeBox(self.anchors[i], len(self.class_names), (self.model_image_size[1], self.model_image_size[0])))
+
+ print('{} model, anchors, and classes loaded.'.format(self.model_path))
+ # 画框设置不同的颜色
+ hsv_tuples = [(x / len(self.class_names), 1., 1.)
+ for x in range(len(self.class_names))]
+ self.colors = list(map(lambda x: colorsys.hsv_to_rgb(*x), hsv_tuples))
+ self.colors = list(
+ map(lambda x: (int(x[0] * 255), int(x[1] * 255), int(x[2] * 255)),
+ self.colors))
+
+ #---------------------------------------------------#
+ # 检测图片
+ #---------------------------------------------------#
+ def detect_image(self, image, cars = [], car_not_vis = []):
+ #---------------------------------------------------------#
+ # 在这里将图像转换成RGB图像,防止灰度图在预测时报错。
+ #---------------------------------------------------------#
+ image = image.convert('RGB')
+
+ image_shape = np.array(np.shape(image)[0:2])
+ #---------------------------------------------------------#
+ # 给图像增加灰条,实现不失真的resize
+ # 也可以直接resize进行识别
+ #---------------------------------------------------------#
+ if self.letterbox_image:
+ crop_img = np.array(letterbox_image(image, (self.model_image_size[1],self.model_image_size[0])))
+ else:
+ crop_img = image.resize((self.model_image_size[1],self.model_image_size[0]), Image.BICUBIC)
+ photo = np.array(crop_img,dtype = np.float32) / 255.0
+ photo = np.transpose(photo, (2, 0, 1))
+ #---------------------------------------------------------#
+ # 添加上batch_size维度
+ #---------------------------------------------------------#
+ images = [photo]
+
+ with torch.no_grad():
+ images = torch.from_numpy(np.asarray(images))
+ if self.cuda:
+ images = images.cuda()
+
+ #---------------------------------------------------------#
+ # 将图像输入网络当中进行预测!
+ #---------------------------------------------------------#
+ outputs = self.net(images)
+ output_list = []
+ for i in range(3):
+ output_list.append(self.yolo_decodes[i](outputs[i]))
+
+ #---------------------------------------------------------#
+ # 将预测框进行堆叠,然后进行非极大抑制
+ #---------------------------------------------------------#
+ output = torch.cat(output_list, 1)
+ batch_detections = non_max_suppression(output, len(self.class_names),
+ conf_thres=self.confidence,
+ nms_thres=self.iou)
+
+ #---------------------------------------------------------#
+ # 如果没有检测出物体,返回原图
+ #---------------------------------------------------------#
+ try:
+ batch_detections = batch_detections[0].cpu().numpy()
+ except:
+ return image
+
+ #---------------------------------------------------------#
+ # 对预测框进行得分筛选
+ #---------------------------------------------------------#
+ top_index = batch_detections[:,4] * batch_detections[:,5] > self.confidence
+ top_conf = batch_detections[top_index,4]*batch_detections[top_index,5]
+ top_label = np.array(batch_detections[top_index,-1],np.int32)
+ top_bboxes = np.array(batch_detections[top_index,:4])
+ top_xmin, top_ymin, top_xmax, top_ymax = np.expand_dims(top_bboxes[:,0],-1),np.expand_dims(top_bboxes[:,1],-1),np.expand_dims(top_bboxes[:,2],-1),np.expand_dims(top_bboxes[:,3],-1)
+
+ #-----------------------------------------------------------------#
+ # 在图像传入网络预测前会进行letterbox_image给图像周围添加灰条
+ # 因此生成的top_bboxes是相对于有灰条的图像的
+ # 我们需要对其进行修改,去除灰条的部分。
+ #-----------------------------------------------------------------#
+ if self.letterbox_image:
+ boxes = yolo_correct_boxes(top_ymin,top_xmin,top_ymax,top_xmax,np.array([self.model_image_size[0],self.model_image_size[1]]),image_shape)
+ else:
+ top_xmin = top_xmin / self.model_image_size[1] * image_shape[1]
+ top_ymin = top_ymin / self.model_image_size[0] * image_shape[0]
+ top_xmax = top_xmax / self.model_image_size[1] * image_shape[1]
+ top_ymax = top_ymax / self.model_image_size[0] * image_shape[0]
+ boxes = np.concatenate([top_ymin,top_xmin,top_ymax,top_xmax], axis=-1)
+
+ font = ImageFont.truetype(font='model_data/simhei.ttf',size=np.floor(3e-2 * np.shape(image)[1] + 0.5).astype('int32'))
+
+ thickness = max((np.shape(image)[0] + np.shape(image)[1]) // self.model_image_size[0], 1)
+
+ # print(cars)
+ tmp_vis = [0] *len(cars)
+ for i, c in enumerate(top_label):
+ predicted_class = self.class_names[c]
+ score = top_conf[i]
+
+ top, left, bottom, right = boxes[i]
+ top = top - 2
+ left = left - 2
+ bottom = bottom + 2
+ right = right + 2
+
+ top = max(0, np.floor(top + 0.5).astype('int32'))
+ left = max(0, np.floor(left + 0.5).astype('int32'))
+ bottom = min(np.shape(image)[0], np.floor(bottom + 0.5).astype('int32'))
+ right = min(np.shape(image)[1], np.floor(right + 0.5).astype('int32'))
+
+ # 画框框
+ # label = '{} {:.2f}'.format(predicted_class, score)
+ label = '{}{:.2f}'.format(predicted_class, score)
+ draw = ImageDraw.Draw(image)
+ label_size = draw.textsize(label, font)
+ label = label.encode('utf-8')
+ # print(label, top, left, bottom, right)
+
+ if top - label_size[1] >= 0:
+ text_origin = np.array([left, top - label_size[1]])
+ else:
+ text_origin = np.array([left, top + 1])
+
+ car = ((left + right) // 2, (top + bottom) // 2)
+ if cars != None:
+ def distance(a, b):
+ return (a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1])
+ id = None
+ mindis = int(1e9)
+ for carid,tmpcar in enumerate(cars):
+ dis = distance(car, tmpcar[-1])
+ if dis <= min(170, mindis):
+ id = carid
+ mindis = dis
+ if id == None or id >= len(tmp_vis):
+ cars.append([car])
+ else:
+ tmp_vis[id] += 1
+ cars[id].append(car)
+ for i in range(len(cars[id]) - 1):
+ draw.line((cars[id][i], cars[id][i + 1]),
+ fill = self.colors[self.class_names.index(predicted_class)],
+ width = 1)
+ else:
+ cars.append([car])
+ # print(cars)
+ for i in range(thickness):
+ draw.rectangle(
+ [left + i, top + i, right - i, bottom - i],
+ outline=self.colors[self.class_names.index(predicted_class)])
+ draw.rectangle(
+ [tuple(text_origin), tuple(text_origin + label_size)],
+ fill=self.colors[self.class_names.index(predicted_class)])
+ draw.text(text_origin, str(label,'UTF-8'), fill=(0, 0, 0), font=font)
+
+ del draw
+ # print(car_not_vis)
+ for id in range(len(tmp_vis)):
+ if tmp_vis[id] == 0:
+ car_not_vis[id] += 1
+ else:
+ car_not_vis[id] = 0
+ for i in range(len(cars) - len(car_not_vis)):
+ car_not_vis.append(0)
+ fail_frame = 5
+ for id in reversed(range(len(car_not_vis))):
+ if car_not_vis[id] == fail_frame:
+ cars.pop(id)
+ car_not_vis.pop(id)
+ return image
+
+ def get_FPS(self, image, test_interval):
+ # 调整图片使其符合输入要求
+ image_shape = np.array(np.shape(image)[0:2])
+
+ #---------------------------------------------------------#
+ # 给图像增加灰条,实现不失真的resize
+ # 也可以直接resize进行识别
+ #---------------------------------------------------------#
+ if self.letterbox_image:
+ crop_img = np.array(letterbox_image(image, (self.model_image_size[1],self.model_image_size[0])))
+ else:
+ crop_img = image.convert('RGB')
+ crop_img = crop_img.resize((self.model_image_size[1],self.model_image_size[0]), Image.BICUBIC)
+ photo = np.array(crop_img,dtype = np.float32) / 255.0
+ photo = np.transpose(photo, (2, 0, 1))
+ #---------------------------------------------------------#
+ # 添加上batch_size维度
+ #---------------------------------------------------------#
+ images = [photo]
+
+ with torch.no_grad():
+ images = torch.from_numpy(np.asarray(images))
+ if self.cuda:
+ images = images.cuda()
+ outputs = self.net(images)
+ output_list = []
+ for i in range(3):
+ output_list.append(self.yolo_decodes[i](outputs[i]))
+ output = torch.cat(output_list, 1)
+ batch_detections = non_max_suppression(output, len(self.class_names),
+ conf_thres=self.confidence,
+ nms_thres=self.iou)
+ try:
+ batch_detections = batch_detections[0].cpu().numpy()
+ top_index = batch_detections[:,4]*batch_detections[:,5] > self.confidence
+ top_conf = batch_detections[top_index,4]*batch_detections[top_index,5]
+ top_label = np.array(batch_detections[top_index,-1],np.int32)
+ top_bboxes = np.array(batch_detections[top_index,:4])
+ top_xmin, top_ymin, top_xmax, top_ymax = np.expand_dims(top_bboxes[:,0],-1),np.expand_dims(top_bboxes[:,1],-1),np.expand_dims(top_bboxes[:,2],-1),np.expand_dims(top_bboxes[:,3],-1)
+
+ if self.letterbox_image:
+ boxes = yolo_correct_boxes(top_ymin,top_xmin,top_ymax,top_xmax,np.array([self.model_image_size[0],self.model_image_size[1]]),image_shape)
+ else:
+ top_xmin = top_xmin / self.model_image_size[1] * image_shape[1]
+ top_ymin = top_ymin / self.model_image_size[0] * image_shape[0]
+ top_xmax = top_xmax / self.model_image_size[1] * image_shape[1]
+ top_ymax = top_ymax / self.model_image_size[0] * image_shape[0]
+ boxes = np.concatenate([top_ymin,top_xmin,top_ymax,top_xmax], axis=-1)
+
+ except:
+ pass
+
+ t1 = time.time()
+ for _ in range(test_interval):
+ with torch.no_grad():
+ outputs = self.net(images)
+ output_list = []
+ for i in range(3):
+ output_list.append(self.yolo_decodes[i](outputs[i]))
+ output = torch.cat(output_list, 1)
+ batch_detections = non_max_suppression(output, len(self.class_names),
+ conf_thres=self.confidence,
+ nms_thres=self.iou)
+ try:
+ batch_detections = batch_detections[0].cpu().numpy()
+ top_index = batch_detections[:,4]*batch_detections[:,5] > self.confidence
+ top_conf = batch_detections[top_index,4]*batch_detections[top_index,5]
+ top_label = np.array(batch_detections[top_index,-1],np.int32)
+ top_bboxes = np.array(batch_detections[top_index,:4])
+ top_xmin, top_ymin, top_xmax, top_ymax = np.expand_dims(top_bboxes[:,0],-1),np.expand_dims(top_bboxes[:,1],-1),np.expand_dims(top_bboxes[:,2],-1),np.expand_dims(top_bboxes[:,3],-1)
+
+ if self.letterbox_image:
+ boxes = yolo_correct_boxes(top_ymin,top_xmin,top_ymax,top_xmax,np.array([self.model_image_size[0],self.model_image_size[1]]),image_shape)
+ else:
+ top_xmin = top_xmin / self.model_image_size[1] * image_shape[1]
+ top_ymin = top_ymin / self.model_image_size[0] * image_shape[0]
+ top_xmax = top_xmax / self.model_image_size[1] * image_shape[1]
+ top_ymax = top_ymax / self.model_image_size[0] * image_shape[0]
+ boxes = np.concatenate([top_ymin,top_xmin,top_ymax,top_xmax], axis=-1)
+
+ except:
+ pass
+
+ t2 = time.time()
+ tact_time = (t2 - t1) / test_interval
+ return tact_time
+
diff --git a/src/airsim_control/custom_env.py b/src/airsim_control/custom_env.py
index a098499eb6..4a1308290c 100644
--- a/src/airsim_control/custom_env.py
+++ b/src/airsim_control/custom_env.py
@@ -5,12 +5,24 @@
import cv2
import math
-# 目标出口位置 (注意:这里要填换算成“米”后的单位)
-# UE坐标: X=790cm, Y=3360cm -> AirSim: X=7.9m, Y=33.6m
-EXIT_POS = np.array([7.9, 33.6])
-
-# 允许的误差半径 (到达终点附近 2 米就算成功)
-GOAL_THRESHOLD = 2.0
+# =========================================================
+# 1. 坐标系精准换算
+# =========================================================
+# 请确保这三个坐标是你 UE 里显示的真实数值 (单位: cm)
+UE_START = np.array([1180.0, 610.0, 28.0]) # 出生点
+UE_GOAL = np.array([790.0, 3360.0, -50.0]) # 终点位置
+
+# 计算 AirSim 中的相对目标向量 (单位: 米)
+# 逻辑: (目标 - 起点) / 100
+# 结果大约是: [-3.9, 27.5, -0.78]
+TARGET_POS_AIRSIM = (UE_GOAL - UE_START) / 100.0
+
+print(f"========================================")
+print(f"环境配置信息:")
+print(f"1. UE 目标坐标 (cm): {UE_GOAL}")
+print(f"2. AirSim 相对目标 (m): {TARGET_POS_AIRSIM}")
+print(f"3. 判定半径: 5 米 (球体)")
+print(f"========================================")
class AirSimMazeEnv(gym.Env):
@@ -21,47 +33,47 @@ def __init__(self):
self.client = airsim.MultirotorClient()
self.client.confirmConnection()
- # 动作空间: [前进速度(0~1), 转向速度(-1~1)]
- # 我们不控制后退,只控制前进和左右转,这样更容易走出迷宫
- self.action_space = spaces.Box(low=np.array([0, -1]), high=np.array([1, 1]), dtype=np.float32)
+ # 动作空间
+ # [0]: 前进速度 (0 ~ 4 m/s)
+ # [1]: 转向速度 (-60 ~ 60 deg/s)
+ self.action_space = spaces.Box(
+ low=np.array([0, -1]),
+ high=np.array([1, 1]),
+ dtype=np.float32
+ )
- # 观测空间 (多模态):
- # 1. image: 84x84 的深度图
- # 2. lidar: 180 个维度的距离向量 (模拟前方180度扇区)
+ # 观测空间 (为了兼容你之前的模型,Lidar 保持 high=50)
self.observation_space = spaces.Dict({
"image": spaces.Box(low=0, high=255, shape=(84, 84, 1), dtype=np.uint8),
"lidar": spaces.Box(low=0, high=50, shape=(180,), dtype=np.float32)
})
- self.state = None
self.last_dist = None
def step(self, action):
- # 1. 执行动作
- # 将动作映射到实际物理参数
- fwd_vel = float(action[0]) * 3.0 # 最大速度 3 m/s
- yaw_rate = float(action[1]) * 45 # 最大转向角速度 45度/s
-
-
-
- # 新的代码 (锁定高度在 -1.5 米):
- # 注意: NED坐标系中,负数是空中。-1.5 表示离地 1.5 米。
+ # --- 1. 执行动作 ---
+ fwd_vel = float(action[0]) * 4.0
+ yaw_rate = float(action[1]) * 60
+
+ # 高度策略:
+ # 目标 Z 约为 -0.78米。
+ # 我们让无人机飞在 -1.5米 (离地1.5米),这样它就在目标球体(半径5米)的内部
+ # 不需要专门去对齐高度,只要飞过去就能触发
self.client.moveByVelocityZBodyFrameAsync(
vx=fwd_vel,
vy=0,
- z=-1.5, # <--- 强制固定在这个高度
+ z=-1.5,
duration=0.1,
yaw_mode=airsim.YawMode(True, yaw_rate)
).join()
- # ------------------- 修改结束 -------------------
- # 2. 获取观测
+ # --- 2. 获取观测 ---
obs = self._get_obs()
- # 3. 计算奖励和结束条件
- reward, done = self._compute_reward_and_done()
+ # --- 3. 计算奖励 ---
+ reward, done = self._compute_reward_and_done(obs)
- truncated = False # 对于 Gym 新版本
+ truncated = False
return obs, reward, done, truncated, {}
def reset(self, seed=None, options=None):
@@ -69,96 +81,129 @@ def reset(self, seed=None, options=None):
self.client.reset()
self.client.enableApiControl(True)
self.client.armDisarm(True)
- # 起飞到固定高度
+
+ # 起飞并悬停
self.client.takeoffAsync().join()
- self.client.moveToZAsync(-1.5, 1).join() # 保持在 1.5米高度
+ self.client.moveToZAsync(-1.5, 1).join()
- # 初始化距离用于计算奖励
- pos = self.client.getMultirotorState().kinematics_estimated.position
- self.last_dist = np.linalg.norm(np.array([pos.x_val, pos.y_val]) - EXIT_POS)
+ # 初始化距离 (3D距离)
+ state = self.client.getMultirotorState().kinematics_estimated.position
+ curr_pos = np.array([state.x_val, state.y_val, state.z_val])
+ self.last_dist = np.linalg.norm(curr_pos - TARGET_POS_AIRSIM)
return self._get_obs(), {}
def _get_obs(self):
- # --- 获取摄像头数据 (深度图) ---
+ # === 图像处理 ===
+ img_obs = np.zeros((84, 84, 1), dtype=np.uint8)
responses = self.client.simGetImages([
airsim.ImageRequest("front_center_custom", airsim.ImageType.DepthPlanar, True)
])
-
if responses:
- img1d = np.array(responses[0].image_data_float, dtype=np.float32)
- img1d = np.clip(img1d, 0, 20) # 限制最大深度为20米,归一化效果更好
- img2d = img1d.reshape(responses[0].height, responses[0].width)
- # 缩放到 84x84
- img_resize = cv2.resize(img2d, (84, 84))
- # 转换为 0-255 uint8 供 CNN 使用
- img_uint8 = (img_resize / 20.0 * 255).astype(np.uint8)
- img_obs = np.expand_dims(img_uint8, axis=-1)
- else:
- img_obs = np.zeros((84, 84, 1), dtype=np.uint8)
-
- # --- 获取 Lidar 数据 (并处理为 1D 向量) ---
- lidar_data = self.client.getLidarData("lidar_1")
- points = np.array(lidar_data.point_cloud, dtype=np.float32)
-
- # 初始化 180 个 bins (代表前方 -90度 到 +90度)
- lidar_scan = np.ones(180) * 20.0 # 默认最远距离 20米
-
- if len(points) > 3:
- points = np.reshape(points, (-1, 3))
- # 我们只关心局部坐标系下的 X(前) 和 Y(右)
- x = points[:, 0]
- y = points[:, 1]
-
- # 转换为极坐标: 角度和距离
- angles = np.arctan2(y, x) * 180 / np.pi # -180 到 180
- dists = np.linalg.norm(points[:, :2], axis=1)
-
- # 筛选前方 -90 到 90 度的点
- valid_mask = (angles >= -90) & (angles < 90)
- valid_angles = angles[valid_mask]
- valid_dists = dists[valid_mask]
-
- # 将角度映射到 0-179 的索引
- indices = ((valid_angles + 90).astype(int))
- indices = np.clip(indices, 0, 179)
-
- # 更新每个角度扇区的最小距离 (避障需要知道最近的障碍物)
- for i, d in zip(indices, valid_dists):
- if d < lidar_scan[i]:
- lidar_scan[i] = d
+ response = responses[0]
+ if response.width > 0 and response.height > 0:
+ try:
+ img1d = np.array(response.image_data_float, dtype=np.float32)
+ if img1d.size == response.width * response.height:
+ img1d = np.clip(img1d, 0, 20)
+ img2d = img1d.reshape(response.height, response.width)
+ img_resize = cv2.resize(img2d, (84, 84))
+ img_uint8 = (img_resize / 20.0 * 255).astype(np.uint8)
+ img_obs = np.expand_dims(img_uint8, axis=-1)
+ except Exception:
+ pass
+
+ # === Lidar 处理 ===
+ lidar_scan = np.ones(180) * 20.0
+ try:
+ lidar_data = self.client.getLidarData("lidar_1")
+ points = np.array(lidar_data.point_cloud, dtype=np.float32)
+ if len(points) > 3:
+ points = np.reshape(points, (-1, 3))
+ x = points[:, 0]
+ y = points[:, 1]
+ angles = np.arctan2(y, x) * 180 / np.pi
+ dists = np.linalg.norm(points[:, :2], axis=1)
+
+ valid_mask = (angles >= -90) & (angles < 90)
+ valid_angles = angles[valid_mask]
+ valid_dists = dists[valid_mask]
+
+ indices = ((valid_angles + 90).astype(int))
+ indices = np.clip(indices, 0, 179)
+ for i, d in zip(indices, valid_dists):
+ if d < lidar_scan[i]:
+ lidar_scan[i] = d
+ except Exception:
+ pass
return {"image": img_obs, "lidar": lidar_scan}
- def _compute_reward_and_done(self):
- # 获取状态
- collision = self.client.simGetCollisionInfo().has_collided
- pos = self.client.getMultirotorState().kinematics_estimated.position
- curr_dist = np.linalg.norm(np.array([pos.x_val, pos.y_val]) - EXIT_POS)
+ def _compute_reward_and_done(self, obs):
+ # 1. 获取当前 3D 位置
+ collision_info = self.client.simGetCollisionInfo()
+ state = self.client.getMultirotorState().kinematics_estimated.position
+ curr_pos = np.array([state.x_val, state.y_val, state.z_val])
+
+ # 2. 计算到【正方体】的 3D 距离
+ dist_to_goal = np.linalg.norm(curr_pos - TARGET_POS_AIRSIM)
+
+ # 计算到【出生点】的距离 (用于判断是否飞太远)
+ dist_from_start = np.linalg.norm(curr_pos)
+ # 计算正方体到出生点的理论距离
+ goal_distance_from_start = np.linalg.norm(TARGET_POS_AIRSIM)
reward = 0
done = False
- # 1. 碰撞惩罚 (大惩罚并结束)
- if collision:
+ # --- A. 撞墙判定 ---
+ if collision_info.has_collided:
reward = -50.0
done = True
+ print(f"❌ 撞墙!")
return reward, done
- # 2. 到达终点奖励
- if curr_dist < GOAL_THRESHOLD:
- reward = 100.0
+ # --- B. 成功判定: 进入正方体 5米 范围内 ---
+ if dist_to_goal < 5.0:
+ reward = 100.0 # 大奖
+ done = True # 结束回合 -> 触发 reset -> 回到出生点
+ print(f"✅ 成功到达终点范围! (误差: {dist_to_goal:.2f}m)")
+ return reward, done
+
+ # --- C. 防止乱飞 (Geofence) ---
+ # 逻辑: 如果无人机飞行的距离,比目标距离还要远 15 米,说明它已经飞过头并开始乱跑了
+ # 强制结束,让它回出生点重来
+ if dist_from_start > (goal_distance_from_start + 15.0):
+ reward = -20.0
+ done = True
+ print(f"⚠️ 飞出边界 (距离起点 {dist_from_start:.1f}m),强制重置")
+ return reward, done
+
+ # 补充: 如果高度异常 (Z > 5米, 钻地; Z < -10米, 飞太高)
+ if curr_pos[2] > 5 or curr_pos[2] < -10:
+ reward = -20.0
done = True
- print(">>> REACHED EXIT! <<<")
+ print("⚠️ 高度异常")
return reward, done
- # 3. 引导奖励 (Progress Reward)
- # 如果比上一步离终点近,给正奖励;远了给负奖励
- dist_improvement = self.last_dist - curr_dist
- reward += dist_improvement * 10.0
- self.last_dist = curr_dist
+ # --- D. 引导奖励 ---
+ # 只要离正方体近了就给分
+ if self.last_dist is not None:
+ improvement = self.last_dist - dist_to_goal
+ reward += improvement * 10.0
+
+ self.last_dist = dist_to_goal
- # 4. 时间步惩罚 (迫使它快速移动)
+ # --- E. 避障惩罚 (防止死路) ---
+ min_obstacle_dist = np.min(obs['lidar'])
+ if min_obstacle_dist < 1.5:
+ penalty = (1.5 - min_obstacle_dist) * 0.5
+ reward -= penalty
+
+ # --- F. 时间惩罚 ---
reward -= 0.05
- return reward, done
\ No newline at end of file
+ return reward, done
+
+ def close(self):
+ self.client.enableApiControl(False)
diff --git a/src/airsim_control/train.py b/src/airsim_control/train.py
index 3314fd4b48..1470d7fa9f 100644
--- a/src/airsim_control/train.py
+++ b/src/airsim_control/train.py
@@ -1,46 +1,94 @@
+import glob
+import os
from stable_baselines3 import PPO
from stable_baselines3.common.callbacks import CheckpointCallback
from custom_env import AirSimMazeEnv
-import os
-# 创建日志目录
-log_dir = "./airsim_logs/"
-os.makedirs(log_dir, exist_ok=True)
+# === 配置路径 (使用你提供的绝对路径) ===
+MODELS_DIR = r"D:\Others\MyAirsimprojects\models"
+LOG_DIR = r"D:\Others\MyAirsimprojects\airsim_logs"
+
+# 确保目录存在
+os.makedirs(MODELS_DIR, exist_ok=True)
+os.makedirs(LOG_DIR, exist_ok=True)
+
+
+def get_latest_model_path(path_dir):
+ """
+ 辅助函数:查找指定目录下最新的 .zip 模型文件
+ """
+ # 获取目录下所有 .zip 文件
+ list_of_files = glob.glob(os.path.join(path_dir, '*.zip'))
+
+ if not list_of_files:
+ return None
+
+ # 按照文件修改时间排序,取最新的一个
+ latest_file = max(list_of_files, key=os.path.getctime)
+ return latest_file
def main():
# 实例化环境
env = AirSimMazeEnv()
- # 定义模型
- # MultiInputPolicy 会自动检测 Dict 输入
- # learning_rate: 学习率,通常 3e-4 是个不错的起点
- # n_steps: 每次更新收集的步数
- model = PPO(
- "MultiInputPolicy",
- env,
- verbose=1,
- tensorboard_log=log_dir,
- learning_rate=0.0003,
- batch_size=64,
- n_steps=2048,
- gamma=0.99
- )
+ # 1. 尝试寻找最新的模型
+ latest_model_path = get_latest_model_path(MODELS_DIR)
- # 保存检查点的回调函数 (每 10000 步保存一次)
+ if latest_model_path:
+ print(f"--------------------------------------------------")
+ print(f"检测到已存在的模型,正在加载: {latest_model_path}")
+ print(f"将恢复之前的训练进度...")
+ print(f"--------------------------------------------------")
+
+ # === 加载旧模型 ===
+ # 注意:这里传入 env 和 tensorboard_log 是为了确保训练环境和日志路径正确
+ model = PPO.load(latest_model_path, env=env, tensorboard_log=LOG_DIR)
+
+ # 标记:不需要重置步数,我们要接着画图
+ reset_timesteps = False
+
+ else:
+ print(f"--------------------------------------------------")
+ print(f"未找到已有模型,正在初始化新模型 (From Scratch)...")
+ print(f"--------------------------------------------------")
+
+ # === 创建新模型 ===
+ model = PPO(
+ "MultiInputPolicy",
+ env,
+ verbose=1,
+ tensorboard_log=LOG_DIR,
+ learning_rate=0.0003,
+ batch_size=64,
+ n_steps=2048,
+ gamma=0.99
+ )
+ # 标记:新模型需要从 0 开始计数
+ reset_timesteps = True
+
+ # 定义回调函数
checkpoint_callback = CheckpointCallback(
save_freq=10000,
- save_path='./models/',
+ save_path=MODELS_DIR, # 使用你指定的 models 路径
name_prefix='drone_maze'
)
- print("开始训练...")
- # 训练步数建议:简单的迷宫可能需要 20万-50万步,复杂的需要更多
- model.learn(total_timesteps=500000, callback=checkpoint_callback)
+ print("开始/继续 训练...")
+
+ # === 开始训练 ===
+ # reset_num_timesteps=False 关键!
+ # 如果是加载的模型,设为 False 可以让 Tensorboard 的曲线接上之前的,而不是从 0 开始乱画。
+ model.learn(
+ total_timesteps=500000,
+ callback=checkpoint_callback,
+ reset_num_timesteps=reset_timesteps
+ )
# 保存最终模型
- model.save("drone_maze_final")
- print("训练完成,模型已保存。")
+ final_save_path = os.path.join(MODELS_DIR, "drone_maze_final")
+ model.save(final_save_path)
+ print(f"训练完成,最终模型已保存至: {final_save_path}")
if __name__ == "__main__":
diff --git a/src/biomechanical_hcl_smart_simulation_platform/beatsvr_bimanual_eval.py b/src/biomechanical_hcl_smart_simulation_platform/beatsvr_bimanual_eval.py
deleted file mode 100644
index 89c0406c10..0000000000
--- a/src/biomechanical_hcl_smart_simulation_platform/beatsvr_bimanual_eval.py
+++ /dev/null
@@ -1,223 +0,0 @@
-import os
-import numpy as np
-from stable_baselines3 import PPO
-import re
-import argparse
-import scipy.ndimage
-from collections import defaultdict
-import matplotlib.pyplot as pp
-import cv2
-
-from uitb.utils.logger import StateLogger, ActionLogger
-from uitb.simulator import Simulator
-
-#from uitb.bm_models.effort_models import CumulativeFatigue3CCr, ConsumedEndurance
-
-
-def natural_sort(l):
- convert = lambda text: int(text) if text.isdigit() else text.lower()
- alphanum_key = lambda key: [convert(c) for c in re.split('([0-9]+)', key)]
- return sorted(l, key=alphanum_key)
-
-
-### DEPRECATED, use simulator.render() instead
-# def grab_pip_image(simulator):
-# # Grab an image from both 'for_testing' camera and 'oculomotor' camera, and display them 'picture-in-picture'
-#
-# # Grab images
-# img, _ = simulator._GUI_camera.render()
-#
-# ocular_img = None
-# for module in simulator.perception.perception_modules:
-# if module.modality == "vision":
-# # TODO would be better to have a class function that returns "human-viewable" rendering of the observation;
-# # e.g. in case the vision model has two cameras, or returns a combination of rgb + depth images etc.
-# ocular_img, _ = module._camera.render()
-#
-# if ocular_img is not None:
-#
-# # Resample
-# resample_factor = 2
-# resample_height = ocular_img.shape[0]*resample_factor
-# resample_width = ocular_img.shape[1]*resample_factor
-# resampled_img = np.zeros((resample_height, resample_width, 3), dtype=np.uint8)
-# for channel in range(3):
-# resampled_img[:, :, channel] = scipy.ndimage.zoom(ocular_img[:, :, channel], resample_factor, order=0)
-#
-# # Embed ocular image into free image
-# i = simulator._GUI_camera.height - resample_height
-# j = simulator._GUI_camera.width - resample_width
-# img[i:, j:] = resampled_img
-#
-# return img
-
-
-if __name__ == "__main__":
-
- parser = argparse.ArgumentParser(description='Evaluate a policy.')
- parser.add_argument('simulator_folder', type=str,
- help='the simulation folder')
- parser.add_argument('--action_sample_freq', type=float, default=20,
- help='action sample frequency (how many times per second actions are sampled from policy, default: 20)')
- parser.add_argument('--checkpoint', type=str, default=None,
- help='filename of a specific checkpoint (default: None, latest checkpoint is used)')
- parser.add_argument('--num_episodes', type=int, default=10,
- help='how many episodes are evaluated (default: 10)')
- parser.add_argument('--uncloned', dest="cloned", action='store_false', help='use source code instead of files from cloned simulator module')
- parser.add_argument('--app_condition', type=str, default=None,
- help="can be used to override the 'condition' argument passed to a Unity app")
- parser.add_argument('--record', action='store_true', help='enable recording')
- parser.add_argument('--out_file', type=str, default='evaluate.mp4',
- help='output file for recording if recording is enabled (default: ./evaluate.mp4)')
- parser.add_argument('--logging', action='store_true', help='enable logging')
- parser.add_argument('--state_log_file', default='state_log',
- help='output file for state log if logging is enabled (default: ./state_log)')
- parser.add_argument('--action_log_file', default='action_log',
- help='output file for action log if logging is enabled (default: ./action_log)')
- args = parser.parse_args()
-
- # Define directories
- checkpoint_dir = os.path.join(args.simulator_folder, 'checkpoints')
- evaluate_dir = os.path.join(args.simulator_folder, 'evaluate')
-
- # Make sure output dir exists
- os.makedirs(evaluate_dir, exist_ok=True)
-
- # Override run parameters
- run_params = dict()
- run_params["action_sample_freq"] = args.action_sample_freq
- run_params["evaluate"] = True
-
- run_params["unity_record_gameplay"] = args.record #False
- run_params["unity_logging"] = True
- run_params["unity_output_folder"] = evaluate_dir
- if args.app_condition is not None:
- run_params["app_args"] = ['-condition', args.app_condition]
- # run_params["unity_random_seed"] = 123
-
- # Embed visual observations into main mp4 or store as separate mp4 files
- render_mode_perception = "separate" if run_params["unity_record_gameplay"] else "embed"
-
- # Use deterministic actions?
- deterministic = False
-
- # Initialise simulator
- simulator = Simulator.get(args.simulator_folder, render_mode="rgb_array_list", render_mode_perception=render_mode_perception, run_parameters=run_params, use_cloned=args.cloned)
-
- # ## Change effort model #TODO: delete
- # simulator.bm_model._effort_model = CumulativeFatigue3CCr(simulator.bm_model, dt=simulator._run_parameters["dt"])
-
- print(f"run parameters are: {simulator.run_parameters}")
-
- # Load latest model if filename not given
- _policy_loaded = False
- if args.checkpoint is not None:
- model_file = args.checkpoint
- _policy_loaded = True
- else:
- try:
- files = natural_sort(os.listdir(checkpoint_dir))
- model_file = files[-1]
- _policy_loaded = True
- except (FileNotFoundError, IndexError):
- print("No checkpoint found. Will continue evaluation with randomly sampled controls.")
-
- if _policy_loaded:
- # Load policy TODO should create a load method for uitb.rl.BaseRLModel
- print(f'Loading model: {os.path.join(checkpoint_dir, model_file)}\n')
- model = PPO.load(os.path.join(checkpoint_dir, model_file))
-
- # Set callbacks to match the value used for this training point (if the simulator had any)
- simulator.update_callbacks(model.num_timesteps)
-
- if args.logging:
- # Initialise log
- state_logger = StateLogger(args.num_episodes, keys=simulator.get_state().keys())
-
- # Actions are logged separately to make things easier
- action_logger = ActionLogger(args.num_episodes)
-
- # Visualise evaluations
- # statistics = defaultdict(list)
- for episode_idx in range(args.num_episodes):
-
- print(f"Run episode {episode_idx + 1}/{args.num_episodes}.")
-
- # Reset environment
- obs, info = simulator.reset()
- terminated = False
- truncated = False
- reward = 0
-
- if args.logging:
- state = simulator.get_state()
- state_logger.log(episode_idx, state)
-
- # Loop until episode ends
- while not terminated and not truncated:
- # #print(f"Episode {episode_idx}: {simulator.get_episode_statistics_str()}")
- # print(reward)
-
- if _policy_loaded:
- # Get actions from policy
- action, _internal_policy_state = model.predict(obs, deterministic=deterministic)
- else:
- # choose random action from action space
- action = simulator.action_space.sample()
-
- # Take a step
- obs, r, terminated, truncated, info = simulator.step(action)
- reward += r
-
- if args.logging:
- action_logger.log(episode_idx,
- {"steps": state["steps"], "timestep": state["timestep"], "action": action.copy(),
- "reward": r})
- state = simulator.get_state()
- state.update(info)
- state_logger.log(episode_idx, state)
-
- print(reward)
- # print(f"Episode {episode_idx}: {simulator.get_episode_statistics_str()}")
-
- # episode_statistics = simulator.get_episode_statistics()
- # for key in episode_statistics:
- # statistics[key].append(episode_statistics[key])
-
- # print(f'Averages over {args.num_episodes} episodes (std in parenthesis):',
- # ', '.join(['{}: {:.2f} ({:.2f})'.format(k, np.mean(v), np.std(v)) for k, v in statistics.items()]))
-
- if args.logging:
- # Output log
- state_logger.save(os.path.join(evaluate_dir, args.state_log_file))
- action_logger.save(os.path.join(evaluate_dir, args.action_log_file))
- print(f'Log files have been saved files {os.path.join(evaluate_dir, args.state_log_file)}.pickle and '
- f'{os.path.join(evaluate_dir, args.action_log_file)}.pickle')
-
- if args.record:
- simulator._GUI_camera.write_video_set_path(os.path.join(evaluate_dir, args.out_file))
-
- # Write the video
- # simulator._camera.write_video(imgs, os.path.join(evaluate_dir, args.out_file))
- _imgs = simulator.render()
- for _img in _imgs:
- simulator._GUI_camera.write_video_add_frame(_img)
-
- simulator._GUI_camera.write_video_close()
- print(f'A recording has been saved to file {os.path.join(evaluate_dir, args.out_file)}')
-
- # Write additional videos for each perception module camera (only if simulator._render_mode_perception == "separate")
- if simulator._render_mode_perception == "separate":
- _perception_imgs = simulator.get_render_stack_perception()
- for _module_name, _imgs in _perception_imgs.items():
- _out_file = os.path.splitext(args.out_file)[0] + f"_{_module_name.replace('/', '-')}" + os.path.splitext(args.out_file)[1]
-
- fourcc = cv2.VideoWriter_fourcc(*'mp4v')
- out = cv2.VideoWriter(os.path.join(evaluate_dir, _out_file), fourcc, simulator._GUI_camera._fps, (_imgs[0].shape[1], _imgs[0].shape[0]))
- # out.open(_out_file)
- for img in _imgs:
- out.write(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
- out.release()
- print(f'A recording has been saved to file {os.path.join(evaluate_dir, _out_file)}')
-
- simulator.close()
diff --git a/src/biomechanical_hcl_smart_simulation_platform/main_beatsvr_bimanual_eval.py b/src/biomechanical_hcl_smart_simulation_platform/main_beatsvr_bimanual_eval.py
new file mode 100644
index 0000000000..baf9cbd629
--- /dev/null
+++ b/src/biomechanical_hcl_smart_simulation_platform/main_beatsvr_bimanual_eval.py
@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+"""
+beatsvr_bimanual_eval - single entrypoint
+
+This script is the ONLY runnable entry for:
+- training
+- evaluation
+- bimanual + envCamera/mainCamera evaluation visualization
+"""
+
+from __future__ import annotations
+import argparse
+import sys
+
+from pipelines.train import run_train
+from pipelines.eval import run_eval
+from pipelines.record_multiview import run_record
+from utils.export_gifs import run_export_gifs
+
+
+def build_parser() -> argparse.ArgumentParser:
+ parser = argparse.ArgumentParser(
+ prog="beatsvr_bimanual_eval",
+ description="Single entrypoint for BeatSVR bimanual evaluation and visualization."
+ )
+ sub = parser.add_subparsers(dest="cmd", required=True)
+
+ # ---------------- train ----------------
+ p_train = sub.add_parser("train", help="Train BeatSVR bimanual policy")
+ p_train.add_argument("--config", required=True, help="Path to training config")
+ p_train.add_argument("--seed", type=int, default=0)
+ p_train.add_argument("--total_steps", type=int, default=1_000_000)
+ p_train.set_defaults(func=run_train)
+
+ # ---------------- eval ----------------
+ p_eval = sub.add_parser("eval", help="Evaluate trained policy")
+ p_eval.add_argument("--simulator_dir", required=True)
+ p_eval.add_argument("--checkpoint", default="", help="Optional model checkpoint")
+ p_eval.add_argument("--episodes", type=int, default=10)
+ p_eval.set_defaults(func=run_eval)
+
+ # ---------------- record ----------------
+ p_rec = sub.add_parser(
+ "record",
+ help="Run bimanual evaluation and record envCamera + mainCamera"
+ )
+ p_rec.add_argument("--simulator_dir", required=True)
+ p_rec.add_argument("--episodes", type=int, default=1)
+ p_rec.add_argument("--max_steps", type=int, default=2000)
+ p_rec.add_argument("--fps", type=int, default=20)
+ p_rec.add_argument("--out_dir", default="evaluate")
+ p_rec.add_argument("--checkpoint", default="", help="Optional model checkpoint")
+ p_rec.set_defaults(func=run_record)
+
+ # ---------------- export gif ----------------
+ p_gif = sub.add_parser("export_gif", help="Convert evaluation MP4 to GIF")
+ p_gif.add_argument("--inputs", nargs="+", required=True)
+ p_gif.add_argument("--out_dir", default=".")
+ p_gif.add_argument("--fps", type=int, default=15)
+ p_gif.add_argument("--width", type=int, default=640)
+ p_gif.set_defaults(func=run_export_gifs)
+
+ return parser
+
+
+def main(argv: list[str] | None = None) -> int:
+ parser = build_parser()
+ args = parser.parse_args(argv)
+ args.func(args)
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/src/box/moblARMslndex.py b/src/box/moblARMslndex.py
new file mode 100644
index 0000000000..950f646641
--- /dev/null
+++ b/src/box/moblARMslndex.py
@@ -0,0 +1,270 @@
+"""
+MoblArmsIndex 生物力学模型 - 修改版
+这是基于 MoBL ARMS 模型的 MuJoCo 实现,专门用于手指指向任务
+"""
+
+import sys
+import os
+
+# ================ 动态导入 BaseBMModel ================
+try:
+ # 尝试1: 从原始相对位置导入
+ from ..base import BaseBMModel
+ print("✓ 使用相对导入: from ..base import BaseBMModel")
+except ImportError:
+ try:
+ # 尝试2: 从绝对路径导入(假设在 uitb 包中)
+ from uitb.base import BaseBMModel
+ print("✓ 使用绝对导入: from uitb.base import BaseBMModel")
+ except ImportError:
+ try:
+ # 尝试3: 从常见路径导入
+ import sys
+ import os
+
+ # 将项目根目录添加到路径
+ current_dir = os.path.dirname(os.path.abspath(__file__))
+ project_root = os.path.dirname(os.path.dirname(current_dir))
+ sys.path.insert(0, project_root)
+
+ from base import BaseBMModel
+ print(f"✓ 使用项目根目录导入: from base import BaseBMModel (路径: {project_root})")
+ except ImportError as e:
+ # 尝试4: 搜索并动态导入
+ print("⚠ 无法自动导入 BaseBMModel,尝试动态查找...")
+
+ # 搜索 base.py 文件
+ base_path = None
+ search_dirs = [
+ os.path.dirname(os.path.dirname(os.path.abspath(__file__))), # 上一级目录
+ os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))), # 上上级目录
+ os.path.join(os.path.dirname(__file__), '..'), # 相对路径
+ ]
+
+ for search_dir in search_dirs:
+ possible_path = os.path.join(search_dir, 'base.py')
+ if os.path.exists(possible_path):
+ base_path = possible_path
+ break
+
+ if base_path:
+ # 动态导入
+ import importlib.util
+ spec = importlib.util.spec_from_file_location("base_module", base_path)
+ base_module = importlib.util.module_from_spec(spec)
+ sys.modules[spec.name] = base_module
+ spec.loader.exec_module(base_module)
+ BaseBMModel = base_module.BaseBMModel
+ print(f"✓ 动态导入成功: {base_path}")
+ else:
+ # 如果都失败,创建简化的 BaseBMModel
+ print("⚠ 无法找到 BaseBMModel,使用简化版本")
+
+ class BaseBMModel:
+ """简化的 BaseBMModel 基类"""
+ def __init__(self, model, data, **kwargs):
+ self.model = model
+ self.data = data
+
+ def _update(self, model, data):
+ """更新方法 - 子类应重写"""
+ pass
+
+ @classmethod
+ def _get_floor(cls):
+ """获取地板 - 子类应重写"""
+ return None
+
+ BaseBMModel = BaseBMModel
+ print("✓ 使用简化版本的 BaseBMModel")
+
+# ================ 继续原来的代码 ================
+import numpy as np
+import mujoco
+
+
+class MoblArmsIndex(BaseBMModel):
+ """
+ 基于 MoBL ARMS 模型的生物力学模型
+
+ 来源:
+ - 原始 OpenSim 模型: https://simtk.org/frs/?group_id=657
+ - MuJoCo 转换工具: https://github.com/aikkala/O2MConverter
+
+ 说明:
+ 此模型与 uitb/bm_models/mobl_arms 中的模型相同,
+ 但食指处于弯曲状态并包含力传感器。
+ """
+
+ def __init__(self, model, data, **kwargs):
+ """
+ 初始化 MoblArmsIndex 模型
+
+ 参数:
+ model: MuJoCo 模型实例
+ data: MuJoCo 数据实例
+ **kwargs: 额外参数
+ - shoulder_variant: 肩部变体,可选 "none" (默认) 或 "patch-v1"
+ """
+ super().__init__(model, data, **kwargs)
+
+ # 设置肩部变体
+ # 使用 "none" 作为默认值
+ # 使用 "patch-v1" 可以获得更合理的运动外观(未经全面测试)
+ self.shoulder_variant = kwargs.get("shoulder_variant", "none")
+
+ print(f"✓ MoblArmsIndex 初始化完成,肩部变体: {self.shoulder_variant}")
+
+ def _update(self, model, data):
+ """
+ 更新模型状态
+
+ 此方法在每个时间步被调用,用于更新肩部约束。
+
+ 参数:
+ model: MuJoCo 模型实例
+ data: MuJoCo 数据实例
+ """
+ # 更新肩部等式约束
+ if self.shoulder_variant.startswith("patch"):
+ # 约束1: shoulder1_r2_con 约束的数据更新
+ model.equality("shoulder1_r2_con").data[1] = \
+ -((np.pi - 2 * data.joint('shoulder_elv').qpos) / np.pi)
+
+ # patch-v2 变体有额外的约束
+ if self.shoulder_variant == "patch-v2":
+ # 动态调整肩部旋转范围
+ data.joint('shoulder_rot').range[:] = \
+ np.array([-np.pi / 2, np.pi / 9]) - \
+ 2 * np.min((data.joint('shoulder_elv').qpos,
+ np.pi - data.joint('shoulder_elv').qpos)) / np.pi \
+ * data.joint('elv_angle').qpos
+
+ # 执行前向计算,更新物理状态
+ mujoco.mj_forward(model, data)
+
+ # 打印调试信息(可选)
+ if hasattr(self, 'debug') and self.debug:
+ print(f"更新肩部约束: shoulder_elv={data.joint('shoulder_elv').qpos:.3f}, "
+ f"约束值={model.equality('shoulder1_r2_con').data[1]:.3f}")
+
+ @classmethod
+ def _get_floor(cls):
+ """
+ 获取地板配置
+
+ 此模型不包含地板,返回 None。
+
+ 返回:
+ None: 表示此模型没有地板
+ """
+ return None
+
+ def get_force_sensor_data(self, data, sensor_name="index_force_sensor"):
+ """
+ 获取力传感器数据
+
+ 参数:
+ data: MuJoCo 数据实例
+ sensor_name: 传感器名称,默认为 "index_force_sensor"
+
+ 返回:
+ numpy.ndarray: 传感器数据,如果没有找到传感器则返回 None
+ """
+ try:
+ sensor_id = self.model.sensor(sensor_name).id
+ return data.sensordata[sensor_id]
+ except Exception as e:
+ if hasattr(self, 'debug') and self.debug:
+ print(f"⚠ 无法获取力传感器数据: {e}")
+ return None
+
+ def set_debug_mode(self, debug=True):
+ """
+ 设置调试模式
+
+ 参数:
+ debug: 是否启用调试模式
+ """
+ self.debug = debug
+
+ def __str__(self):
+ """
+ 返回模型的字符串表示
+
+ 返回:
+ str: 模型描述
+ """
+ return (f"MoblArmsIndex 模型 (肩部变体: {self.shoulder_variant})\n"
+ f"描述: 基于 MoBL ARMS 的上肢模型,食指弯曲并包含力传感器")
+
+
+# ================ 测试代码 ================
+if __name__ == "__main__":
+ """
+ 直接运行此文件的测试代码
+ """
+ print("=" * 60)
+ print("MoblArmsIndex 模型测试")
+ print("=" * 60)
+
+ # 测试导入
+ print("1. 测试导入和类定义:")
+ print(f" 类名: {MoblArmsIndex.__name__}")
+ print(f" 基类: {MoblArmsIndex.__bases__[0].__name__}")
+ print(f" 文档: {MoblArmsIndex.__doc__.strip().split('\n')[0]}")
+
+ # 测试创建实例(需要 MuJoCo 模型)
+ try:
+ import mujoco
+ import numpy as np
+
+ print("\n2. 测试创建实例:")
+
+ # 创建一个简单的 MuJoCo 模型用于测试
+ xml_string = """
+
+
+
+
+
+
+
+
+ """
+
+ model = mujoco.MjModel.from_xml_string(xml_string)
+ data = mujoco.MjData(model)
+
+ # 创建模型实例
+ bm_model = MoblArmsIndex(model, data, shoulder_variant="none")
+
+ print(f" ✓ 成功创建 MoblArmsIndex 实例")
+ print(f" 肩部变体: {bm_model.shoulder_variant}")
+
+ # 测试更新方法
+ print("\n3. 测试更新方法:")
+ bm_model._update(model, data)
+ print(" ✓ _update 方法执行成功")
+
+ # 测试获取地板
+ print("\n4. 测试获取地板:")
+ floor = bm_model._get_floor()
+ print(f" 地板配置: {floor}")
+
+ # 测试字符串表示
+ print("\n5. 测试字符串表示:")
+ print(f" {bm_model}")
+
+ print("\n" + "=" * 60)
+ print("所有测试通过!")
+ print("=" * 60)
+
+ except Exception as e:
+ print(f"\n✗ 测试过程中出现错误: {type(e).__name__}: {e}")
+ import traceback
+ traceback.print_exc()
+
+ print("\n" + "=" * 60)
+ print("注意: 缺少 MuJoCo 模型文件,但导入测试成功")
+ print("=" * 60)
diff --git a/src/box/perception/moblARMslndex.py b/src/box/perception/moblARMslndex.py
new file mode 100644
index 0000000000..299bbb4d67
--- /dev/null
+++ b/src/box/perception/moblARMslndex.py
@@ -0,0 +1,372 @@
+"""
+MoblArmsIndex 简化版本 - 只生成人体上肢模型运动视频
+完全修复版
+"""
+
+import numpy as np
+import mujoco
+import imageio
+import os
+import subprocess
+import sys
+
+def install_ffmpeg():
+ """安装必要的视频编码依赖"""
+ try:
+ # 尝试导入 imageio-ffmpeg
+ import imageio_ffmpeg
+ print("✓ imageio-ffmpeg 已安装")
+ return True
+ except ImportError:
+ print("正在安装 imageio-ffmpeg...")
+ try:
+ subprocess.check_call([sys.executable, "-m", "pip", "install", "imageio[ffmpeg]"])
+ print("✓ imageio[ffmpeg] 安装成功")
+ return True
+ except Exception as e:
+ print(f"⚠ 安装失败: {e}")
+ print("将使用 GIF 格式替代 MP4")
+ return False
+
+# ================ 简化的 BaseBMModel 类 ================
+class BaseBMModel:
+ """简化的 BaseBMModel 基类"""
+ def __init__(self, model, data, **kwargs):
+ self.model = model
+ self.data = data
+
+ def _update(self, model, data):
+ """更新方法 - 子类应重写"""
+ pass
+
+# ================ MoblArmsIndex 模型 ================
+class MoblArmsIndex(BaseBMModel):
+ """
+ 基于 MoBL ARMS 模型的生物力学模型
+ 只专注于生成上肢运动视频
+ """
+
+ def __init__(self, model, data, **kwargs):
+ super().__init__(model, data, **kwargs)
+ print("✓ MoblArmsIndex 上肢模型初始化完成")
+
+ def _update(self, model, data):
+ """更新模型状态 - 简化版本"""
+ # 这里可以添加任何必要的模型更新逻辑
+ pass
+
+# ================ 主程序:只生成视频 ================
+def main():
+ print("=" * 60)
+ print("人体上肢模型运动视频生成器")
+ print("=" * 60)
+
+ # 检查并安装必要依赖
+ has_ffmpeg = install_ffmpeg()
+
+ # 1. 创建更真实的上肢XML模型(修复了所有问题)
+ xml_string = """
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ """
+
+ # 2. 加载模型
+ try:
+ model = mujoco.MjModel.from_xml_string(xml_string)
+ data = mujoco.MjData(model)
+ print("✓ MuJoCo 模型加载成功")
+ print(f" 关节数: {model.njnt}")
+ print(f" 几何体数: {model.ngeom}")
+ print(f" 执行器数: {model.nu}")
+ except Exception as e:
+ print(f"❌ 模型加载失败: {e}")
+ return
+
+ # 3. 创建生物力学模型实例
+ bm_model = MoblArmsIndex(model, data)
+
+ # 4. 设置仿真参数
+ fps = 30
+ duration = 5 # 5秒仿真
+ total_steps = int(duration / model.opt.timestep)
+
+ print(f"\n仿真参数:")
+ print(f" 时长: {duration}秒")
+ print(f" 时间步长: {model.opt.timestep}")
+ print(f" 总步数: {total_steps}")
+ print(f" 视频帧率: {fps} FPS")
+
+ # 5. 设置渲染器(使用较小的分辨率避免问题)
+ try:
+ # 创建渲染器(使用640x480避免离屏缓冲区问题)
+ renderer = mujoco.Renderer(model, height=480, width=640)
+
+ # 设置相机视角
+ camera = mujoco.MjvCamera()
+ camera.azimuth = 30 # 水平角度
+ camera.elevation = -20 # 垂直角度
+ camera.distance = 2.5 # 距离
+ camera.lookat = np.array([0.2, 0, 1.2]) # 看向点
+
+ # 初始渲染
+ renderer.update_scene(data, camera=camera)
+ print("✓ 渲染器创建成功")
+ except Exception as e:
+ print(f"❌ 渲染器创建失败: {e}")
+ return
+
+ # 6. 运行仿真并捕获视频帧
+ frames = []
+
+ print("\n开始仿真并录制视频...")
+
+ for step in range(total_steps):
+ # 计算时间(用于控制运动)
+ t = step * model.opt.timestep
+
+ # 协同运动控制
+ # 肩部运动(缓慢的波浪)
+ shoulder_pitch = 0.3 * np.sin(2 * np.pi * 0.3 * t) + 0.1
+ shoulder_roll = 0.2 * np.sin(2 * np.pi * 0.2 * t + 0.5)
+
+ # 肘部运动
+ elbow = 0.6 * np.sin(2 * np.pi * 0.4 * t) - 1.0
+
+ # 手腕运动
+ wrist = 0.1 * np.sin(2 * np.pi * 0.6 * t)
+
+ # 手指运动序列
+ if t < 1:
+ # 初始:手指弯曲
+ index_angle = -0.4
+ thumb_angle = 0.1
+ elif t < 3:
+ # 指向阶段
+ phase = (t - 1) / 2
+ index_angle = 0.1 * np.sin(2 * np.pi * 1.0 * t) - 0.1
+ thumb_angle = 0.15 * np.sin(2 * np.pi * 0.8 * t)
+ else:
+ # 返回阶段
+ phase = (t - 3) / 2
+ index_angle = -0.4 * (1 - phase)
+ thumb_angle = 0.1 * (1 - phase)
+
+ # 应用控制信号
+ data.ctrl[0] = shoulder_pitch # 肩部俯仰
+ data.ctrl[1] = shoulder_roll # 肩部滚动
+ data.ctrl[2] = elbow # 肘部
+ data.ctrl[3] = wrist # 手腕
+ data.ctrl[4] = index_angle # 食指
+ data.ctrl[5] = thumb_angle # 拇指
+
+ # 步进仿真
+ mujoco.mj_step(model, data)
+
+ # 定期捕获帧
+ if step % 2 == 0: # 每2步捕获一帧,获得15fps(GIF比较流畅)
+ try:
+ # 动态调整相机视角
+ if t < 2:
+ camera.azimuth = 30 + 15 * t
+ elif t < 4:
+ camera.elevation = -20 + 10 * np.sin(t * 0.5)
+
+ # 更新场景并渲染
+ renderer.update_scene(data, camera=camera)
+ frame = renderer.render()
+ frames.append(frame)
+ except:
+ # 如果失败,使用默认视角
+ try:
+ renderer.update_scene(data)
+ frame = renderer.render()
+ frames.append(frame)
+ except:
+ pass
+
+ # 显示进度
+ if step % 100 == 0:
+ progress = (step / total_steps) * 100
+ print(f"进度: {progress:.1f}%", end='\r')
+
+ print(f"进度: 100.0% - 仿真完成!")
+
+ # 7. 生成视频文件
+ if frames:
+ try:
+ # 确定保存路径(桌面)
+ desktop = os.path.join(os.path.expanduser("~"), "Desktop")
+ if not os.path.exists(desktop):
+ desktop = os.path.dirname(os.path.abspath(__file__))
+
+ # 首先尝试生成MP4(如果有FFMPEG)
+ if has_ffmpeg:
+ try:
+ video_path = os.path.join(desktop, "human_arm_motion.mp4")
+
+ # 保存MP4视频
+ print(f"\n生成MP4视频中...")
+ imageio.mimsave(
+ video_path,
+ frames,
+ fps=15, # GIF帧率较低
+ codec='libx264',
+ quality=8
+ )
+ print(f"✓ MP4视频已保存: {video_path}")
+ print(f" 文件大小: {os.path.getsize(video_path)/1024/1024:.1f} MB")
+
+ except Exception as mp4_error:
+ print(f"⚠ MP4生成失败: {mp4_error}")
+ # 回退到GIF
+ has_ffmpeg = False
+
+ # 如果没有FFMPEG或MP4生成失败,生成GIF
+ if not has_ffmpeg:
+ gif_path = os.path.join(desktop, "human_arm_motion.gif")
+
+ print(f"\n生成GIF动画中...")
+ # 保存为GIF(质量稍低但兼容性好)
+ imageio.mimsave(
+ gif_path,
+ frames,
+ fps=15, # GIF帧率较低
+ subrectangles=True # 优化GIF大小
+ )
+ print(f"✓ GIF动画已保存: {gif_path}")
+ print(f" 文件大小: {os.path.getsize(gif_path)/1024:.1f} KB")
+
+ except Exception as e:
+ print(f"❌ 文件保存失败: {e}")
+ else:
+ print("⚠ 没有捕获到视频帧")
+
+ print("\n" + "=" * 60)
+ print("✅ 任务完成!")
+ print("=" * 60)
+
+ # 8. 打开文件所在目录(仅Windows)
+ if 'video_path' in locals() and os.path.exists(video_path):
+ print(f"📁 MP4视频文件: {video_path}")
+ try:
+ os.startfile(os.path.dirname(video_path))
+ print("📂 已打开文件所在目录")
+ except:
+ pass
+ elif 'gif_path' in locals() and os.path.exists(gif_path):
+ print(f"📁 GIF动画文件: {gif_path}")
+ try:
+ os.startfile(os.path.dirname(gif_path))
+ print("📂 已打开文件所在目录")
+ except:
+ pass
+
+# ================ 运行主程序 ================
+if __name__ == "__main__":
+ # 检查必要依赖
+ try:
+ import mujoco
+ import numpy as np
+ import imageio
+ print("✓ 核心库已安装")
+ except ImportError as e:
+ print(f"❌ 缺少必要的库: {e}")
+ print("\n请安装以下依赖:")
+ print("pip install mujoco numpy imageio")
+ exit(1)
+
+ try:
+ main()
+ except KeyboardInterrupt:
+ print("\n⚠ 用户中断")
+ except Exception as e:
+ print(f"\n❌ 运行出错: {type(e).__name__}: {e}")
+ import traceback
+ traceback.print_exc()
+
\ No newline at end of file
diff --git a/src/car_navigation_system/README.md b/src/car_navigation_system/README.md
index 4580a6a70c..7912e8cb97 100644
--- a/src/car_navigation_system/README.md
+++ b/src/car_navigation_system/README.md
@@ -1,11 +1,12 @@
# 多模态 CARLA 导航避障系统
## 项目简介
-本项目基于 CARLA 模拟器与神经网络技术,实现了具备多传感器融合能力的智能车辆导航避障系统。系统集成前视摄像头、第三视角摄像头与激光雷达(LiDAR),通过多模态数据感知环境,结合智能避障算法,实现车辆自主行驶与障碍物规避功能。
+本项目基于 CARLA 模拟器与神经网络技术,实现了具备多传感器融合能力的智能车辆导航避障系统。系统集成前视摄像头、第三视角摄像头与障碍物检测模块,通过多模态数据感知环境,结合神经网络与传统控制算法,实现车辆自主行驶与障碍物规避功能。
## 核心功能
-- 多传感器融合感知:集成 RGB 摄像头(前视 + 第三视角)与 32 线激光雷达,全面获取环境信息
-- 智能避障算法:基于激光雷达点云数据,分区域检测障碍物,动态选择最优避障方向(左 / 右)
-- 可视化监控:实时显示前视画面、第三视角跟随画面、LiDAR 鸟瞰图,叠加关键行驶数据
-- 人机交互控制:支持键盘手动干预,兼容自动 / 手动切换
+- 多模态感知系统:集成 RGB 摄像头(前视 + 第三视角)与障碍物检测器,全面获取环境信息
+- 智能避障算法:基于改进神经网络控制器与传统控制器双模式,动态检测并规避前方障碍物
+- 实时可视化:实时显示第三视角画面,叠加障碍物检测结果、车速、控制状态等关键信息
+- 双模式控制:支持神经网络模式与传统控制模式切换,可根据环境选择最优控制策略
+- 智能恢复机制:内置车辆卡住检测与自动恢复系统,保障行驶稳定性
## 环境配置
- 操作系统:Windows 10/11 或 Ubuntu 20.04/22.04
- Python 版本:3.7
@@ -51,12 +52,26 @@
- a 手动左转向
- d 手动右转向
- r 重置转向角度(回正)
-
## 系统架构
1. 环境初始化模块
-连接 CARLA 服务器(默认 localhost:2000)。
-加载 Town01 地图,设置同步模式(固定时间步长 0.05s)。
-配置天气参数(多云、无降水、太阳高度角)。
+- 连接 CARLA 服务器(默认 localhost:2000)
+- 加载 Town01 地图,设置同步模式(固定时间步长 0.1s)
+- 配置天气参数(云量30%、无降水、太阳高度角70°)
2. 智能体生成模块
-主车辆:特斯拉 Model3(红色),关闭自动驾驶,由自定义算法控制。
-障碍物车辆:随机生成 6 辆不同类型车辆,开启自动驾驶,分散分布在地图中。
+- 主车辆:特斯拉 Model3(红色),关闭自动驾驶,由自定义控制算法控制
+- NPC车辆:随机生成3辆不同类型车辆,开启自动驾驶,分散在50米外起始位置
+3. 传感器系统
+- 前视摄像头:90°视野,分辨率640×480,用于神经网络输入
+- 第三视角摄像头:110°视野,分辨率640×480,用于可视化监控
+- 障碍物检测器:实时检测前方±60°范围内50米内障碍物
+4. 控制系统
+- 神经网络控制器:基于改进的SimpleDrivingNetwork,融合图像与障碍物信息
+- 传统控制器:基于路点跟踪的经典控制算法,稳定性更高
+- 障碍物避障逻辑:紧急刹车、减速跟随、转向避让多级策略
+5. 可视化与监控
+- 实时显示第三视角画面
+- 可视化障碍物位置与距离
+- 显示车速、控制模式、油门/刹车/转向值
+- 安全区域标记与卡住警告
+## 联系方式
+邮箱:2985835251@qq.com
\ No newline at end of file
diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py
index f9d8ddb25d..740b1b3c8c 100644
--- a/src/car_navigation_system/main.py
+++ b/src/car_navigation_system/main.py
@@ -14,16 +14,10 @@
import os
-
# 修复1: 简化的神经网络架构(增加障碍物检测通道)
class SimpleDrivingNetwork(nn.Module):
"""
简化的驾驶网络 - 增加障碍物感知
-=======
-# 修复1: 简化的神经网络架构
-class SimpleDrivingNetwork(nn.Module):
- """
-
"""
def __init__(self):
@@ -40,7 +34,6 @@ def __init__(self):
nn.AdaptiveAvgPool2d((4, 4))
)
-
# 状态信息维度: 速度 + 转向历史 + 障碍物信息
state_dim = 7 # 增加障碍物相关维度
@@ -50,14 +43,6 @@ def __init__(self):
nn.ReLU(),
nn.Dropout(0.1), # 添加dropout防止过拟合
nn.Linear(128, 64),
-
- # 状态信息维度: 速度 + 转向历史
- state_dim = 4
-
- # 融合层
- self.fc_layers = nn.Sequential(
- nn.Linear(32 * 4 * 4 + state_dim, 64),
-
nn.ReLU(),
nn.Linear(64, 32),
nn.ReLU(),
@@ -80,7 +65,6 @@ def forward(self, image, state):
return torch.cat([throttle_brake, steer], dim=1)
-
# 新增:障碍物检测器类
class ObstacleDetector:
def __init__(self, world, vehicle, max_distance=50.0):
@@ -219,11 +203,6 @@ def visualize_obstacles(self, image, vehicle_transform):
# 修复2: 改进的神经网络控制器(整合障碍物检测)
class ImprovedNeuralController:
def __init__(self, obstacle_detector=None):
-
-# 修复2: 改进的神经网络控制器
-class ImprovedNeuralController:
- def __init__(self):
-
self.device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
print(f"使用设备: {self.device}")
@@ -234,7 +213,6 @@ def __init__(self):
# 控制历史,用于平滑
self.control_history = deque(maxlen=5)
-
# 障碍物检测器
self.obstacle_detector = obstacle_detector
@@ -243,13 +221,11 @@ def __init__(self):
self.last_brake = 0.0
self.last_steer = 0.0
-
# 避障参数
self.emergency_brake_distance = 5.0 # 紧急刹车距离
self.safe_following_distance = 8.0 # 安全跟车距离
self.obstacle_avoidance_steer = 0.0
-
def preprocess_image(self, image):
"""修复图像预处理"""
if image is None:
@@ -266,21 +242,16 @@ def preprocess_image(self, image):
print(f"图像预处理错误: {e}")
return torch.zeros((1, 3, 120, 160), device=self.device)
-
def preprocess_state(self, speed, steer_history, obstacle_info):
"""修复状态预处理,加入障碍物信息"""
has_obstacle = 1.0 if obstacle_info['has_obstacle'] else 0.0
normalized_distance = min(obstacle_info['distance'] / 50.0, 1.0) if obstacle_info['has_obstacle'] else 1.0
normalized_angle = obstacle_info['relative_angle'] / 60.0 if obstacle_info['has_obstacle'] else 0.0
- def preprocess_state(self, speed, steer_history):
- """修复状态预处理"""
-
state_data = [
speed / 20.0, # 归一化速度
steer_history[-1] if steer_history else 0.0, # 最近转向
steer_history[-2] if len(steer_history) > 1 else 0.0, # 前一次转向
-
np.mean(steer_history) if steer_history else 0.0, # 平均转向
has_obstacle, # 是否有障碍物
normalized_distance, # 归一化距离
@@ -288,8 +259,12 @@ def preprocess_state(self, speed, steer_history):
]
return torch.tensor(state_data, device=self.device).unsqueeze(0)
+ # 在 ImprovedNeuralController 类的 apply_obstacle_avoidance 方法中,修改以下部分:
+ def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed):
+ """应用避障逻辑 - 优化版本"""
+
def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed):
- """应用避障逻辑"""
+
if not obstacle_info['has_obstacle']:
return throttle, brake, steer
@@ -299,59 +274,100 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, obstacle_info, speed)
# 紧急情况:前方有近距离障碍物
if distance < self.emergency_brake_distance:
print(f"紧急刹车!距离障碍物: {distance:.1f}m")
+
+ return 0.0, 1.0, 0.0 # 紧急情况下保持直行,只刹车
+
+
+ return 0.0, 1.0, 0.0 # 紧急情况下保持直行,只刹车
+
return 0.0, 1.0, steer # 全力刹车
+
+
# 中距离障碍物:减速并准备转向
elif distance < self.safe_following_distance:
# 计算安全速度比例
- safe_speed_ratio = (distance - 2.0) / (self.safe_following_distance - 2.0)
+
+ safe_speed_ratio = (distance - 3.0) / (self.safe_following_distance - 3.0)
+
+
+ safe_speed_ratio = (distance - 3.0) / (self.safe_following_distance - 3.0)
safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio))
# 如果当前速度过高,减速
- if speed > 5.0 * safe_speed_ratio:
+ target_speed = 15.0 * safe_speed_ratio # 目标速度最大15km/h
+ current_speed_kmh = speed * 3.6
+
+ if current_speed_kmh > target_speed:
throttle = 0.0
- brake = 0.3
+ brake = 0.4 * ((current_speed_kmh - target_speed) / current_speed_kmh)
+ else:
+ throttle = 0.3 * safe_speed_ratio
+ brake = 0.0
# 如果障碍物在正前方,尝试轻微转向避开
- if abs(angle) < 10: # 正前方±10度内
+ if abs(angle) < 15: # 正前方±15度内
# 根据障碍物距离决定转向幅度
- avoid_steer = 0.5 if angle >= 0 else -0.5
- # 平滑转向
- steer = 0.7 * steer + 0.3 * avoid_steer
+ avoid_factor = max(0, 1.0 - distance / self.safe_following_distance)
+ avoid_steer = 0.3 * avoid_factor if angle >= 0 else -0.3 * avoid_factor
+ # 平滑转向 - 保持更多原始转向
+ steer = 0.8 * steer + 0.2 * avoid_steer
# 远距离障碍物:轻微调整
- elif distance < 20.0:
+ elif distance < 25.0:
# 轻微减速
- if speed > 10.0:
- throttle *= 0.8
+ if speed > 8.0:
+ throttle *= 0.7
# 如果障碍物在正前方,轻微转向
- if abs(angle) < 15:
- avoid_steer = 0.2 if angle >= 0 else -0.2
+ if abs(angle) < 20:
+ avoid_steer = 0.15 if angle >= 0 else -0.15
+ steer = 0.9 * steer + 0.1 * avoid_steer
+
+ safe_speed_ratio = (distance - 2.0) / (self.safe_following_distance - 2.0)
+
+ safe_speed_ratio = max(0.1, min(1.0, safe_speed_ratio))
+
+ # 如果当前速度过高,减速
+ target_speed = 15.0 * safe_speed_ratio # 目标速度最大15km/h
+ current_speed_kmh = speed * 3.6
+
+ if current_speed_kmh > target_speed:
+ throttle = 0.0
+ brake = 0.4 * ((current_speed_kmh - target_speed) / current_speed_kmh)
+ else:
+ throttle = 0.3 * safe_speed_ratio
+ brake = 0.0
+
+ # 如果障碍物在正前方,尝试轻微转向避开
+ if abs(angle) < 15: # 正前方±15度内
+ # 根据障碍物距离决定转向幅度
+ avoid_factor = max(0, 1.0 - distance / self.safe_following_distance)
+ avoid_steer = 0.3 * avoid_factor if angle >= 0 else -0.3 * avoid_factor
+ # 平滑转向 - 保持更多原始转向
steer = 0.8 * steer + 0.2 * avoid_steer
+ # 远距离障碍物:轻微调整
+ elif distance < 25.0:
+ # 轻微减速
+ if speed > 8.0:
+ throttle *= 0.7
+
+ # 如果障碍物在正前方,轻微转向
+ if abs(angle) < 20:
+ avoid_steer = 0.15 if angle >= 0 else -0.15
+ steer = 0.9 * steer + 0.1 * avoid_steer
+
return throttle, brake, steer
def get_control(self, image, speed, steer_history, obstacle_info):
"""修复控制生成逻辑,加入避障"""
-
- np.mean(steer_history) if steer_history else 0.0 # 平均转向
- ]
- return torch.tensor(state_data, device=self.device).unsqueeze(0)
-
- def get_control(self, image, speed, steer_history):
- """修复控制生成逻辑"""
-
try:
with torch.no_grad():
# 预处理
img_tensor = self.preprocess_image(image)
-
state_tensor = self.preprocess_state(speed, steer_history, obstacle_info)
- state_tensor = self.preprocess_state(speed, steer_history)
-
-
# 神经网络推理
control_output = self.model(img_tensor, state_tensor)
@@ -365,13 +381,11 @@ def get_control(self, image, speed, steer_history):
brake = max(0.0, min(0.5, brake)) # 限制最大刹车
steer = max(-0.5, min(0.5, steer)) # 限制转向幅度
-
# 应用避障逻辑
throttle, brake, steer = self.apply_obstacle_avoidance(
throttle, brake, steer, obstacle_info, speed
)
-
return throttle, brake, steer
except Exception as e:
@@ -380,30 +394,22 @@ def get_control(self, image, speed, steer_history):
return 0.3, 0.0, 0.0
-
# 修复5: 传统控制器作为备份(整合障碍物检测)
class TraditionalController:
"""可靠的传统控制逻辑"""
def __init__(self, world, obstacle_detector=None):
-
-# 修复5: 传统控制器作为备份
-class TraditionalController:
- """可靠的传统控制逻辑"""
-
- def __init__(self, world):
-
self.world = world
self.map = world.get_map()
self.waypoint_distance = 10.0
self.last_waypoint = None
-
self.obstacle_detector = obstacle_detector
self.emergency_brake_distance = 6.0
self.safe_following_distance = 10.0
+ # 在 TraditionalController 类的 apply_obstacle_avoidance 方法中,修改以下部分:
def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_info):
- """传统控制器的避障逻辑"""
+ """传统控制器的避障逻辑 - 优化版本"""
if not obstacle_info['has_obstacle']:
return throttle, brake, steer
@@ -421,49 +427,45 @@ def apply_obstacle_avoidance(self, throttle, brake, steer, vehicle, obstacle_inf
# 减速跟随
elif distance < self.safe_following_distance:
# 计算所需的安全距离(基于速度)
- required_distance = max(5.0, vehicle_speed * 0.3) # 0.3秒车距
+ required_distance = max(5.0, vehicle_speed * 0.4) # 增加到0.4秒车距
if distance < required_distance:
# 距离太近,减速
+ speed_ratio = distance / required_distance
if vehicle_speed > 10:
throttle = 0.0
- brake = 0.4
+ brake = 0.6 * (1.0 - speed_ratio)
else:
- throttle = 0.1
+ throttle = 0.2 * speed_ratio
brake = 0.0
# 如果障碍物在正前方,尝试变道
- if abs(angle) < 15:
- # 获取当前车道和相邻车道
+ if abs(angle) < 20: # 放宽角度范围
location = vehicle.get_location()
waypoint = self.map.get_waypoint(location)
- # 尝试获取左侧车道
+ # 检查相邻车道是否可用
left_lane = waypoint.get_left_lane()
right_lane = waypoint.get_right_lane()
+ # 优先选择转向较小的方向
if left_lane and left_lane.lane_type == carla.LaneType.Driving:
- steer = -0.3 # 向左变道
+
+ steer = -0.25
elif right_lane and right_lane.lane_type == carla.LaneType.Driving:
- steer = 0.3 # 向右变道
+ steer = 0.25
else:
- # 没有可用的相邻车道,轻微转向避开
- steer = 0.2 if angle >= 0 else -0.2
+
+ steer = 0.15 if angle >= 0 else -0.15
return throttle, brake, steer
def get_control(self, vehicle):
"""基于路点的传统控制,整合避障"""
-
-
- def get_control(self, vehicle):
- """基于路点的传统控制"""
-
# 获取车辆状态
transform = vehicle.get_transform()
location = vehicle.get_location()
velocity = vehicle.get_velocity()
-
speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2) * 3.6 # km/h
# 获取障碍物信息
@@ -471,9 +473,6 @@ def get_control(self, vehicle):
if self.obstacle_detector:
obstacle_info = self.obstacle_detector.get_obstacle_info()
- speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2)
-
-
# 获取路点
waypoint = self.map.get_waypoint(location, project_to_road=True)
next_waypoints = waypoint.next(self.waypoint_distance)
@@ -501,7 +500,6 @@ def get_control(self, vehicle):
angle = math.atan2(local_y, local_x)
steer = np.clip(angle / math.radians(45), -1.0, 1.0)
-
# 速度控制(基于障碍物距离调整)
throttle = 0.0
brake = 0.0
@@ -556,24 +554,10 @@ def get_control(self, vehicle):
throttle, brake, steer, vehicle, obstacle_info
)
- # 速度控制
- if speed < 5.0: # 18 km/h
- throttle = 0.6
- brake = 0.0
- elif speed < 10.0: # 36 km/h
- throttle = 0.3
- brake = 0.0
- else:
- throttle = 0.1
- brake = 0.1
-
-
return throttle, brake, steer
-
# CARLA初始化部分...
-# CARLA初始化部分保持不变...
# 连接到本地CARLA服务器,端口2000
client = carla.Client('localhost', 2000)
client.set_timeout(15.0)
@@ -616,17 +600,78 @@ def get_control(self, vehicle):
print(f"车辆生成在位置: {spawn_point.location}")
-# 生成障碍物车辆
+# 改进的NPC车辆生成和设置
+print("生成NPC车辆...")
obstacle_count = 3
-for i in range(obstacle_count):
- if i >= len(spawn_points):
- break
- other_vehicles = blueprint_library.filter('vehicle.*')
- other_vehicle_bp = np.random.choice(other_vehicles)
- spawn_idx = (i + 15) % len(spawn_points)
- other_vehicle = world.try_spawn_actor(other_vehicle_bp, spawn_points[spawn_idx])
- if other_vehicle:
- other_vehicle.set_autopilot(True)
+npc_vehicles = [] # 存储NPC车辆以便后续管理
+
+# 获取所有可用的车辆蓝图
+vehicle_blueprints = blueprint_library.filter('vehicle.*')
+
+# 选择远离主车辆的出生点(避免直接堵塞)
+valid_spawn_points = []
+main_spawn_location = spawn_point.location
+
+for point in spawn_points:
+ distance = main_spawn_location.distance(point.location)
+ # 选择距离主车辆50米以上的出生点,避免直接碰撞
+ if distance > 50.0:
+ valid_spawn_points.append(point)
+ if len(valid_spawn_points) >= obstacle_count:
+ break
+
+# 如果找不到足够远的点,使用所有点
+if len(valid_spawn_points) < obstacle_count:
+ print("警告:找不到足够远的出生点,使用所有可用点")
+ valid_spawn_points = spawn_points[:obstacle_count]
+
+# 生成NPC车辆
+for i in range(min(obstacle_count, len(valid_spawn_points))):
+ try:
+ # 随机选择车辆类型(排除特斯拉,使场景更丰富)
+ available_blueprints = [bp for bp in vehicle_blueprints if 'tesla' not in bp.id]
+ if not available_blueprints:
+ available_blueprints = vehicle_blueprints
+
+ npc_bp = random.choice(available_blueprints)
+
+ # 设置车辆颜色
+ if npc_bp.has_attribute('color'):
+ colors = npc_bp.get_attribute('color').recommended_values
+ if colors:
+ npc_bp.set_attribute('color', random.choice(colors))
+
+ # 生成车辆
+ npc_vehicle = world.try_spawn_actor(npc_bp, valid_spawn_points[i])
+
+ if npc_vehicle:
+ # 启用自动驾驶模式并设置速度限制
+ npc_vehicle.set_autopilot(True)
+
+ # 设置NPC车辆的速度限制(让它们能正常行驶)
+ traffic_manager = client.get_trafficmanager()
+ traffic_manager.set_global_distance_to_leading_vehicle(2.5) # 设置跟车距离
+ traffic_manager.set_random_device_seed(12345) # 设置随机种子确保一致性
+
+ # 为每个NPC车辆设置不同的速度限制(避免所有车速度相同导致堵塞)
+ target_speed = random.uniform(30.0, 50.0) # 30-50 km/h
+ traffic_manager.vehicle_percentage_speed_difference(npc_vehicle, random.uniform(-10, 10))
+
+ # 设置NPC车辆的驾驶行为(更安全、更智能)
+ traffic_manager.auto_lane_change(npc_vehicle, True) # 允许自动变道
+ traffic_manager.distance_to_leading_vehicle(npc_vehicle, 3.0) # 设置跟车距离
+ traffic_manager.collision_detection(npc_vehicle, world, True) # 启用碰撞检测
+
+ npc_vehicles.append(npc_vehicle)
+ print(f"生成NPC车辆 {i + 1}: {npc_bp.id} 在位置 {valid_spawn_points[i].location}")
+
+ # 短暂暂停,避免生成时的碰撞
+ time.sleep(0.1)
+
+ except Exception as e:
+ print(f"生成NPC车辆 {i + 1} 时出错: {e}")
+
+print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆")
# 配置传感器(简化配置)
third_camera_bp = blueprint_library.find('sensor.camera.rgb')
@@ -661,7 +706,6 @@ def third_camera_callback(image):
third_image = array[:, :, :3]
-
def front_camera_callback(image):
global front_image
array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
@@ -694,33 +738,6 @@ def front_camera_callback(image):
# 障碍物信息历史
obstacle_history = deque(maxlen=5)
-
-def front_camera_callback(image):
- global front_image
- array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
- array = np.reshape(array, (image.height, image.width, 4))
- front_image = array[:, :, :3]
-
-
-third_camera.listen(third_camera_callback)
-front_camera.listen(front_camera_callback)
-
-time.sleep(2.0)
-
-# 修复6: 初始化控制器
-nn_controller = ImprovedNeuralController()
-traditional_controller = TraditionalController(world)
-
-# 控制变量
-throttle = 0.3 # 更保守的初始油门
-steer = 0.0
-brake = 0.0
-NEURAL_NETWORK_MODE = False # 默认使用传统控制,更稳定
-
-# 转向历史,用于平滑
-steer_history = deque(maxlen=10)
-
-
print("初始化车辆状态...")
vehicle.set_simulate_physics(True)
@@ -733,31 +750,65 @@ def front_camera_callback(image):
hand_brake=False
))
+
+# 添加NPC车辆管理函数
+def check_and_reset_stuck_npcs():
+ """检查并重置卡住的NPC车辆"""
+ for npc in npc_vehicles:
+ try:
+ npc_speed = math.sqrt(
+ npc.get_velocity().x ** 2 + npc.get_velocity().y ** 2 + npc.get_velocity().z ** 2) * 3.6
+ # 如果NPC车辆速度过低(小于1km/h)且没有碰撞,可能是卡住了
+ if npc_speed < 1.0:
+ print(f"检测到NPC车辆 {npc.id} 可能卡住,尝试重置...")
+ # 获取当前位置
+ current_transform = npc.get_transform()
+ # 寻找最近的可用出生点
+ closest_point = None
+ min_distance = float('inf')
+ for point in spawn_points:
+ distance = point.location.distance(current_transform.location)
+ if distance < min_distance and distance > 10.0: # 避免重置到太近的位置
+ min_distance = distance
+ closest_point = point
+
+ if closest_point:
+ npc.set_transform(closest_point)
+ npc.set_autopilot(True)
+ print(f"重置NPC车辆 {npc.id} 到新位置")
+ except:
+ pass
+
+
try:
print("自动驾驶系统启动 - 初始模式: 传统控制")
print("控制键: q-退出, m-切换控制模式, r-重置车辆, t-传统模式, n-神经网络模式")
+ print(f"当前有 {len(npc_vehicles)} 辆NPC车辆在运行")
frame_count = 0
stuck_count = 0
last_position = vehicle.get_location()
success_count = 0 # 成功运行计数器
-
collision_count = 0 # 碰撞计数器
last_collision_time = 0 # 上次碰撞时间
-
+ last_npc_check_time = 0 # 上次检查NPC的时间
# 主循环
while True:
world.tick()
frame_count += 1
+ # 定期检查NPC车辆状态(每100帧检查一次)
+ if frame_count - last_npc_check_time > 100:
+ check_and_reset_stuck_npcs()
+ last_npc_check_time = frame_count
+
# 获取车辆状态
vehicle_transform = vehicle.get_transform()
vehicle_location = vehicle.get_location()
vehicle_velocity = vehicle.get_velocity()
vehicle_speed = math.sqrt(vehicle_velocity.x ** 2 + vehicle_velocity.y ** 2 + vehicle_velocity.z ** 2)
-
# 检测障碍物
obstacle_info = obstacle_detector.get_obstacle_info()
obstacle_history.append(obstacle_info)
@@ -772,11 +823,6 @@ def front_camera_callback(image):
f"模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}, "
f"障碍物={'有' if obstacle_info['has_obstacle'] else '无'}")
-
- print(
- f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, 模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}")
-
-
# 修复8: 改进的卡住检测
current_position = vehicle_location
distance_moved = current_position.distance(last_position)
@@ -789,15 +835,6 @@ def front_camera_callback(image):
stuck_count = 0
success_count += 1 # 成功运行一帧
-
- last_position = current_position
-
- # 修复9: 更智能的卡住恢复
- if stuck_count > 15: # 1.5秒后认为卡住
- print("检测到车辆卡住,执行恢复程序...")
-
-
-
last_position = current_position
# 修复9: 更智能的卡住恢复
@@ -810,7 +847,6 @@ def front_camera_callback(image):
))
time.sleep(0.5)
-
# 检查是否有前方障碍物
if obstacle_info['has_obstacle'] and obstacle_info['distance'] < 10:
print("前方有障碍物,尝试倒车...")
@@ -849,37 +885,6 @@ def front_camera_callback(image):
# 记录转向历史
steer_history.append(steer)
-
-
- # 然后尝试不同方向的脱困
- recovery_steer = random.choice([-0.5, 0.5]) # 随机选择方向
- vehicle.apply_control(carla.VehicleControl(
- throttle=0.8, steer=recovery_steer, brake=0.0, hand_brake=False
- ))
- time.sleep(1.0)
-
- stuck_count = 0
- success_count = 0
-
- # 每成功运行100帧显示一次状态
- if success_count % 100 == 0:
- print(f"已成功运行 {success_count} 帧")
-
- # 控制逻辑
- if NEURAL_NETWORK_MODE:
- # 神经网络控制
- nn_throttle, nn_brake, nn_steer = nn_controller.get_control(
- front_image, vehicle_speed, steer_history
- )
-
- # 修复10: 更激进的控制平滑
- throttle = 0.3 * throttle + 0.7 * nn_throttle
- brake = 0.3 * brake + 0.7 * nn_brake
- steer = 0.2 * steer + 0.8 * nn_steer
-
- # 记录转向历史
- steer_history.append(steer)
-
else:
# 传统控制 - 更稳定
throttle, brake, steer = traditional_controller.get_control(vehicle)
@@ -900,7 +905,6 @@ def front_camera_callback(image):
if third_image is not None:
display_image = third_image.copy()
-
# 可视化障碍物检测结果
display_image = obstacle_detector.visualize_obstacles(display_image, vehicle_transform)
@@ -910,7 +914,6 @@ def front_camera_callback(image):
cv2.putText(display_image, f"Mode: {'Neural' if NEURAL_NETWORK_MODE else 'Traditional'}",
(10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
cv2.putText(display_image, f"Throttle: {throttle:.2f}", (10, 90),
-
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
@@ -935,20 +938,6 @@ def front_camera_callback(image):
cv2.imshow('自动驾驶系统 - 带障碍物检测', display_image)
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
- cv2.putText(display_image, f"Steer: {steer:.2f}", (10, 120),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
- cv2.putText(display_image, f"Brake: {brake:.2f}", (10, 150),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
-
- # 卡住警告
- if stuck_count > 5:
- cv2.putText(display_image, "STUCK DETECTED!", (10, 180),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
-
- cv2.imshow('自动驾驶系统 - 修复版', display_image)
-
-
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
break
@@ -969,15 +958,11 @@ def front_camera_callback(image):
brake = 1.0
stuck_count = 0
success_count = 0
-
collision_count = 0
steer_history.clear()
obstacle_history.clear()
print("车辆已重置")
- steer_history.clear()
-
-
time.sleep(0.01)
except KeyboardInterrupt:
@@ -996,7 +981,10 @@ def front_camera_callback(image):
# 销毁actor
for actor in world.get_actors():
if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('sensor.'):
- actor.destroy()
+ try:
+ actor.destroy()
+ except:
+ pass
# 恢复设置
settings.synchronous_mode = False
diff --git a/src/carla_slam_gmapping/README.md b/src/carla_slam_gmapping/README.md
index 3973b3231a..abb80cebcb 100644
--- a/src/carla_slam_gmapping/README.md
+++ b/src/carla_slam_gmapping/README.md
@@ -1,8 +1,8 @@
# 人车模拟
-# CARLA 全局路径规划节点
+# CARLA 全局路径规划和SLAM节点
实现基于 CARLA 模拟器的车辆全局路径规划功能,支持从车辆自身位置(通过里程计输入)到地图中随机目标点的路径生成,并通过 ROS 2 话题可视化规划结果,模拟用户预订行程后车辆的全局路径规划过程。
-
+新增支持 3D 建图功能,集成 GMapping 算法实现三维环境下的地图构建。
## 功能概述
该节点是 CARLA 自动驾驶仿真系统中的核心模块之一,主要功能包括:
@@ -10,7 +10,7 @@
- **路径规划服务**:提供 ROS 2 服务 `plan_to_random_goal`,接收起始点里程计信息,生成到随机目标点的全局路径
- **路径可视化**:通过 `visualization_marker` 话题发布路径标记,在 RViz 中以绿色线条直观展示规划结果
- **鲁棒性保障**:包含路径有效性校验(确保路径长度满足需求)、异常捕获和错误日志输出,避免无限循环或崩溃
-
+- **3D 建图**:集成 GMapping 算法,利用激光雷达数据进行实时三维地图构建
## 环境配置
### 基础环境
@@ -45,8 +45,11 @@
```bash
sudo apt install -y python3-colcon-common-extensions
```
-
-
+5. **GMapping 依赖** 安装 slam_gmapping 包以支持 3D 建图功能:
+'''
+bash
+sudo apt install ros-foxy-slam-gmapping
+'''
## 快速开始
### 1. 工作空间准备
假设你的 ROS 2 工作空间为 `carla_ws`,将该节点所在的功能包 `carla_global_planner` 放入 `src` 目录:
@@ -83,10 +86,11 @@ cd ~/carla_ws
# 激活 ROS 2 环境和工作空间环境
source /opt/ros/foxy/setup.bash
source install/setup.bash
-# 启动节点
-ros2 run carla_global_planner carla_global_planner_node
+# 启动节点和建图功能
+ros2 launch carla_global_planner carla_slam_launch.py
```
- 节点启动成功后,会输出日志 `CARLA全局路径规划服务已启动`
+- GMapping 建图节点同时启动,开始接收激光雷达数据并构建地图
- 若脚本卡住无报错,说明节点正常运行(等待服务请求);若报错 "CARLA客户端初始化失败",需检查 CARLA 服务器是否已启动,或连接地址/端口是否正确
#### 步骤 3:调用路径规划服务
@@ -101,7 +105,7 @@ ros2 service call /plan_to_random_goal carla_global_planner/srv/PlanGlobalPath "
- 服务调用成功后,节点会输出路径信息(如 `成功规划路径,包含XX个路点`)
#### 步骤 4:可视化路径
-打开新终端,启动 RViz 查看路径:
+打开新终端,启动 RViz 查看路径和建图结果:
```bash
source /opt/ros/foxy/setup.bash
rviz2
@@ -110,7 +114,32 @@ rviz2
1. 设置 `Fixed Frame` 为 `map`
2. 添加 `Marker` 显示项,设置 `Topic` 为 `/visualization_marker`
3. 路径会以绿色线条显示在场景中
-
+4. 添加 Map 显示项,设置 Topic 为 /map 查看实时构建的地图
+
+3D 建图算法说明
+GMapping 算法原理
+本项目采用经典的 GMapping 算法实现 3D 建图功能。GMapping 是一种基于粒子滤波的 SLAM( simultaneous localization and mapping)算法,它通过以下步骤实现地图构建:
+
+粒子滤波: 使用多个粒子表示机器人的可能位姿,每个粒子都携带一个地图副本
+运动模型更新: 根据里程计数据预测每个粒子的新位姿
+观测模型评估: 利用激光雷达扫描数据评估每个粒子的权重
+重采样: 根据权重重新分布粒子集,保留高似然的粒子
+地图更新: 基于最优粒子的位姿估计更新全局地图
+数据流
+输入传感器数据:
+激光雷达扫描数据 (/scan)
+里程计数据 (/odom)
+输出:
+2D 占据栅格地图 (/map)
+机器人位姿估计 (/map 坐标系下的变换)
+参数配置
+GMapping 算法的关键参数已在 launch 文件中预设,可根据具体场景调整:
+
+maxUrange: 激光雷达最大使用距离 (单位: 米)
+sigma: 扫描匹配的标准差
+particles: 粒子数量
+delta: 地图分辨率 (单位: 米/cell)
+temporalUpdate: 地图更新频率
## 核心接口说明
### 1. ROS 2 服务
@@ -131,9 +160,12 @@ nav_msgs/Path path
```
### 2. ROS 2 话题
-| 话题名称 | 消息类型 | 功能描述 |
-|------------------------|-----------------------------------|-------------------------------------------|
-| `/visualization_marker`| `visualization_msgs/Marker` | 发布路径可视化标记(绿色 LINE_STRIP 类型) |
+话题名称 消息类型 功能描述
+/visualization_marker visualization_msgs/Marker 发布路径可视化标记(绿色 LINE_STRIP 类型)
+/scan sensor_msgs/LaserScan 激光雷达扫描数据,用于建图
+/odom nav_msgs/Odometry 里程计数据,用于定位和建图
+/map nav_msgs/OccupancyGrid 构建的 2D 占据栅格地图
+
## 常见问题排查
@@ -157,6 +189,11 @@ nav_msgs/Path path
4. **编译报错 "找不到 carla_global_planner/srv/PlanGlobalPath"**
- 确认 `srv` 目录下已定义 `PlanGlobalPath.srv` 文件,且 `CMakeLists.txt`/`package.xml` 已正确配置服务依赖
+5. **RViz 中看不到地图**
+
+确认 Map 显示项已添加且 Topic 设置为 /map
+检查是否有地图数据发布:ros2 topic echo /map
+确保激光雷达数据正常:ros2 topic echo /scan
## 代码结构说明
@@ -168,6 +205,8 @@ carla_global_planner/
│ └── PlanGlobalPath.srv # 路径规划服务定义
├── utilities/
│ └── planner.py # 路径计算工具(compute_route_waypoints 函数)
+├── launch/
+│ └── carla_slam_launch.py # 启动文件,包含建图功能
├── CMakeLists.txt # 编译配置
└── package.xml # 依赖和包信息配置
```
@@ -186,10 +225,13 @@ carla_global_planner/
- 关键逻辑变更需同步更新 README.md
2. **功能扩展建议**
- - 支持自定义目标点(而非仅随机目标点)
- - 增加路径规划算法选择(如 A*、RRT*)
- - 添加路径平滑处理,优化车辆行驶轨迹
- - 增加单元测试(基于 `pytest` 和 `ros2 test`)
+ 支持自定义目标点(而非仅随机目标点)
+ 增加路径规划算法选择(如 A*、RRT*)
+ 添加路径平滑处理,优化车辆行驶轨迹
+ 增加单元测试(基于 pytest 和 ros2 test)
+ 集成其他 SLAM 算法(如 Hector SLAM、Karto SLAM)以提供更多建图选项
+
+
3. **提交流程**
准备提交代码前,请确保:
@@ -202,4 +244,5 @@ carla_global_planner/
1. [CARLA 官方文档](https://carla.readthedocs.io/en/0.9.15/) - 了解 CARLA Python API 和地图数据结构
2. [ROS 2 Foxy 官方文档](https://docs.ros.org/en/foxy/) - 学习 ROS 2 服务、话题和节点开发
3. [nav_msgs/Path 消息定义](https://docs.ros.org/en/foxy/api/nav_msgs/html/msg/Path.html) - 路径消息格式说明
-4. [visualization_msgs/Marker 消息定义](https://docs.ros.org/en/foxy/api/visualization_msgs/html/msg/Marker.html) - 可视化标记格式说明
\ No newline at end of file
+4. [visualization_msgs/Marker 消息定义](https://docs.ros.org/en/foxy/api/visualization_msgs/html/msg/Marker.html) - 可视化标记格式说明
+5. slam_gmapping 官方文档 - GMapping 算法详细说明
\ No newline at end of file
diff --git a/src/carla_slam_gmapping/main.py b/src/carla_slam_gmapping/main.py
index 1e6eb639dd..bad0e3a8a1 100644
--- a/src/carla_slam_gmapping/main.py
+++ b/src/carla_slam_gmapping/main.py
@@ -3,11 +3,7 @@
CARLA全局路径规划节点 - 增强版
提供从起始点到随机目标点的路径规划服务,并将规划结果通过ROS消息发布和可视化
新增功能:
-- 路径长度估算
-- 连接状态监控
-- 改进的错误处理
-- 性能统计
-- 配置参数化
+
"""
import rclpy
@@ -23,6 +19,7 @@
import time # 新增:用于性能统计
import math # 新增:用于距离计算
from tf_transformations import quaternion_from_euler
+from typing import Optional, Tuple, List, Any
class GlobalPlannerNode(Node):
@@ -134,63 +131,74 @@ def _publish_performance_stats(self):
def plan_path_cb(self, request, response):
"""
- 路径规划服务回调函数 - 增强版
- 接收起始点,规划到随机目标点的路径并返回
- 新增性能统计和更好的错误处理
+
+ 不足:原方法过长,包含多个职责,难以维护和测试。
+ 优化:拆分为子方法,提高可读性和可维护性。
"""
- start_time = time.time() # 新增:开始计时
+ start_time = time.time()
- # 新增:检查CARLA连接状态
- if not self.carla_connected or not self.map:
- self.get_logger().error("CARLA连接未建立或地图未初始化,无法规划路径")
- response.success = False # 假设服务定义中有success字段
+ if not self._validate_connection(response):
return response
try:
- # 将ROS坐标转换为CARLA坐标
- start_location = self._ros_to_carla_location(request.start)
- start_wp = self.map.get_waypoint(start_location)
-
- if not start_wp: # 新增:检查起始路点有效性
+ start_wp = self._get_start_waypoint(request.start)
+ if not start_wp:
self.get_logger().error("无法在起始位置找到有效的路点")
return response
- # 规划足够长的路径
- min_waypoints = self.get_parameter('min_waypoints').value
- max_attempts = self.get_parameter('max_planning_attempts').value
-
- route, goal_wp = self._get_valid_route(start_wp, min_waypoints, max_attempts)
+ route, goal_wp = self._plan_route(start_wp)
if not route:
self.get_logger().error("无法生成有效的路径")
return response
- # 新增:计算路径总长度
- total_distance = self._calculate_path_distance(route)
-
- # 构建路径消息并可视化
- path_msg = self._build_path_message(route)
- self._visualize_path_and_goal(path_msg, goal_wp) # 修改:同时可视化路径和目标点
-
+ path_msg = self._process_route(route, goal_wp)
response.path = path_msg
- # 新增:更新性能统计
- planning_time = time.time() - start_time
- self.last_planning_time = planning_time
- self.total_planning_time += planning_time
- self.planning_count += 1
+ self._update_performance_stats(time.time() - start_time, len(route), self._calculate_path_distance(route))
- self.get_logger().info(
- f"成功规划路径 - "
- f"路点数: {len(route)}, "
- f"总距离: {total_distance:.2f}m, "
- f"用时: {planning_time:.3f}s"
- )
return response
except Exception as e:
self.get_logger().error(f"路径规划过程中发生错误: {str(e)}")
return response
+ def _validate_connection(self, response) -> bool:
+ """验证CARLA连接状态"""
+ if not self.carla_connected or not self.map:
+ self.get_logger().error("CARLA连接未建立或地图未初始化,无法规划路径")
+ response.success = False
+ return False
+ return True
+
+ def _get_start_waypoint(self, start_msg) -> Optional[carla.Waypoint]:
+ """获取起始路点"""
+ start_location = self._ros_to_carla_location(start_msg)
+ return self.map.get_waypoint(start_location)
+
+ def _plan_route(self, start_wp: carla.Waypoint) -> Tuple[Optional[List], Optional[carla.Waypoint]]:
+ """规划路径"""
+ min_waypoints = self.get_parameter('min_waypoints').value
+ max_attempts = self.get_parameter('max_planning_attempts').value
+ return self._get_valid_route(start_wp, min_waypoints, max_attempts)
+
+ def _process_route(self, route: List, goal_wp: Optional[carla.Waypoint]) -> Path:
+ """处理路径:构建消息并可视化"""
+ path_msg = self._build_path_message(route)
+ self._visualize_path_and_goal(path_msg, goal_wp)
+ return path_msg
+
+ def _update_performance_stats(self, planning_time: float, waypoint_count: int, total_distance: float):
+ """更新性能统计"""
+ self.last_planning_time = planning_time
+ self.total_planning_time += planning_time
+ self.planning_count += 1
+ self.get_logger().info(
+ f"成功规划路径 - "
+ f"路点数: {waypoint_count}, "
+ f"总距离: {total_distance:.2f}m, "
+ f"用时: {planning_time:.3f}s"
+ )
+
def _ros_to_carla_location(self, odom_msg):
"""将ROS里程计消息中的位置转换为CARLA坐标"""
return carla.Location(
@@ -201,7 +209,7 @@ def _ros_to_carla_location(self, odom_msg):
def _get_valid_route(self, start_wp, min_waypoints=50, max_attempts=10):
"""
- 获取有效的路径 - 增强版
+ 获取有效的路径 -
返回路径和目标路点元组
尝试多次生成路径,直到满足最小路点数量要求或达到最大尝试次数
"""
@@ -245,7 +253,6 @@ def _get_valid_route(self, start_wp, min_waypoints=50, max_attempts=10):
if len(route) >= min_waypoints:
return route, goal_wp # 找到满足要求的路径,直接返回
- # 返回最佳路径(即使不满足最小长度要求)
if best_route:
self.get_logger().warning(
f"达到最大尝试次数({max_attempts}),返回最长路径 "
@@ -362,10 +369,10 @@ def _create_path_marker(self, header, action=Marker.ADD, points=None, line_width
marker.color.g = 1.0 # 绿色
marker.color.b = 0.0
- # 新增:设置生存时间,自动清理旧标记
+
marker.lifetime.sec = 60 # 60秒后自动删除
- # 添加点集(仅用于ADD动作)
+
if points and action == Marker.ADD:
marker.points = points
diff --git a/src/carla_tracking/README.md b/src/carla_tracking/README.md
new file mode 100644
index 0000000000..952f4185f4
--- /dev/null
+++ b/src/carla_tracking/README.md
@@ -0,0 +1,146 @@
+# CARLA_YOLO_ObjectTracking
+
+## 项目概述
+本项目实现了基于YOLOv5算法和CARLA仿真器的自动驾驶目标检测与跟踪功能。通过CARLA仿真环境生成动态交通场景,利用YOLOv5模型实时检测车辆、摩托车、公交车、卡车等交通参与者,结合内置SORT跟踪算法实现目标的连续追踪,并通过OpenCV实时可视化检测与跟踪结果。
+
+### 核心功能:
+⦁ 基于YOLOv5模型(支持yolov5s、yolov5m)进行实时目标检测,默认置信度阈值0.5
+⦁ 集成SORT跟踪算法(内置实现,无需额外依赖),实现对检测目标的稳定跟踪,支持多目标ID持续标注
+⦁ 与CARLA仿真器深度集成,自动生成主车辆和NPC交通流,挂载RGB相机采集实时图像
+⦁ 支持同步模式运行,保证检测帧率与仿真环境一致(默认0.05s/帧)
+⦁ 实时可视化检测/跟踪结果,显示目标类别、置信度及跟踪ID
+
+## 安装步骤
+
+### 前置条件:
+⦁ Python 3.7+(推荐3.7,与CARLA兼容性更佳)
+⦁ CARLA仿真器(支持0.9.x系列版本,版本需与Python API匹配)
+⦁ NVIDIA显卡(支持CUDA 11.3+,可选,用于加速模型推理)
+
+### 操作步骤:
+
+1. 下载并安装CARLA仿真器:
+```plaintext
+https://github.com/carla-simulator/carla/releases
+```
+选择适合操作系统的版本,解压至任意路径(如D:\CARLA)
+
+2. 克隆或下载本项目代码:
+```bash
+git clone <项目仓库地址>
+cd CARLA_YOLO_ObjectTracking
+```
+
+3. 安装Python依赖库:
+```bash
+# 安装PyTorch(支持CUDA加速)
+pip install torch torchvision --extra-index-url https://download.pytorch.org/whl/cu113
+
+# 安装YOLOv5依赖
+pip install ultralytics
+
+# 安装其他依赖
+pip install numpy opencv-python scipy carla
+
+# 若使用CPU版本PyTorch
+pip install torch torchvision
+```
+
+4. 安装CARLA Python API:
+```bash
+# 进入CARLA安装目录下的PythonAPI路径
+cd D:\CARLA\PythonAPI\carla\dist
+# 安装对应版本的egg文件(需与Python版本匹配)
+easy_install carla-<版本号>-py3.7-<平台>.egg
+```
+
+5. 下载YOLOv5预训练模型:
+通过百度网盘链接下载模型权重:
+```plaintext
+链接:https://pan.baidu.com/s/13huHiEmjYYekRDs08elz5Q?pwd=5wg9
+```
+下载后将模型文件(yolov5s.pt/yolov5m.pt)放在项目根目录
+
+## 使用方法
+
+1. 启动CARLA仿真器:
+```bash
+cd D:\CARLA
+# Windows系统
+CarlaUE4.exe
+# Linux系统
+./CarlaUE4.sh
+```
+等待地图加载完成(显示3D城市场景)
+
+2. 运行主程序:
+```bash
+# 基础运行(默认yolov5s模型+SORT跟踪器)
+python main.py
+
+# 可选参数配置
+python main.py--model yolov5m--tracker sort--host localhost--port 2000
+```
+
+3. 操作指令:
+⦁ 按`q`键:退出程序
+⦁ 程序运行时会自动生成主车辆、NPC车辆及相机,实时显示检测跟踪结果
+
+## 功能说明
+
+1. 目标检测:
+⦁ 支持检测类别:汽车(car)、摩托车(motorcycle)、公交车(bus)、卡车(truck)(对应COCO数据集ID:2、3、5、7)
+⦁ 模型选择:yolov5s(轻量快速)、yolov5m(精度更高,速度稍慢)
+⦁ 检测结果包含边界框、类别名称及置信度
+
+2. 目标跟踪:
+⦁ 采用内置SORT跟踪算法,通过卡尔曼滤波预测目标运动轨迹
+⦁ 基于IOU匹配实现检测框与跟踪目标的关联
+⦁ 跟踪结果包含持续的目标ID,支持目标消失后重新出现的ID匹配
+⦁ 核心参数可配置:max_age(最大消失帧数)、min_hits(最小命中数)、iou_threshold(IOU匹配阈值)
+
+3. CARLA交互:
+⦁ 自动清理仿真环境中的残留车辆和静态车辆,保证场景纯净
+⦁ 主车辆开启自动驾驶模式,相机挂载于主车辆前方(x=1.5m,z=2.0m)
+⦁ 自动生成NPC车辆并开启自动驾驶,构建动态交通场景(默认生成10辆)
+⦁ spectator视角自动跟随主车辆(后上方5m,俯角30°),方便观察全局场景
+⦁ 同步模式运行,固定帧率0.05s/帧(20FPS),保证检测稳定性
+
+## 项目结构
+⦁ main.py:主程序脚本,包含CARLA连接、目标检测、跟踪及可视化逻辑
+⦁ yolov5s.pt/yolov5m.pt:YOLOv5预训练模型(需手动下载至项目根目录)
+⦁ 内置核心组件:
+ ⦁ KalmanFilter:卡尔曼滤波器实现,用于目标运动状态预测与更新
+ ⦁ Track:跟踪目标实体类,维护单目标的边界框、ID、生命周期等信息
+ ⦁ Sort:SORT跟踪器核心实现,处理多目标匹配与跟踪状态管理
+ ⦁ 工具函数:包含边界框绘制、相机投影矩阵构建、CARLA环境清理等功能
+
+## 常见问题
+
+### CARLA连接失败:
+⦁ 确保CARLA仿真器已启动,且端口与脚本参数一致(默认2000)
+⦁ 检查CARLA版本与Python API版本是否匹配
+⦁ 关闭防火墙或添加端口例外规则
+
+### 模型加载错误:
+⦁ 确认模型文件已下载并放在项目根目录
+⦁ 检查模型文件名与代码中指定的名称一致
+⦁ 网络问题导致模型自动下载失败时,可手动下载并放置到指定路径
+
+### 性能问题:
+⦁ 检测速度慢:可切换为yolov5s模型,或降低相机分辨率(代码中修改camera_bp参数)
+⦁ 仿真卡顿:减少NPC车辆数量(修改spawn_npcs函数的count参数)
+⦁ 内存占用高:关闭不必要的后台程序,或减小批量处理规模
+
+### 可视化窗口无响应:
+⦁ 确保OpenCV版本兼容(推荐4.5.x系列)
+⦁ 检查是否存在未处理的异常导致主循环中断
+⦁ 尝试重新启动CARLA仿真器和程序
+
+## 参考文档
+⦁ [CARLA官方文档](https://carla.readthedocs.io/)
+⦁ [YOLOv5官方文档](https://docs.ultralytics.com/yolov5/)
+⦁ [SORT跟踪算法论文](https://arxiv.org/abs/1602.00763)
+
+## 许可协议
+本项目基于MIT许可协议开源,详情参见LICENSE文件。
\ No newline at end of file
diff --git a/src/carla_tracking/main.py b/src/carla_tracking/main.py
new file mode 100644
index 0000000000..ac50158755
--- /dev/null
+++ b/src/carla_tracking/main.py
@@ -0,0 +1,472 @@
+import argparse
+import carla
+import os
+import queue
+import random
+import cv2
+import numpy as np
+import torch
+from collections import deque
+
+
+# -------------------------- 内置 SORT 跟踪器(无需安装任何依赖) --------------------------
+class KalmanFilter:
+ def __init__(self):
+ self.dt = 1.0
+ self.x = np.zeros((4, 1)) # [x, y, vx, vy]
+ self.F = np.array([[1, 0, self.dt, 0],
+ [0, 1, 0, self.dt],
+ [0, 0, 1, 0],
+ [0, 0, 0, 1]])
+ self.H = np.array([[1, 0, 0, 0],
+ [0, 1, 0, 0]])
+ self.P = np.eye(4) * 1000
+ self.Q = np.eye(4) * 0.01
+ self.R = np.eye(2) * 10
+
+ def predict(self):
+ self.x = np.dot(self.F, self.x)
+ self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
+ return self.x[:2]
+
+ def update(self, z):
+ y = z - np.dot(self.H, self.x)
+ S = np.dot(np.dot(self.H, self.P), self.H.T) + self.R
+ K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
+ self.x = self.x + np.dot(K, y)
+ self.P = self.P - np.dot(np.dot(K, self.H), self.P)
+ return self.x[:2]
+
+
+class Track:
+ def __init__(self, box, track_id):
+ self.id = track_id
+ self.kf = KalmanFilter()
+ self.x1, self.y1, self.x2, self.y2 = box
+ self.center = np.array([[(self.x1 + self.x2) / 2], [(self.y1 + self.y2) / 2]])
+ self.kf.x[:2] = self.center
+ self.width = self.x2 - self.x1
+ self.height = self.y2 - self.y1
+ self.hits = 1
+ self.age = 0
+
+ def predict(self):
+ self.age += 1
+ center = self.kf.predict()
+ self.x1 = center[0, 0] - self.width / 2
+ self.y1 = center[1, 0] - self.height / 2
+ self.x2 = self.x1 + self.width
+ self.y2 = self.y1 + self.height
+ return [self.x1, self.y1, self.x2, self.y2]
+
+ def update(self, box):
+ self.x1, self.y1, self.x2, self.y2 = box
+ self.center = np.array([[(self.x1 + self.x2) / 2], [(self.y1 + self.y2) / 2]])
+ self.kf.update(self.center)
+ self.width = self.x2 - self.x1
+ self.height = self.y2 - self.y1
+ self.hits += 1
+ self.age = 0
+
+ def get_box(self):
+ return [self.x1, self.y1, self.x2, self.y2]
+
+
+class Sort:
+ def __init__(self, max_age=3, min_hits=2, iou_threshold=0.3):
+ self.max_age = max_age
+ self.min_hits = min_hits
+ self.iou_threshold = iou_threshold
+ self.tracks = []
+ self.next_id = 1
+
+ def update(self, detections):
+ if len(detections) == 0:
+ for track in self.tracks:
+ track.age += 1
+ self.tracks = [t for t in self.tracks if t.age <= self.max_age]
+ return np.array([[t.x1, t.y1, t.x2, t.y2, t.id] for t in self.tracks if t.hits >= self.min_hits])
+
+ if len(self.tracks) == 0:
+ for det in detections:
+ self.tracks.append(Track(det[:4], self.next_id))
+ self.next_id += 1
+ return np.array([[t.x1, t.y1, t.x2, t.y2, t.id] for t in self.tracks])
+
+ track_boxes = np.array([t.get_box() for t in self.tracks])
+ iou_matrix = self._iou_batch(track_boxes, detections[:, :4])
+
+ matches, unmatched_tracks, unmatched_dets = self._hungarian_algorithm(iou_matrix)
+
+ for track_idx, det_idx in matches:
+ if iou_matrix[track_idx, det_idx] >= self.iou_threshold:
+ self.tracks[track_idx].update(detections[det_idx][:4])
+
+ for track_idx in unmatched_tracks:
+ self.tracks[track_idx].age += 1
+
+ for det_idx in unmatched_dets:
+ self.tracks.append(Track(detections[det_idx][:4], self.next_id))
+ self.next_id += 1
+
+ self.tracks = [t for t in self.tracks if t.age <= self.max_age]
+ return np.array([[t.x1, t.y1, t.x2, t.y2, t.id] for t in self.tracks if t.hits >= self.min_hits])
+
+ def _iou_batch(self, b1, b2):
+ b1_x1, b1_y1, b1_x2, b1_y2 = b1[:, 0], b1[:, 1], b1[:, 2], b1[:, 3]
+ b2_x1, b2_y1, b2_x2, b2_y2 = b2[:, 0], b2[:, 1], b2[:, 2], b2[:, 3]
+
+ inter_x1 = np.maximum(b1_x1[:, None], b2_x1[None, :])
+ inter_y1 = np.maximum(b1_y1[:, None], b2_y1[None, :])
+ inter_x2 = np.minimum(b1_x2[:, None], b2_x2[None, :])
+ inter_y2 = np.minimum(b1_y2[:, None], b2_y2[None, :])
+
+ inter_area = np.maximum(0, inter_x2 - inter_x1) * np.maximum(0, inter_y2 - inter_y1)
+ b1_area = (b1_x2 - b1_x1) * (b1_y2 - b1_y1)
+ b2_area = (b2_x2 - b2_x1) * (b2_y2 - b2_y1)
+
+ return inter_area / (b1_area[:, None] + b2_area[None, :] - inter_area + 1e-6)
+
+ def _hungarian_algorithm(self, cost_matrix):
+ from scipy.optimize import linear_sum_assignment
+ row_ind, col_ind = linear_sum_assignment(-cost_matrix)
+ matches = np.array(list(zip(row_ind, col_ind)))
+ unmatched_tracks = [i for i in range(cost_matrix.shape[0]) if i not in matches[:, 0]]
+ unmatched_dets = [i for i in range(cost_matrix.shape[1]) if i not in matches[:, 1]]
+ return matches, unmatched_tracks, unmatched_dets
+
+
+# -------------------------- YOLOv5 检测模型(需安装 ultralytics) --------------------------
+from ultralytics import YOLO
+
+
+# -------------------------- 核心工具函数(内置,无需创建文件夹) --------------------------
+def draw_bounding_boxes(image, boxes, labels, class_names, track_ids=None, probs=None):
+ """绘制检测/跟踪框(含类别、置信度、跟踪ID)"""
+ for i, (box, label) in enumerate(zip(boxes, labels)):
+ x1, y1, x2, y2 = map(int, box)
+ # 绘制矩形框
+ cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2)
+ # 拼接文本信息
+ conf_text = f"{probs[i]:.2f}" if (probs is not None and i < len(probs)) else ""
+ label_text = f"{class_names[label]} {conf_text}".strip()
+ if track_ids and i < len(track_ids):
+ label_text += f"_ID:{track_ids[i]}"
+ # 绘制文本背景(避免遮挡)
+ text_size = cv2.getTextSize(label_text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2)[0]
+ cv2.rectangle(image, (x1, y1 - text_size[1] - 5), (x1 + text_size[0], y1), (0, 255, 0), -1)
+ cv2.putText(image, label_text, (x1, y1 - 3), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
+ return image
+
+
+def build_projection_matrix(w, h, fov):
+ """构建相机投影矩阵"""
+ focal = w / (2.0 * np.tan(fov * np.pi / 360.0))
+ return np.array([[focal, 0, w / 2], [0, focal, h / 2], [0, 0, 1]])
+
+
+def clear_npc(world):
+ """清理所有NPC车辆"""
+ if not world:
+ return
+ for actor in world.get_actors().filter('vehicle.*'):
+ try:
+ # 老版本CARLA无role_name属性,直接销毁非自车车辆
+ actor.destroy()
+ except:
+ pass
+
+
+def clear_static_vehicle(world):
+ """清理静态车辆"""
+ if not world:
+ return
+ for actor in world.get_actors().filter('static.vehicle.*'):
+ try:
+ actor.destroy()
+ except:
+ pass
+
+
+def clear(world, camera):
+ """清理所有资源"""
+ if camera:
+ try:
+ camera.destroy()
+ except:
+ pass
+ clear_npc(world)
+ clear_static_vehicle(world)
+
+
+# -------------------------- CARLA 相关核心函数 --------------------------
+def camera_callback(image, rgb_image_queue):
+ """相机回调函数:将图像存入队列"""
+ rgb_image_queue.put(np.reshape(np.copy(image.raw_data), (image.height, image.width, 4)))
+
+
+def setup_carla_client(host='localhost', port=2000):
+ """连接CARLA服务器并设置同步模式"""
+ client = carla.Client(host, port)
+ client.set_timeout(10.0)
+ world = client.get_world()
+ # 设置同步模式(保证帧率稳定)
+ settings = world.get_settings()
+ settings.synchronous_mode = True
+ settings.fixed_delta_seconds = 0.05
+ world.apply_settings(settings)
+ return world, client
+
+
+def spawn_ego_vehicle(world):
+ """生成主车辆(自车)- 兼容老版本CARLA"""
+ if not world:
+ return None
+ bp_lib = world.get_blueprint_library()
+ # 兼容不同CARLA版本的车辆蓝图
+ try:
+ vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020')
+ except:
+ vehicle_bp = random.choice(
+ [bp for bp in bp_lib.filter('vehicle') if int(bp.get_attribute('number_of_wheels')) == 4])
+
+ spawn_points = world.get_map().get_spawn_points()
+ if not spawn_points:
+ print("警告:没有可用的车辆生成点!")
+ return None
+
+ # 尝试生成车辆(多次尝试避免生成失败)
+ vehicle = None
+ for _ in range(5):
+ vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points))
+ if vehicle:
+ break
+ if vehicle:
+ vehicle.set_autopilot(True)
+ print("主车辆生成成功!")
+ else:
+ print("警告:无法生成主车辆!")
+ return vehicle
+
+
+def spawn_camera(world, vehicle):
+ """生成相机传感器(挂载在自车)"""
+ if not world or not vehicle:
+ return None, None, None
+ bp_lib = world.get_blueprint_library()
+ camera_bp = bp_lib.find('sensor.camera.rgb')
+ # 设置相机参数(降低分辨率提升CPU性能)
+ camera_bp.set_attribute('image_size_x', '480')
+ camera_bp.set_attribute('image_size_y', '360')
+ camera_bp.set_attribute('fov', '90')
+ camera_bp.set_attribute('sensor_tick', '0.05') # 与世界帧率一致
+
+ # 相机安装位置(自车前方1.5米,高度2米)
+ camera_init_trans = carla.Transform(carla.Location(x=1.5, z=2.0))
+ camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle)
+
+ # 图像队列(异步获取图像)
+ image_queue = queue.Queue()
+ camera.listen(lambda image: camera_callback(image, image_queue))
+ return camera, image_queue, camera_bp
+
+
+def spawn_npcs(world, count=10):
+ """生成NPC车辆(数量适中,避免CPU卡顿)"""
+ if not world:
+ return
+ bp_lib = world.get_blueprint_library()
+ vehicle_bp = bp_lib.filter('vehicle')
+ car_bp = [bp for bp in vehicle_bp if int(bp.get_attribute('number_of_wheels')) == 4]
+ spawn_points = world.get_map().get_spawn_points()
+ if not car_bp or not spawn_points:
+ print("警告:无法生成NPC车辆!")
+ return
+
+ spawned_count = 0
+ for _ in range(count):
+ npc = world.try_spawn_actor(random.choice(car_bp), random.choice(spawn_points))
+ if npc:
+ npc.set_autopilot(True)
+ spawned_count += 1
+ print(f"成功生成 {spawned_count} 辆NPC车辆")
+
+
+def load_detection_model(model_type):
+ """加载YOLOv5检测模型(自动下载权重)"""
+ device = 'cuda' if torch.cuda.is_available() else 'cpu'
+ # 加载YOLOv5模型(自动缓存权重)
+ if model_type == 'yolov5s':
+ model = YOLO("yolov5s.pt")
+ elif model_type == 'yolov5m':
+ model = YOLO("yolov5m.pt")
+ else:
+ raise ValueError(f"不支持的模型类型:{model_type}")
+ model.to(device)
+ print(f"检测模型加载成功(设备:{device})")
+ return model, model.names
+
+
+def setup_tracker(tracker_type):
+ """初始化跟踪器(默认SORT,稳定高效)"""
+ if tracker_type == 'sort':
+ return Sort(max_age=3, min_hits=2, iou_threshold=0.3), None
+ else:
+ raise ValueError(f"不支持的跟踪器类型:{tracker_type}")
+
+
+# -------------------------- 主函数(最终稳定版本) --------------------------
+def main():
+ parser = argparse.ArgumentParser(description='CARLA 目标检测与跟踪(兼容老版本,零额外依赖)')
+ parser.add_argument('--model', type=str, default='yolov5s', choices=['yolov5s', 'yolov5m'],
+ help='检测模型(yolov5s更轻量)')
+ parser.add_argument('--tracker', type=str, default='sort', choices=['sort'], help='跟踪器(仅保留稳定的SORT)')
+ parser.add_argument('--host', type=str, default='localhost', help='CARLA服务器地址')
+ parser.add_argument('--port', type=int, default=2000, help='CARLA服务器端口')
+ args = parser.parse_args()
+
+ # 提前初始化所有变量,避免未定义错误
+ world = None
+ camera = None
+ vehicle = None
+ image_queue = None
+
+ try:
+ # 1. 初始化设备
+ device = 'cuda' if torch.cuda.is_available() else 'cpu'
+ print(f"当前使用设备:{device}")
+
+ # 2. 连接CARLA服务器(必须先启动CARLA)
+ print(f"正在连接CARLA服务器 {args.host}:{args.port}...")
+ world, client = setup_carla_client(args.host, args.port)
+ spectator = world.get_spectator()
+ print("CARLA服务器连接成功!")
+
+ # 3. 清理环境(避免残留车辆)
+ clear_npc(world)
+ clear_static_vehicle(world)
+ print("环境清理完成!")
+
+ # 4. 生成主车辆
+ vehicle = spawn_ego_vehicle(world)
+ if not vehicle:
+ print("无法生成主车辆,程序退出!")
+ return
+
+ # 5. 生成相机传感器
+ camera, image_queue, camera_bp = spawn_camera(world, vehicle)
+ if not camera:
+ print("无法生成相机,程序退出!")
+ return
+ print("相机传感器生成成功!")
+
+ # 6. 生成NPC车辆
+ spawn_npcs(world, count=10)
+
+ # 7. 加载检测模型和跟踪器
+ model, class_names = load_detection_model(args.model)
+ tracker, _ = setup_tracker(args.tracker)
+
+ # 8. 主循环(检测+跟踪)
+ print("开始目标检测与跟踪(按 'q' 键退出程序)")
+ while True:
+ # 同步CARLA世界时钟
+ world.tick()
+
+ # 移动视角到自车上方(方便观察)
+ ego_transform = vehicle.get_transform()
+ spectator_transform = carla.Transform(
+ ego_transform.transform(carla.Location(x=-5, z=8)),
+ carla.Rotation(yaw=ego_transform.rotation.yaw - 180, pitch=-30)
+ )
+ spectator.set_transform(spectator_transform)
+
+ # 获取相机图像(跳过空队列)
+ if image_queue.empty():
+ continue
+ origin_image = image_queue.get()
+ # CARLA图像默认BGRA格式,转换为RGB
+ image = cv2.cvtColor(origin_image, cv2.COLOR_BGRA2RGB)
+ height, width, _ = image.shape
+
+ # -------------------------- 目标检测(YOLOv5) --------------------------
+ results = model(image, conf=0.5) # 置信度阈值0.5,过滤低置信度结果
+ boxes, labels, probs = [], [], []
+ for r in results:
+ for box in r.boxes:
+ # 解析边界框(x1,y1,x2,y2)
+ x1, y1, x2, y2 = box.xyxy[0].cpu().numpy()
+ # 解析置信度和类别
+ conf = box.conf[0].cpu().numpy()
+ cls = int(box.cls[0].cpu().numpy())
+ # 只保留车辆相关类别(COCO数据集:car=2, motorcycle=3, bus=5, truck=7)
+ if cls in [2, 3, 5, 7]:
+ boxes.append([x1, y1, x2, y2])
+ labels.append(cls)
+ probs.append(conf)
+
+ # 转换为numpy数组(避免空列表报错)
+ boxes = np.array(boxes) if boxes else np.array([])
+ labels = np.array(labels) if labels else np.array([])
+ probs = np.array(probs) if probs else np.array([])
+
+ # -------------------------- 目标跟踪(SORT) --------------------------
+ if args.tracker == 'sort' and len(boxes) > 0:
+ # 转换为SORT需要的格式:[x1,y1,x2,y2,conf]
+ dets = np.hstack([boxes, probs.reshape(-1, 1)]) if len(probs) > 0 else boxes
+ # 更新跟踪器
+ track_results = tracker.update(dets)
+ # 解析跟踪结果
+ track_boxes = []
+ track_ids = []
+ for track in track_results:
+ x1, y1, x2, y2, track_id = track
+ track_boxes.append([x1, y1, x2, y2])
+ track_ids.append(int(track_id))
+ # 绘制跟踪框(所有跟踪目标统一标注为car类别)
+ if track_boxes:
+ image = draw_bounding_boxes(
+ image, track_boxes,
+ labels=[2] * len(track_boxes), # 2对应COCO的car类别
+ class_names=class_names,
+ track_ids=track_ids,
+ probs=[0.9] * len(track_boxes) # 跟踪结果默认高置信度
+ )
+ elif len(boxes) > 0:
+ # 无跟踪时,仅绘制检测结果
+ image = draw_bounding_boxes(image, boxes, labels, class_names, probs=probs)
+
+ # -------------------------- 显示结果 --------------------------
+ cv2.imshow(f'CARLA {args.model} + {args.tracker}', cv2.cvtColor(image, cv2.COLOR_RGB2BGR))
+ # 按'q'退出(必须加cv2.waitKey,否则窗口卡死)
+ if cv2.waitKey(1) & 0xFF == ord('q'):
+ print("用户触发退出程序...")
+ break
+
+ except Exception as e:
+ print(f"程序运行出错:{str(e)}")
+ print("报错提示:1. 确保CARLA服务器已启动;2. 确保carla包版本与服务器一致;3. 确保已安装所有依赖")
+ finally:
+ # 安全清理所有资源
+ print("正在清理资源...")
+ if camera:
+ try:
+ camera.destroy()
+ except:
+ pass
+ if world:
+ # 恢复CARLA异步模式
+ settings = world.get_settings()
+ settings.synchronous_mode = False
+ world.apply_settings(settings)
+ # 清理车辆
+ clear_npc(world)
+ clear_static_vehicle(world)
+ # 关闭图像窗口
+ cv2.destroyAllWindows()
+ print("资源清理完成,程序正常退出!")
+
+
+if __name__ == "__main__":
+ main()
+
diff --git a/src/driverless_car/image_seg/main_seg.py b/src/driverless_car/image_seg/main_seg.py
new file mode 100644
index 0000000000..2c0dc476d6
--- /dev/null
+++ b/src/driverless_car/image_seg/main_seg.py
@@ -0,0 +1,301 @@
+import torch
+import torch.nn as nn
+import torch.optim as optim
+from torch.utils.data import Dataset, DataLoader
+import torchvision.transforms as transforms
+import matplotlib.pyplot as plt
+import numpy as np
+from PIL import Image, ImageDraw
+import random
+from tqdm import tqdm
+
+# --------------------------
+# 基础配置:设备选择+可视化设置
+# --------------------------
+# 自动选择GPU/CPU,提速训练
+device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
+print(f'使用设备:{device}')
+
+# 解决Matplotlib中文显示问题
+plt.rcParams["font.family"] = ["SimHei", "Arial"]
+plt.rcParams["axes.unicode_minus"] = False
+plt.switch_backend('TkAgg') # 解决PyCharm可视化问题
+
+# --------------------------
+# 步骤1:生成模拟无人机航拍数据集(道路/非道路二分类)
+# --------------------------
+class DroneRoadDataset(Dataset):
+ """模拟无人机航拍道路分割数据集(自动生成图像和掩码)"""
+ def __init__(self, num_samples=100, image_size=(128, 128), transform=None):
+ self.num_samples = num_samples
+ self.image_size = image_size
+ self.transform = transform
+ # 预生成所有图像和掩码
+ self.images, self.masks = self._generate_data()
+
+ def _generate_data(self):
+ """生成模拟无人机航拍图像和对应的分割掩码"""
+ images = []
+ masks = []
+ w, h = self.image_size
+
+ for _ in range(self.num_samples):
+ # 1. 创建图像(模拟航拍:浅灰色背景,道路为深灰色)
+ img = Image.new('RGB', (w, h), color=(230, 230, 230)) # 非道路背景
+ mask = Image.new('L', (w, h), color=0) # 掩码:0=非道路,255=道路
+ draw_img = ImageDraw.Draw(img)
+ draw_mask = ImageDraw.Draw(mask)
+
+ # 绘制主道路(随机角度的矩形,模拟航拍道路)
+ road_angle = random.randint(-30, 30) # 道路倾斜角度
+ road_width = random.randint(20, 30)
+ # 道路中心坐标
+ cx, cy = w // 2, h // 2
+ # 绘制道路(填充深灰色)
+ # 简化处理:用水平道路替代旋转,降低计算量(新手友好)
+ road_x1 = cx - road_width // 2
+ road_x2 = cx + road_width // 2
+ draw_img.rectangle([0, cy - road_width//2, w, cy + road_width//2], fill=(100, 100, 100))
+ draw_mask.rectangle([0, cy - road_width//2, w, cy + road_width//2], fill=255)
+
+ # 绘制次要元素(建筑/行人,非道路)
+ # 建筑:随机矩形(浅棕色)
+ for _ in range(random.randint(2, 5)):
+ bx1 = random.randint(10, w-10)
+ by1 = random.randint(10, h-10)
+ bx2 = bx1 + random.randint(10, 20)
+ by2 = by1 + random.randint(10, 20)
+ draw_img.rectangle([bx1, by1, bx2, by2], fill=(160, 120, 80))
+ # 行人:随机小圆点(红色)
+ for _ in range(random.randint(3, 8)):
+ px = random.randint(10, w-10)
+ py = random.randint(10, h-10)
+ draw_img.ellipse([px-2, py-2, px+2, py+2], fill=(255, 0, 0))
+
+ images.append(img)
+ masks.append(mask)
+
+ return images, masks
+
+ def __len__(self):
+ return self.num_samples
+
+ def __getitem__(self, idx):
+ img = self.images[idx]
+ mask = self.masks[idx]
+
+ # 数据变换:转为张量+归一化
+ if self.transform:
+ img = self.transform(img)
+ # 掩码归一化到0/1(二分类)
+ mask = transforms.ToTensor()(mask)
+ mask = (mask > 0).float() # 255→1,0→0
+
+ return img, mask
+
+# 定义数据变换
+transform = transforms.Compose([
+ transforms.Resize((128, 128)),
+ transforms.ToTensor(),
+ transforms.Normalize(mean=[0.5, 0.5, 0.5], std=[0.5, 0.5, 0.5])
+])
+
+# 创建数据集和数据加载器
+train_dataset = DroneRoadDataset(num_samples=100, transform=transform)
+train_loader = DataLoader(train_dataset, batch_size=8, shuffle=True)
+
+# --------------------------
+# 步骤2:定义轻量化UNet(UNet-small)
+# --------------------------
+class DoubleConv(nn.Module):
+ """UNet的基本模块:两次卷积+BN+ReLU(轻量化:减少通道数)"""
+ def __init__(self, in_channels, out_channels):
+ super().__init__()
+ self.double_conv = nn.Sequential(
+ nn.Conv2d(in_channels, out_channels, kernel_size=3, padding=1),
+ nn.BatchNorm2d(out_channels),
+ nn.ReLU(inplace=True),
+ nn.Conv2d(out_channels, out_channels, kernel_size=3, padding=1),
+ nn.BatchNorm2d(out_channels),
+ nn.ReLU(inplace=True)
+ )
+
+ def forward(self, x):
+ return self.double_conv(x)
+
+class Down(nn.Module):
+ """下采样:最大池化+DoubleConv"""
+ def __init__(self, in_channels, out_channels):
+ super().__init__()
+ self.maxpool_conv = nn.Sequential(
+ nn.MaxPool2d(2),
+ DoubleConv(in_channels, out_channels)
+ )
+
+ def forward(self, x):
+ return self.maxpool_conv(x)
+
+class Up(nn.Module):
+ """上采样:反卷积+拼接+DoubleConv(轻量化:减少通道数)"""
+ def __init__(self, in_channels, out_channels):
+ super().__init__()
+ # 反卷积上采样
+ self.up = nn.ConvTranspose2d(in_channels, in_channels//2, kernel_size=2, stride=2)
+ self.conv = DoubleConv(in_channels, out_channels)
+
+ def forward(self, x1, x2):
+ x1 = self.up(x1)
+ # 拼接(对应UNet的skip connection)
+ x = torch.cat([x2, x1], dim=1)
+ return self.conv(x)
+
+class OutConv(nn.Module):
+ """输出层:1x1卷积,将通道数转为1(二分类)"""
+ def __init__(self, in_channels, out_channels):
+ super().__init__()
+ self.conv = nn.Conv2d(in_channels, out_channels, kernel_size=1)
+
+ def forward(self, x):
+ return self.conv(x)
+
+class UNetSmall(nn.Module):
+ """轻量化UNet:减少通道数和下采样层数,适合小图像分割"""
+ def __init__(self, n_channels=3, n_classes=1):
+ super().__init__()
+ self.n_channels = n_channels
+ self.n_classes = n_classes
+
+ # 编码器(下采样):通道数从3→16→32
+ self.inc = DoubleConv(n_channels, 16)
+ self.down1 = Down(16, 32)
+ # 移除原UNet的深层下采样,减少计算量
+
+ # 解码器(上采样):通道数从32→16
+ self.up1 = Up(32, 16)
+ self.outc = OutConv(16, n_classes)
+
+ # Sigmoid激活,输出0-1之间的概率
+ self.sigmoid = nn.Sigmoid()
+
+ def forward(self, x):
+ # 编码器
+ x1 = self.inc(x)
+ x2 = self.down1(x1)
+
+ # 解码器
+ x = self.up1(x2, x1)
+ logits = self.outc(x)
+ return self.sigmoid(logits)
+
+# 初始化模型并移到设备上
+model = UNetSmall(n_channels=3, n_classes=1).to(device)
+
+# --------------------------
+# 步骤3:定义损失函数和优化器
+# --------------------------
+# 二分类损失:BCELoss(适用于概率输出)
+criterion = nn.BCELoss()
+# 优化器:Adam,学习率适中
+optimizer = optim.Adam(model.parameters(), lr=1e-3)
+
+# --------------------------
+# 步骤4:训练模型(少量epochs,快速收敛)
+# --------------------------
+def train_model(model, loader, criterion, optimizer, epochs=10):
+ model.train()
+ train_losses = []
+ for epoch in range(epochs):
+ running_loss = 0.0
+ # 进度条显示训练进度
+ pbar = tqdm(loader, desc=f'Epoch {epoch+1}/{epochs}', unit='batch')
+ for images, masks in pbar:
+ images = images.to(device)
+ masks = masks.to(device)
+
+ # 前向传播
+ outputs = model(images)
+ loss = criterion(outputs, masks)
+
+ # 反向传播+优化
+ optimizer.zero_grad()
+ loss.backward()
+ optimizer.step()
+
+ running_loss += loss.item() * images.size(0)
+ # 更新进度条显示损失
+ pbar.set_postfix(loss=loss.item())
+
+ epoch_loss = running_loss / len(loader.dataset)
+ train_losses.append(epoch_loss)
+ print(f'Epoch {epoch+1} 平均损失:{epoch_loss:.4f}')
+
+ print('\n训练完成!')
+ return model, train_losses
+
+# 开始训练(10个epoch,快速收敛)
+model, train_losses = train_model(model, train_loader, criterion, optimizer, epochs=10)
+
+# --------------------------
+# 步骤5:可视化分割结果(原图+掩码+预测结果)
+# --------------------------
+def visualize_segmentation(model, dataset, num_samples=5):
+ """可视化分割结果:三图并列展示"""
+ model.eval()
+ fig, axes = plt.subplots(num_samples, 3, figsize=(15, 5*num_samples))
+ fig.suptitle('无人机航拍道路分割结果(原图→真实掩码→预测结果)', fontsize=16, fontweight='bold')
+
+ with torch.no_grad():
+ for i in range(num_samples):
+ # 取数据
+ img, mask = dataset[i]
+ img_np = img.permute(1, 2, 0).cpu().numpy()
+ # 反归一化:(x*0.5)+0.5 → 0-1
+ img_np = img_np * 0.5 + 0.5
+ mask_np = mask.squeeze().cpu().numpy()
+
+ # 模型预测
+ img_input = img.unsqueeze(0).to(device)
+ pred = model(img_input)
+ pred_np = pred.squeeze().cpu().numpy()
+ # 二值化:大于0.5为道路(1),否则为非道路(0)
+ pred_np = (pred_np > 0.5).astype(np.float32)
+
+ # 绘制原图
+ axes[i, 0].imshow(img_np)
+ axes[i, 0].set_title('无人机航拍原图', fontsize=12)
+ axes[i, 0].axis('off')
+
+ # 绘制真实掩码(伪色彩:道路为红色)
+ axes[i, 1].imshow(mask_np, cmap='Reds')
+ axes[i, 1].set_title('真实道路掩码', fontsize=12)
+ axes[i, 1].axis('off')
+
+ # 绘制预测结果(伪色彩:道路为红色)
+ axes[i, 2].imshow(pred_np, cmap='Reds')
+ axes[i, 2].set_title('模型预测结果', fontsize=12)
+ axes[i, 2].axis('off')
+
+ # 保存结果图
+ plt.tight_layout()
+ plt.savefig('drone_segmentation_results.png', bbox_inches='tight', dpi=100)
+ plt.show()
+
+# 可视化分割结果
+visualize_segmentation(model, train_dataset, num_samples=5)
+
+# --------------------------
+# 额外:可视化训练损失曲线
+# --------------------------
+def plot_loss_curve(losses):
+ """绘制训练损失曲线"""
+ plt.figure(figsize=(10, 5))
+ plt.plot(range(1, len(losses)+1), losses, marker='o', color='b', label='训练损失')
+ plt.xlabel('Epoch')
+ plt.ylabel('Loss')
+ plt.title('无人机分割模型训练损失曲线', fontsize=14)
+ plt.legend()
+ plt.grid(True)
+ plt.savefig('train_loss_curve.png', bbox_inches='tight')
+ plt.show()
+
+plot_loss_curve(train_losses)
\ No newline at end of file
diff --git a/src/enhance_pedestrian_safety/annotation_generator.py b/src/enhance_pedestrian_safety/annotation_generator.py
new file mode 100644
index 0000000000..d443854d96
--- /dev/null
+++ b/src/enhance_pedestrian_safety/annotation_generator.py
@@ -0,0 +1,168 @@
+import json
+import os
+import numpy as np
+from datetime import datetime
+
+
+class AnnotationGenerator:
+ """标注生成器"""
+
+ def __init__(self, output_dir):
+ self.output_dir = output_dir
+ self.annotations_dir = os.path.join(output_dir, "annotations")
+ os.makedirs(self.annotations_dir, exist_ok=True)
+
+ self.frame_annotations = {}
+ self.object_counter = 0
+
+ def detect_objects(self, world, frame_num, timestamp):
+ """检测场景中的物体"""
+ annotations = {
+ 'frame_id': frame_num,
+ 'timestamp': timestamp,
+ 'objects': [],
+ 'camera_info': {}
+ }
+
+ try:
+ actors = world.get_actors()
+
+ for actor in actors:
+ obj_type = actor.type_id
+
+ if 'vehicle' in obj_type or 'walker' in obj_type:
+ obj_info = self._extract_object_info(actor)
+ if obj_info:
+ annotations['objects'].append(obj_info)
+ self.object_counter += 1
+
+ self._save_annotations(frame_num, annotations)
+ return annotations
+
+ except Exception as e:
+ print(f"物体检测失败: {e}")
+ return annotations
+
+ def _extract_object_info(self, actor):
+ """提取物体信息"""
+ try:
+ bbox = actor.bounding_box
+ location = actor.get_location()
+ velocity = actor.get_velocity()
+
+ obj_info = {
+ 'id': actor.id,
+ 'type': actor.type_id,
+ 'class': self._get_object_class(actor.type_id),
+ 'location': {
+ 'x': float(location.x),
+ 'y': float(location.y),
+ 'z': float(location.z)
+ },
+ 'velocity': {
+ 'x': float(velocity.x),
+ 'y': float(velocity.y),
+ 'z': float(velocity.z)
+ },
+ 'bounding_box': {
+ 'extent': {
+ 'x': float(bbox.extent.x),
+ 'y': float(bbox.extent.y),
+ 'z': float(bbox.extent.z)
+ }
+ },
+ 'attributes': {
+ 'is_alive': actor.is_alive
+ }
+ }
+
+ return obj_info
+
+ except Exception as e:
+ return None
+
+ def _get_object_class(self, type_id):
+ """获取物体类别"""
+ type_lower = type_id.lower()
+
+ if 'vehicle' in type_lower:
+ if 'tesla' in type_lower:
+ return 'car'
+ elif 'audi' in type_lower:
+ return 'car'
+ elif 'mini' in type_lower:
+ return 'car'
+ elif 'mercedes' in type_lower:
+ return 'car'
+ elif 'nissan' in type_lower:
+ return 'car'
+ elif 'bmw' in type_lower:
+ return 'car'
+ else:
+ return 'vehicle'
+
+ elif 'walker' in type_lower:
+ return 'pedestrian'
+
+ elif 'traffic' in type_lower:
+ return 'traffic_light'
+
+ else:
+ return 'unknown'
+
+ def _save_annotations(self, frame_num, annotations):
+ """保存标注到文件"""
+ filename = f"frame_{frame_num:06d}.json"
+ filepath = os.path.join(self.annotations_dir, filename)
+
+ with open(filepath, 'w', encoding='utf-8') as f:
+ json.dump(annotations, f, indent=2, ensure_ascii=False)
+
+ # 同时更新总标注文件
+ self.frame_annotations[frame_num] = annotations
+ self._update_master_annotation()
+
+ def _update_master_annotation(self):
+ """更新主标注文件"""
+ master_file = os.path.join(self.annotations_dir, "annotations.json")
+
+ master_data = {
+ 'total_frames': len(self.frame_annotations),
+ 'total_objects': self.object_counter,
+ 'frames': list(self.frame_annotations.keys()),
+ 'created': datetime.now().isoformat()
+ }
+
+ with open(master_file, 'w', encoding='utf-8') as f:
+ json.dump(master_data, f, indent=2, ensure_ascii=False)
+
+ def generate_summary(self):
+ """生成标注摘要"""
+ vehicle_count = 0
+ pedestrian_count = 0
+ other_count = 0
+
+ for frame_data in self.frame_annotations.values():
+ for obj in frame_data.get('objects', []):
+ if obj['class'] in ['car', 'vehicle']:
+ vehicle_count += 1
+ elif obj['class'] == 'pedestrian':
+ pedestrian_count += 1
+ else:
+ other_count += 1
+
+ summary = {
+ 'total_frames': len(self.frame_annotations),
+ 'total_objects': self.object_counter,
+ 'vehicles': vehicle_count,
+ 'pedestrians': pedestrian_count,
+ 'other_objects': other_count,
+ 'average_objects_per_frame': self.object_counter / len(
+ self.frame_annotations) if self.frame_annotations else 0
+ }
+
+ summary_file = os.path.join(self.annotations_dir, "summary.json")
+ with open(summary_file, 'w', encoding='utf-8') as f:
+ json.dump(summary, f, indent=2, ensure_ascii=False)
+
+ return summary
\ No newline at end of file
diff --git a/src/enhance_pedestrian_safety/config_manager.py b/src/enhance_pedestrian_safety/config_manager.py
new file mode 100644
index 0000000000..e2c5410e34
--- /dev/null
+++ b/src/enhance_pedestrian_safety/config_manager.py
@@ -0,0 +1,116 @@
+import json
+import os
+import argparse
+
+
+class ConfigManager:
+ """配置管理器"""
+
+ @staticmethod
+ def load_config(config_file=None):
+ """加载配置文件"""
+ config = {
+ 'scenario': {
+ 'name': 'intersection',
+ 'town': 'Town10HD',
+ 'weather': 'clear',
+ 'time_of_day': 'noon',
+ 'duration': 60,
+ 'seed': 42
+ },
+ 'traffic': {
+ 'ego_vehicles': 1,
+ 'background_vehicles': 8,
+ 'pedestrians': 10,
+ 'traffic_lights': True,
+ 'pedestrian_behavior': {
+ 'crossing_probability': 0.3,
+ 'walking_speed': 1.2
+ }
+ },
+ 'sensors': {
+ 'vehicle_cameras': 4,
+ 'infrastructure_cameras': 4,
+ 'image_size': [1280, 720],
+ 'capture_interval': 2.0,
+ 'fov': 90
+ },
+ 'output': {
+ 'data_dir': 'cvips_dataset',
+ 'save_raw': True,
+ 'save_stitched': True,
+ 'save_metadata': True,
+ 'save_annotations': False,
+ 'compression': False
+ }
+ }
+
+ if config_file and os.path.exists(config_file):
+ try:
+ with open(config_file, 'r') as f:
+ if config_file.endswith('.json'):
+ user_config = json.load(f)
+ else:
+ # 尝试导入yaml,如果失败则跳过
+ try:
+ import yaml
+ user_config = yaml.safe_load(f)
+ except ImportError:
+ print("警告: PyYAML未安装,无法加载YAML配置文件")
+ print("请安装: pip install pyyaml")
+ user_config = {}
+ ConfigManager._deep_update(config, user_config)
+ except Exception as e:
+ print(f"配置文件加载失败: {e}")
+
+ return config
+
+ @staticmethod
+ def _deep_update(original, update):
+ """深度更新配置字典"""
+ for key, value in update.items():
+ if key in original and isinstance(original[key], dict) and isinstance(value, dict):
+ ConfigManager._deep_update(original[key], value)
+ else:
+ original[key] = value
+
+ @staticmethod
+ def save_config(config, filepath):
+ """保存配置到文件"""
+ with open(filepath, 'w') as f:
+ if filepath.endswith('.json'):
+ json.dump(config, f, indent=2)
+ else:
+ try:
+ import yaml
+ yaml.dump(config, f, default_flow_style=False)
+ except ImportError:
+ print("警告: PyYAML未安装,无法保存YAML配置文件")
+ json.dump(config, f, indent=2)
+
+ @staticmethod
+ def merge_args(config, args):
+ """合并命令行参数到配置"""
+ if args.scenario:
+ config['scenario']['name'] = args.scenario
+ if args.town:
+ config['scenario']['town'] = args.town
+ if args.weather:
+ config['scenario']['weather'] = args.weather
+ if args.time_of_day:
+ config['scenario']['time_of_day'] = args.time_of_day
+ if args.duration:
+ config['scenario']['duration'] = args.duration
+ if hasattr(args, 'seed') and args.seed:
+ config['scenario']['seed'] = args.seed
+
+ if args.num_vehicles:
+ config['traffic']['background_vehicles'] = args.num_vehicles
+ if args.num_pedestrians:
+ config['traffic']['pedestrians'] = args.num_pedestrians
+ if hasattr(args, 'ego_vehicles') and args.ego_vehicles:
+ config['traffic']['ego_vehicles'] = args.ego_vehicles
+ if args.capture_interval:
+ config['sensors']['capture_interval'] = args.capture_interval
+
+ return config
\ No newline at end of file
diff --git a/src/enhance_pedestrian_safety/data_validator.py b/src/enhance_pedestrian_safety/data_validator.py
new file mode 100644
index 0000000000..faef32f1ce
--- /dev/null
+++ b/src/enhance_pedestrian_safety/data_validator.py
@@ -0,0 +1,328 @@
+import os
+import json
+
+
+class DataValidator:
+ """数据验证器"""
+
+ @staticmethod
+ def validate_dataset(data_dir):
+ """验证整个数据集"""
+ print(f"验证数据集: {data_dir}")
+
+ validation_results = {
+ 'directory_structure': DataValidator._check_directory_structure(data_dir),
+ 'raw_images': DataValidator._validate_raw_images(data_dir),
+ 'stitched_images': DataValidator._validate_stitched_images(data_dir),
+ 'annotations': DataValidator._validate_annotations(data_dir),
+ 'metadata': DataValidator._validate_metadata(data_dir)
+ }
+
+ # 计算总体评分
+ validation_results['overall_score'] = DataValidator._calculate_score(validation_results)
+
+ DataValidator._save_validation_report(data_dir, validation_results)
+ DataValidator._print_validation_report(validation_results)
+
+ return validation_results
+
+ @staticmethod
+ def _check_directory_structure(data_dir):
+ """检查目录结构"""
+ required_dirs = [
+ "raw/vehicle",
+ "raw/infrastructure",
+ "stitched",
+ "annotations",
+ "metadata"
+ ]
+
+ missing_dirs = []
+ for dir_path in required_dirs:
+ full_path = os.path.join(data_dir, dir_path)
+ if not os.path.exists(full_path):
+ missing_dirs.append(dir_path)
+
+ status = 'PASS' if len(missing_dirs) == 0 else 'FAIL'
+
+ result = {
+ 'status': status,
+ 'missing_directories': missing_dirs,
+ 'required_directories': required_dirs
+ }
+
+ return result
+
+ @staticmethod
+ def _validate_raw_images(data_dir):
+ """验证原始图像"""
+ raw_dirs = ["vehicle", "infrastructure"]
+ results = {}
+
+ for raw_dir in raw_dirs:
+ path = os.path.join(data_dir, "raw", raw_dir)
+ if not os.path.exists(path):
+ results[raw_dir] = {'status': 'MISSING', 'count': 0, 'errors': []}
+ continue
+
+ camera_dirs = [d for d in os.listdir(path) if os.path.isdir(os.path.join(path, d))]
+ total_images = 0
+ errors = []
+
+ for camera_dir in camera_dirs:
+ camera_path = os.path.join(path, camera_dir)
+ if not os.path.isdir(camera_path):
+ continue
+
+ images = [f for f in os.listdir(camera_path) if f.endswith(('.png', '.jpg', '.jpeg'))]
+
+ for img_file in images:
+ img_path = os.path.join(camera_path, img_file)
+ try:
+ # 尝试导入PIL,如果失败则跳过验证
+ try:
+ from PIL import Image
+ with Image.open(img_path) as img:
+ img.verify()
+ width, height = img.size
+ if width == 0 or height == 0:
+ errors.append(f"无效尺寸: {img_file}")
+ except ImportError:
+ # PIL未安装,只检查文件是否存在
+ if not os.path.exists(img_path):
+ errors.append(f"文件不存在: {img_file}")
+ except Exception as e:
+ errors.append(f"损坏的图像: {img_file} - {str(e)}")
+
+ total_images += len(images)
+
+ if len(errors) == 0:
+ status = 'PASS'
+ elif len(errors) < 3:
+ status = 'WARNING'
+ else:
+ status = 'FAIL'
+
+ results[raw_dir] = {
+ 'status': status,
+ 'count': total_images,
+ 'errors': errors
+ }
+
+ return results
+
+ @staticmethod
+ def _validate_stitched_images(data_dir):
+ """验证拼接图像"""
+ stitched_dir = os.path.join(data_dir, "stitched")
+
+ if not os.path.exists(stitched_dir):
+ return {'status': 'MISSING', 'count': 0, 'errors': []}
+
+ images = [f for f in os.listdir(stitched_dir) if f.endswith(('.jpg', '.jpeg', '.png'))]
+ errors = []
+
+ for img_file in images:
+ img_path = os.path.join(stitched_dir, img_file)
+ try:
+ # 尝试导入PIL
+ try:
+ from PIL import Image
+ with Image.open(img_path) as img:
+ img.verify()
+ width, height = img.size
+ if width != 1300 or height != 740:
+ errors.append(f"尺寸不匹配: {img_file} ({width}x{height})")
+ except ImportError:
+ # PIL未安装,只检查文件
+ if not os.path.exists(img_path):
+ errors.append(f"文件不存在: {img_file}")
+ except Exception as e:
+ errors.append(f"损坏的图像: {img_file} - {str(e)}")
+
+ if len(errors) == 0:
+ status = 'PASS'
+ elif len(errors) < 3:
+ status = 'WARNING'
+ else:
+ status = 'FAIL'
+
+ return {
+ 'status': status,
+ 'count': len(images),
+ 'errors': errors
+ }
+
+ @staticmethod
+ def _validate_annotations(data_dir):
+ """验证标注文件"""
+ annotations_dir = os.path.join(data_dir, "annotations")
+
+ if not os.path.exists(annotations_dir):
+ return {'status': 'MISSING', 'count': 0, 'errors': []}
+
+ json_files = [f for f in os.listdir(annotations_dir) if f.endswith('.json')]
+ errors = []
+ valid_files = 0
+
+ for json_file in json_files:
+ json_path = os.path.join(annotations_dir, json_file)
+ try:
+ with open(json_path, 'r') as f:
+ data = json.load(f)
+
+ # 基本结构验证
+ if json_file == "annotations.json":
+ required_keys = ['total_frames', 'total_objects', 'frames', 'created']
+ elif json_file == "summary.json":
+ required_keys = ['total_frames', 'total_objects', 'vehicles', 'pedestrians']
+ else:
+ required_keys = ['frame_id', 'timestamp', 'objects']
+
+ for key in required_keys:
+ if key not in data:
+ errors.append(f"缺少字段 {key} 在 {json_file}")
+
+ valid_files += 1
+
+ except Exception as e:
+ errors.append(f"无效的JSON文件: {json_file} - {str(e)}")
+
+ if len(errors) == 0:
+ status = 'PASS'
+ elif len(errors) < 3:
+ status = 'WARNING'
+ else:
+ status = 'FAIL'
+
+ return {
+ 'status': status,
+ 'count': valid_files,
+ 'errors': errors
+ }
+
+ @staticmethod
+ def _validate_metadata(data_dir):
+ """验证元数据"""
+ metadata_dir = os.path.join(data_dir, "metadata")
+
+ if not os.path.exists(metadata_dir):
+ return {'status': 'MISSING', 'count': 0, 'errors': []}
+
+ json_files = [f for f in os.listdir(metadata_dir) if f.endswith('.json')]
+ errors = []
+ valid_files = 0
+
+ for json_file in json_files:
+ json_path = os.path.join(metadata_dir, json_file)
+ try:
+ with open(json_path, 'r') as f:
+ data = json.load(f)
+
+ # 验证基本结构
+ if 'scenario' not in data or 'collection' not in data:
+ errors.append(f"元数据结构不完整: {json_file}")
+
+ valid_files += 1
+
+ except Exception as e:
+ errors.append(f"无效的元数据文件: {json_file} - {str(e)}")
+
+ if len(errors) == 0:
+ status = 'PASS'
+ elif len(errors) < 3:
+ status = 'WARNING'
+ else:
+ status = 'FAIL'
+
+ return {
+ 'status': status,
+ 'count': valid_files,
+ 'errors': errors
+ }
+
+ @staticmethod
+ def _calculate_score(results):
+ """计算总体评分"""
+ weights = {
+ 'directory_structure': 0.2,
+ 'raw_images': 0.3,
+ 'stitched_images': 0.2,
+ 'annotations': 0.15,
+ 'metadata': 0.15
+ }
+
+ score = 0
+ for key, weight in weights.items():
+ if key not in results:
+ # 如果某个结果不存在,给予最低分
+ score += 30 * weight
+ continue
+
+ result = results[key]
+ if 'status' not in result:
+ # 如果status不存在,给予最低分
+ score += 30 * weight
+ continue
+
+ if result['status'] == 'PASS':
+ score += 100 * weight
+ elif result['status'] == 'WARNING':
+ score += 70 * weight
+ elif result['status'] in ['FAIL', 'MISSING']:
+ score += 30 * weight
+ else:
+ # 其他未知状态
+ score += 50 * weight
+
+ return round(score, 1)
+
+ @staticmethod
+ def _save_validation_report(data_dir, results):
+ """保存验证报告"""
+ report_path = os.path.join(data_dir, "validation_report.json")
+
+ with open(report_path, 'w', encoding='utf-8') as f:
+ json.dump(results, f, indent=2, ensure_ascii=False)
+
+ @staticmethod
+ def _print_validation_report(results):
+ """打印验证报告"""
+ print("\n" + "=" * 60)
+ print("数据集验证报告")
+ print("=" * 60)
+
+ for key, result in results.items():
+ if key == 'overall_score':
+ continue
+
+ print(f"\n{key.replace('_', ' ').title()}:")
+
+ if 'status' in result:
+ print(f" 状态: {result['status']}")
+ else:
+ print(f" 状态: 未知")
+
+ if 'count' in result:
+ print(f" 数量: {result['count']}")
+
+ if 'errors' in result and result['errors']:
+ print(f" 错误 ({len(result['errors'])}):")
+ for error in result['errors'][:3]: # 只显示前3个错误
+ print(f" - {error}")
+ if len(result['errors']) > 3:
+ print(f" ... 还有 {len(result['errors']) - 3} 个错误")
+
+ print(f"\n总体评分: {results.get('overall_score', 0)}/100")
+
+ overall_score = results.get('overall_score', 0)
+ if overall_score >= 90:
+ print("✓ 数据集质量优秀")
+ elif overall_score >= 70:
+ print("✓ 数据集质量良好")
+ elif overall_score >= 50:
+ print("⚠ 数据集质量一般")
+ else:
+ print("✗ 数据集质量较差")
+
+ print("=" * 60)
\ No newline at end of file
diff --git a/src/enhance_pedestrian_safety/main.py b/src/enhance_pedestrian_safety/main.py
index 41616f043f..75a9e71d6f 100644
--- a/src/enhance_pedestrian_safety/main.py
+++ b/src/enhance_pedestrian_safety/main.py
@@ -6,561 +6,602 @@
import traceback
import math
import threading
+import json
from datetime import datetime
-from PIL import Image, ImageDraw, ImageFont
+from collections import deque
from carla_utils import setup_carla_path, import_carla_module
+from config_manager import ConfigManager
+from annotation_generator import AnnotationGenerator
+from data_validator import DataValidator
carla_egg_path, remaining_argv = setup_carla_path()
carla = import_carla_module()
+class Logger:
+ @staticmethod
+ def info(msg):
+ print(f"[INFO] {msg}")
+
+ @staticmethod
+ def warning(msg):
+ print(f"[WARNING] {msg}")
+
+ @staticmethod
+ def error(msg):
+ print(f"[ERROR] {msg}")
+
+ @staticmethod
+ def debug(msg):
+ print(f"[DEBUG] {msg}")
+
+
+class Config:
+ VEHICLE_TYPES = [
+ 'vehicle.tesla.model3',
+ 'vehicle.audi.tt',
+ 'vehicle.mini.cooperst',
+ 'vehicle.nissan.micra'
+ ]
+
+
+class WeatherManager:
+ @staticmethod
+ def create_weather(weather_type, time_of_day):
+ weather = carla.WeatherParameters()
+
+ if weather_type == 'clear':
+ weather.cloudiness = 10.0
+ elif weather_type == 'rainy':
+ weather.cloudiness = 90.0
+ weather.precipitation = 80.0
+ elif weather_type == 'cloudy':
+ weather.cloudiness = 70.0
+
+ if time_of_day == 'noon':
+ weather.sun_altitude_angle = 75
+ elif time_of_day == 'sunset':
+ weather.sun_altitude_angle = 15
+ elif time_of_day == 'night':
+ weather.sun_altitude_angle = -20
+
+ return weather
+
+
class ImageStitcher:
def __init__(self, output_dir):
self.output_dir = output_dir
- self.stitched_dir = os.path.join(output_dir, "stitched_images")
+ self.stitched_dir = os.path.join(output_dir, "stitched")
os.makedirs(self.stitched_dir, exist_ok=True)
- self.font = self._load_font()
+ def stitch(self, image_paths, frame_num, view_type="vehicle"):
+ try:
+ from PIL import Image, ImageDraw
+ except ImportError:
+ Logger.warning("PIL未安装,跳过图像拼接")
+ return False
- def _load_font(self):
- font_paths = [
- "arial.ttf",
- "/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf",
- "/System/Library/Fonts/Helvetica.ttc"
- ]
+ positions = [(10, 10), (660, 10), (10, 390), (660, 390)]
- for font_path in font_paths:
- try:
- return ImageFont.truetype(font_path, 20)
- except:
- continue
- return ImageFont.load_default()
-
- def stitch_ego_vehicle_images(self, image_paths, frame_num):
- positions = {
- 'front_wide': (10, 10),
- 'front_narrow': (660, 10),
- 'right_side': (10, 390),
- 'left_side': (660, 390)
- }
+ canvas = Image.new('RGB', (640 * 2 + 20, 360 * 2 + 20), (40, 40, 40))
+ draw = ImageDraw.Draw(canvas)
- images = []
- for cam_name in positions.keys():
- img_path = image_paths.get(cam_name)
+ for idx, (cam_name, img_path) in enumerate(list(image_paths.items())[:4]):
if img_path and os.path.exists(img_path):
try:
img = Image.open(img_path).resize((640, 360))
- images.append((cam_name, img))
except:
- images.append((cam_name, Image.new('RGB', (640, 360), (100, 100, 100))))
+ img = Image.new('RGB', (640, 360), (80, 80, 80))
else:
- images.append((cam_name, Image.new('RGB', (640, 360), (100, 100, 100))))
+ img = Image.new('RGB', (640, 360), (80, 80, 80))
- if len(images) < 4:
- return False
+ canvas.paste(img, positions[idx])
+ draw.text((positions[idx][0] + 5, positions[idx][1] + 5),
+ cam_name, fill=(255, 255, 200))
- canvas = Image.new('RGB', (640 * 2 + 20, 360 * 2 + 20), (50, 50, 50))
+ output_path = os.path.join(self.stitched_dir, f"{view_type}_{frame_num:06d}.jpg")
+ canvas.save(output_path, "JPEG", quality=90)
+ return True
- for cam_name, img in images:
- x, y = positions[cam_name]
- canvas.paste(img, (x, y))
- draw = ImageDraw.Draw(canvas)
- label = cam_name.replace('_', ' ').title()
- draw.text((x + 10, y + 10), label, fill=(255, 255, 255), font=self.font)
- draw = ImageDraw.Draw(canvas)
- title = f"CVIPS - Frame {frame_num:04d}"
- timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
- draw.text((canvas.width // 2 - 150, 5), title, fill=(255, 255, 255), font=self.font)
- draw.text((10, canvas.height - 30), timestamp, fill=(200, 200, 200), font=self.font)
+class TrafficSystem:
+ def __init__(self, world, config):
+ self.world = world
+ self.config = config
+ self.vehicles = []
+ self.pedestrians = []
- output_path = os.path.join(self.stitched_dir, f"ego_stitched_{frame_num:04d}.jpg")
- canvas.save(output_path, "JPEG", quality=95)
+ seed = config['scenario'].get('seed', 42)
+ random.seed(seed)
- return True
+ def spawn_ego_vehicle(self):
+ blueprint_lib = self.world.get_blueprint_library()
+ for vtype in Config.VEHICLE_TYPES:
+ if blueprint_lib.filter(vtype):
+ vehicle_bp = random.choice(blueprint_lib.filter(vtype))
+ break
+ else:
+ vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*'))
-class PedestrianController:
- STATE_WAITING = "waiting"
- STATE_CROSSING = "crossing"
- STATE_WALKING = "walking"
- STATE_STOPPED = "stopped"
+ spawn_points = self.world.get_map().get_spawn_points()
+ if not spawn_points:
+ return None
- def __init__(self, world):
- self.world = world
- self.pedestrians = []
- self.running = True
+ spawn_point = random.choice(spawn_points)
+ try:
+ vehicle = self.world.spawn_actor(vehicle_bp, spawn_point)
+ vehicle.set_autopilot(True)
+ vehicle.apply_control(carla.VehicleControl(throttle=0.2))
+ Logger.info(f"主车: {vehicle.type_id}")
+ return vehicle
+ except Exception as e:
+ Logger.warning(f"主车生成失败: {e}")
+ return None
- def spawn_pedestrian(self, location, behavior_type="crossing"):
+ def spawn_background_vehicles(self):
blueprint_lib = self.world.get_blueprint_library()
- ped_bps = list(blueprint_lib.filter('walker.pedestrian.*'))
+ spawn_points = self.world.get_map().get_spawn_points()
- if not ped_bps:
- return None
+ if not spawn_points:
+ return 0
- ped_bp = random.choice(ped_bps)
- location.z += 1.0
+ num_vehicles = min(self.config['traffic']['background_vehicles'], 10)
+ spawned = 0
- try:
- pedestrian = self.world.spawn_actor(ped_bp, carla.Transform(location))
- controller_bp = blueprint_lib.find('controller.ai.walker')
- controller = self.world.spawn_actor(controller_bp, carla.Transform(), attach_to=pedestrian)
- controller.start()
-
- behavior_state = {
- 'state': self.STATE_WAITING if behavior_type in ["crossing", "hesitant"] else self.STATE_WALKING,
- 'wait_start': time.time(),
- 'wait_duration': random.uniform(2.0, 5.0),
- 'target': None,
- 'original': location,
- 'behavior': behavior_type
- }
+ for _ in range(num_vehicles):
+ try:
+ vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*'))
+ spawn_point = random.choice(spawn_points)
+ vehicle = self.world.spawn_actor(vehicle_bp, spawn_point)
+ vehicle.set_autopilot(True)
+ self.vehicles.append(vehicle)
+ spawned += 1
+ except:
+ pass
- if behavior_type == "walking":
- behavior_state['target'] = self._get_random_location()
+ Logger.info(f"背景车辆: {spawned} 辆")
+ return spawned
- self.pedestrians.append((pedestrian, controller, behavior_state))
- return pedestrian
+ def spawn_pedestrians(self, center_location):
+ blueprint_lib = self.world.get_blueprint_library()
- except Exception as e:
- print(f"生成行人失败: {e}")
- return None
+ num_peds = min(self.config['traffic']['pedestrians'], 8)
+ spawned = 0
- def _get_random_location(self):
- try:
- return self.world.get_random_location_from_navigation()
- except:
- return None
+ for _ in range(num_peds):
+ try:
+ ped_bps = list(blueprint_lib.filter('walker.pedestrian.*'))
+ if not ped_bps:
+ continue
- def update_behaviors(self):
- current_time = time.time()
+ ped_bp = random.choice(ped_bps)
- for pedestrian, controller, state in self.pedestrians:
- if not pedestrian.is_alive or not controller.is_alive:
- continue
+ angle = random.uniform(0, 2 * math.pi)
+ distance = random.uniform(5.0, 12.0)
- if state['behavior'] == "crossing":
- self._update_crossing(pedestrian, controller, state, current_time)
- elif state['behavior'] == "hesitant":
- self._update_hesitant(pedestrian, controller, state, current_time)
- else:
- self._update_walking(pedestrian, controller, state, current_time)
-
- def _update_crossing(self, pedestrian, controller, state, current_time):
- if state['state'] == self.STATE_WAITING:
- if current_time - state['wait_start'] >= state['wait_duration']:
- target = carla.Location(
- x=state['original'].x + random.uniform(15.0, 25.0),
- y=state['original'].y + random.uniform(-5.0, 5.0),
- z=state['original'].z
+ location = carla.Location(
+ x=center_location.x + distance * math.cos(angle),
+ y=center_location.y + distance * math.sin(angle),
+ z=center_location.z + 0.5
)
- state['target'] = target
- state['state'] = self.STATE_CROSSING
- state['cross_start'] = current_time
- controller.go_to_location(target)
-
- elif state['state'] == self.STATE_CROSSING:
- distance = pedestrian.get_location().distance(state['target'])
- if distance < 2.0 or current_time - state['cross_start'] > 15.0:
- state['state'] = self.STATE_STOPPED
-
- def _update_hesitant(self, pedestrian, controller, state, current_time):
- if state['state'] == self.STATE_WAITING:
- if current_time - state['wait_start'] >= state['wait_duration']:
- target = self._get_random_location()
- if target:
- state['target'] = target
- state['state'] = self.STATE_WALKING
- state['walk_start'] = current_time
- state['walk_duration'] = random.uniform(3.0, 8.0)
- controller.go_to_location(target)
-
- elif state['state'] == self.STATE_WALKING:
- if current_time - state['walk_start'] >= state['walk_duration']:
- state['state'] = self.STATE_WAITING
- state['wait_start'] = current_time
- state['wait_duration'] = random.uniform(1.0, 4.0)
-
- def _update_walking(self, pedestrian, controller, state, current_time):
- if state['state'] == self.STATE_WALKING and state['target']:
- distance = pedestrian.get_location().distance(state['target'])
- if distance < 2.0:
- new_target = self._get_random_location()
- if new_target:
- state['target'] = new_target
- controller.go_to_location(new_target)
-
- def start_updates(self):
- def update_loop():
- while self.running:
- try:
- self.update_behaviors()
- time.sleep(0.5)
- except Exception as e:
- print(f"行为更新错误: {e}")
- time.sleep(1.0)
- threading.Thread(target=update_loop, daemon=True).start()
+ pedestrian = self.world.spawn_actor(ped_bp, carla.Transform(location))
+ self.pedestrians.append(pedestrian)
+ spawned += 1
+ except Exception as e:
+ Logger.debug(f"行人生成失败: {e}")
+
+ Logger.info(f"行人: {spawned} 个")
+ return spawned
def cleanup(self):
- self.running = False
- for pedestrian, controller, _ in self.pedestrians:
+ Logger.info("清理交通...")
+
+ for vehicle in self.vehicles:
+ try:
+ if vehicle.is_alive:
+ vehicle.destroy()
+ except:
+ pass
+
+ for pedestrian in self.pedestrians:
try:
- if controller.is_alive:
- controller.stop()
- controller.destroy()
if pedestrian.is_alive:
pedestrian.destroy()
except:
pass
+
+ self.vehicles.clear()
self.pedestrians.clear()
-class DataGenerator:
- def __init__(self, args):
- self.args = args
- self.client = None
- self.world = None
- self.actors = []
+class SensorSystem:
+ def __init__(self, world, config, data_dir):
+ self.world = world
+ self.config = config
+ self.data_dir = data_dir
self.sensors = []
- self.frame_count = 0
- self.last_capture_time = 0
- self.setup_output_directory()
- self.stitcher = ImageStitcher(self.output_dir)
- self.ped_controller = None
+ self.frame_counter = 0
+ self.last_capture_time = 0
- self.image_buffer = {}
+ self.vehicle_buffer = {}
+ self.infra_buffer = {}
self.buffer_lock = threading.Lock()
- def setup_output_directory(self):
- timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
- self.output_dir = os.path.join("cvips_data", f"{self.args.scenario}_{timestamp}")
+ self.image_stitcher = ImageStitcher(data_dir)
+
+ self.camera_configs = {
+ 'vehicle': {
+ 'front_wide': {'loc': (2.0, 0, 1.8), 'rot': (0, -3, 0), 'fov': 100},
+ 'front_narrow': {'loc': (2.0, 0, 1.6), 'rot': (0, 0, 0), 'fov': 60},
+ 'right_side': {'loc': (0.5, 1.0, 1.5), 'rot': (0, -2, 45), 'fov': 90},
+ 'left_side': {'loc': (0.5, -1.0, 1.5), 'rot': (0, -2, -45), 'fov': 90}
+ },
+ 'infrastructure': [
+ {'name': 'north', 'offset': (0, -20, 12), 'rotation': (0, -25, 180)},
+ {'name': 'south', 'offset': (0, 20, 12), 'rotation': (0, -25, 0)},
+ {'name': 'east', 'offset': (20, 0, 12), 'rotation': (0, -25, -90)},
+ {'name': 'west', 'offset': (-20, 0, 12), 'rotation': (0, -25, 90)}
+ ]
+ }
- self.raw_dirs = {}
- for view in ['front_wide', 'front_narrow', 'right_side', 'left_side']:
- dir_path = os.path.join(self.output_dir, "raw", "ego_vehicle", view)
- os.makedirs(dir_path, exist_ok=True)
- self.raw_dirs[view] = dir_path
+ def setup_vehicle_cameras(self, vehicle):
+ if not vehicle:
+ return 0
- print(f"数据输出目录: {self.output_dir}")
+ installed = 0
+ for cam_name, config_data in self.camera_configs['vehicle'].items():
+ if self._create_camera(cam_name, config_data, vehicle, 'vehicle'):
+ installed += 1
- def connect_to_server(self):
- for attempt in range(1, 6):
- try:
- self.client = carla.Client('localhost', 2000)
- self.client.set_timeout(15.0)
+ Logger.info(f"车辆摄像头: {installed}")
+ return installed
- if self.args.town:
- self.world = self.client.load_world(self.args.town)
- else:
- self.world = self.client.get_world()
+ def setup_infrastructure_cameras(self, center_location):
+ installed = 0
- settings = self.world.get_settings()
- settings.synchronous_mode = False
- self.world.apply_settings(settings)
+ for cam_config in self.camera_configs['infrastructure']:
+ sensor_config = {
+ 'loc': (
+ center_location.x + cam_config['offset'][0],
+ center_location.y + cam_config['offset'][1],
+ center_location.z + cam_config['offset'][2]
+ ),
+ 'rot': cam_config['rotation'],
+ 'fov': 90
+ }
- print(f"连接成功! 地图: {self.world.get_map().name}")
- return True
+ if self._create_camera(cam_config['name'], sensor_config, None, 'infrastructure'):
+ installed += 1
- except Exception as e:
- print(f"尝试 {attempt}/5 失败: {str(e)[:80]}")
- if attempt < 5:
- time.sleep(3)
+ Logger.info(f"基础设施摄像头: {installed}")
+ return installed
- return False
+ def _create_camera(self, name, config, parent, sensor_type):
+ try:
+ blueprint = self.world.get_blueprint_library().find('sensor.camera.rgb')
+ blueprint.set_attribute('image_size_x', '1280')
+ blueprint.set_attribute('image_size_y', '720')
+ blueprint.set_attribute('fov', str(config.get('fov', 90)))
- def setup_scene(self):
- self.ped_controller = PedestrianController(self.world)
+ location = carla.Location(config['loc'][0], config['loc'][1], config['loc'][2])
+ rotation = carla.Rotation(config['rot'][0], config['rot'][1], config['rot'][2])
+ transform = carla.Transform(location, rotation)
- self.set_weather()
- time.sleep(2.0)
+ if parent:
+ camera = self.world.spawn_actor(blueprint, transform, attach_to=parent)
+ else:
+ camera = self.world.spawn_actor(blueprint, transform)
- ego_vehicle = self.spawn_ego_vehicle()
- if not ego_vehicle:
- return None
+ save_dir = os.path.join(self.data_dir, "raw", sensor_type, name)
+ os.makedirs(save_dir, exist_ok=True)
- self.spawn_pedestrians()
- self.spawn_background_vehicles()
- time.sleep(5.0)
+ callback = self._create_callback(save_dir, name, sensor_type)
+ camera.listen(callback)
- self.ped_controller.start_updates()
- return ego_vehicle
+ self.sensors.append(camera)
+ return True
- def set_weather(self):
- weather = carla.WeatherParameters()
+ except Exception as e:
+ Logger.warning(f"创建摄像头 {name} 失败: {e}")
+ return False
- if self.args.weather == 'clear':
- weather.sun_altitude_angle = 75
- weather.cloudiness = 5.0
- elif self.args.weather == 'rainy':
- weather.sun_altitude_angle = 40
- weather.cloudiness = 90.0
- weather.precipitation = 60.0
- else:
- weather.sun_altitude_angle = 60
- weather.cloudiness = 70.0
+ def _create_callback(self, save_dir, name, sensor_type):
+ capture_interval = self.config['sensors']['capture_interval']
- if self.args.time_of_day == 'night':
- weather.sun_altitude_angle = -10
- elif self.args.time_of_day == 'sunset':
- weather.sun_altitude_angle = 5
+ def callback(image):
+ current_time = time.time()
- self.world.set_weather(weather)
+ if current_time - self.last_capture_time >= capture_interval:
+ self.frame_counter += 1
+ self.last_capture_time = current_time
- def spawn_ego_vehicle(self):
- blueprint_lib = self.world.get_blueprint_library()
- vehicle_types = ['vehicle.tesla.model3', 'vehicle.audi.tt', 'vehicle.mini.cooperst']
+ filename = os.path.join(save_dir, f"{name}_{self.frame_counter:06d}.png")
+ image.save_to_disk(filename, carla.ColorConverter.Raw)
- vehicle_bp = None
- for vtype in vehicle_types:
- if blueprint_lib.filter(vtype):
- vehicle_bp = random.choice(blueprint_lib.filter(vtype))
- break
+ with self.buffer_lock:
+ if sensor_type == 'vehicle':
+ self.vehicle_buffer[name] = filename
+ if len(self.vehicle_buffer) >= 4:
+ self.image_stitcher.stitch(self.vehicle_buffer, self.frame_counter, 'vehicle')
+ self.vehicle_buffer.clear()
+ else:
+ self.infra_buffer[name] = filename
+ if len(self.infra_buffer) >= 4:
+ self.image_stitcher.stitch(self.infra_buffer, self.frame_counter, 'infrastructure')
+ self.infra_buffer.clear()
- if not vehicle_bp:
- vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*'))
+ return callback
- spawn_points = self.world.get_map().get_spawn_points()
- if not spawn_points:
- return None
+ def get_frame_count(self):
+ return self.frame_counter
- spawn_point = random.choice(spawn_points)
+ def cleanup(self):
+ Logger.info(f"清理 {len(self.sensors)} 个传感器...")
+ for sensor in self.sensors:
+ try:
+ sensor.stop()
+ sensor.destroy()
+ except:
+ pass
+ self.sensors.clear()
- try:
- vehicle = self.world.spawn_actor(vehicle_bp, spawn_point)
- self.actors.append(vehicle)
- vehicle.set_autopilot(True)
- vehicle.apply_control(carla.VehicleControl(throttle=0.2, steer=0.0))
- print(f"主车辆: {vehicle.type_id}")
- return vehicle
- except Exception as e:
- print(f"生成主车辆失败: {e}")
- return None
+class DataCollector:
+ def __init__(self, config):
+ self.config = config
+ self.client = None
+ self.world = None
+ self.ego_vehicle = None
+ self.scene_center = None
- def spawn_pedestrians(self):
- spawn_points = self.world.get_map().get_spawn_points()
- if not spawn_points:
- return
+ self.setup_directories()
- print(f"生成 {self.args.num_smart_pedestrians} 个行人...")
+ self.traffic_system = None
+ self.sensor_system = None
- behaviors = ['crossing', 'hesitant', 'walking']
+ self.start_time = None
+ self.is_running = False
- for _ in range(self.args.num_smart_pedestrians):
- behavior = random.choice(behaviors)
- spawn_point = random.choice(spawn_points)
+ def setup_directories(self):
+ scenario = self.config['scenario']
+ timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
- if self.ped_controller.spawn_pedestrian(spawn_point.location, behavior):
- self.actors.append(self.ped_controller.pedestrians[-1][0])
+ self.output_dir = os.path.join(
+ self.config['output']['data_dir'],
+ f"{scenario['name']}_{scenario['town']}_{timestamp}"
+ )
- def spawn_background_vehicles(self):
- blueprint_lib = self.world.get_blueprint_library()
- spawn_points = self.world.get_map().get_spawn_points()
+ for subdir in ["raw/vehicle", "raw/infrastructure", "stitched", "metadata"]:
+ os.makedirs(os.path.join(self.output_dir, subdir), exist_ok=True)
- if not spawn_points:
- return
+ Logger.info(f"数据目录: {self.output_dir}")
- spawned = 0
- for _ in range(min(5, self.args.num_background_vehicles)):
+ def connect(self):
+ for attempt in range(1, 6):
try:
- vehicle_bp = random.choice(blueprint_lib.filter('vehicle.*'))
- spawn_point = random.choice(spawn_points)
+ self.client = carla.Client('localhost', 2000)
+ self.client.set_timeout(10.0)
- vehicle = self.world.spawn_actor(vehicle_bp, spawn_point)
- self.actors.append(vehicle)
- vehicle.set_autopilot(True)
- spawned += 1
- except:
- pass
+ town = self.config['scenario']['town']
+ self.world = self.client.load_world(town)
- print(f"背景车辆: {spawned} 辆")
+ settings = self.world.get_settings()
+ settings.synchronous_mode = False
+ self.world.apply_settings(settings)
- def setup_cameras(self, vehicle):
- if not vehicle:
- return False
+ Logger.info(f"连接成功: {town}")
+ return True
- blueprint_lib = self.world.get_blueprint_library()
+ except Exception as e:
+ Logger.warning(f"连接尝试 {attempt}/5 失败: {str(e)[:50]}")
+ time.sleep(2)
- camera_configs = [
- ('front_wide', carla.Location(x=2.0, z=1.8), carla.Rotation(pitch=-3.0), 100),
- ('front_narrow', carla.Location(x=2.0, z=1.6), carla.Rotation(pitch=0), 60),
- ('right_side', carla.Location(x=0.5, y=1.0, z=1.5), carla.Rotation(pitch=-2.0, yaw=45), 90),
- ('left_side', carla.Location(x=0.5, y=-1.0, z=1.5), carla.Rotation(pitch=-2.0, yaw=-45), 90),
- ]
+ return False
- installed = 0
+ def setup_scene(self):
+ weather_cfg = self.config['scenario']
+ weather = WeatherManager.create_weather(weather_cfg['weather'], weather_cfg['time_of_day'])
+ self.world.set_weather(weather)
+ Logger.info(f"天气: {weather_cfg['weather']}, 时间: {weather_cfg['time_of_day']}")
- for name, location, rotation, fov in camera_configs:
- try:
- camera_bp = blueprint_lib.find('sensor.camera.rgb')
- camera_bp.set_attribute('image_size_x', '1280')
- camera_bp.set_attribute('image_size_y', '720')
- camera_bp.set_attribute('fov', str(fov))
-
- camera = self.world.spawn_actor(
- camera_bp,
- carla.Transform(location, rotation),
- attach_to=vehicle
- )
+ spawn_points = self.world.get_map().get_spawn_points()
+ if spawn_points:
+ self.scene_center = spawn_points[len(spawn_points) // 2].location
+ else:
+ self.scene_center = carla.Location(0, 0, 0)
- def make_callback(save_dir, cam_name):
- def callback(image):
- current_time = time.time()
+ self.traffic_system = TrafficSystem(self.world, self.config)
- if current_time - self.last_capture_time >= self.args.capture_interval:
- self.frame_count += 1
- self.last_capture_time = current_time
+ self.ego_vehicle = self.traffic_system.spawn_ego_vehicle()
+ if not self.ego_vehicle:
+ Logger.error("主车生成失败")
+ return False
- raw_filename = f"{save_dir}/{cam_name}_{self.frame_count:04d}.png"
- image.save_to_disk(raw_filename, carla.ColorConverter.Raw)
+ self.traffic_system.spawn_background_vehicles()
+ self.traffic_system.spawn_pedestrians(self.scene_center)
- with self.buffer_lock:
- self.image_buffer[cam_name] = raw_filename
+ time.sleep(2.0)
+ return True
- if len(self.image_buffer) == 4:
- self.stitcher.stitch_ego_vehicle_images(self.image_buffer, self.frame_count)
- self.image_buffer.clear()
+ def setup_sensors(self):
+ self.sensor_system = SensorSystem(self.world, self.config, self.output_dir)
- return callback
+ vehicle_cams = self.sensor_system.setup_vehicle_cameras(self.ego_vehicle)
+ infra_cams = self.sensor_system.setup_infrastructure_cameras(self.scene_center)
- camera.listen(make_callback(self.raw_dirs[name], name))
- self.actors.append(camera)
- self.sensors.append(camera)
- installed += 1
+ total_cams = vehicle_cams + infra_cams
+ if total_cams == 0:
+ Logger.error("没有摄像头安装成功")
+ return False
- except Exception as e:
- print(f"安装 {name} 摄像头失败: {e}")
+ Logger.info(f"摄像头总数: {total_cams}")
+ return True
- print(f"摄像头安装: {installed}/4")
- return installed == 4
+ def run_collection(self):
+ duration = self.config['scenario']['duration']
+ Logger.info(f"开始数据收集,时长: {duration}秒")
- def collect_data(self):
- print(f"\n开始数据收集...")
- print(f"时长: {self.args.total_dura.tion}秒, 间隔: {self.args.capture_interval}秒")
+ self.start_time = time.time()
+ self.is_running = True
- start_time = time.time()
- self.frame_count = 0
- self.last_capture_time = start_time
+ last_update = time.time()
try:
- while time.time() - start_time < self.args.total_duration:
- elapsed = time.time() - start_time
- remaining = self.args.total_duration - elapsed
+ while time.time() - self.start_time < duration and self.is_running:
+ current_time = time.time()
+ elapsed = current_time - self.start_time
- if int(elapsed) % 10 == 0:
- progress = (elapsed / self.args.total_duration) * 100
- print(f"进度: {elapsed:.0f}/{self.args.total_duration}秒 ({progress:.1f}%) | "
- f"批次: {self.frame_count} | 剩余: {remaining:.0f}秒")
+ if current_time - last_update >= 5.0:
+ frames = self.sensor_system.get_frame_count()
+ progress = (elapsed / duration) * 100
- time.sleep(0.1)
+ Logger.info(f"进度: {elapsed:.0f}/{duration}秒 ({progress:.1f}%) | 帧数: {frames}")
+ last_update = current_time
- print(f"\n数据收集完成! 总批次: {self.frame_count}")
+ time.sleep(0.05)
except KeyboardInterrupt:
- print(f"\n数据收集中断, 已收集 {self.frame_count} 批次")
+ Logger.info("数据收集被用户中断")
+ finally:
+ self.is_running = False
+ elapsed = time.time() - self.start_time
+
+ frames = self.sensor_system.get_frame_count() if self.sensor_system else 0
+ Logger.info(f"收集完成: {frames}帧, 用时: {elapsed:.1f}秒")
+
+ self._save_metadata(frames, elapsed)
+ self._print_summary()
+
+ def _save_metadata(self, total_frames, elapsed_time):
+ metadata = {
+ 'scenario': self.config['scenario'],
+ 'traffic': self.config['traffic'],
+ 'sensors': self.config['sensors'],
+ 'collection': {
+ 'duration': elapsed_time,
+ 'total_frames': total_frames,
+ 'frame_rate': total_frames / elapsed_time if elapsed_time > 0 else 0
+ }
+ }
- self.display_summary()
+ meta_path = os.path.join(self.output_dir, "metadata", "collection_info.json")
+ with open(meta_path, 'w', encoding='utf-8') as f:
+ json.dump(metadata, f, indent=2, ensure_ascii=False)
- def display_summary(self):
+ Logger.info(f"元数据保存: {meta_path}")
+
+ def _print_summary(self):
print("\n" + "=" * 60)
- print("数据收集摘要:")
+ print("数据收集摘要")
print("=" * 60)
- stitched_dir = os.path.join(self.output_dir, "stitched_images")
+ stitched_dir = os.path.join(self.output_dir, "stitched")
if os.path.exists(stitched_dir):
- stitched_files = [f for f in os.listdir(stitched_dir) if f.endswith('.jpg')]
- print(f"拼接图像: {len(stitched_files)} 张")
+ files = [f for f in os.listdir(stitched_dir) if f.endswith('.jpg')]
+ print(f"拼接图像: {len(files)} 张")
+
+ raw_dirs = ["vehicle", "infrastructure"]
+ for raw_dir in raw_dirs:
+ path = os.path.join(self.output_dir, "raw", raw_dir)
+ if os.path.exists(path):
+ total = 0
+ for root, dirs, files in os.walk(path):
+ total += len([f for f in files if f.endswith('.png')])
+ print(f"原始图像 ({raw_dir}): {total} 张")
+
+ print(f"\n输出目录: {self.output_dir}")
+ print("=" * 60)
- raw_dir = os.path.join(self.output_dir, "raw")
- if os.path.exists(raw_dir):
- total_raw = 0
- for root, dirs, files in os.walk(raw_dir):
- total_raw += len([f for f in files if f.endswith('.png')])
- print(f"原始图像: {total_raw} 张")
+ def cleanup(self):
+ Logger.info("清理场景...")
- print(f"\n数据目录: {self.output_dir}")
+ if self.sensor_system:
+ self.sensor_system.cleanup()
- def cleanup(self):
- if self.ped_controller:
- self.ped_controller.cleanup()
+ if self.traffic_system:
+ self.traffic_system.cleanup()
- for sensor in self.sensors:
+ if self.ego_vehicle and self.ego_vehicle.is_alive:
try:
- sensor.stop()
+ self.ego_vehicle.destroy()
except:
pass
- destroyed = 0
- for actor in self.actors:
- try:
- if actor.is_alive:
- actor.destroy()
- destroyed += 1
- except:
- pass
+ Logger.info("清理完成")
- print(f"清理 {destroyed} 个actor")
- self.actors.clear()
- self.sensors.clear()
+ # 运行数据验证
+ if self.config['output'].get('validate_data', True):
+ DataValidator.validate_dataset(self.output_dir)
def main():
- parser = argparse.ArgumentParser(description='CVIPS 数据生成器')
+ parser = argparse.ArgumentParser(description='CVIPS 数据收集系统')
- parser.add_argument('--scenario', type=str, default='pedestrian_scene', help='场景名称')
+ parser.add_argument('--config', type=str, help='配置文件路径')
+ parser.add_argument('--scenario', type=str, default='urban_scene', help='场景名称')
parser.add_argument('--town', type=str, default='Town10HD',
- choices=['Town03', 'Town04', 'Town05', 'Town10HD'], help='CARLA地图')
+ choices=['Town03', 'Town04', 'Town05', 'Town10HD'], help='地图')
parser.add_argument('--weather', type=str, default='clear',
- choices=['clear', 'rainy', 'cloudy'], help='天气条件')
+ choices=['clear', 'rainy', 'cloudy'], help='天气')
parser.add_argument('--time-of-day', type=str, default='noon',
choices=['noon', 'sunset', 'night'], help='时间')
- parser.add_argument('--num-smart-pedestrians', type=int, default=6, help='行人数')
- parser.add_argument('--num-background-vehicles', type=int, default=4, help='背景车辆数')
- parser.add_argument('--total-duration', type=int, default=90, help='总时长(秒)')
- parser.add_argument('--capture-interval', type=float, default=2.5, help='捕捉间隔(秒)')
+ parser.add_argument('--num-vehicles', type=int, default=8, help='背景车辆数')
+ parser.add_argument('--num-pedestrians', type=int, default=10, help='行人数')
+ parser.add_argument('--duration', type=int, default=60, help='收集时长(秒)')
+ parser.add_argument('--capture-interval', type=float, default=2.0, help='捕捉间隔(秒)')
+ parser.add_argument('--seed', type=int, help='随机种子')
+ parser.add_argument('--validate-data', action='store_true', help='启用数据验证')
args = parser.parse_args(remaining_argv)
- if args.capture_interval < 2.0:
- args.capture_interval = 2.0
+ config = ConfigManager.load_config(args.config)
+ config = ConfigManager.merge_args(config, args)
+
+ if args.validate_data:
+ config['output']['validate_data'] = True
+
+ print("\n" + "=" * 60)
+ print("CVIPS 数据收集系统")
+ print("=" * 60)
- print(f"\n场景配置:")
- print(f" 场景: {args.scenario}")
- print(f" 地图: {args.town}")
- print(f" 天气/时间: {args.weather}/{args.time_of_day}")
- print(f" 行人: {args.num_smart_pedestrians}个")
- print(f" 车辆: {args.num_background_vehicles}辆")
- print(f" 时长: {args.total_duration}秒")
- print(f" 间隔: {args.capture_interval}秒")
+ print(f"场景: {config['scenario']['name']}")
+ print(f"地图: {config['scenario']['town']}")
+ print(f"天气/时间: {config['scenario']['weather']}/{config['scenario']['time_of_day']}")
+ print(f"时长: {config['scenario']['duration']}秒")
+ print(f"交通: {config['traffic']['background_vehicles']}车辆 + {config['traffic']['pedestrians']}行人")
+ print(f"验证: {'启用' if config['output'].get('validate_data', True) else '禁用'}")
- generator = DataGenerator(args)
+ collector = DataCollector(config)
try:
- if not generator.connect_to_server():
+ if not collector.connect():
+ print("连接CARLA服务器失败")
return
- ego_vehicle = generator.setup_scene()
- if not ego_vehicle:
- generator.cleanup()
+ if not collector.setup_scene():
+ print("场景设置失败")
+ collector.cleanup()
return
- if not generator.setup_cameras(ego_vehicle):
- generator.cleanup()
+ if not collector.setup_sensors():
+ print("传感器设置失败")
+ collector.cleanup()
return
- generator.collect_data()
+ collector.run_collection()
except KeyboardInterrupt:
- print("\n程序中断")
+ print("\n程序被用户中断")
except Exception as e:
print(f"\n运行错误: {e}")
traceback.print_exc()
finally:
- generator.cleanup()
- print(f"\n数据保存到: {generator.output_dir}")
+ collector.cleanup()
+ print(f"\n数据集已保存到: {collector.output_dir}")
if __name__ == "__main__":
diff --git a/src/gesture_controll_drone/main.py b/src/gesture_controll_drone/main.py
index ad1a8a770b..63c2ade13d 100644
--- a/src/gesture_controll_drone/main.py
+++ b/src/gesture_controll_drone/main.py
@@ -10,46 +10,53 @@ class VirtualDrone:
"""
虚拟无人机模拟器类
负责创建无人机可视化界面、管理无人机状态、执行控制命令
-
- 属性:
- width/height: 窗口尺寸
- screen: pygame显示窗口
- position: 无人机2D坐标 [x, y]
- altitude: 无人机高度(m)
- battery: 电池电量(%)
- is_flying: 飞行状态标识
- speed: 移动速度
- 各类颜色/字体配置
"""
+ # 窗口配置常量
+ WINDOW_WIDTH = 400
+ WINDOW_HEIGHT = 300
+
+ # 无人机初始状态常量
+ INIT_POSITION = [200, 150]
+ INIT_ALTITUDE = 0
+ INIT_BATTERY = 100
+ SPEED = 3
+ # 颜色常量
+ BG_COLOR = (30, 30, 50)
+ GROUND_COLOR = (50, 50, 70)
+ DRONE_COLOR_FLYING = (0, 255, 0)
+ DRONE_COLOR_GROUND = (255, 100, 100)
+ PROPELLER_COLOR = (200, 200, 200)
+ TEXT_COLOR = (255, 255, 255)
+
+ # 渲染常量
+ DRONE_RADIUS = 15
+ PROPELLER_RADIUS = 6
+ GROUND_HEIGHT = 100
+ BATTERY_CONSUMPTION_RATE = 0.05
+
def __init__(self):
"""初始化pygame环境和无人机初始状态"""
- pygame.init()
+ try:
+ pygame.init()
+ except pygame.error as e:
+ print(f"Pygame初始化失败: {e}")
+ raise
# 窗口配置
- self.width, self.height = 400, 300
- self.screen = pygame.display.set_mode((self.width, self.height))
+ self.screen = pygame.display.set_mode((self.WINDOW_WIDTH, self.WINDOW_HEIGHT))
pygame.display.set_caption("虚拟无人机模拟器")
# 无人机初始状态
- self.position = [200, 150] # 初始位置居中
- self.altitude = 0 # 初始高度为0(地面)
- self.battery = 100 # 初始电量100%
- self.is_flying = False # 初始未飞行
- self.speed = 3 # 移动速度
+ self.position = self.INIT_POSITION.copy()
+ self.altitude = self.INIT_ALTITUDE
+ self.battery = self.INIT_BATTERY
+ self.is_flying = False
# 视觉样式配置
- self._init_visual_config()
+ self.font = pygame.font.Font(None, 24)
- print("虚拟无人机模拟器已启动")
-
- def _init_visual_config(self):
- """初始化颜色和字体配置(提取独立函数)"""
- self.bg_color = (30, 30, 50)
- self.drone_color_flying = (0, 255, 0) # 飞行中绿色
- self.drone_color_ground = (255, 100, 100) # 地面红色
- self.text_color = (255, 255, 255) # 白色文字
- self.font = pygame.font.Font(None, 24) # 默认字体,24号大小
+ print("✅ 虚拟无人机模拟器已启动")
def execute_command(self, command):
"""
@@ -63,72 +70,82 @@ def execute_command(self, command):
"""
result = False
- # 命令执行逻辑(功能完全不变)
- if command == "起飞" and not self.is_flying:
- self.is_flying = True
- self.altitude = 10
- print("🛫 无人机起飞")
- result = True
+ try:
+ # 命令执行逻辑
+ if command == "起飞" and not self.is_flying:
+ self.is_flying = True
+ self.altitude = 10
+ print("🛫 无人机起飞")
+ result = True
+
+ elif command == "降落" and self.is_flying:
+ self.is_flying = False
+ self.altitude = 0
+ print("🛬 无人机降落")
+ result = True
+
+ elif command == "前进" and self.is_flying:
+ self.position[1] = max(50, self.position[1] - self.SPEED)
+ self.altitude = min(50, self.altitude + 0.5)
+ print("➡️ 无人机前进")
+ result = True
+
+ elif command == "上升" and self.is_flying:
+ self.altitude = min(100, self.altitude + 10)
+ print(f"⬆️ 无人机上升 | 当前高度: {self.altitude}m")
+ result = True
+
+ elif command == "紧急停止":
+ self.is_flying = False
+ self.altitude = 0
+ print("🚨 紧急停止!")
+ result = True
+
+ # 模拟电池消耗(仅飞行时消耗)
+ if self.is_flying:
+ self.battery = max(0, self.battery - self.BATTERY_CONSUMPTION_RATE)
+
+ except Exception as e:
+ print(f"❌ 执行命令 '{command}' 时出错: {e}")
+ result = False
- elif command == "降落" and self.is_flying:
- self.is_flying = False
- self.altitude = 0
- print("🛬 无人机降落")
- result = True
+ return result
+
+ def draw(self):
+ """绘制无人机界面和状态信息(视觉渲染主函数)"""
+ try:
+ # 清屏
+ self.screen.fill(self.BG_COLOR)
- elif command == "前进" and self.is_flying:
- self.position[1] = max(50, self.position[1] - self.speed)
- self.altitude = min(50, self.altitude + 0.5)
- print("➡️ 无人机前进")
- result = True
+ # 绘制地面
+ pygame.draw.rect(
+ self.screen,
+ self.GROUND_COLOR,
+ (0, self.WINDOW_HEIGHT - self.GROUND_HEIGHT, self.WINDOW_WIDTH, self.GROUND_HEIGHT)
+ )
- elif command == "上升" and self.is_flying:
- self.altitude = min(100, self.altitude + 10)
- print(f"⬆️ 无人机上升 | 当前高度: {self.altitude}m")
- result = True
+ # 绘制无人机(根据飞行状态切换颜色)
+ drone_color = self.DRONE_COLOR_FLYING if self.is_flying else self.DRONE_COLOR_GROUND
+ drone_y = self.WINDOW_HEIGHT - 120 - self.altitude * 2
- elif command == "紧急停止":
- self.is_flying = False
- self.altitude = 0
- print("🚨 紧急停止!")
- result = True
+ # 绘制无人机主体(圆形)
+ pygame.draw.circle(self.screen, drone_color, (self.position[0], drone_y), self.DRONE_RADIUS)
- # 模拟电池消耗(仅飞行时消耗)
- if self.is_flying:
- self.battery = max(0, self.battery - 0.05)
+ # 绘制无人机螺旋桨(四个小圆形)
+ self._draw_drone_propellers(drone_y)
- return result
-
- def draw(self):
- """绘制无人机界面和状态信息(视觉渲染主函数)"""
- # 清屏
- self.screen.fill(self.bg_color)
-
- # 绘制地面
- pygame.draw.rect(self.screen, (50, 50, 70), (0, self.height - 100, self.width, 100))
-
- # 绘制无人机(根据飞行状态切换颜色)
- drone_color = self.drone_color_flying if self.is_flying else self.drone_color_ground
- drone_y = self.height - 120 - self.altitude * 2
-
- # 绘制无人机主体(圆形)
- pygame.draw.circle(self.screen, drone_color, (self.position[0], drone_y), 15)
-
- # 绘制无人机螺旋桨(四个小圆形)
- self._draw_drone_propellers(drone_y)
-
- # 绘制状态信息和控制说明
- self._draw_status_info()
- self._draw_control_instructions()
-
- # 更新显示
- pygame.display.flip()
+ # 绘制状态信息和控制说明
+ self._draw_status_info()
+ self._draw_control_instructions()
+
+ # 更新显示
+ pygame.display.flip()
+
+ except Exception as e:
+ print(f"❌ 绘制界面时出错: {e}")
def _draw_drone_propellers(self, drone_y):
- """绘制无人机螺旋桨(提取独立函数)"""
- propeller_color = (200, 200, 200) # 螺旋桨颜色
- propeller_radius = 6 # 螺旋桨半径
-
+ """绘制无人机螺旋桨"""
# 四个螺旋桨位置
prop_positions = [
(self.position[0] - 20, drone_y - 12),
@@ -138,10 +155,10 @@ def _draw_drone_propellers(self, drone_y):
]
for pos in prop_positions:
- pygame.draw.circle(self.screen, propeller_color, pos, propeller_radius)
+ pygame.draw.circle(self.screen, self.PROPELLER_COLOR, pos, self.PROPELLER_RADIUS)
def _draw_status_info(self):
- """绘制无人机状态信息(提取独立函数)"""
+ """绘制无人机状态信息"""
status = "飞行中" if self.is_flying else "在地面"
texts = [
f"状态: {status}",
@@ -152,11 +169,11 @@ def _draw_status_info(self):
# 逐行绘制状态文本
for i, text in enumerate(texts):
- text_surface = self.font.render(text, True, self.text_color)
+ text_surface = self.font.render(text, True, self.TEXT_COLOR)
self.screen.blit(text_surface, (10, 10 + i * 25))
def _draw_control_instructions(self):
- """绘制控制说明文本(提取独立函数)"""
+ """绘制控制说明文本"""
controls = [
"控制说明:",
"张开手掌 - 起飞",
@@ -168,40 +185,61 @@ def _draw_control_instructions(self):
# 逐行绘制控制说明
for i, control in enumerate(controls):
- text_surface = self.font.render(control, True, self.text_color)
- self.screen.blit(text_surface, (self.width - 200, 10 + i * 25))
+ text_surface = self.font.render(control, True, self.TEXT_COLOR)
+ self.screen.blit(text_surface, (self.WINDOW_WIDTH - 200, 10 + i * 25))
def process_events(self):
"""处理pygame窗口事件(如关闭窗口)"""
- for event in pygame.event.get():
- if event.type == QUIT:
- return False
- return True
+ try:
+ for event in pygame.event.get():
+ if event.type == QUIT:
+ return False
+ return True
+ except Exception as e:
+ print(f"❌ 处理窗口事件时出错: {e}")
+ return False
class GestureRecognizer:
"""
手势识别器类
基于MediaPipe实现手部关键点检测,识别预设手势并转换为控制命令
-
- 属性:
- mp_hands: MediaPipe手部检测模块
- hands: 手部检测处理器
- mp_drawing: 关键点绘制工具
- cap: 摄像头捕获对象
"""
+ # 摄像头配置常量
+ CAMERA_WIDTH = 640
+ CAMERA_HEIGHT = 480
+ CAMERA_INDICES_TO_TRY = [0, 1, 2, 3, 4]
+
+ # 手势检测常量
+ HAND_DETECTION_CONFIDENCE = 0.6
+ HAND_TRACKING_CONFIDENCE = 0.5
+ MAX_HANDS = 1
+ OK_GESTURE_DISTANCE_THRESHOLD = 30
+ FINGER_BENT_THRESHOLD = 20
+
+ # 关键点索引常量
+ THUMB_TIP = 4
+ INDEX_FINGER_TIP = 8
+ MIDDLE_FINGER_TIP = 12
+ RING_FINGER_TIP = 16
+ PINKY_TIP = 20
def __init__(self):
"""初始化MediaPipe手部检测和摄像头"""
self.mp_hands = mp.solutions.hands
- # 配置手部检测参数(优化参数命名,语义不变)
- self.hands = self.mp_hands.Hands(
- static_image_mode=False, # 视频流模式
- max_num_hands=1, # 仅检测单只手
- min_detection_confidence=0.6, # 检测置信度阈值
- min_tracking_confidence=0.5 # 跟踪置信度阈值
- )
self.mp_drawing = mp.solutions.drawing_utils
- self.cap = None # 摄像头对象初始化为None
+ self.cap = None
+
+ # 初始化手部检测器
+ try:
+ self.hands = self.mp_hands.Hands(
+ static_image_mode=False,
+ max_num_hands=self.MAX_HANDS,
+ min_detection_confidence=self.HAND_DETECTION_CONFIDENCE,
+ min_tracking_confidence=self.HAND_TRACKING_CONFIDENCE
+ )
+ except Exception as e:
+ print(f"❌ MediaPipe手部检测器初始化失败: {e}")
+ raise
def initialize_camera(self):
"""
@@ -210,24 +248,23 @@ def initialize_camera(self):
返回:
bool: 摄像头初始化是否成功
"""
- print("初始化摄像头...")
- self.cap = cv2.VideoCapture(0)
+ print("🔍 初始化摄像头...")
- # 如果索引0失败,尝试1-4索引
- if not self.cap.isOpened():
- for cam_index in range(1, 5):
+ for cam_index in self.CAMERA_INDICES_TO_TRY:
+ try:
self.cap = cv2.VideoCapture(cam_index)
if self.cap.isOpened():
- print(f"找到摄像头在索引 {cam_index}")
- break
- else:
- raise Exception("无法找到可用的摄像头")
-
- # 设置摄像头分辨率
- self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
- self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
- print("摄像头初始化成功")
- return True
+ # 设置摄像头分辨率
+ self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.CAMERA_WIDTH)
+ self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.CAMERA_HEIGHT)
+ print(f"✅ 找到摄像头在索引 {cam_index}")
+ print("✅ 摄像头初始化成功")
+ return True
+ except Exception as e:
+ print(f"⚠️ 摄像头索引 {cam_index} 初始化失败: {e}")
+ continue
+
+ raise Exception("❌ 无法找到可用的摄像头")
def detect_gesture(self, frame):
"""
@@ -241,76 +278,52 @@ def detect_gesture(self, frame):
gesture: 识别到的手势名称
command: 对应的控制命令
"""
- # 转换颜色空间(BGR -> RGB)
- rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
- results = self.hands.process(rgb_frame)
-
gesture = "未检测到手势"
command = "等待"
- # 如果检测到手部关键点
- if results.multi_hand_landmarks:
- for hand_landmarks in results.multi_hand_landmarks:
- # 绘制手部关键点和连接线
- self.mp_drawing.draw_landmarks(
- frame, hand_landmarks, self.mp_hands.HAND_CONNECTIONS)
-
- # 提取关键点坐标
- landmarks = self._extract_landmarks(hand_landmarks, frame.shape)
-
- # 识别手势并转换为命令
- gesture = self._improved_classify_gesture(landmarks)
- command = self._gesture_to_command(gesture)
+ try:
+ # 转换颜色空间(BGR -> RGB)
+ rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+ results = self.hands.process(rgb_frame)
+
+ # 如果检测到手部关键点
+ if results.multi_hand_landmarks:
+ for hand_landmarks in results.multi_hand_landmarks:
+ # 绘制手部关键点和连接线
+ self.mp_drawing.draw_landmarks(
+ frame, hand_landmarks, self.mp_hands.HAND_CONNECTIONS)
+
+ # 提取关键点坐标
+ landmarks = self._extract_landmarks(hand_landmarks, frame.shape)
+
+ # 识别手势并转换为命令
+ gesture = self._improved_classify_gesture(landmarks)
+ command = self._gesture_to_command(gesture)
+
+ except Exception as e:
+ print(f"❌ 手势检测时出错: {e}")
return frame, gesture, command
def _extract_landmarks(self, hand_landmarks, frame_shape):
- """
- 提取手部关键点的像素坐标
-
- 参数:
- hand_landmarks: MediaPipe手部关键点对象
- frame_shape: 视频帧形状 (h, w, c)
-
- 返回:
- list: 关键点像素坐标列表 [(x1,y1), (x2,y2), ...]
- """
+ """提取手部关键点的像素坐标"""
h, w, _ = frame_shape
landmarks = []
for lm in hand_landmarks.landmark:
- # 将归一化坐标转换为像素坐标
px = int(lm.x * w)
py = int(lm.y * h)
landmarks.append((px, py))
return landmarks
def _improved_classify_gesture(self, landmarks):
- """
- 改进的手势分类算法(支持OK手势检测)
-
- 参数:
- landmarks: 手部关键点像素坐标列表
-
- 返回:
- str: 识别到的手势名称
- """
+ """改进的手势分类算法(支持OK手势检测)"""
# 校验关键点数量
if not landmarks or len(landmarks) < 21:
return "未检测到手势"
- # 定义关键点索引常量(提高可读性)
- THUMB_TIP = 4 # 拇指指尖
- INDEX_FINGER_TIP = 8 # 食指指尖
- MIDDLE_FINGER_TIP = 12 # 中指指尖
- RING_FINGER_TIP = 16 # 无名指指尖
- PINKY_TIP = 20 # 小指指尖
-
# 获取关键点位坐标
- thumb_tip = landmarks[THUMB_TIP]
- index_tip = landmarks[INDEX_FINGER_TIP]
- middle_tip = landmarks[MIDDLE_FINGER_TIP]
- ring_tip = landmarks[RING_FINGER_TIP]
- pinky_tip = landmarks[PINKY_TIP]
+ thumb_tip = landmarks[self.THUMB_TIP]
+ index_tip = landmarks[self.INDEX_FINGER_TIP]
# 检测各手指是否伸直
fingers = self._detect_extended_fingers(landmarks)
@@ -320,7 +333,7 @@ def _improved_classify_gesture(self, landmarks):
# 检测OK手势(拇指和食指接触,其他手指弯曲)
thumb_index_dist = np.sqrt((thumb_tip[0]-index_tip[0])**2 + (thumb_tip[1]-index_tip[1])**2)
- if thumb_index_dist < 30 and extended_fingers <= 3:
+ if thumb_index_dist < self.OK_GESTURE_DISTANCE_THRESHOLD and extended_fingers <= 3:
# 检查中指、无名指、小指是否弯曲
other_fingers_bent = self._check_other_fingers_bent(landmarks)
if other_fingers_bent:
@@ -339,23 +352,15 @@ def _improved_classify_gesture(self, landmarks):
return "其他手势"
def _detect_extended_fingers(self, landmarks):
- """
- 检测每根手指是否伸直
-
- 参数:
- landmarks: 手部关键点坐标列表
-
- 返回:
- list: 手指伸直状态 [拇指, 食指, 中指, 无名指, 小指]
- """
+ """检测每根手指是否伸直"""
fingers = []
# 拇指:比较指尖和IP关节的x坐标
- fingers.append(landmarks[4][0] < landmarks[3][0])
+ fingers.append(landmarks[self.THUMB_TIP][0] < landmarks[self.THUMB_TIP-1][0])
# 其他手指:比较指尖和PIP关节的y坐标
- finger_tips = [8, 12, 16, 20]
- finger_pips = [6, 10, 14, 18]
+ finger_tips = [self.INDEX_FINGER_TIP, self.MIDDLE_FINGER_TIP, self.RING_FINGER_TIP, self.PINKY_TIP]
+ finger_pips = [self.INDEX_FINGER_TIP-2, self.MIDDLE_FINGER_TIP-2, self.RING_FINGER_TIP-2, self.PINKY_TIP-2]
for tip, pip in zip(finger_tips, finger_pips):
fingers.append(landmarks[tip][1] < landmarks[pip][1])
@@ -364,25 +369,17 @@ def _detect_extended_fingers(self, landmarks):
def _check_other_fingers_bent(self, landmarks):
"""检查中指、无名指、小指是否弯曲(OK手势辅助检测)"""
- finger_tips = [12, 16, 20]
- finger_pips = [10, 14, 18]
+ finger_tips = [self.MIDDLE_FINGER_TIP, self.RING_FINGER_TIP, self.PINKY_TIP]
+ finger_pips = [self.MIDDLE_FINGER_TIP-2, self.RING_FINGER_TIP-2, self.PINKY_TIP-2]
for tip, pip in zip(finger_tips, finger_pips):
- # 如果手指伸直超过20像素,判定为未弯曲
- if landmarks[tip][1] < landmarks[pip][1] - 20:
+ # 如果手指伸直超过阈值,判定为未弯曲
+ if landmarks[tip][1] < landmarks[pip][1] - self.FINGER_BENT_THRESHOLD:
return False
return True
def _gesture_to_command(self, gesture):
- """
- 将识别到的手势映射为无人机控制命令
-
- 参数:
- gesture: 手势名称
-
- 返回:
- str: 控制命令
- """
+ """将识别到的手势映射为无人机控制命令"""
command_map = {
"张开手掌": "起飞",
"握拳": "降落",
@@ -395,52 +392,60 @@ def _gesture_to_command(self, gesture):
return command_map.get(gesture, "等待")
def release_camera(self):
- """释放摄像头资源"""
- if self.cap:
- self.cap.release()
+ """释放摄像头资源(容错处理)"""
+ try:
+ if self.cap and self.cap.isOpened():
+ self.cap.release()
+ print("✅ 摄像头资源已释放")
+ except Exception as e:
+ print(f"⚠️ 释放摄像头时出错: {e}")
class GestureDroneSystem:
"""
手势控制无人机主系统类
整合手势识别和无人机模拟器,提供完整的交互流程
"""
+ # 系统配置常量
+ COMMAND_INTERVAL = 1.0 # 命令执行最小间隔(秒)
+ EXIT_KEY = ord('q') # 退出程序按键
def __init__(self):
"""初始化手势识别器和无人机模拟器"""
self.gesture_recognizer = GestureRecognizer()
self.drone_simulator = VirtualDrone()
- self.is_running = False # 系统运行状态
+ self.is_running = False
def initialize(self):
- """
- 初始化整个系统
-
- 返回:
- bool: 初始化是否成功
- """
+ """初始化整个系统"""
print("=" * 50)
- print("手势控制无人机系统")
+ print("🤖 手势控制无人机系统")
print("=" * 50)
- # 初始化摄像头
- if not self.gesture_recognizer.initialize_camera():
- return False
+ try:
+ # 初始化摄像头
+ if not self.gesture_recognizer.initialize_camera():
+ return False
+
+ # 打印使用说明
+ self._print_usage_instructions()
- # 打印使用说明
- self._print_usage_instructions()
-
- return True
+ return True
+
+ except Exception as e:
+ print(f"❌ 系统初始化失败: {e}")
+ self.cleanup()
+ return False
def _print_usage_instructions(self):
- """打印系统使用说明(提取独立函数)"""
- print("\n系统初始化完成!")
- print("\n手势控制说明:")
+ """打印系统使用说明"""
+ print("\n✅ 系统初始化完成!")
+ print("\n📋 手势控制说明:")
print("✋ 张开手掌 - 起飞")
print("✊ 握拳 - 降落")
print("👆 食指指向 - 前进")
print("✌️ 胜利手势 - 上升")
print("👌 OK手势 - 紧急停止")
- print("\n按 'q' 键退出程序")
+ print(f"\n⌨️ 按 '{chr(self.EXIT_KEY)}' 键退出程序")
print("=" * 50)
def run(self):
@@ -449,13 +454,12 @@ def run(self):
return
self.is_running = True
- print("开始手势控制...")
+ print("▶️ 开始手势控制...")
# 性能统计变量
frame_count = 0
start_time = time.time()
last_command_time = 0
- command_interval = 1.0 # 命令执行间隔(防止重复执行)
try:
while self.is_running:
@@ -466,8 +470,9 @@ def run(self):
# 读取摄像头帧
ret, frame = self.gesture_recognizer.cap.read()
if not ret:
- print("无法读取摄像头帧")
- break
+ print("⚠️ 无法读取摄像头帧,重试中...")
+ time.sleep(0.1)
+ continue
# 帧计数+1,水平翻转帧(镜像显示)
frame_count += 1
@@ -478,25 +483,28 @@ def run(self):
# 执行控制命令(带时间间隔限制)
current_time = time.time()
- if (current_time - last_command_time > command_interval and
+ if (current_time - last_command_time > self.COMMAND_INTERVAL and
command != "等待"):
if self.drone_simulator.execute_command(command):
print(f"✅ 执行命令: {command}")
last_command_time = current_time
elif command != "等待":
- print(f"⏳ 识别到: {gesture} -> {command}")
+ print(f"⏳ 识别到: {gesture} -> {command} (冷却中)")
# 显示帧信息、更新无人机界面
self._display_info(processed_frame, gesture, command, frame_count, start_time)
- cv2.imshow('手势识别摄像头', processed_frame)
+ cv2.imshow('📷 手势识别摄像头', processed_frame)
self.drone_simulator.draw()
# 检测退出按键
- if cv2.waitKey(1) & 0xFF == ord('q'):
+ if cv2.waitKey(1) & 0xFF == self.EXIT_KEY:
+ print("\n🛑 用户请求退出程序")
break
+ except KeyboardInterrupt:
+ print("\n🛑 用户中断程序执行")
except Exception as e:
- print(f"运行时错误: {e}")
+ print(f"❌ 系统运行时错误: {e}")
finally:
self.cleanup()
@@ -509,18 +517,19 @@ def _display_info(self, frame, gesture, command, frame_count, start_time):
elapsed_time = time.time() - start_time
fps = frame_count / elapsed_time if elapsed_time > 0 else 0
- # 绘制文本信息(优化坐标和样式)
- cv2.putText(frame, f"手势: {gesture}", (10, 30),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
-
- cv2.putText(frame, f"命令: {command}", (10, 60),
- cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
-
- cv2.putText(frame, f"FPS: {fps:.1f}", (10, 90),
- cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
+ # 绘制文本信息
+ info_texts = [
+ (f"🤘 手势: {gesture}", (10, 30), (0, 255, 0), 0.7, 2),
+ (f"🎮 命令: {command}", (10, 60), (0, 255, 255), 0.7, 2),
+ (f"⚡ FPS: {fps:.1f}", (10, 90), (255, 255, 255), 0.6, 2),
+ (f"按 '{chr(self.EXIT_KEY)}' 退出", (10, 450), (255, 255, 255), 0.5, 1)
+ ]
- cv2.putText(frame, "按 'q' 退出", (10, 450),
- cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
+ for text, pos, color, scale, thickness in info_texts:
+ cv2.putText(
+ frame, text, pos,
+ cv2.FONT_HERSHEY_SIMPLEX, scale, color, thickness
+ )
def _show_performance_stats(self, start_time, frame_count):
"""显示系统性能统计信息"""
@@ -528,22 +537,42 @@ def _show_performance_stats(self, start_time, frame_count):
avg_fps = frame_count / total_time if total_time > 0 else 0
print("\n" + "=" * 50)
- print("性能统计")
+ print("📊 性能统计")
print("=" * 50)
- print(f"总运行时间: {total_time:.2f} 秒")
- print(f"处理帧数: {frame_count}")
- print(f"平均FPS: {avg_fps:.2f}")
+ print(f"⏱️ 总运行时间: {total_time:.2f} 秒")
+ print(f"🖼️ 处理帧数: {frame_count}")
+ print(f"⚡ 平均FPS: {avg_fps:.2f}")
print("=" * 50)
def cleanup(self):
"""清理系统资源(摄像头、窗口、pygame)"""
self.is_running = False
- self.gesture_recognizer.release_camera()
- cv2.destroyAllWindows()
- pygame.quit()
- print("系统已关闭")
+ print("\n🧹 正在清理系统资源...")
+
+ try:
+ self.gesture_recognizer.release_camera()
+ except Exception as e:
+ print(f"⚠️ 释放摄像头资源时出错: {e}")
+
+ try:
+ cv2.destroyAllWindows()
+ print("✅ OpenCV窗口已关闭")
+ except Exception as e:
+ print(f"⚠️ 关闭OpenCV窗口时出错: {e}")
+
+ try:
+ pygame.quit()
+ print("✅ Pygame资源已释放")
+ except Exception as e:
+ print(f"⚠️ 退出Pygame时出错: {e}")
+
+ print("✅ 系统已安全关闭")
if __name__ == "__main__":
# 创建并运行系统
- drone_system = GestureDroneSystem()
- drone_system.run()
+ try:
+ drone_system = GestureDroneSystem()
+ drone_system.run()
+ except Exception as e:
+ print(f"❌ 程序执行失败: {e}")
+ sys.exit(1)
diff --git a/src/gesture_controll_drone/requirements.txt b/src/gesture_controll_drone/requirements.txt
index 853857c78d..a6a207d7e8 100644
--- a/src/gesture_controll_drone/requirements.txt
+++ b/src/gesture_controll_drone/requirements.txt
@@ -4,3 +4,5 @@ mediapipe==0.8.9
# 数据处理和数学计算
numpy==1.21.0
+#模拟无人机
+pygame==2.6.1
diff --git a/src/humanoid_loco_mujoco/main.py b/src/humanoid_loco_mujoco/main.py
index 658c36a02e..8d2b980da3 100644
--- a/src/humanoid_loco_mujoco/main.py
+++ b/src/humanoid_loco_mujoco/main.py
@@ -1,16 +1,101 @@
import numpy as np
from loco_mujoco.task_factories import ImitationFactory, LAFAN1DatasetConf, DefaultDatasetConf, AMASSDatasetConf
+from loco_mujoco.core import ObservationType
+randomization_config = {
+ # gravity
+ "randomize_gravity": True,
+ "gravity_range": [9.51, 10.11],
+ # geom properties
+ "randomize_geom_friction_tangential": True,
+ "geom_friction_tangential_range": [0.8, 1.2],
+ "randomize_geom_friction_torsional": True,
+ "geom_friction_torsional_range": [0.003, 0.007],
+ "randomize_geom_friction_rolling": True,
+ "geom_friction_rolling_range": [0.00008, 0.00012],
+ "randomize_geom_damping": True,
+ "geom_damping_range": [72, 88],
+ "randomize_geom_stiffness": True,
+ "geom_stiffness_range": [900, 1100],
+
+ # joint properties
+ "randomize_joint_damping": True,
+ "joint_damping_range": [0.3, 1.5],
+ "randomize_joint_stiffness": True,
+ "joint_stiffness_range": [0.9, 1.1],
+ "randomize_joint_friction_loss": True,
+ "joint_friction_loss_range": [0.0, 0.2],
+ "randomize_joint_armature": True,
+ "joint_armature_range": [0.08, 0.12],
+
+ # base mass
+ "randomize_base_mass": True,
+ "base_mass_to_add_range": [-2.0, 2.0],
+
+ # COM
+ "randomize_com_displacement": True,
+ "com_displacement_range": [-0.15, 0.15],
+
+ # link mass
+ "randomize_link_mass": True,
+ "link_mass_multiplier_range": {
+ "root_body": [0.5, 1.9],
+ "other_bodies": [0.8, 1.2],
+ },
+
+ # PD Gains (if PDControl is used)
+ "add_p_gains_noise": True,
+ "add_d_gains_noise": True,
+ "p_gains_noise_scale": 0.1,
+ "d_gains_noise_scale": 0.1,
+
+ # Observation Noise
+ "add_joint_pos_noise": True,
+ "joint_pos_noise_scale": 0.003,
+ "add_joint_vel_noise": True,
+ "joint_vel_noise_scale": 0.08,
+ "add_gravity_noise": True,
+ "gravity_noise_scale": 0.015,
+ "add_free_joint_lin_vel_noise": True,
+ "lin_vel_noise_scale": 0.1,
+ "add_free_joint_ang_vel_noise": True,
+ "ang_vel_noise_scale": 0.02,
+}
+observation_spec = [
+ # prioritized observations
+ ObservationType.FreeJointPosNoXY(obs_name="free_joint", xml_name="root", group="prioritized", allow_randomization=False),
+ ObservationType.FreeJointVel(obs_name="free_joint_vel", xml_name="root", group="prioritized", allow_randomization=False),
+ # 修正左髋关节名称(之前的KeyError问题)
+ ObservationType.JointPos(obs_name="joint_pos", xml_name="hip_flexion_l", group="prioritized", allow_randomization=False),
+ # 修正右髋关节名称
+ ObservationType.JointVel(obs_name="joint_vel1", xml_name="hip_flexion_r", group="prioritized", allow_randomization=False),
+ # 修正左膝关节名称
+ ObservationType.JointVel(obs_name="joint_vel2", xml_name="knee_angle_l", group="prioritized", allow_randomization=False),
+ # 将head替换为有效的身体名称(例如torso_link)
+ ObservationType.BodyPos(obs_name="head_pos", xml_name="torso_link", group="prioritized", allow_randomization=False),
+ # policy observations --> these will be noisy
+ ObservationType.ProjectedGravityVector(obs_name="proj_grav", xml_name="root", group="policy", allow_randomization=True),
+ ObservationType.FreeJointVel(obs_name="free_joint_vel_pi", xml_name="root", group="policy", allow_randomization=True),
+ # 修正左髋关节名称
+ ObservationType.JointPos(obs_name="joint_pos_pi", xml_name="hip_flexion_l", group="policy", allow_randomization=True),
+ # 修正右髋关节名称
+ ObservationType.JointVel(obs_name="joint_vel1_pi", xml_name="hip_flexion_r", group="policy", allow_randomization=True),
+ # 修正左膝关节名称
+ ObservationType.JointVel(obs_name="joint_vel2_pi", xml_name="knee_angle_l", group="policy", allow_randomization=True),
+ # 将head替换为有效的身体名称(例如torso_link)
+ ObservationType.BodyPos(obs_name="head_pos_pi", xml_name="torso_link", group="policy", allow_randomization=True),
+ ObservationType.LastAction(obs_name="last_action_pi", group="policy", allow_randomization=True)
+]
# # example --> you can add as many datasets as you want in the lists!
env = ImitationFactory.make("UnitreeH1",
default_dataset_conf=DefaultDatasetConf(["squat", "walk"]),
- lafan1_dataset_conf=LAFAN1DatasetConf(["dance2_subject4"]),
- # if SMPL and AMASS are installed, you can use the following:
- # amass_dataset_conf=AMASSDatasetConf(["DanceDB/DanceDB/20120911_TheodorosSourmelis/Capoeira_Theodoros_v2_C3D_poses",
- # "KIT/12/WalkInClockwiseCircle11_poses",
- # "HUMAN4D/HUMAN4D/Subject3_Medhi/INF_JumpingJack_S3_01_poses",
- # 'KIT/359/walking_fast05_poses']),
+ terrain_type="RoughTerrain",
+ terrain_params=dict(random_min_height=-0.1,random_max_height=0.1,random_downsampled_scale=0.5),
+ domain_randomization_type="DefaultRandomizer",
+ domain_randomization_params=randomization_config,
+ observation_spec=observation_spec,
+ control_type="PDControl", control_params=dict(p_gain=100, d_gain=1),
n_substeps=20)
env.play_trajectory(n_episodes=3, n_steps_per_episode=500, render=True)
diff --git a/src/humanoid_loco_mujoco/ros2/robot_controller.py b/src/humanoid_loco_mujoco/ros2/robot_controller.py
new file mode 100644
index 0000000000..43dcf9b4ef
--- /dev/null
+++ b/src/humanoid_loco_mujoco/ros2/robot_controller.py
@@ -0,0 +1,95 @@
+import sys
+import select
+import rclpy
+from rclpy.node import Node
+from my_interfaces.srv import RobotCtrl
+
+# 全局变量(提前初始化,避免未绑定)
+current_action_key = 'stop'
+action_switch_flag = False
+
+
+class RobotController(Node):
+ def __init__(self, name):
+ super().__init__(name)
+ # 创建服务客户端,连接机器人控制服务
+ self.client = self.create_client(RobotCtrl, 'robot_control')
+ # 等待服务上线
+ while not self.client.wait_for_service(timeout_sec=1.0):
+ self.get_logger().info('等待服务 "robot_control" 启动...')
+ self.get_logger().info("操作提示:w=行走 | s=下蹲 | q=退出 | 其他键=停止")
+
+ def result_callback(self, future):
+ """服务调用结果的回调函数"""
+
+ response = future.result()
+ self.get_logger().info(f'指令执行成功:{response.message}')
+
+
+ def send_request(self, action):
+ """发送控制指令到服务端"""
+
+ request = RobotCtrl.Request()
+ request.command = action # 传递动作指令
+ self.client.call_async(request).add_done_callback(self.result_callback)
+ self.get_logger().info(f'已发送指令:{action}')
+
+
+
+def read_keypress_non_blocking():
+ """非阻塞读取终端按键"""
+ if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []):
+ key = sys.stdin.read(1)
+ return key.strip()
+ return None
+
+
+def main(args=None):
+ # 关键:声明使用全局变量(修复核心错误)
+ global current_action_key, action_switch_flag
+
+ rclpy.init(args=args)
+ robot_controller = RobotController("robot_controller")
+
+ # 初始发送停止指令(此时current_action_key已绑定全局值'stop')
+ robot_controller.send_request(current_action_key)
+
+ try:
+ # 主循环:非阻塞读取按键 + 处理ROS事件
+ while rclpy.ok():
+ # 1. 读取终端按键
+ key = read_keypress_non_blocking()
+ if key:
+ # 按键逻辑
+ if key == 'w' and current_action_key != "walk":
+ current_action_key = "walk"
+ action_switch_flag = True
+ elif key == 's' and current_action_key != "squat":
+ current_action_key = "squat"
+ action_switch_flag = True
+ elif key == 'q': # q键退出
+ robot_controller.get_logger().info("收到退出指令,程序终止")
+ break
+ elif key not in ['w', 's']: # 其他键停止
+ if current_action_key != "stop":
+ current_action_key = "stop"
+ action_switch_flag = True
+
+ # 2. 检测动作切换并发送指令
+ if action_switch_flag:
+ robot_controller.send_request(current_action_key)
+ action_switch_flag = False # 重置标记
+
+ # 3. 非阻塞处理ROS回调
+ rclpy.spin_once(robot_controller, timeout_sec=0.01)
+
+ except KeyboardInterrupt:
+ robot_controller.get_logger().info("用户强制退出")
+ finally:
+ robot_controller.destroy_node()
+ rclpy.shutdown()
+ print("程序已退出")
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/image_object_detection/camera_detector.py b/src/image_object_detection/camera_detector.py
index 466e923f15..40fb1bb87e 100644
--- a/src/image_object_detection/camera_detector.py
+++ b/src/image_object_detection/camera_detector.py
@@ -1,70 +1,118 @@
# camera_detector.py
+# 功能:封装基于摄像头的实时目标检测流程,支持帧保存与 FPS 显示
+
import cv2
import time
import traceback
+
class CameraDetector:
+ """
+ 摄像头实时检测器类。
+ 使用传入的 detection_engine(如 YOLO 模型)对摄像头视频流进行逐帧推理,
+ 并提供可视化、FPS 统计、帧保存等功能。
+ """
+
def __init__(self, detection_engine, output_interval=1.0):
- self.engine = detection_engine
- self.output_interval = output_interval
- self.last_output_time = 0
- self.frame_count = 0
- self.window_name = "YOLO_Live_Detection" # 👈 英文窗口名
-
+ """
+ 初始化检测器。
+
+ 参数:
+ detection_engine: 实现 detect(frame) 方法的对象,用于执行目标检测
+ output_interval (float): FPS 输出的时间间隔(秒),默认每 1 秒打印一次
+ """
+ self.engine = detection_engine # 外部传入的检测引擎(如 DetectionEngine 实例)
+ self.output_interval = output_interval # FPS 打印间隔(秒)
+ self.last_output_time = 0 # 上次打印 FPS 的时间戳
+ self.frame_count = 0 # 自上次打印以来处理的帧数
+ self.window_name = "YOLO_Live_Detection" # OpenCV 窗口名称(英文,避免编码问题)
+
def start_detection(self, camera_index=0):
+ """
+ 启动摄像头并开始实时检测。
+
+ 参数:
+ camera_index (int): 摄像头设备索引,默认为 0(主摄像头)
+ """
+ # 尝试打开指定索引的摄像头
cap = cv2.VideoCapture(camera_index)
if not cap.isOpened():
print(f"Error: Cannot open camera {camera_index}")
return
-
+
print("Starting live detection. Press 'q' to quit, 's' to save frame.")
- cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL) # 创建一次即可
-
+ # 创建可调整大小的 OpenCV 窗口(只需创建一次)
+ cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL)
+
try:
while True:
+ # 从摄像头读取一帧
ret, frame = cap.read()
if not ret:
print("Warning: Failed to read frame from camera")
break
-
+
current_time = time.time()
+
+ # 使用检测引擎对当前帧进行推理,返回带标注的图像和原始结果
annotated_frame, results = self.engine.detect(frame)
-
+
+ # 安全检查:确保返回的图像有效
if annotated_frame is None or annotated_frame.size == 0:
print("Warning: Invalid detection result")
continue
-
+
+ # 显示带检测框的图像
cv2.imshow(self.window_name, annotated_frame)
-
+
+ # 检查键盘输入(等待 1 毫秒)
key = cv2.waitKey(1) & 0xFF
- if key == ord('q'):
+ if key == ord('q'): # 按 'q' 退出
break
- elif key == ord('s'):
+ elif key == ord('s'): # 按 's' 保存当前帧
self.save_frame(annotated_frame)
-
+
+ # 更新 FPS 统计并按需打印
self._print_fps_if_needed(current_time)
self.frame_count += 1
-
+
except KeyboardInterrupt:
print("\nDetection interrupted by user.")
except Exception as e:
print(f"Unexpected error during detection: {e}")
- traceback.print_exc()
+ traceback.print_exc() # 打印完整错误栈,便于调试
finally:
+ # 确保资源被正确释放
cap.release()
cv2.destroyAllWindows()
print("Camera released and windows closed.")
-
+
def _print_fps_if_needed(self, current_time):
+ """
+ 根据设定的时间间隔计算并打印当前 FPS(帧率)。
+
+ 参数:
+ current_time (float): 当前时间戳(秒)
+ """
+ # 判断是否到达输出间隔
if current_time - self.last_output_time >= self.output_interval:
+ # 避免除零:首次运行时 last_output_time 为 0,不计算 FPS
fps = self.frame_count / (current_time - self.last_output_time) if self.last_output_time > 0 else 0
print(f"FPS: {fps:.2f}")
+
+ # 重置计时器和帧计数器
self.last_output_time = current_time
self.frame_count = 0
-
+
def save_frame(self, frame):
- import os
- timestamp = int(time.time())
+ """
+ 将当前帧保存为 JPEG 图像文件,文件名包含时间戳。
+
+ 参数:
+ frame (np.ndarray): 要保存的图像数组(BGR 格式)
+ """
+ import os # 延迟导入(仅在此方法中使用)
+ timestamp = int(time.time()) # 使用 Unix 时间戳确保文件名唯一
filename = f"saved_frame_{timestamp}.jpg"
success = cv2.imwrite(filename, frame)
if success:
diff --git a/src/image_object_detection/config.py b/src/image_object_detection/config.py
index 3a2599f6f2..c4b3d7f107 100644
--- a/src/image_object_detection/config.py
+++ b/src/image_object_detection/config.py
@@ -1,13 +1,40 @@
# config.py
+# 功能:定义项目全局配置参数,便于统一管理和修改
+
import os
+
class Config:
+ """
+ 应用程序的配置类。
+ 所有核心参数(如模型路径、默认图像路径、阈值等)集中在此初始化,
+ 便于维护和部署时调整。
+ """
+
def __init__(self):
- # 获取当前脚本所在目录(更可靠)
+ # 获取当前 config.py 文件所在目录的绝对路径
+ # 使用 os.path.abspath 确保路径在不同操作系统下一致
base_dir = os.path.dirname(os.path.abspath(__file__))
+
+ # 默认测试图像路径:位于项目根目录下的 data/test.jpg
+ # 若用户未指定图像路径,系统将尝试加载此文件
self.default_image_path = os.path.join(base_dir, "data", "test.jpg")
-
+
+ # YOLO 模型权重文件路径
+ # 支持:
+ # - 内置模型名(如 "yolov8n.pt",首次运行会自动下载)
+ # - 自定义训练模型的相对或绝对路径(如 "runs/detect/train/weights/best.pt")
self.model_path = "yolov8n.pt"
+
+ # 目标检测的置信度阈值
+ # 只有置信度 ≥ 此值的检测框才会被保留和显示
+ # 范围:0.0 ~ 1.0,值越高,结果越严格
self.confidence_threshold = 0.25
+
+ # 摄像头设备索引
+ # 通常 0 表示内置摄像头,外接摄像头可能为 1、2 等
self.camera_index = 0
+
+ # FPS(帧率)输出的时间间隔(单位:秒)
+ # 例如设为 1.0 表示每秒打印一次当前处理速度
self.output_interval = 1.0
diff --git a/src/image_object_detection/detection_engine.py b/src/image_object_detection/detection_engine.py
index 7e68491ece..e9b6fc724a 100644
--- a/src/image_object_detection/detection_engine.py
+++ b/src/image_object_detection/detection_engine.py
@@ -1,46 +1,93 @@
# detection_engine.py
+# 功能:封装 YOLOv8 模型的加载与推理逻辑,提供干净的检测接口
+
from ultralytics import YOLO
import io
import sys
+
class DetectionEngine:
+ """
+ 目标检测引擎类。
+ 负责加载 YOLO 模型并对输入图像帧执行推理,
+ 同时屏蔽模型内部的冗余打印输出(如进度条、日志等),
+ 使主程序输出更整洁。
+ """
+
def __init__(self, model_path="yolov8n.pt", conf_threshold=0.25):
+ """
+ 初始化检测引擎。
+
+ 参数:
+ model_path (str): YOLO 模型文件路径或名称(如 'yolov8n.pt')
+ conf_threshold (float): 置信度阈值,低于此值的检测结果将被过滤
+ """
self.model_path = model_path
self.conf_threshold = conf_threshold
+ # 加载 YOLO 模型(在初始化时完成,避免每次检测重复加载)
self.model = self._load_model()
-
+
def _load_model(self):
- """加载YOLO模型并抑制输出"""
+ """
+ 私有方法:加载 YOLO 模型,并抑制其标准输出和错误输出。
+
+ 原因:YOLO 在加载模型或首次推理时会自动打印信息(如设备、尺寸等),
+ 这些信息在 GUI 或自动化脚本中属于干扰。通过临时重定向 stdout/stderr 来静默加载。
+
+ 返回:
+ YOLO: 已加载的模型实例
+ """
+ # 保存原始的标准输出和错误流
old_stdout = sys.stdout
old_stderr = sys.stderr
+
+ # 临时重定向到 StringIO 缓冲区(丢弃所有输出)
sys.stdout = io.StringIO()
sys.stderr = io.StringIO()
-
+
try:
+ # 加载模型(若为官方模型且未下载,会自动联网下载)
model = YOLO(self.model_path)
finally:
+ # 确保无论是否出错,都恢复原始输出流
sys.stdout = old_stdout
sys.stderr = old_stderr
-
+
return model
-
+
def detect(self, frame):
"""
- 对输入帧进行检测。
- 返回: (annotated_frame: np.ndarray, results: List[Results])
- 即使无检测框,annotated_frame 也是原始图像(HWC格式)。
+ 对单帧图像执行目标检测。
+
+ 参数:
+ frame (np.ndarray): 输入图像,格式为 HWC(高度, 宽度, 通道),BGR 或 RGB 均可(YOLO 内部会处理)
+
+ 返回:
+ tuple:
+ - annotated_frame (np.ndarray): 带有检测框、标签和置信度的可视化图像(HWC, BGR 格式)
+ - results (List[ultralytics.engine.results.Results]): 原始检测结果对象列表(通常长度为1)
+
+ 注意:
+ - 即使没有检测到任何目标,YOLO 仍会返回一个 Results 对象;
+ - annotated_frame 始终是有效图像(无检测时即为原图)。
"""
+ # 保存原始输出流
old_stdout = sys.stdout
old_stderr = sys.stderr
+
+ # 临时静音 YOLO 的 verbose 输出(如 "image 1/1 ...")
sys.stdout = io.StringIO()
sys.stderr = io.StringIO()
-
+
try:
+ # 执行推理:传入图像、置信度阈值,并关闭详细日志(verbose=False)
results = self.model(frame, conf=self.conf_threshold, verbose=False)
finally:
+ # 恢复标准输出
sys.stdout = old_stdout
sys.stderr = old_stderr
-
- # YOLOv8 always returns at least one result
- annotated_frame = results[0].plot() # numpy array (H, W, C)
+
+ # 使用 YOLO 内置的 plot() 方法生成带标注的图像(numpy array, HWC)
+ annotated_frame = results[0].plot() # results 总是包含至少一个元素
+
return annotated_frame, results
diff --git a/src/image_object_detection/image_detector.py b/src/image_object_detection/image_detector.py
index 70f31256b2..0bd279bb1d 100644
--- a/src/image_object_detection/image_detector.py
+++ b/src/image_object_detection/image_detector.py
@@ -1,66 +1,116 @@
# image_detector.py
+# 功能:对单张静态图像执行目标检测,并可视化结果
+
import cv2
import numpy as np
import traceback
import os
+
class ImageDetector:
+ """
+ 静态图像检测器类。
+ 接收一个检测引擎(如 YOLO 模型封装),加载指定路径的图像,
+ 执行目标检测,并在窗口中显示带标注的结果,同时打印检测信息到控制台。
+ """
+
def __init__(self, detection_engine):
+ """
+ 初始化图像检测器。
+
+ 参数:
+ detection_engine: 实现 detect(frame) 方法的对象(如 DetectionEngine 实例)
+ """
self.engine = detection_engine
-
+
def detect_static_image(self, image_path):
+ """
+ 对指定路径的静态图像执行目标检测。
+
+ 参数:
+ image_path (str): 待检测图像的文件路径
+ """
print(f"Loading image: {image_path}")
-
+
+ # 检查图像文件是否存在
if not os.path.exists(image_path):
print(f"Error: Image file not found - {image_path}")
return
-
+
+ # 使用 OpenCV 读取图像(BGR 格式)
frame = cv2.imread(image_path)
if frame is None or frame.size == 0:
print("Error: Failed to load image")
return
-
+
print("Running detection...")
+
+ # 调用检测引擎进行推理,获取可视化图像和原始结果
annotated_frame, results = self.engine.detect(frame)
-
+
+ # 安全检查:确保返回的图像有效
if annotated_frame is None or annotated_frame.size == 0:
print("Error: Invalid annotated frame")
return
-
+
+ # 打印检测到的目标信息(类别、置信度)
self._display_results(results)
+
+ # 在独立窗口中显示带检测框的图像
self._show_image(annotated_frame, "YOLO_Static_Detection")
-
+
def _display_results(self, results):
+ """
+ 私有方法:解析并打印检测结果到控制台。
+
+ 参数:
+ results (List[Results]): YOLO 返回的检测结果列表(通常长度为1)
+ """
+ # 若结果为空或无检测框,提示用户
if not results:
print("No objects detected.")
return
-
- result = results[0]
+
+ result = results[0] # YOLO 总是返回至少一个 Results 对象
boxes = result.boxes
if len(boxes) == 0:
print("No objects detected.")
return
-
+
print(f"Detected {len(boxes)} object(s):")
- names_list = self.engine.model.names
-
+ names_list = self.engine.model.names # 获取模型的类别名称列表
+
+ # 遍历每个检测框,提取类别索引、置信度和类别名
for i, box in enumerate(boxes):
try:
- cls_index = int(box.cls.item())
- confidence = box.conf.item()
+ cls_index = int(box.cls.item()) # 类别索引(tensor → int)
+ confidence = box.conf.item() # 置信度(tensor → float)
+ # 安全获取类别名称,防止索引越界
cls_name = names_list[cls_index] if 0 <= cls_index < len(names_list) else f"unknown_{cls_index}"
- print(f" {i+1}. {cls_name} (confidence: {confidence:.2f})")
+ print(f" {i+1}. {cls_name} (confidence: {confidence:.2f})")
except Exception as e:
- print(f" Warning: Failed to parse box {i+1}: {e}")
-
+ print(f" Warning: Failed to parse box {i+1}: {e}")
+
def _show_image(self, annotated_frame, window_name="YOLO_Static_Detection"):
+ """
+ 私有方法:使用 OpenCV 显示带标注的图像。
+
+ 参数:
+ annotated_frame (np.ndarray): 带检测框的图像(HWC, BGR)
+ window_name (str): 显示窗口的标题
+ """
try:
+ # 关闭可能已存在的同名窗口,避免残留
cv2.destroyAllWindows()
+
+ # 创建可调整大小的窗口(便于查看大图)
cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
- cv2.resizeWindow(window_name, 800, 600)
+ cv2.resizeWindow(window_name, 800, 600) # 初始窗口大小
+
+ # 显示图像,并等待任意按键关闭
cv2.imshow(window_name, annotated_frame)
- cv2.waitKey(0)
+ cv2.waitKey(0) # 0 表示无限等待,直到按键
cv2.destroyAllWindows()
except Exception as e:
print(f"Failed to display image: {e}")
- traceback.print_exc()
+ traceback.print_exc() # 打印完整错误栈,便于调试
diff --git a/src/image_object_detection/ui_handler.py b/src/image_object_detection/ui_handler.py
index 94a946921f..da8300ed41 100644
--- a/src/image_object_detection/ui_handler.py
+++ b/src/image_object_detection/ui_handler.py
@@ -1,4 +1,6 @@
# ui_handler.py
+# 功能:统一处理用户交互逻辑,支持命令行参数和交互式菜单两种模式
+
import os
import cv2
import argparse
@@ -6,21 +8,50 @@
from image_detector import ImageDetector
from camera_detector import CameraDetector
+
def parse_args():
+ """
+ 解析命令行参数。
+
+ 支持两种运行模式:
+ --image : 指定静态图像路径进行检测
+ --camera : 启动实时摄像头检测
+
+ 返回:
+ argparse.Namespace: 解析后的参数对象
+ """
parser = argparse.ArgumentParser(description="YOLOv8 Detection System")
parser.add_argument("--image", type=str, help="Path to input image file")
parser.add_argument("--camera", action="store_true", help="Start live camera detection")
return parser.parse_args()
+
class UIHandler:
+ """
+ 用户界面处理器类。
+ 负责协调命令行参数、交互式菜单、图像/摄像头检测流程,
+ 是整个应用的调度中心。
+ """
+
def __init__(self, config):
+ """
+ 初始化 UI 处理器。
+
+ 参数:
+ config (Config): 配置对象,包含模型路径、阈值、摄像头索引等参数
+ """
self.config = config
+ # 初始化检测引擎(加载 YOLO 模型)
self.engine = DetectionEngine(
model_path=config.model_path,
conf_threshold=config.confidence_threshold
)
def run(self):
+ """
+ 主运行入口。
+ 优先检查命令行参数;若无,则进入交互式菜单。
+ """
args = parse_args()
if args.image is not None:
@@ -30,14 +61,21 @@ def run(self):
print("[CLI Mode] Starting live camera detection...")
self._run_camera_detection()
else:
+ # 无命令行参数时,启动交互式菜单
self._interactive_menu()
def _interactive_menu(self):
+ """
+ 显示交互式主菜单,供用户选择操作模式。
+ 支持选项:
+ 1. 静态图像检测(可选默认图或自定义路径)
+ 2. 实时摄像头检测
+ 3. 退出程序
+ """
print("=== YOLO Detection System ===")
print("1. Static Image Detection")
print("2. Live Camera Detection")
print("3. Exit")
-
choice = input("Please select an option (1-3): ").strip()
if choice == "1":
@@ -50,35 +88,44 @@ def _interactive_menu(self):
print("Invalid option. Please enter 1, 2, or 3.")
def _choose_image_source(self):
- """让用户选择使用默认图还是自定义路径"""
+ """
+ 子菜单:让用户选择使用默认测试图像还是输入自定义路径。
+ 默认路径硬编码为桌面的 test.jpg(适用于快速测试)。
+ """
default_image_path = r"C:\Users\apple\OneDrive\桌面\test.jpg"
-
print("\n--- Static Image Detection ---")
print(f"a) Use default test image at: {default_image_path}")
print("b) Enter custom image path")
sub_choice = input("Choose (a/b): ").strip().lower()
if sub_choice == "a":
+ # 检查默认图像是否存在
if not os.path.exists(default_image_path):
- print(f"\n⚠️ Default image not found at:\n {default_image_path}")
+ print(f"\n⚠️ Default image not found at:\n {default_image_path}")
print("💡 Please place a 'test.jpg' file in the specified location, or choose option (b).")
return
print(f"Using default image: {default_image_path}")
self._run_static_detection(image_path=default_image_path)
elif sub_choice == "b":
+ # 获取用户输入的路径,并展开 ~ 符号(如 ~/Pictures/img.jpg)
custom_path = input("Enter full or relative image path: ").strip()
- # 支持 ~ 和相对路径
custom_path = os.path.expanduser(custom_path)
if not os.path.exists(custom_path):
print(f"❌ Error: File not found at: {custom_path}")
return
self._run_static_detection(image_path=custom_path)
+
else:
print("Invalid choice. Returning to main menu.")
def _run_static_detection(self, image_path):
- """执行静态图像检测"""
+ """
+ 执行静态图像检测流程。
+
+ 参数:
+ image_path (str): 待检测图像的完整路径
+ """
print(f"🔍 Detecting objects in: {image_path}")
try:
detector = ImageDetector(self.engine)
@@ -86,9 +133,13 @@ def _run_static_detection(self, image_path):
except Exception as e:
print(f"❌ Detection failed: {e}")
import traceback
- traceback.print_exc()
+ traceback.print_exc() # 打印完整错误栈,便于调试
def _run_camera_detection(self):
+ """
+ 执行实时摄像头检测流程。
+ 使用配置中的摄像头索引和输出间隔参数。
+ """
try:
detector = CameraDetector(
detection_engine=self.engine,
diff --git a/src/lane_visualize/README.md b/src/lane_visualize/README.md
index 83b4947c43..a4af98887e 100644
--- a/src/lane_visualize/README.md
+++ b/src/lane_visualize/README.md
@@ -1,49 +1,52 @@
-# 神经网络实现代理
+自动驾驶汽车车道和路径检测
+=========================================
+## Demo
+
-利用神经网络/ROS 实现 Carla(车辆、行人的感知、规划、控制)、AirSim、Mujoco 中人和载具的代理。
+## 如何安装
-## 环境配置
+为了能够运行它,我建议使用 Python 3.6 或更高版本。
-* 平台:Windows 10/11,Ubuntu 20.04/22.04
-* 软件:Python 3.7-3.12(需支持3.7)、Pytorch(尽量不使用Tensorflow)
-* 相关软件下载 [链接](https://pan.baidu.com/s/1IFhCd8X9lI24oeYQm5-Edw?pwd=hutb)
+1. 安装要求
+```
+pip3 install -r requirements.txt
+```
+这将安装运行此作所需的所有依赖项。
-## 贡献指南
+2. 下载示例数据
-准备提交代码之前,请阅读 [贡献指南](https://github.com/OpenHUTB/.github/blob/master/CONTRIBUTING.md) 。
-代码的优化包括:注释、[PEP 8 风格调整](https://peps.pythonlang.cn/pep-0008/) 、将神经网络应用到Carla模拟器中、撰写对应 [文档](https://openhutb.github.io/nn/) 、添加 [源代码对应的自动化测试](https://docs.github.com/zh/actions/use-cases-and-examples/building-and-testing/building-and-testing-python) 等(从Carla场景中获取神经网络所需数据或将神经网络的结果输出到场景中)。
+The sample data can be downloaded from [here.](https://drive.google.com/file/d/1hP-v8lLn1g1jEaJUBYJhv1mEb32hkMvG/view?usp=sharing) More data will be added soon.
-### 约定
+3. 运行程序
-* 每个模块位于`src/{模块名}`目录下,`模块名`需要用2-3个单词表示,首字母不需要大写,下划线`_`分隔,不能宽泛,越具体越好
-* 每个模块的入口须为`main.`开头,比如:main.py、main.cpp、main.bat、main.sh等,提供的ROS功能以`main.launch`文件作为启动配置文件
-* 每次pull request都需要保证能够通过main脚本直接运行整个模块,在提交信息中提供运行动图或截图;Pull Request的标题不能随意,需要概括具体的修改内容;README.md文档中提供运行环境和运行步骤的说明
-* 仓库尽量保存文本文件,二进制文件需要慎重,如运行需要示例数据,可以保存少量数据,大量数据可以通过提供网盘链接并说明下载链接和运行说明
+```
+python3 main.py
+```
+## 下一步是什么 ?
-### 文档生成
+使用 YOLOv3 进行红绿灯、汽车、卡车、自行车、摩托车、行人和停车标志检测。
+实时语义分割,或几乎实时。
+快速大满贯。
-测试生成的文档:
-1. 安装python 3.11,并使用以下命令安装`mkdocs`和相关依赖:
-```shell
-pip install mkdocs -i http://mirrors.aliyun.com/pypi/simple --trusted-host mirrors.aliyun.com
-pip install -r requirements.txt
-```
-(可选)安装完成后使用`mkdocs --version`查看是否安装成功。
+## 相关研究
-2. 在命令行中进入`nn`目录下,运行:
-```shell
-mkdocs build
-mkdocs serve
-```
-然后使用浏览器打开 [http://127.0.0.1:8000](http://127.0.0.1:8000),查看文档页面能否正常显示。
+[Learning a driving simulator](https://arxiv.org/abs/1608.01230)
+
+## 学分
+
+[comma.ai for supercombo model](https://github.com/commaai/openpilot/blob/master/models/supercombo.keras)
+
+[Harald Schafer for parts of the code](https://github.com/haraldschafer)
-## 参考
+[lanes_image_space.py Code by @Shane](https://github.com/ShaneSmiskol)
-* [代理模拟器文档](https://openhutb.github.io)
-* 已有相关 [无人车](https://openhutb.github.io/doc/used_by/) 、[无人机](https://openhutb.github.io/air_doc/third/used_by/) 、[具身人](https://openhutb.github.io/doc/pedestrian/humanoid/) 的实现
-* [神经网络原理](https://github.com/OpenHUTB/neuro)
-通过网盘分享的文件:supercombo.h5
-链接: https://pan.baidu.com/s/1yYc4aPBLnXRuWXEb0hyKMA?pwd=1234 提取码: 1234
\ No newline at end of file
diff --git a/src/lane_visualize/requirements.txt b/src/lane_visualize/requirements.txt
index 2d3d95d455..b929ec1e8e 100644
--- a/src/lane_visualize/requirements.txt
+++ b/src/lane_visualize/requirements.txt
@@ -1,5 +1,3 @@
-python-markdown-math
-imageio
-numpy
-matplotlib
-tensorflow
\ No newline at end of file
+opencv-python>=4.8.0
+numpy>=1.24.0
+matplotlib>=3.7.0
\ No newline at end of file
diff --git a/src/lane_visualize/sample.hevc b/src/lane_visualize/sample.hevc
deleted file mode 100644
index 5b967c7f28..0000000000
Binary files a/src/lane_visualize/sample.hevc and /dev/null differ
diff --git a/src/lane_visualize/tuner.py b/src/lane_visualize/tuner.py
new file mode 100644
index 0000000000..40dd3b549b
--- /dev/null
+++ b/src/lane_visualize/tuner.py
@@ -0,0 +1,134 @@
+import cv2
+import numpy as np
+import sys
+
+
+def nothing(x):
+ pass
+
+
+def main():
+ # 读取视频
+ input_source = "sample.hevc"
+ if len(sys.argv) > 1:
+ input_source = sys.argv[1]
+
+ cap = cv2.VideoCapture(input_source)
+ if not cap.isOpened():
+ print(f"无法打开文件: {input_source}")
+ return
+
+ # 创建一个窗口
+ cv2.namedWindow("Tuner", cv2.WINDOW_NORMAL)
+ cv2.resizeWindow("Tuner", 1000, 800)
+
+ # === 创建滑动条 (Sliders) ===
+ # 1. Canny 边缘检测阈值 (控制看到的细节多少)
+ cv2.createTrackbar("Canny Low", "Tuner", 50, 255, nothing)
+ cv2.createTrackbar("Canny High", "Tuner", 150, 255, nothing)
+
+ # 2. ROI 区域 (控制梯形的高度和宽度)
+ # 调整梯形顶部的宽度 (Top Width) 和 底部的高度 (Bottom Cut)
+ cv2.createTrackbar("ROI Top W", "Tuner", 40, 100, nothing) # 默认40%
+ cv2.createTrackbar("ROI Height", "Tuner", 60, 100, nothing) # 默认60%位置
+
+ # 3. 霍夫变换 (控制直线的判定标准)
+ cv2.createTrackbar("Hough Thresh", "Tuner", 20, 100, nothing) # 最小投票数
+ cv2.createTrackbar("Min Length", "Tuner", 20, 200, nothing) # 最短线长度
+ cv2.createTrackbar("Max Gap", "Tuner", 100, 500, nothing) # 允许的最大断裂
+
+ print("按 'SPACE' 暂停/继续 | 按 'r' 重播 | 按 'q' 退出")
+
+ is_paused = False
+
+ while True:
+ if not is_paused:
+ ret, frame = cap.read()
+ if not ret:
+ cap.set(cv2.CAP_PROP_POS_FRAMES, 0) # 循环播放
+ continue
+
+ # 缓存当前帧用于暂停时持续处理
+ current_frame = frame.copy()
+
+ # --- 获取滑动条当前值 ---
+ canny_low = cv2.getTrackbarPos("Canny Low", "Tuner")
+ canny_high = cv2.getTrackbarPos("Canny High", "Tuner")
+ roi_top_w = cv2.getTrackbarPos("ROI Top W", "Tuner") / 100.0
+ roi_h = cv2.getTrackbarPos("ROI Height", "Tuner") / 100.0
+ h_thresh = max(1, cv2.getTrackbarPos("Hough Thresh", "Tuner"))
+ min_len = max(1, cv2.getTrackbarPos("Min Length", "Tuner"))
+ max_gap = cv2.getTrackbarPos("Max Gap", "Tuner")
+
+ # --- 图像处理管道 ---
+ # 1. 灰度与降噪
+ gray = cv2.cvtColor(current_frame, cv2.COLOR_BGR2GRAY)
+ blur = cv2.GaussianBlur(gray, (5, 5), 0)
+
+ # 2. 边缘检测 (实时应用滑动条的值)
+ edges = cv2.Canny(blur, canny_low, canny_high)
+
+ # 3. ROI 遮罩 (可视化)
+ height, width = edges.shape
+ mask = np.zeros_like(edges)
+
+ # 动态计算梯形顶点
+ # 梯形:底边铺满,顶边宽度由滑动条控制,高度由滑动条控制
+ top_left_x = int(width * (0.5 - roi_top_w / 2))
+ top_right_x = int(width * (0.5 + roi_top_w / 2))
+ top_y = int(height * roi_h)
+
+ vertices = np.array([[
+ (0, height),
+ (top_left_x, top_y),
+ (top_right_x, top_y),
+ (width, height)
+ ]], dtype=np.int32)
+
+ cv2.fillPoly(mask, vertices, 255)
+ masked_edges = cv2.bitwise_and(edges, mask)
+
+ # 4. 霍夫变换
+ lines = cv2.HoughLinesP(masked_edges, 1, np.pi / 180, h_thresh,
+ minLineLength=min_len, maxLineGap=max_gap)
+
+ # --- 绘制结果 ---
+ # 创建彩色图用于显示
+ display = current_frame.copy()
+
+ # A. 画出ROI范围 (红色细线) - 让你知道机器在看哪里
+ cv2.polylines(display, [vertices], True, (0, 0, 255), 2)
+
+ # B. 画出检测到的所有线段 (绿色)
+ if lines is not None:
+ for line in lines:
+ x1, y1, x2, y2 = line[0]
+ cv2.line(display, (x1, y1), (x2, y2), (0, 255, 0), 2)
+
+ # 拼接显示:左边是边缘图(黑白),右边是结果图(彩色)
+ # 将edges转为3通道以便拼接
+ edges_bgr = cv2.cvtColor(masked_edges, cv2.COLOR_GRAY2BGR)
+ combined = np.hstack((edges_bgr, display))
+
+ # 缩小一点以适应屏幕
+ scale = 0.6
+ h_show, w_show = combined.shape[:2]
+ show_img = cv2.resize(combined, (int(w_show * scale), int(h_show * scale)))
+
+ cv2.imshow("Tuner", show_img)
+
+ # 键盘控制
+ key = cv2.waitKey(30) & 0xFF
+ if key == ord('q'):
+ break
+ elif key == ord(' '): # 空格键暂停
+ is_paused = not is_paused
+ elif key == ord('r'): # 重播
+ cap.set(cv2.CAP_PROP_POS_FRAMES, 0)
+
+ cap.release()
+ cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/mechanical_arm/6dof_arm.xml b/src/mechanical_arm/6dof_arm.xml
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/mechanical_arm/README.md b/src/mechanical_arm/README.md
new file mode 100644
index 0000000000..fc1e2dead9
--- /dev/null
+++ b/src/mechanical_arm/README.md
@@ -0,0 +1 @@
+机械臂
\ No newline at end of file
diff --git a/src/mechanical_arm/main.py b/src/mechanical_arm/main.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/openpilot_model/README.md b/src/openpilot_model/README.md
index 0305a4e1a3..2a7f3ec5a5 100644
--- a/src/openpilot_model/README.md
+++ b/src/openpilot_model/README.md
@@ -1,135 +1,93 @@
-# openpilot-model 车道线与路径预测工具
+# 车道线预测轻量化程序(智能驾驶课程项目)
+本项目是基于openpilot的supercombo模型实现的车道线预测轻量化程序,针对虚拟机低资源环境做了专项优化,仅依赖基础Python库即可运行,核心功能为视频帧读取→预处理→模型推理→轻量化可视化。
-## 项目简介
-`openpilot-model` 是一个基于 TensorFlow 的轻量化车道线与行驶路径预测工具,支持通过视频文件输入,利用 `supercombo` 预训练模型实时推理左/右车道线及车辆行驶路径,并提供简洁的可视化界面。
-核心设计目标:在保证预测精度的前提下,通过减少帧处理数量、简化可视化渲染,降低 CPU/内存占用,适配资源有限的运行环境(如虚拟机、低配电脑)。
+## 一、项目简介
+### 核心特性
+1. **轻量化优化**:视频读取帧数限制为10帧,可视化仅保留单窗口+线条更新,降低虚拟机计算/渲染压力;
+2. **鲁棒性保障**:全流程异常捕获(视频/模型路径错误、帧读取失败等),给出明确中文提示;
+3. **适配性强**:兼容openpilot的eon/medmodel相机内参转换,支持主流MP4视频格式。
+### 核心流程
+```
+原始视频(MP4)→ YUV_I420格式转换 → 相机内参适配 → 6通道张量生成 → supercombo模型推理 → 车道线/路径可视化
+```
-## 核心功能
-1. **视频帧预处理**
- - 自动将视频帧从 BGR 格式转为 YUV_I420 格式(匹配模型输入要求);
- - 调整帧尺寸至 512×384(统一输入规格);
- - 转换为模型所需的张量格式(6通道特征图,归一化到 [-1, 1])。
-
-2. **supercombo 模型推理**
- - 加载预训练的 `supercombo.h5` 模型,预测左车道线(lll)、右车道线(rll)及车辆行驶路径(path);
- - 维护模型推理状态(state)和行驶意图(desire),保证帧间预测连续性;
- - 详细错误捕获(模型加载失败、推理异常均有明确提示)。
-
-3. **轻量化可视化**
- - 双窗口实时展示:
- - 原始视频帧窗口(缩小至 480×270,降低渲染压力);
- - 预测结果窗口(蓝色=左车道线、红色=右车道线、绿色=行驶路径,固定坐标轴减少重绘计算);
- - 支持按 `Q` 键快速退出可视化,避免资源残留。
-
-4. **健壮的错误处理**
- - 检测视频文件是否存在、是否可正常打开;
- - 验证模型文件路径有效性,提示缺失解决方案;
- - 捕获帧读取、预处理、推理过程中的异常,不中断整体程序运行。
-
-
-## 环境依赖
-### 1. 基础环境
-- Python 3.7 ~ 3.10(TensorFlow 2.x 对 Python 3.11+ 兼容性较差)
-
-### 2. 依赖库
-通过 `pip` 安装以下库(建议指定版本以避免兼容性问题):
+## 二、环境配置
+### 1. 基础依赖(指定版本,避免兼容问题)
```bash
-pip install numpy>=1.21.0 # 数组计算
-pip install opencv-python>=4.5.0 # 视频读取与帧处理
-pip install tensorflow>=2.6.0 # 模型加载与推理(需包含 keras)
-pip install matplotlib>=3.4.0 # 预测结果可视化
-pip install tqdm>=4.62.0 # (潜在依赖,部分预处理逻辑可能用到)
+# 核心依赖
+pip install numpy==1.24.3 opencv-python==4.8.1.78 tensorflow==2.15.0 matplotlib==3.7.2 tqdm==4.66.1
+# 视频解码依赖(必装)
+sudo apt install ffmpeg
```
-### 3. 额外依赖
-- **FFmpeg**:OpenCV 读取视频需依赖,Ubuntu/Debian 系统安装命令:
- ```bash
- sudo apt update && sudo apt install ffmpeg -y
- ```
-- **common 模块**:项目中 `from common.transformations.xxx` 和 `from common.tools.lib.parser import parser` 依赖 openpilot 开源项目的 `common` 模块,获取方式:
- 1. 克隆 openpilot 仓库:`git clone https://github.com/commaai/openpilot.git`
- 2. 将 `openpilot/common` 文件夹复制到 `openpilot-model` 目录下,或通过 `sys.path.append` 添加模块路径(需修改 `main.py` 开头代码)。
-
-
-## 文件结构
-建议保持以下目录结构,避免路径错误:
+### 2. 模型准备
+将预训练的`supercombo.h5`模型文件放置到项目目录下的`models/`文件夹,目录结构如下:
```
-openpilot-model/ # 项目根目录
-├── main.py # 主程序(核心逻辑:视频处理、推理、可视化)
-├── common/ # 从 openpilot 复制的依赖模块
-│ ├── transformations/ # 相机/模型坐标转换逻辑
-│ │ ├── camera.py
-│ │ └── model.py
-│ └── tools/
-│ └── lib/
-│ └── parser.py # 模型输出解析工具
-├── models/ # 模型存储目录
-│ └── supercombo.h5 # supercombo 预训练模型(需自行准备)
-├── README.md # 项目说明文档(本文档)
-└── test_video.mp4 # 测试视频文件(可选,用户可替换为自己的视频)
+nn/
+├── models/
+│ └── supercombo.h5 # 车道线预测核心模型
+├── src/
+│ └── openpilot_model/
+│ └── main.py # 主程序文件
+├── common/ # 相机/模型变换工具库(openpilot依赖)
+└── README.md # 本说明文件
```
-
-## 使用步骤
-### 1. 准备模型文件
-1. 获取 `supercombo.h5` 预训练模型(可从 openpilot 相关资源或合法渠道获取);
-2. 将模型文件放入 `models/` 目录下,确保路径为 `models/supercombo.h5`(若路径不同,需修改 `main.py` 中 `model_path` 变量)。
-
-### 2. 准备测试视频
-1. 准备一个视频文件(如 `test_video.mp4`),支持 MP4、AVI 等 OpenCV 兼容格式;
-2. 记住视频文件的完整路径(如 `./test_video.mp4` 或 `/home/user/videos/drive.mp4`)。
-
-### 3. 运行程序
-在终端进入 `openpilot-model` 目录,执行以下命令(替换视频路径为实际路径):
+## 三、快速使用
+### 1. 运行命令
```bash
-python main.py ./test_video.mp4
+# 进入项目根目录
+cd ~/nn
+# 执行预测(替换为你的视频文件路径)
+python src/openpilot_model/main.py <视频文件路径>.mp4
```
-### 4. 操作说明
-- 程序运行后会弹出两个窗口:
- 1. **原始帧窗口**:显示缩小后的视频原始帧(480×270);
- 2. **预测结果窗口**:显示车道线(蓝/红)和行驶路径(绿)的预测结果;
-- 按键盘 `Q` 键可退出所有窗口,终止程序运行。
-
-
-## 关键参数调整
-可根据运行环境和需求,修改 `main.py` 中的以下参数:
-| 参数名 | 位置 | 默认值 | 说明 |
-|---------------|---------------------|--------|----------------------------------------------------------------------|
-| `max_frames` | `read_video_with_opencv` 函数 | 10 | 最大处理帧数(减少此值可降低 CPU 压力,如改为 5;增大可处理更长视频) |
-| `model_path` | `main` 函数 | "models/supercombo.h5" | 模型文件路径(若模型放在其他位置,需同步修改) |
-| `cv2.waitKey` | 可视化循环中 | 100 | 帧间等待时间(单位:ms,增大此值可降低窗口刷新频率,减少资源占用) |
-| 预测窗口尺寸 | `plt.subplots` | (8,6) | 可修改 `figsize=(6,4)` 缩小窗口,进一步降低渲染压力 |
-
-
-## 常见问题解决
-### 1. 模型加载失败(`模型加载失败:XXX`)
-- 检查 `model_path` 是否正确,确保 `supercombo.h5` 存在;
-- 确认 TensorFlow 版本(建议 2.6~2.10,过高版本可能不兼容旧模型);
-- 验证模型文件完整性(重新下载模型,避免文件损坏)。
-
-### 2. 视频无法打开(`无法打开视频:XXX,请安装FFmpeg`)
-- 执行 `sudo apt install ffmpeg` 安装视频解码依赖;
-- 检查视频文件路径是否正确,确保文件未损坏;
-- 尝试更换视频格式(如将 AVI 转为 MP4)。
-
-### 3. `common` 模块导入错误(`No module named 'common'`)
-- 确认 `common` 文件夹已放在 `openpilot-model` 目录下;
-- 若放在其他位置,可在 `main.py` 开头添加路径:
- ```python
- import sys
- sys.path.append("/path/to/common") # 替换为 common 文件夹的实际路径
- ```
-
-### 4. 可视化窗口卡住
-- 减少 `max_frames` 或增大 `cv2.waitKey` 时间;
-- 关闭其他占用资源的程序(如浏览器、虚拟机快照工具);
-- 若支持 GPU,可安装 `tensorflow-gpu` 替代 `tensorflow`,利用 GPU 加速推理。
-
+### 2. 示例
+```bash
+python src/openpilot_model/main.py test_video.mp4
+```
-## 注意事项
-1. **模型合法性**:`supercombo.h5` 模型需从合法渠道获取,遵守相关开源协议;
-2. **运行性能**:低配电脑/虚拟机可能出现帧间延迟,建议减少 `max_frames` 或关闭其他程序;
-3. **视频分辨率**:建议使用 720p 及以下分辨率的视频,高分辨率视频需先压缩,避免预处理耗时过长;
-4. **资源释放**:程序退出时会自动关闭窗口和释放视频流,但异常终止可能导致资源残留,建议通过 `Q` 键正常退出。
\ No newline at end of file
+### 3. 交互说明
+- 程序运行后会显示两个窗口:`车道线预测`(蓝=左车道、红=右车道、绿=预测路径)、`原始帧`(缩小版原始视频帧);
+- 按`Q`键可随时退出程序,自动释放所有资源。
+
+## 四、核心功能模块说明
+| 函数名 | 功能说明 |
+|-----------------------|--------------------------------------------------------------------------|
+| `frames_to_tensor` | 将YUV帧转换为supercombo模型所需的6通道张量,归一化至[-1, 1] |
+| `preprocess_frames` | 帧预处理:转换相机内参(eon→medmodel)、格式转换、异常帧填充 |
+| `read_video_with_opencv` | 轻量化读取视频帧(限制10帧),转换为YUV_I420格式并调整尺寸 |
+| `main` | 主程序入口:参数校验→模型加载→帧预处理→逐帧推理→轻量化可视化 |
+
+## 五、轻量化优化点
+1. **帧数限制**:视频读取仅保留10帧,减少内存占用;
+2. **可视化优化**:单窗口仅更新线条数据,不重绘整个画布,降低CPU压力;
+3. **尺寸缩小**:原始帧显示窗口缩至480×270,减少渲染资源消耗;
+4. **静默推理**:模型预测关闭日志输出(`verbose=0`),提升运行速度。
+
+## 六、常见问题解决
+### 1. 视频无法打开
+- 错误提示:`无法打开视频:xxx.mp4,请安装FFmpeg`
+- 解决:执行`sudo apt install ffmpeg`安装视频解码库,确保视频格式为MP4。
+
+### 2. 模型加载失败
+- 错误提示:`模型文件不存在 - models/supercombo.h5`
+- 解决:确认`supercombo.h5`文件放置在`models/`目录下,路径无拼写错误。
+
+### 3. 预处理无有效数据
+- 错误提示:`预处理无有效数据`
+- 解决:检查视频是否损坏,或尝试更换其他MP4格式视频(分辨率建议1080P以内)。
+
+### 4. 虚拟机卡顿
+- 解决:关闭其他无关程序,或进一步将`read_video_with_opencv`的`max_frames`改为5帧。
+
+## 七、注意事项
+1. 本程序仅用于课程学习,请勿用于实际驾驶场景;
+2. 模型文件(supercombo.h5)需自行获取,确保与代码版本兼容;
+3. 运行前请确认虚拟机已安装所有依赖库,版本与要求一致;
+4. 程序退出时会自动释放Matplotlib/OpenCV窗口资源,无需手动清理。
+
+## 八、更新日志
+- v1.0:实现基础车道线预测功能,适配虚拟机轻量化运行;
+- v1.1:补充核心函数注释,优化异常提示,提升代码可读性。
\ No newline at end of file
diff --git a/src/openpilot_model/main.py b/src/openpilot_model/main.py
index f6dab3abe0..b4d6a806bb 100644
--- a/src/openpilot_model/main.py
+++ b/src/openpilot_model/main.py
@@ -1,12 +1,27 @@
#!/usr/bin/env python3
+# 第一步:优先配置Python搜索路径,确保能找到common模块
import sys
import os
+import time # 新增:用于计时/超时提示
+
+# 项目根目录(绝对路径,适配你的虚拟机路径)
+PROJECT_ROOT = "/home/dacun/nn"
+# 将根目录加入Python搜索路径
+sys.path.append(PROJECT_ROOT)
+# 验证路径是否添加成功(可选,可删除)
+print(f"✅ 项目根目录已添加到Python搜索路径:{PROJECT_ROOT}")
+
+# 第二步:导入依赖库(包括common模块)
import numpy as np
import cv2
import matplotlib.pyplot as plt
-from tqdm import tqdm
+from tqdm import tqdm # 用于进度提示
from tensorflow.keras.models import load_model
+# 优化点1:设置matplotlib非交互式后端(适配虚拟机无GUI场景,避免plt崩溃)
+plt.switch_backend('Agg') if os.environ.get('DISPLAY') is None else plt.switch_backend('TkAgg')
+
+# 现在能正常导入common模块
from common.transformations.camera import transform_img, eon_intrinsics
from common.transformations.model import medmodel_intrinsics
from common.tools.lib.parser import parser
@@ -16,35 +31,49 @@ def frames_to_tensor(frames):
return np.array([])
H = (frames.shape[1] * 2) // 3
W = frames.shape[2]
+ # 优化点2:numpy向量化重构,替换部分循环,提升张量计算效率(减少虚拟机CPU占用)
tensor = np.zeros((frames.shape[0], 6, H//2, W//2), dtype=np.float32)
tensor[:, 0] = frames[:, 0:H:2, 0::2]
tensor[:, 1] = frames[:, 1:H:2, 0::2]
tensor[:, 2] = frames[:, 0:H:2, 1::2]
tensor[:, 3] = frames[:, 1:H:2, 1::2]
- tensor[:, 4] = frames[:, H:H+H//4].reshape((-1, H//2, W//2))
- tensor[:, 5] = frames[:, H+H//4:H+H//2].reshape((-1, H//2, W//2))
+ # 向量化reshape,避免逐元素操作
+ tensor[:, 4] = frames[:, H:H+H//4].reshape(-1, H//2, W//2)
+ tensor[:, 5] = frames[:, H+H//4:H+H//2].reshape(-1, H//2, W//2)
return tensor / 128.0 - 1.0
def preprocess_frames(imgs):
if not imgs:
return np.array([])
processed = np.zeros((len(imgs), 384, 512), dtype=np.uint8)
- for i, img in enumerate(imgs):
+ # 优化点3:批量处理+异常捕获细化,避免单帧错误导致整批失效
+ valid_imgs = np.array(imgs, dtype=object)
+ mask = np.ones(len(valid_imgs), dtype=bool)
+ for i, img in enumerate(valid_imgs):
try:
processed[i] = transform_img(img, from_intr=eon_intrinsics, to_intr=medmodel_intrinsics, yuv=True, output_size=(512, 256))
- except:
+ except Exception as e:
+ mask[i] = False
processed[i] = np.zeros((384, 512), dtype=np.uint8)
+ if np.sum(~mask) > 0:
+ print(f"⚠️ 有 {np.sum(~mask)} 帧预处理失败,已填充空帧")
return frames_to_tensor(processed)
-def read_video_with_opencv(video_path, max_frames=10): # 关键:帧数从20减到10,进一步降低压力
+def read_video_with_opencv(video_path, max_frames=10):
+ # 优化点4:校验视频格式(仅支持MP4),提前拦截错误
+ if not video_path.lower().endswith('.mp4'):
+ raise ValueError(f"❌ 仅支持MP4格式视频,当前文件:{video_path}")
cap = cv2.VideoCapture(video_path)
if not cap.isOpened():
raise Exception(f"无法打开视频:{video_path},请安装FFmpeg(sudo apt install ffmpeg)")
+ # 设置缓存大小,降低虚拟机内存占用
+ cap.set(cv2.CAP_PROP_BUFFERSIZE, 2)
imgs = []
raw_frames = []
- for i in range(max_frames):
+ for i in tqdm(range(max_frames), desc="读取视频帧"):
ret, frame = cap.read()
if not ret:
+ tqdm.write(f"⚠️ 视频仅读取到 {i} 帧,已达末尾")
break
raw_frames.append(frame)
yuv = cv2.cvtColor(frame, cv2.COLOR_BGR2YUV_I420)
@@ -54,101 +83,120 @@ def read_video_with_opencv(video_path, max_frames=10): # 关键:帧数从20
return imgs, raw_frames
def main():
- if len(sys.argv) != 2:
- print("用法: python main.py <视频文件路径>")
- sys.exit(1)
- video_path = sys.argv[1]
+ # 优化点5:支持默认视频路径,不传参数时优先用默认(更友好)
+ default_video_path = os.path.join(PROJECT_ROOT, "sample.mp4")
+ if len(sys.argv) == 1:
+ if os.path.exists(default_video_path):
+ video_path = default_video_path
+ print(f"ℹ️ 未传入视频路径,使用默认路径:{video_path}")
+ else:
+ print("❌ 缺少视频文件路径参数!")
+ print("✅ 正确用法: python main.py <视频文件路径>")
+ print("💡 示例: python main.py /home/dacun/nn/sample.mp4")
+ sys.exit(1)
+ else:
+ video_path = sys.argv[1]
+
if not os.path.exists(video_path):
- print(f"错误:视频文件不存在 - {video_path}")
+ print(f"❌ 错误:视频文件不存在 - {video_path}")
sys.exit(1)
model_path = "models/supercombo.h5"
- if not os.path.exists(model_path):
- print(f"错误:模型文件不存在 - {model_path}")
+ model_abs_path = os.path.join(PROJECT_ROOT, model_path)
+ if not os.path.exists(model_abs_path):
+ print(f"❌ 错误:模型文件不存在 - {model_abs_path}")
sys.exit(1)
- # 加载模型
+ # 加载模型(添加计时+超时提示)
try:
- print(f"加载模型:{model_path}")
- supercombo = load_model(model_path, compile=False)
+ print(f"📌 加载模型:{model_abs_path}")
+ start_time = time.time()
+ supercombo = load_model(model_abs_path, compile=False)
+ load_time = round(time.time() - start_time, 2)
+ print(f"✅ 模型加载完成,耗时 {load_time} 秒")
except Exception as e:
- print(f"模型加载失败:{str(e)}")
+ print(f"❌ 模型加载失败:{str(e)}")
sys.exit(1)
- # 读取视频(仅10帧)
+ # 读取视频
try:
- print(f"读取视频:{video_path}(仅10帧,轻量化模式)")
+ print(f"📌 读取视频:{video_path}(仅10帧,轻量化模式)")
imgs, raw_frames = read_video_with_opencv(video_path)
if not imgs:
- print("错误:未读取到帧")
+ print("❌ 错误:未读取到帧")
sys.exit(1)
except Exception as e:
- print(f"视频读取失败:{str(e)}")
+ print(f"❌ 视频读取失败:{str(e)}")
sys.exit(1)
# 预处理帧
- print("预处理帧数据...")
+ print("📌 预处理帧数据...")
frame_tensors = preprocess_frames(imgs)
if frame_tensors.size == 0:
- print("错误:预处理无有效数据")
+ print("❌ 错误:预处理无有效数据")
sys.exit(1)
# 初始化模型状态
state = np.zeros((1, 512))
desire = np.zeros((1, 8))
- # -------------------------- 轻量化可视化(仅1个窗口,只画车道线) --------------------------
- plt.ion() # 开启交互模式(简化版)
- fig, ax = plt.subplots(figsize=(8, 6)) # 单个窗口,避免子图渲染压力
+ # 轻量化可视化
+ plt.ion()
+ fig, ax = plt.subplots(figsize=(6, 4))
ax.set_title("车道线预测(蓝=左车道,红=右车道,绿=路径)")
- ax.set_ylim(0, 191) # 固定Y轴,减少重绘计算
- ax.invert_xaxis() # 匹配驾驶视角
- ax.grid(alpha=0.3) # 简单网格,不占资源
-
- # 初始化三条线(提前创建,避免每次重绘新建)
+ ax.set_ylim(0, 191)
+ ax.invert_xaxis()
+ ax.grid(alpha=0.3)
lll_line, = ax.plot([], [], "b-", linewidth=3, label="左车道线")
rll_line, = ax.plot([], [], "r-", linewidth=3, label="右车道线")
path_line, = ax.plot([], [], "g-", linewidth=2, label="预测路径")
ax.legend()
- # -------------------------------------------------------------------
-
- # 逐帧推理+轻量化可视化
- print(f"\n开始推理+可视化(共{len(frame_tensors)-1}帧,按Q键退出)...")
- for i in range(len(frame_tensors) - 1):
- try:
- # 模型推理
- inputs = [np.vstack(frame_tensors[i:i+2])[None], desire, state]
- outs = supercombo.predict(inputs, verbose=0)
- parsed = parser(outs)
- state = outs[-1]
-
- # -------------------------- 仅更新线的数据,不重绘整个窗口 --------------------------
- lll_line.set_data(parsed["lll"][0], range(192)) # 只更新左车道线数据
- rll_line.set_data(parsed["rll"][0], range(192)) # 只更新右车道线数据
- path_line.set_data(parsed["path"][0], range(192))# 只更新路径数据
- fig.canvas.draw() # 轻量重绘(只更改造变的部分)
- fig.canvas.flush_events() # 强制刷新窗口,避免卡住
- # -------------------------------------------------------------------
-
- # 显示原始帧(简化版,用Matplotlib显示,避免OpenCV额外窗口)
- if i < len(raw_frames):
- # 新建一个小窗口显示原始帧,减少渲染压力
- cv2.imshow("原始帧", cv2.resize(raw_frames[i], (480, 270))) # 缩小尺寸
- if cv2.waitKey(100) & 0xFF == ord('q'): # 延长等待时间,给CPU喘息
- print("用户按Q键退出")
- break
-
- print(f"✅ 帧 {i+1}/{len(frame_tensors)-1} 完成")
- except Exception as e:
- print(f"⚠️ 帧 {i+1} 失败:{str(e)}")
- continue
-
- # 释放资源(简化版)
- print("\n🎉 处理完成!")
- plt.ioff()
- plt.close()
- cv2.destroyAllWindows()
+ # 逐帧推理+可视化
+ total_frames = len(frame_tensors) - 1
+ print(f"\n🚀 开始推理+可视化(共{total_frames}帧,按Q键/5秒无操作自动退出)...")
+ try:
+ for i in range(total_frames):
+ try:
+ inputs = [np.vstack(frame_tensors[i:i+2])[None], desire, state]
+ outs = supercombo.predict(inputs, verbose=0)
+ parsed = parser(outs)
+ state = outs[-1]
+
+ # 更新线条数据
+ lll_line.set_data(parsed["lll"][0], range(192))
+ rll_line.set_data(parsed["rll"][0], range(192))
+ path_line.set_data(parsed["path"][0], range(192))
+ fig.canvas.draw()
+ fig.canvas.flush_events()
+
+ # 优化点6:CV2窗口5秒无操作自动关闭,避免虚拟机卡死
+ if i < len(raw_frames):
+ cv2.imshow("原始帧", cv2.resize(raw_frames[i], (480, 270)))
+ key = cv2.waitKey(100) & 0xFF
+ if key == ord('q'):
+ print("🛑 用户按Q键退出")
+ break
+ # 5秒无操作自动退出(100ms*50=5秒)
+ if i % 50 == 0 and i != 0:
+ print("⚠️ 5秒无操作,自动退出...")
+ break
+
+ print(f"✅ 帧 {i+1}/{total_frames} 完成")
+
+ except Exception as e:
+ print(f"⚠️ 帧 {i+1} 失败:{str(e)}")
+ continue
+ finally:
+ # 彻底释放资源(适配虚拟机)
+ print("\n🧹 释放资源中...")
+ plt.ioff()
+ plt.close(fig)
+ cv2.destroyAllWindows()
+ # 强制清除CV2窗口残留
+ for _ in range(2):
+ cv2.waitKey(1)
+ print("🎉 处理完成!")
if __name__ == "__main__":
- main()
\ No newline at end of file
+ main()
diff --git a/src/robotic_arm_grasping/grasp.py b/src/robotic_arm_grasping/grasp.py
new file mode 100644
index 0000000000..75c3bda721
--- /dev/null
+++ b/src/robotic_arm_grasping/grasp.py
@@ -0,0 +1,407 @@
+import os
+import shutil
+import time
+import numpy as np
+import mujoco
+# 路径
+MODEL_PATH = r"C:\Users\龙忠梁\Downloads\mujoco-3.3.7-windows-x86_64\model\robotic_arm\arm_with_gripper.xml"
+
+def resolve_model_path(path: str) -> str:
+ """处理包含中文字符的路径"""
+ if not os.path.exists(path):
+ raise FileNotFoundError(f"模型文件不存在: {path}")
+
+ # 检查路径是否包含非ASCII字符
+ if any(ord(ch) > 127 for ch in path):
+ temp_dir = r"C:\temp_mujoco_project"
+ os.makedirs(temp_dir, exist_ok=True)
+ temp_path = os.path.join(temp_dir, "arm_with_gripper.xml")
+ shutil.copy2(path, temp_path)
+ print(f"已将模型复制到临时路径: {temp_path}")
+ return temp_path
+ return path
+
+def set_initial_pose(model, data):
+ """设置机械臂的初始姿态(立起来)"""
+ # 关节初始位置(弧度)
+ initial_qpos = np.zeros(model.nq)
+
+ # 设置6个主要关节的初始位置
+ joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"]
+ initial_angles = [0.0, 0.785, -1.57, 0.0, 0.0, 0.0]
+
+ for i, joint_name in enumerate(joint_names):
+ joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
+ if joint_id != -1:
+ qpos_addr = model.jnt_qposadr[joint_id]
+ initial_qpos[qpos_addr] = initial_angles[i]
+
+ # 设置抓取器初始为半张开状态
+ finger_joints = ["finger_1_joint", "finger_2_joint", "finger_3_joint",
+ "finger_1_proximal_joint", "finger_2_proximal_joint", "finger_3_proximal_joint",
+ "finger_1_distal_joint", "finger_2_distal_joint", "finger_3_distal_joint"]
+
+ for joint_name in finger_joints:
+ joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
+ if joint_id != -1:
+ qpos_addr = model.jnt_qposadr[joint_id]
+ if "slide" in joint_name: # 滑动关节初始位置
+ initial_qpos[qpos_addr] = 0.02 # 稍微张开
+ elif "proximal" in joint_name: # 近端关节
+ initial_qpos[qpos_addr] = 0.3 # 轻微弯曲
+ elif "distal" in joint_name: # 远端关节
+ initial_qpos[qpos_addr] = 0.2 # 轻微弯曲
+
+ data.qpos[:] = initial_qpos
+ mujoco.mj_forward(model, data)
+
+def move_arm_to_workspace(model, data, viewer):
+ """将机械臂移动到工作空间中央位置"""
+ print("正在移动机械臂到工作位置...")
+
+ # 控制机械臂移动到合适的工作位置
+ steps = 300
+ for i in range(steps):
+ # 简单的控制:保持抓取器位置,移动机械臂到合适位置
+ if model.nu >= 6:
+ # 设置机械臂关节的控制信号
+ # 调整机械臂姿态,使其处于更好的抓取位置
+ data.ctrl[0] = 0.0 # joint_1
+ data.ctrl[1] = 0.5 # joint_2 - 肩部
+ data.ctrl[2] = -0.3 # joint_3 - 肘部
+ data.ctrl[3] = 0.0 # joint_4
+ data.ctrl[4] = 0.2 # joint_5
+ data.ctrl[5] = 0.0 # joint_6
+
+ mujoco.mj_step(model, data)
+
+ # 同步视图
+ if viewer is not None:
+ viewer.sync()
+
+ time.sleep(model.opt.timestep * 0.5)
+
+ print("✓ 机械臂已移动到工作位置")
+
+def control_gripper_demo(model, data, viewer):
+ """演示爪勾的抓取功能,修正控制逻辑确保三个手指都向中心点弯曲"""
+
+ print("=== 爪勾抓取功能演示 ===")
+ print("修正控制逻辑,确保三个手指都向中心点弯曲")
+
+ # 首先,打印所有抓取器相关关节的信息,以便调试
+ print("\n抓取器关节信息:")
+ for i in range(model.nu):
+ joint_id = model.actuator_trnid[i, 0]
+ if joint_id != -1:
+ joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id)
+ if joint_name and "finger" in joint_name:
+ print(f" 控制通道 {i}: 控制关节 '{joint_name}' (ID: {joint_id})")
+
+ # 演示1:直接控制所有抓取器电机,观察效果
+ print("\n1. 测试所有抓取器控制通道...")
+
+ # 先清零所有控制
+ data.ctrl[:] = 0.0
+
+ # 测试张开:使用负值
+ print(" 测试张开动作 (使用负控制信号)...")
+ for i in range(300):
+ # 设置所有抓取器控制为负值
+ for j in range(6, model.nu): # 抓取器控制从索引6开始
+ data.ctrl[j] = -1.0
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 测试闭合:使用正值
+ print(" 测试闭合动作 (使用正控制信号)...")
+ for i in range(300):
+ # 设置所有抓取器控制为正值
+ for j in range(6, model.nu): # 抓取器控制从索引6开始
+ data.ctrl[j] = 1.0
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 重置为张开状态
+ print(" 重置为张开状态...")
+ for i in range(300):
+ for j in range(6, model.nu):
+ data.ctrl[j] = -1.0
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 演示2:尝试分别控制三个手指
+ print("\n2. 分别控制三个手指...")
+
+ # 创建控制通道到手指的映射
+ control_to_finger = {}
+ for i in range(model.nu):
+ joint_id = model.actuator_trnid[i, 0]
+ if joint_id != -1:
+ joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id)
+ if joint_name:
+ # 检查属于哪个手指
+ if "finger_1" in joint_name:
+ control_to_finger[i] = 1
+ elif "finger_2" in joint_name:
+ control_to_finger[i] = 2
+ elif "finger_3" in joint_name:
+ control_to_finger[i] = 3
+
+ # 手指1闭合
+ print(" 手指1闭合...")
+ for i in range(200):
+ for ctrl_idx, finger_num in control_to_finger.items():
+ if finger_num == 1:
+ data.ctrl[ctrl_idx] = 1.5 # 正值闭合
+ else:
+ data.ctrl[ctrl_idx] = 0.0
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 手指2闭合
+ print(" 手指2闭合...")
+ for i in range(200):
+ for ctrl_idx, finger_num in control_to_finger.items():
+ if finger_num == 2:
+ data.ctrl[ctrl_idx] = 1.5 # 正值闭合
+ else:
+ data.ctrl[ctrl_idx] = 0.0
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 手指3闭合
+ print(" 手指3闭合...")
+ for i in range(200):
+ for ctrl_idx, finger_num in control_to_finger.items():
+ if finger_num == 3:
+ data.ctrl[ctrl_idx] = 1.5 # 正值闭合
+ else:
+ data.ctrl[ctrl_idx] = 0.0
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 所有手指张开
+ print(" 所有手指张开...")
+ for i in range(300):
+ for j in range(6, model.nu):
+ data.ctrl[j] = -1.0 # 负值张开
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 演示3:尝试不同的控制策略
+ print("\n3. 尝试不同的控制策略...")
+
+ # 策略1:区分滑动关节和旋转关节
+ print(" 策略1: 区分滑动关节和旋转关节...")
+
+ # 首先识别不同类型的关节
+ slide_ctrl_indices = []
+ proximal_ctrl_indices = []
+ distal_ctrl_indices = []
+
+ for i in range(model.nu):
+ joint_id = model.actuator_trnid[i, 0]
+ if joint_id != -1:
+ joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id)
+ if joint_name:
+ if "slide" in joint_name or ("joint" in joint_name and "proximal" not in joint_name and "distal" not in joint_name):
+ slide_ctrl_indices.append(i)
+ elif "proximal" in joint_name:
+ proximal_ctrl_indices.append(i)
+ elif "distal" in joint_name:
+ distal_ctrl_indices.append(i)
+
+ print(f" 找到 {len(slide_ctrl_indices)} 个滑动关节控制")
+ print(f" 找到 {len(proximal_ctrl_indices)} 个近端关节控制")
+ print(f" 找到 {len(distal_ctrl_indices)} 个远端关节控制")
+
+ # 完全张开
+ print(" 完全张开...")
+ for i in range(300):
+ for idx in slide_ctrl_indices:
+ data.ctrl[idx] = -2.0 # 负值可能使滑动关节向外
+ for idx in proximal_ctrl_indices:
+ data.ctrl[idx] = -1.5 # 负值可能使旋转关节伸直
+ for idx in distal_ctrl_indices:
+ data.ctrl[idx] = -1.0 # 负值可能使远端关节伸直
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 完全闭合(尝试不同的组合)
+ print(" 尝试闭合 (组合1)...")
+ for i in range(300):
+ for idx in slide_ctrl_indices:
+ data.ctrl[idx] = 2.0 # 正值可能使滑动关节向内
+ for idx in proximal_ctrl_indices:
+ data.ctrl[idx] = 1.5 # 正值可能使旋转关节弯曲
+ for idx in distal_ctrl_indices:
+ data.ctrl[idx] = 1.0 # 正值可能使远端关节弯曲
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 尝试相反的组合
+ print(" 尝试闭合 (组合2 - 相反符号)...")
+ for i in range(300):
+ for idx in slide_ctrl_indices:
+ data.ctrl[idx] = -2.0 # 负值可能使滑动关节向内
+ for idx in proximal_ctrl_indices:
+ data.ctrl[idx] = -1.5 # 负值可能使旋转关节弯曲
+ for idx in distal_ctrl_indices:
+ data.ctrl[idx] = -1.0 # 负值可能使远端关节弯曲
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 固定机械臂主体,只控制夹爪动作
+ print(" 固定机械臂主体,只控制夹爪动作...")
+ # 锁定前6个关节(机械臂主体)
+ for j in range(6):
+ data.ctrl[j] = 0.0 # 固定机械臂主体不动
+
+ # 增加多次抓取循环演示
+ print(" 增加多次抓取循环演示...")
+ for cycle in range(5): # 执行5次抓取循环
+ print(f" 抓取循环 {cycle+1} 开始...")
+
+ # 闭合抓取器 - 更强的控制信号
+ print(f" 闭合抓取器 (循环 {cycle+1})...")
+ for i in range(300): # 增加迭代次数
+ # 固定机械臂主体不动
+ for j in range(6):
+ data.ctrl[j] = 0.0
+
+ # 控制夹爪闭合
+ for j in range(6, model.nu):
+ data.ctrl[j] = 2.0 # 更大的正值确保完全闭合
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 等待一段时间
+ print(f" 保持抓取状态 (循环 {cycle+1})...")
+ for i in range(150): # 增加等待时间
+ # 固定机械臂主体不动
+ for j in range(6):
+ data.ctrl[j] = 0.0
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 张开抓取器 - 更强的控制信号
+ print(f" 张开抓取器 (循环 {cycle+1})...")
+ for i in range(300): # 增加迭代次数
+ # 固定机械臂主体不动
+ for j in range(6):
+ data.ctrl[j] = 0.0
+
+ # 控制夹爪张开
+ for j in range(6, model.nu):
+ data.ctrl[j] = -2.0 # 更大的负值确保完全张开
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 等待一段时间
+ print(f" 保持张开状态 (循环 {cycle+1})...")
+ for i in range(150): # 增加等待时间
+ # 固定机械臂主体不动
+ for j in range(6):
+ data.ctrl[j] = 0.0
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ print(" 多次抓取循环演示完成!")
+
+ # 最终回到张开状态
+ print(" 最终回到张开状态...")
+ for i in range(300):
+ for j in range(6, model.nu):
+ data.ctrl[j] = -1.0 # 使用负值
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ print("\n✓ 爪勾抓取功能演示完成!")
+ print("\n观察结果:")
+ print("1. 如果所有手指都正确向中心弯曲,说明控制信号符号正确")
+ print("2. 如果有手指向外翻,可能需要检查模型中的关节轴设置")
+ print("3. 如果所有手指都上下运动,说明滑动关节轴可能是(0,0,1)而不是水平方向")
+
+def main():
+ """主函数"""
+ # 加载模型
+ model_path = resolve_model_path(MODEL_PATH)
+ model = mujoco.MjModel.from_xml_path(model_path)
+ data = mujoco.MjData(model)
+
+ print("开始机械臂爪勾抓取功能演示")
+ print("修正控制逻辑,测试不同的控制策略")
+
+ # 启动可视化
+ with mujoco.viewer.launch_passive(model, data) as viewer:
+ # 设置相机视角,专注于抓取器
+ viewer.cam.azimuth = 180
+ viewer.cam.elevation = -20
+ viewer.cam.distance = 0.8
+ viewer.cam.lookat[:] = [0.25, 0.0, 0.15]
+
+ print("\n=== 阶段1:初始化机械臂 ===")
+ # 设置机械臂初始姿态
+ set_initial_pose(model, data)
+
+ # 给一些时间让机械臂稳定
+ for _ in range(150):
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep)
+
+ print("✓ 机械臂已初始化")
+
+ print("\n=== 阶段2:爪勾抓取功能演示 ===")
+ # 演示爪勾的抓取功能
+ control_gripper_demo(model, data, viewer)
+
+ print("\n=== 演示完成 ===")
+ print("请在可视化窗口中观察三个手指的运动方向")
+ print("根据观察结果,可能需要调整控制信号的符号或模型设置")
+ print("关闭窗口以结束程序")
+
+ # 保持窗口打开,让用户观察
+ print("\n保持张开状态展示中...")
+ for i in range(300):
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(model.opt.timestep * 0.5)
+
+ # 保持窗口打开
+ while viewer.is_running:
+ time.sleep(0.1)
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file