diff --git a/README.md b/README.md index 83b4947c43..b9bdd894be 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,12 @@ * 相关软件下载 [链接](https://pan.baidu.com/s/1IFhCd8X9lI24oeYQm5-Edw?pwd=hutb) +## 功能模块 + +| 功能类别 | 模块名 | 链接 | 其他(参考链接等) | +| ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------------------------- |-------------------------| +| 规划 | CARLA 全局路径规划和SLAM节点 | [carla_slam_gmapping](https://github.com/OpenHUTB/nn/tree/main/src/carla_slam_gmapping) | - | + ## 贡献指南 准备提交代码之前,请阅读 [贡献指南](https://github.com/OpenHUTB/.github/blob/master/CONTRIBUTING.md) 。 @@ -45,5 +51,4 @@ mkdocs serve * 已有相关 [无人车](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/main.py b/main.py deleted file mode 100644 index 5d447efb56..0000000000 --- a/main.py +++ /dev/null @@ -1,182 +0,0 @@ -import sys -import os -# 替换绝对路径为相对路径:基于main.py所在目录向上找src文件夹(适配任意部署环境) -sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)), '..')) - -import numpy as np -import cv2 -from tensorflow.keras.models import load_model -from common.transformations.camera import transform_img, eon_intrinsics -from common.transformations.model import medmodel_intrinsics -from common.tools.lib.parser import parser - -# 关闭TensorFlow所有冗余警告 -os.environ['TF_CPP_MIN_LOG_LEVEL'] = '3' - -# -------------------------- 核心工具函数 -------------------------- -def frames_to_tensor(frames): - if len(frames) == 0: - return np.array([]) - H = (frames.shape[1] * 2) // 3 - W = frames.shape[2] - 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)) - 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): - try: - processed[i] = transform_img(img, from_intr=eon_intrinsics, to_intr=medmodel_intrinsics, yuv=True, output_size=(512, 256)) - except: - processed[i] = np.zeros((384, 512), dtype=np.uint8) - return frames_to_tensor(processed) - -# -------------------------- 主函数(无参数、全英文、车道线优化) -------------------------- -def main(): - # 1. 初始化显示窗口(800x600,固定尺寸) - win_name = "Lane Line Prediction (Blue=Left | Red=Right | Green=Path)" - cv2.namedWindow(win_name, cv2.WINDOW_NORMAL) - cv2.resizeWindow(win_name, 800, 600) - - # 2. 读取视频(修改为相对路径:main.py所在文件夹下的sample.hevc) - video_path = "./sample.hevc" # 仅改这里:绝对路径→相对路径 - cap = cv2.VideoCapture(video_path) - if not cap.isOpened(): - empty_frame = np.ones((600, 800, 3), dtype=np.uint8) * 255 - cv2.putText(empty_frame, "Cannot open video", (150, 300), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3) - cv2.imshow(win_name, empty_frame) - cv2.waitKey(0) - cv2.destroyAllWindows() - return - - # 读取前10帧(用于推理,保留原始帧和模型输入帧) - raw_display_frames = [] # 用于显示的800x600帧 - model_input_imgs = [] # 用于模型的512x384 YUV帧 - for _ in range(10): - ret, frame = cap.read() - if not ret: - break - # 缩放为显示尺寸(800x600) - display_frame = cv2.resize(frame, (800, 600)) - raw_display_frames.append(display_frame) - # 转换为模型需要的YUV格式并缩放 - yuv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2YUV_I420) - model_frame = cv2.resize(yuv_frame, (512, 384), cv2.INTER_AREA) - model_input_imgs.append(model_frame) - cap.release() - - # 校验帧数是否足够 - if len(raw_display_frames) < 2: - empty_frame = np.ones((600, 800, 3), dtype=np.uint8) * 255 - cv2.putText(empty_frame, "Insufficient video frames", (100, 300), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 0, 255), 3) - cv2.imshow(win_name, empty_frame) - cv2.waitKey(0) - cv2.destroyAllWindows() - return - - # 3. 加载模型(显示英文提示,无乱码) - load_frame = np.ones((600, 800, 3), dtype=np.uint8) * 255 - cv2.putText(load_frame, "Loading model...", (200, 300), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 0), 3) - cv2.imshow(win_name, load_frame) - cv2.waitKey(200) # 刷新显示 - - # 模型路径(修改为相对路径:main.py所在文件夹下的models/supercombo.h5) - model_path = "./models/supercombo.h5" # 仅改这里:绝对路径→相对路径 - try: - supercombo_model = load_model(model_path, compile=False) - except Exception as e: - empty_frame = np.ones((600, 800, 3), dtype=np.uint8) * 255 - cv2.putText(empty_frame, "Model load failed", (180, 300), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3) - cv2.imshow(win_name, empty_frame) - cv2.waitKey(0) - cv2.destroyAllWindows() - return - - # 4. 预处理帧(显示英文提示) - preprocess_frame = np.ones((600, 800, 3), dtype=np.uint8) * 255 - cv2.putText(preprocess_frame, "Preprocessing frames...", (150, 300), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 0), 3) - cv2.imshow(win_name, preprocess_frame) - cv2.waitKey(200) - - frame_tensors = preprocess_frames(model_input_imgs) - if frame_tensors.size == 0: - empty_frame = np.ones((600, 800, 3), dtype=np.uint8) * 255 - cv2.putText(empty_frame, "Preprocessing failed", (180, 300), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 0, 255), 3) - cv2.imshow(win_name, empty_frame) - cv2.waitKey(0) - cv2.destroyAllWindows() - return - - # 5. 模型状态初始化 - model_state = np.zeros((1, 512)) - model_desire = np.zeros((1, 8)) - - # 6. 逐帧推理+绘制(核心优化:车道线右移+放大圆点) - print("✅ Start inference and display (Press Q to exit)") - for i in range(len(frame_tensors) - 1): - # 确保帧存在,避免索引越界 - if i >= len(raw_display_frames): - current_frame = np.ones((600, 800, 3), dtype=np.uint8) * 255 - else: - current_frame = raw_display_frames[i].copy() # 复制原始帧,避免修改 - - try: - # 模型推理(连续两帧作为输入) - input_data = [np.vstack(frame_tensors[i:i+2])[None], model_desire, model_state] - model_output = supercombo_model.predict(input_data, verbose=0) - parsed_result = parser(model_output) - model_state = model_output[-1] - - # -------------------------- 车道线绘制优化 -------------------------- - # 提取模型输出的车道线/路径x坐标 - left_lane_x = parsed_result["lll"][0] - right_lane_x = parsed_result["rll"][0] - path_x = parsed_result["path"][0] - - # 窗口尺寸 - win_h, win_w = 600, 800 - # y坐标映射(0-191 → 0-599) - y_points = np.linspace(0, win_h - 1, 192).astype(int) - # x坐标映射(0-512 → 0-799)+ 右移100像素(解决偏左问题)+ 放大圆点到8px - left_x_mapped = (left_lane_x / 512 * win_w + 100).astype(int) - right_x_mapped = (right_lane_x / 512 * win_w + 100).astype(int) - path_x_mapped = (path_x / 512 * win_w + 100).astype(int) - - # 绘制左车道线(蓝色,8px实心圆) - for x, y in zip(left_x_mapped, y_points): - if 0 <= x < win_w and 0 <= y < win_h: - cv2.circle(current_frame, (x, y), 8, (255, 0, 0), -1) - # 绘制右车道线(红色,8px实心圆) - for x, y in zip(right_x_mapped, y_points): - if 0 <= x < win_w and 0 <= y < win_h: - cv2.circle(current_frame, (x, y), 8, (0, 0, 255), -1) - # 绘制预测路径(绿色,6px实心圆) - for x, y in zip(path_x_mapped, y_points): - if 0 <= x < win_w and 0 <= y < win_h: - cv2.circle(current_frame, (x, y), 6, (0, 255, 0), -1) - - except Exception as e: - # 推理失败时仅打印错误,仍显示原始帧 - print(f"⚠️ Frame {i+1} inference error: {str(e)[:30]}") - - # 强制显示当前帧 - cv2.imshow(win_name, current_frame) - # 按Q退出 - if cv2.waitKey(100) & 0xFF == ord('q'): - print("🛑 Exit by user (Q pressed)") - break - - # 7. 程序收尾 - cv2.destroyAllWindows() - print("🎉 All frames processed successfully!") - -if __name__ == "__main__": - main() 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..2c4315f9ec 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 @@ -110,7 +121,7 @@ def reset(self) -> tuple[float, float, np.ndarray, bool]: self.collision_sensor.listen(lambda e: {self._set_collision()}) self.initialized = True - return self._change_to_left_lane() + return 0.0, 0.0, np.zeros((self.image_height, self.image_width, 3)), False def _set_collision(self) -> None: """Helper to set collision_detected to True. diff --git a/src/AutonomousDriving_RL/eval_agent.py b/src/AutonomousDriving_RL/eval_agent.py index ff589eac57..d7cb9b9eb7 100644 --- a/src/AutonomousDriving_RL/eval_agent.py +++ b/src/AutonomousDriving_RL/eval_agent.py @@ -1,45 +1,63 @@ # eval_agent.py -""" -加载训练好的模型,在 CARLA 中演示智能体驾驶行为 -""" - -import argparse +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(): - parser = argparse.ArgumentParser() - parser.add_argument("--model_path", type=str, default="./checkpoints/best_model.zip", help="模型路径") - parser.add_argument("--steps", type=int, default=200, help="演示步数") - args = parser.parse_args() - - print("🔄 加载环境与模型...") - env = CarlaEnvMultiObs(keep_alive_after_exit=True) # 保留车辆便于观察 - model = PPO.load(args.model_path, env=env) - - print("▶️ 开始驾驶演示(运行 {} 步)...".format(args.steps)) - obs, _ = env.reset() - total_reward = 0.0 - - for step in range(args.steps): - action, _ = model.predict(obs, deterministic=True) - obs, reward, terminated, truncated, info = env.step(action) - total_reward += reward - - # 每 50 步打印状态 - if step % 50 == 0 or step == args.steps - 1: - x, y, vx, vy = obs - speed = np.linalg.norm([vx, vy]) - print(f" Step {step}: 位置=({x:.1f}, {y:.1f}), 速度={speed:.2f} m/s") - - if terminated or truncated: - break - - print(f"✅ 演示完成!总奖励: {total_reward:.2f}") - input("🛑 准备好后,请回到本窗口按 Enter 键退出...") - env.close() + 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__": 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/Carla-YOLO-DeepSort-Multi-Object-Tracking/README.MD b/src/Carla-YOLO-DeepSort-Multi-Object-Tracking/README.MD new file mode 100644 index 0000000000..7b03d8c859 --- /dev/null +++ b/src/Carla-YOLO-DeepSort-Multi-Object-Tracking/README.MD @@ -0,0 +1,37 @@ +# Multi Vehicle tracking in carla simulator + +This repo host a comprehensive solution for multi-object tracking in combination with YOLO and DeepSORT in carla simulator . +YOLOV8 model is trained on Carla dataset , which is avaiable in kaggle (https://www.kaggle.com/datasets/alechantson/carladataset) + +![ezgif com-optimize](https://github.com/Bsornapudi/Carla-YOLO-DeepSort-Multi-Object-Tracking/assets/48683074/c365a981-e314-4cae-b4aa-d234b3de5cfa) + +Following software are required before installing required packages + +1. Carla simulator : Download Carla simulator and follow the instuction guide - https://carla.readthedocs.io/en/latest/start_quickstart/ +2. CUDA : Download and install CUDA - https://developer.nvidia.com/cuda-downloads +3. CuDnn : Install Cuda DNN - https://docs.nvidia.com/deeplearning/cudnn/install-guide/index.html +4. Anacoda : https://www.anaconda.com/download +5. PyMOT : Download / clone this repo for evaluations - https://github.com/Videmo/pymot +6. Create and activate a new virtural conda env + + ``` + conda create --name python=3.8 + conda activate + NOTE : should be the name of your virtural env + ``` +7. Install required packages + ``` + pip install requirements.txt + ``` +8. Once the setup is done Run Calra.exe file to launch simulator +9. One commapnd prompt or launch jupyter from conda prompt +10. Run track.ipynb file in jupyter +11. Run gt_deepsort.ipynv file followed by evaluate.ipynb to generate MOTA and MOTP values +NOTE : + +step to check PyTorch compatibility and install an appropriate version based on their CUDA and cuDNN configurations from this link , scroll down and you will see an option to select your system configs and tis will generate PIP INSTALL for CUDnn+torch which is compatable +https://pytorch.org/ + +Eg : pip3 install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu117 + + diff --git a/src/Carla-YOLO-DeepSort-Multi-Object-Tracking/requirements.txt b/src/Carla-YOLO-DeepSort-Multi-Object-Tracking/requirements.txt new file mode 100644 index 0000000000..5476b2ea17 --- /dev/null +++ b/src/Carla-YOLO-DeepSort-Multi-Object-Tracking/requirements.txt @@ -0,0 +1,61 @@ +asttokens==2.2.1 +backcall==0.2.0 +carla==0.9.14 +certifi==2023.7.22 +charset-normalizer==3.2.0 +colorama==0.4.6 +contourpy==1.1.0 +cycler==0.11.0 +decorator==5.1.1 +easydict==1.10 +executing==1.2.0 +filelock==3.12.2 +fonttools==4.42.0 +idna==3.4 +importlib-resources==6.0.1 +ipdb==0.13.13 +ipython==8.12.2 +jedi==0.19.0 +Jinja2==3.1.2 +kiwisolver==1.4.4 +MarkupSafe==2.1.3 +matplotlib==3.7.2 +matplotlib-inline==0.1.6 +mpmath==1.3.0 +munkres==1.1.4 +networkx==3.1 +numpy==1.24.4 +opencv-python==4.8.0.74 +packaging==23.1 +pandas==2.0.3 +parso==0.8.3 +pascal-voc-writer==0.1.4 +pickleshare==0.7.5 +Pillow==10.0.0 +prompt-toolkit==3.0.39 +psutil==5.9.5 +pure-eval==0.2.2 +py-cpuinfo==9.0.0 +Pygments==2.16.1 +pyparsing==3.0.9 +python-dateutil==2.8.2 +pytz==2023.3 +PyYAML==6.0.1 +requests==2.31.0 +scipy==1.10.1 +seaborn==0.12.2 +six==1.16.0 +stack-data==0.6.2 +sympy==1.12 +tomli==2.0.1 +torch==2.0.1 +torchvision==0.15.2 +tqdm==4.65.2 +traitlets==5.9.0 +typing_extensions==4.7.1 +tzdata==2023.3 +ultralytics==8.0.150 +urllib3==2.0.4 +wcwidth==0.2.6 +zipp==3.16.2 +munkres==1.1.4 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_Accident_Video_Recognition/requirements.txt b/src/Driving_Accident_Video_Recognition/requirements.txt new file mode 100644 index 0000000000..4889493ce2 --- /dev/null +++ b/src/Driving_Accident_Video_Recognition/requirements.txt @@ -0,0 +1,20 @@ +# YOLOv8核心库(必须) +ultralytics>=8.0.0 # 推荐8.2.0(稳定版) + +# 图像处理(必须) +opencv-python>=4.8.0 # 推荐4.8.1.78 + +# 可视化(必须) +matplotlib>=3.0.0 # 推荐3.7.1 + +# PyTorch核心(ultralytics依赖,必须) +torch>=2.0.0 # 推荐2.0.1或2.1.0 +torchvision>=0.15.0 # 与torch版本匹配 + +# 基础数值计算(间接依赖,建议指定) +numpy>=1.24.0 # 推荐1.24.3 + +# 可选(补充依赖,避免隐性报错) +pillow>=8.0.0 # 推荐9.5.0 +psutil>=5.8.0 # ultralytics监控系统资源用 +pyyaml>=6.0 # 自定义训练时解析yaml配置文件 \ No newline at end of file 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_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/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/HCL_interaction_task/mobl_arms_index_choice_reaction/simulator.py b/src/HCL_interaction_task/mobl_arms_index_choice_reaction/simulator.py index 5a48b643d6..12a44016ac 100644 --- a/src/HCL_interaction_task/mobl_arms_index_choice_reaction/simulator.py +++ b/src/HCL_interaction_task/mobl_arms_index_choice_reaction/simulator.py @@ -1,128 +1,183 @@ import mujoco -import mujoco.viewer # 强制导入viewer(放弃兼容旧版本,优先保证运行) +import mujoco.viewer import numpy as np import yaml import time import os -# 解决Linux GLX错误:优先用osmesa,若不行则用glfw(备选) -os.environ['MUJOCO_GL'] = 'osmesa' -# 增加环境变量,避免viewer闪退 -os.environ['MJPYTHON_FRAMEWORK'] = 'gtk3' +# 兼容所有MuJoCo版本+远程环境 +os.environ['MUJOCO_GL'] = 'egl' +os.environ['MJPYTHON_FRAMEWORK'] = 'none' +os.environ['MESA_GL_VERSION_OVERRIDE'] = '3.3' class IndexSimulator: def __init__(self, config_path, model_path): - # 1. 加载配置 with open(config_path, 'r') as f: self.config = yaml.safe_load(f) - # 2. 加载MuJoCo模型 self.model = mujoco.MjModel.from_xml_path(model_path) self.data = mujoco.MjData(self.model) - # 3. 检查qpos长度(关键!避免索引错误) - self.qpos_len = len(self.data.qpos) - print(f"✅ 模型加载成功!qpos长度:{self.qpos_len}") - self.move_joints = 3 if self.qpos_len >=3 else self.qpos_len - - # 4. 初始化核心变量 + # 1. 模型关键信息+关节range(稳定核心) + print("\n📌 模型核心信息:") + print(f"关节总数:{self.model.njnt} | 执行器数:{self.model.nu} | qpos长度:{len(self.data.qpos)}") + + self.key_joints = { + 15: { + "name": "shoulder_rot (肩关节旋转)", + "range": (-1.57, 0.349), + "step": 0.02, + "current_step_dir": 0.02 # 运动方向 + }, + 16: { + "name": "elbow_flexion (肘关节弯曲)", + "range": (0, 2.26893), + "step": 0.03, + "current_step_dir": 0.03 + }, + 17: { + "name": "pro_sup (前臂旋前旋后)", + "range": (-1.5708, 1.5708), + "step": 0.015, + "current_step_dir": 0.015 + } + } + + # 打印关节初始信息 + print("\n📌 关键关节(带range限制):") + for jnt_id, info in self.key_joints.items(): + if jnt_id < len(self.data.qpos): + init_val = (info['range'][0] + info['range'][1]) / 2 + print(f"关节{jnt_id}:{info['name']} | range:{info['range']} | 初始值:{init_val:.2f} rad") + + # 2. 仿真状态(极简,避免段错误) self.is_running = True self.viewer = None - self.button_names = ["button-0", "button-1", "button-2", "button-3"] - self.button_touched = {name: False for name in self.button_names} + self.print_interval = 50 + self.current_step = 0 + self.button_touched = {"button-0":False, "button-1":False, "button-2":False, "button-3":False} self.finger_geom_name = "hand_2distph" def reset(self): - """重置仿真:安全赋值qpos""" + """重置关节到range中间值""" mujoco.mj_resetData(self.model, self.data) - # 安全赋值:只给存在的关节赋值 - if self.qpos_len >= 1: - self.data.qpos[0] = 0.0 # X方向 - if self.qpos_len >= 2: - self.data.qpos[1] = 0.0 # Y方向 - if self.qpos_len >= 3: - self.data.qpos[2] = 0.0 # Z方向 - self.button_touched = {name: False for name in self.button_names} + for jnt_id, info in self.key_joints.items(): + if jnt_id < len(self.data.qpos): + init_val = (info['range'][0] + info['range'][1]) / 2 + self.data.qpos[jnt_id] = init_val + self.key_joints[jnt_id]['current_step_dir'] = info['step'] # 重置方向 + self.current_step = 0 self.is_running = True return self.data.qpos.copy() - def step(self, action=None): - """每一步仿真:安全移动手指""" - # 1. 默认动作:向按钮移动(放慢速度,避免瞬移) - if action is None: - action = np.array([0.001, 0.0, 0.0]) # 把步长从0.01改成0.001,移动更慢更稳 - - # 2. 安全更新qpos(只更新存在的关节) - for i in range(min(len(action), self.move_joints)): - self.data.qpos[i] += action[i] - - # 3. 推进仿真 + def step(self): + """单步仿真:稳定+慢速,无错误""" + if not self.is_running: + return self.data.qpos.copy() + + # 1. 关节慢速摆动(不超range) + for jnt_id, info in self.key_joints.items(): + if jnt_id >= len(self.data.qpos): + continue + + current_val = self.data.qpos[jnt_id] + new_val = current_val + info['current_step_dir'] + + # 到边界反向 + if new_val > info['range'][1]: + self.key_joints[jnt_id]['current_step_dir'] = -info['step'] + new_val = info['range'][1] - 0.01 + elif new_val < info['range'][0]: + self.key_joints[jnt_id]['current_step_dir'] = info['step'] + new_val = info['range'][0] + 0.01 + + # 安全赋值 + self.data.qpos[jnt_id] = np.clip(new_val, info['range'][0], info['range'][1]) + + # 2. 推进仿真 mujoco.mj_step(self.model, self.data) - # 4. 检测碰撞 - self._check_button_collision() + # 3. 定期打印(减少刷屏) + if self.current_step % self.print_interval == 0: + print(f"\n📌 step{self.current_step} 关节角度(直观度数):") + for jnt_id, info in self.key_joints.items(): + if jnt_id < len(self.data.qpos): + rad = self.data.qpos[jnt_id] + deg = rad * 57.3 # 弧度转角度 + print(f" {info['name']}:{rad:.2f} rad ≈ {deg:.0f}°") + + # 4. 渲染(容错处理,避免段错误) + if self.viewer: + try: + self.viewer.sync() + time.sleep(0.03) # 慢速,肉眼看清 + except Exception as e: + print(f"⚠️ 渲染同步警告(不影响仿真):{e}") + pass + self.current_step += 1 return self.data.qpos.copy() def run_simulation(self): - """ - 核心修改:主动创建Viewer+强制循环,直到手动关闭 - """ - print("✅ 仿真器初始化成功!") - print("👉 可视化窗口已弹出,手指正在向按钮移动...(按Ctrl+C终止)") + """核心:仅Ctrl+C中断,无窗口检测(解决段错误)""" + print("\n✅ 仿真启动成功!") + print("👉 手臂会慢速稳定摆动,按【Ctrl+C】终止仿真(关闭窗口需手动Ctrl+C)") + print("👉 关节严格限制在安全范围,无NaN/段错误!") self.reset() - # 主动创建Viewer实例(放弃passive模式,改用主动模式) + # 启动可视化(最大容错) try: - with mujoco.viewer.launch(self.model, self.data) as self.viewer: - # 调整视角 - self.viewer.cam.azimuth = 135 - self.viewer.cam.elevation = -15 - self.viewer.cam.distance = 1.2 - self.viewer.cam.lookat = [0.4, 0.0, 0.4] - - # 强制循环:直到手动关闭窗口/按Ctrl+C - while self.is_running and self.viewer.is_running(): - # 执行仿真步 - self.step() - # 控制帧率(每秒50帧,不会卡) - time.sleep(0.02) - # 同步viewer(主动模式必须手动sync) - self.viewer.sync() - - except KeyboardInterrupt: - # 按Ctrl+C优雅退出 - print("\n⚠️ 检测到Ctrl+C,正在退出仿真...") + self.viewer = mujoco.viewer.launch_passive(self.model, self.data) + # 优化视角 + self.viewer.cam.azimuth = 135 + self.viewer.cam.elevation = -15 + self.viewer.cam.distance = 0.6 + self.viewer.cam.lookat = [0.45, -0.15, 0.8] + print("✅ 可视化窗口启动成功!能看到手臂慢速摆动") except Exception as e: - print(f"⚠️ 可视化启动失败,改用无窗口模式运行:{e}") - # 无窗口模式:循环100秒后退出 - start_time = time.time() - while time.time() - start_time < 100: + print(f"⚠️ 可视化启动失败(不影响动作):{e}") + print("📢 无窗口模式:终端打印关节角度,确认动作真实发生!") + + # ========== 终极循环:仅Ctrl+C中断,无窗口检测 ========== + try: + while self.is_running: self.step() - time.sleep(0.02) + except KeyboardInterrupt: + print("\n\n⚠️ 检测到Ctrl+C,正在优雅退出仿真...") + self.is_running = False + # 清理资源 self.close() - - def _check_button_collision(self): - """检测碰撞(简化版)""" - for i in range(self.data.ncon): - contact = self.data.contact[i] - geom1 = self.model.geom(contact.geom1).name - geom2 = self.model.geom(contact.geom2).name - - for btn_name in self.button_names: - if (btn_name in [geom1, geom2]) and (self.finger_geom_name in [geom1, geom2]): - if not self.button_touched[btn_name]: - color_map = {"button-0":"红", "button-1":"绿", "button-2":"蓝", "button-3":"黄"} - print(f"🎉 碰到【{color_map[btn_name]}按钮】!(继续运行,不会停止)") - self.button_touched[btn_name] = True + print(f"\n✅ 仿真正常结束!共运行{self.current_step}步") + print("📊 最终关节角度:") + for jnt_id, info in self.key_joints.items(): + if jnt_id < len(self.data.qpos): + rad = self.data.qpos[jnt_id] + deg = rad * 57.3 + print(f" {info['name']}:{rad:.2f} rad ≈ {deg:.0f}°") def close(self): - """关闭资源""" + """安全关闭资源,避免段错误""" self.is_running = False if self.viewer: try: - self.viewer.close() + # 不同版本MuJoCo的viewer关闭方式兼容 + if hasattr(self.viewer, 'close'): + self.viewer.close() + else: + mujoco.viewer.close(self.viewer) except: pass - print("\n👋 仿真结束~") \ No newline at end of file + +if __name__ == "__main__": + # 替换为你的配置和模型路径 + CONFIG_PATH = "config.yaml" + MODEL_PATH = "simulation.xml" + + # 启动仿真(包裹try-except,避免核心转储) + try: + sim = IndexSimulator(CONFIG_PATH, MODEL_PATH) + sim.run_simulation() + except Exception as e: + print(f"\n❌ 仿真启动异常:{e}") + print("💡 请检查模型文件路径/配置文件是否正确") \ No newline at end of file diff --git a/src/HCL_interaction_task/requirements.txt b/src/HCL_interaction_task/requirements.txt new file mode 100644 index 0000000000..7108066433 --- /dev/null +++ b/src/HCL_interaction_task/requirements.txt @@ -0,0 +1,5 @@ +mujoco>=2.3.7 +mediapipe>=0.10.9 +opencv-python>=4.8.1 +pyyaml>=6.0.1 +numpy>=1.26.2 \ 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/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/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/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/Test.py b/src/Unmanned_vehicle_AD_DQN/Test.py index d284b8245e..cb53560dbd 100644 --- a/src/Unmanned_vehicle_AD_DQN/Test.py +++ b/src/Unmanned_vehicle_AD_DQN/Test.py @@ -65,7 +65,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"] # 排除统计文件 @@ -150,16 +150,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]: # 转向动作 @@ -176,18 +182,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) @@ -224,6 +236,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 @@ -241,6 +256,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) @@ -259,10 +280,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 @@ -274,10 +299,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): @@ -373,17 +402,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) @@ -406,6 +437,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}") @@ -416,6 +448,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}秒") @@ -499,7 +532,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/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/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/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_python_examples b/src/carla_python_examples new file mode 160000 index 0000000000..eab09dd94b --- /dev/null +++ b/src/carla_python_examples @@ -0,0 +1 @@ +Subproject commit eab09dd94b61b9af8e29acf84975c4cf834b98e0 diff --git a/src/carla_slam_gmapping/README.md b/src/carla_slam_gmapping/README.md index 3973b3231a..bb190045a9 100644 --- a/src/carla_slam_gmapping/README.md +++ b/src/carla_slam_gmapping/README.md @@ -1,8 +1,7 @@ # 人车模拟 -# CARLA 全局路径规划节点 - -实现基于 CARLA 模拟器的车辆全局路径规划功能,支持从车辆自身位置(通过里程计输入)到地图中随机目标点的路径生成,并通过 ROS 2 话题可视化规划结果,模拟用户预订行程后车辆的全局路径规划过程。 +# CARLA 全局路径规划和SLAM节点 +实现基于 CARLA 模拟器的车辆全局路径规划功能,支持从车辆自身位置(通过里程计输入)到地图中随机目标点的路径生成,并通过 ROS 2 话题可视化规划结果,模拟用户预订行程后车辆的全局路径规划过程。 新增支持 3D 建图功能,集成 GMapping 算法实现三维环境下的地图构建。 ## 功能概述 该节点是 CARLA 自动驾驶仿真系统中的核心模块之一,主要功能包括: @@ -10,7 +9,7 @@ - **路径规划服务**:提供 ROS 2 服务 `plan_to_random_goal`,接收起始点里程计信息,生成到随机目标点的全局路径 - **路径可视化**:通过 `visualization_marker` 话题发布路径标记,在 RViz 中以绿色线条直观展示规划结果 - **鲁棒性保障**:包含路径有效性校验(确保路径长度满足需求)、异常捕获和错误日志输出,避免无限循环或崩溃 - +- **3D 建图**:集成 GMapping 算法,利用激光雷达数据进行实时三维地图构建 ## 环境配置 ### 基础环境 @@ -33,7 +32,6 @@ ```bash export PYTHONPATH=$PYTHONPATH:/path/to/your/carla/PythonAPI/carla/dist/carla-0.9.15-py3.8-linux-x86_64.egg ``` - 3. **其他 Python 依赖** 安装路径计算和坐标转换所需依赖: ```bash @@ -45,8 +43,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 +84,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 +103,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,6 +112,98 @@ rviz2 1. 设置 `Fixed Frame` 为 `map` 2. 添加 `Marker` 显示项,设置 `Topic` 为 `/visualization_marker` 3. 路径会以绿色线条显示在场景中 +4. 添加 Map 显示项,设置 Topic 为 /map 查看实时构建的地图 + +## SLAM 算法详解 + +### SLAM 概述 + +SLAM (Simultaneous Localization and Mapping) 是移动机器人领域的一项关键技术,它允许机器人在未知环境中同时构建地图并确定自身位置。这对于自主导航至关重要,因为机器人必须知道自己的位置才能有效地规划路径并避开障碍物。 + +SLAM 问题的核心挑战在于: + +- 机器人不知道自己在哪里(定位问题) +- 机器人不知道环境是什么样子(建图问题) +- 这两个问题相互依赖,必须同时解决 + + + +### GMapping 算法原理 + +本项目采用经典的 GMapping 算法实现 2D 建图功能。GMapping 是一种基于粒子滤波的 SLAM 算法,它通过以下步骤实现地图构建: + +1. **粒子滤波**: 使用多个粒子表示机器人的可能位姿,每个粒子都携带一个地图副本 +2. **运动模型更新**: 根据里程计数据预测每个粒子的新位姿 +3. **观测模型评估**: 利用激光雷达扫描数据评估每个粒子的权重 +4. **重采样**: 根据权重重新分布粒子集,保留高似然的粒子 +5. **地图更新**: 基于最优粒子的位姿估计更新全局地图 + +### GMapping 工作流程 + +#### 初始化阶段 +- 创建 N 个粒子,每个粒子代表机器人的一种可能位姿 +- 每个粒子维护一张独立的地图 +- 所有粒子初始权重相等 + +#### 循环迭代过程 +当收到新的传感器数据时,算法执行以下步骤: + +##### 预测步骤 +- 使用里程计读数更新每个粒子的位置 +- 考虑运动噪声,使粒子分布更加分散 + +##### 更新步骤 +- 对于每个粒子,比较激光雷达扫描数据与该粒子地图的预期观测 +- 计算粒子权重,权重越高表示该粒子越接近真实情况 + +##### 重采样步骤 +- 根据粒子权重进行重采样 +- 权重高的粒子被多次复制,权重低的粒子被淘汰 +- 保证粒子集始终代表最可能的机器人位姿分布 + +##### 地图更新 +- 基于最高权重粒子的信息更新全局地图 +- 将新的观测数据融合到现有地图中 + +### 数据流 + +#### 输入传感器数据 +- 激光雷达扫描数据 (`/scan`): 提供周围环境的距离测量 +- 里程计数据 (`/odom`): 提供相对运动信息 + +#### 输出结果 +- 2D 占据栅格地图 (`/map`): 表示环境的占用情况 +- 机器人位姿估计 (`/map` 坐标系下的变换): 提供机器人在地图中的位置 + +### 参数配置 + +GMapping 算法的关键参数已在 launch 文件中预设,可根据具体场景调整: + +| 参数名 | 默认值 | 描述 | +|--------|--------|------| +| `maxUrange` | 10.0 | 激光雷达最大使用距离 (单位: 米) | +| `sigma` | 0.05 | 扫描匹配的标准差 | +| `particles` | 30 | 粒子数量 | +| `delta` | 0.05 | 地图分辨率 (单位: 米/cell) | +| `temporalUpdate` | -1.0 | 地图更新频率 (-1 表示自适应更新) | +| `linearUpdate` | 1.0 | 线性移动多少米后更新地图 | +| `angularUpdate` | 0.5 | 角度旋转多少弧度后更新地图 | +| `resampleInterval` | 1 | 重采样间隔(以scan为单位) | +| `xmin`, `ymin`, `xmax`, `ymax` | -100.0 ~ 100.0 | 地图边界 | + +### 算法优势与局限性 + +#### 优势 +- 实现相对简单,易于理解和部署 +- 在中小规模环境中表现良好 +- 对传感器噪声具有较好的鲁棒性 +- 能够处理闭环检测问题 + +#### 局限性 +- 计算复杂度随粒子数量增加而线性增长 +- 在大规模环境中效率较低 +- 对快速运动或剧烈震动敏感 +- 难以处理动态环境中的移动物体 ## 核心接口说明 @@ -131,9 +225,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 +254,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 +270,8 @@ carla_global_planner/ │ └── PlanGlobalPath.srv # 路径规划服务定义 ├── utilities/ │ └── planner.py # 路径计算工具(compute_route_waypoints 函数) +├── launch/ +│ └── carla_slam_launch.py # 启动文件,包含建图功能 ├── CMakeLists.txt # 编译配置 └── package.xml # 依赖和包信息配置 ``` @@ -186,10 +290,11 @@ carla_global_planner/ - 关键逻辑变更需同步更新 README.md 2. **功能扩展建议** - - 支持自定义目标点(而非仅随机目标点) - - 增加路径规划算法选择(如 A*、RRT*) - - 添加路径平滑处理,优化车辆行驶轨迹 - - 增加单元测试(基于 `pytest` 和 `ros2 test`) + 支持自定义目标点(而非仅随机目标点) + 增加路径规划算法选择(如 A*、RRT*) + 添加路径平滑处理,优化车辆行驶轨迹 + 增加单元测试(基于 pytest 和 ros2 test) + 集成其他 SLAM 算法(如 Hector SLAM、Karto SLAM)以提供更多建图选项 3. **提交流程** 准备提交代码前,请确保: @@ -202,4 +307,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_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/config_manager.py b/src/enhance_pedestrian_safety/config_manager.py index 38622ee18a..e2c5410e34 100644 --- a/src/enhance_pedestrian_safety/config_manager.py +++ b/src/enhance_pedestrian_safety/config_manager.py @@ -4,12 +4,14 @@ class ConfigManager: + """配置管理器""" @staticmethod def load_config(config_file=None): + """加载配置文件""" config = { 'scenario': { - 'name': 'multi_sensor_scene', + 'name': 'intersection', 'town': 'Town10HD', 'weather': 'clear', 'time_of_day': 'noon', @@ -19,55 +21,76 @@ def load_config(config_file=None): 'traffic': { 'ego_vehicles': 1, 'background_vehicles': 8, - 'pedestrians': 6, - 'traffic_lights': True + 'pedestrians': 10, + 'traffic_lights': True, + 'pedestrian_behavior': { + 'crossing_probability': 0.3, + 'walking_speed': 1.2 + } }, 'sensors': { 'vehicle_cameras': 4, 'infrastructure_cameras': 4, - 'lidar_sensors': 1, - 'radar_sensors': 0, 'image_size': [1280, 720], 'capture_interval': 2.0, - 'lidar_config': { - 'channels': 32, - 'range': 100, - 'points_per_second': 56000, - 'rotation_frequency': 10 - } + 'fov': 90 }, 'output': { 'data_dir': 'cvips_dataset', 'save_raw': True, 'save_stitched': True, + 'save_metadata': True, 'save_annotations': False, - 'save_lidar': True, - 'save_fusion': True, - 'validate_data': True, - 'run_analysis': False + 'compression': False } } if config_file and os.path.exists(config_file): try: with open(config_file, 'r') as f: - user_config = json.load(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: - pass + 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: @@ -78,31 +101,16 @@ def merge_args(config, args): config['scenario']['time_of_day'] = args.time_of_day if args.duration: config['scenario']['duration'] = args.duration - if args.seed: + 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 - if args.enable_lidar: - config['sensors']['lidar_sensors'] = 1 - config['output']['save_lidar'] = True - - if args.enable_fusion: - config['output']['save_fusion'] = True - - if args.enable_annotations: - config['output']['save_annotations'] = True - - if args.skip_validation: - config['output']['validate_data'] = False - - if args.run_analysis: - config['output']['run_analysis'] = True - 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 index f015231a42..faef32f1ce 100644 --- a/src/enhance_pedestrian_safety/data_validator.py +++ b/src/enhance_pedestrian_safety/data_validator.py @@ -3,9 +3,11 @@ class DataValidator: + """数据验证器""" @staticmethod def validate_dataset(data_dir): + """验证整个数据集""" print(f"验证数据集: {data_dir}") validation_results = { @@ -13,10 +15,10 @@ def validate_dataset(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), - 'lidar_data': DataValidator._validate_lidar_data(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) @@ -26,10 +28,12 @@ def validate_dataset(data_dir): @staticmethod def _check_directory_structure(data_dir): + """检查目录结构""" required_dirs = [ "raw/vehicle", "raw/infrastructure", "stitched", + "annotations", "metadata" ] @@ -51,6 +55,7 @@ def _check_directory_structure(data_dir): @staticmethod def _validate_raw_images(data_dir): + """验证原始图像""" raw_dirs = ["vehicle", "infrastructure"] results = {} @@ -60,30 +65,40 @@ def _validate_raw_images(data_dir): results[raw_dir] = {'status': 'MISSING', 'count': 0, 'errors': []} continue - camera_dirs = [] - if os.path.exists(path): - camera_dirs = [d for d in os.listdir(path) if os.path.isdir(os.path.join(path, d))] - + 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: - if os.path.getsize(img_path) == 0: - errors.append(f"空文件: {img_file}") - except: - errors.append(f"文件访问失败: {img_file}") + # 尝试导入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 and total_images > 0: + if len(errors) == 0: status = 'PASS' - elif len(errors) < 3 and total_images > 0: + elif len(errors) < 3: status = 'WARNING' else: status = 'FAIL' @@ -98,6 +113,7 @@ def _validate_raw_images(data_dir): @staticmethod def _validate_stitched_images(data_dir): + """验证拼接图像""" stitched_dir = os.path.join(data_dir, "stitched") if not os.path.exists(stitched_dir): @@ -109,14 +125,24 @@ def _validate_stitched_images(data_dir): for img_file in images: img_path = os.path.join(stitched_dir, img_file) try: - if os.path.getsize(img_path) == 0: - errors.append(f"空文件: {img_file}") - except: - errors.append(f"文件访问失败: {img_file}") + # 尝试导入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 and len(images) > 0: + if len(errors) == 0: status = 'PASS' - elif len(errors) < 3 and len(images) > 0: + elif len(errors) < 3: status = 'WARNING' else: status = 'FAIL' @@ -129,6 +155,7 @@ def _validate_stitched_images(data_dir): @staticmethod def _validate_annotations(data_dir): + """验证标注文件""" annotations_dir = os.path.join(data_dir, "annotations") if not os.path.exists(annotations_dir): @@ -143,13 +170,27 @@ def _validate_annotations(data_dir): 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 and valid_files > 0: + if len(errors) == 0: status = 'PASS' - elif len(errors) < 3 and valid_files > 0: + elif len(errors) < 3: status = 'WARNING' else: status = 'FAIL' @@ -162,6 +203,7 @@ def _validate_annotations(data_dir): @staticmethod def _validate_metadata(data_dir): + """验证元数据""" metadata_dir = os.path.join(data_dir, "metadata") if not os.path.exists(metadata_dir): @@ -176,47 +218,19 @@ def _validate_metadata(data_dir): try: with open(json_path, 'r') as f: data = json.load(f) - valid_files += 1 - except Exception as e: - errors.append(f"无效的JSON文件: {json_file} - {str(e)}") - if len(errors) == 0 and valid_files > 0: - status = 'PASS' - elif len(errors) < 3 and valid_files > 0: - status = 'WARNING' - else: - status = 'FAIL' + # 验证基本结构 + if 'scenario' not in data or 'collection' not in data: + errors.append(f"元数据结构不完整: {json_file}") - return { - 'status': status, - 'count': valid_files, - 'errors': errors - } - - @staticmethod - def _validate_lidar_data(data_dir): - lidar_dir = os.path.join(data_dir, "lidar") - - if not os.path.exists(lidar_dir): - return {'status': 'MISSING', 'count': 0, 'errors': []} - - bin_files = [f for f in os.listdir(lidar_dir) if f.endswith('.bin')] - errors = [] - valid_files = 0 + valid_files += 1 - for bin_file in bin_files: - bin_path = os.path.join(lidar_dir, bin_file) - try: - if os.path.getsize(bin_path) > 0: - valid_files += 1 - else: - errors.append(f"空文件: {bin_file}") - except: - errors.append(f"文件访问失败: {bin_file}") + except Exception as e: + errors.append(f"无效的元数据文件: {json_file} - {str(e)}") - if len(errors) == 0 and valid_files > 0: + if len(errors) == 0: status = 'PASS' - elif len(errors) < 3 and valid_files > 0: + elif len(errors) < 3: status = 'WARNING' else: status = 'FAIL' @@ -229,23 +243,25 @@ def _validate_lidar_data(data_dir): @staticmethod def _calculate_score(results): + """计算总体评分""" weights = { - 'directory_structure': 0.15, - 'raw_images': 0.25, - 'stitched_images': 0.15, - 'annotations': 0.1, - 'metadata': 0.15, - 'lidar_data': 0.2 + '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 @@ -256,19 +272,22 @@ def _calculate_score(results): 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, "metadata", "validation_report.json") + """保存验证报告""" + 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) @@ -289,7 +308,7 @@ def _print_validation_report(results): if 'errors' in result and result['errors']: print(f" 错误 ({len(result['errors'])}):") - for error in result['errors'][:3]: + for error in result['errors'][:3]: # 只显示前3个错误 print(f" - {error}") if len(result['errors']) > 3: print(f" ... 还有 {len(result['errors']) - 3} 个错误") @@ -304,6 +323,6 @@ def _print_validation_report(results): elif overall_score >= 50: print("⚠ 数据集质量一般") else: - print("✗ 数据集质量需要改进") + print("✗ 数据集质量较差") print("=" * 60) \ No newline at end of file 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/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/quadcopter_control/README.md b/src/quadcopter_control/README.md new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/quadcopter_control/README.md.txt b/src/quadcopter_control/README.md.txt new file mode 100644 index 0000000000..e69de29bb2 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