diff --git a/README.md b/README.md index d362f30b07..bf523710f5 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,12 @@ | 规划 | 车辆全局路径规划 | [carla_slam_gmapping](https://github.com/OpenHUTB/nn/tree/main/src/carla_slam_gmapping) | - | +## 功能模块 + +| 功能类别 | 模块名 | 链接 | 其他(参考链接等) | +| ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------------------------- |-------------------------| +| 规划 | 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) 。 @@ -50,5 +56,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/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/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/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/HCL_interaction_task/README.md b/src/HCL_interaction_task/README.md new file mode 100644 index 0000000000..806c611cfb --- /dev/null +++ b/src/HCL_interaction_task/README.md @@ -0,0 +1,228 @@ +# 生物模型交互与ROS 2集成项目 + +## 项目概述 + +本项目融合了基于MuJoCo的人体上肢物理仿真与ROS 2机器人操作系统,实现了兼具高精度手势交互与分布式通信能力的综合仿真系统。项目核心包含两大功能模块: +1. **食指指向追踪**(`mobl_arms_index_pointing`):通过MediaPipe手部追踪驱动食指指向指定目标(如小球)的仿真功能 +- **功能演示**: +![ezgif-72b271013158aed5](https://github.com/user-attachments/assets/937c5994-4cfe-42ae-8f77-2eb0ebb1b41a) + +2. **面板选择反应**(`mobl_arms_index_choice_reaction`):新增的多目标面板交互模块,支持食指对虚拟面板进行选择与反应测试 +- **功能演示**: +![ezgif-283f8d513dd75e78](https://github.com/user-attachments/assets/6edd413e-8566-4b36-a8c0-fd6650ceee10) + + +## 主要特点 + +### 通用特点 +- **高精度骨骼模型**:包含上肢完整骨骼结构,细化手部掌骨、指骨(近节/中节/远节)的几何与物理参数 +- **增强型肌肉驱动**:基于FDPI、EDM等食指相关肌肉群的肌腱路径定义,提升动作真实性 +- **实时手势映射**:集成MediaPipe手部追踪,将真实手势实时映射到虚拟模型 +- **ROS 2分布式通信**:通过ROS 2节点实现关节数据发布、感知数据处理与外部系统交互 +- **模块化设计**:保持与[User-in-the-Box](https://github.com/User-in-the-Box/user-in-the-box)原项目的兼容性 + +### 食指指向模块(`mobl_arms_index_pointing`) +- **目标追踪功能**:支持配置单一目标坐标(如小球),实现食指自动指向指定位置 +- **动态轨迹规划**:根据目标位置实时计算手指运动轨迹,确保平滑自然的指向动作 + +### 面板选择反应模块(`mobl_arms_index_choice_reaction`) +- **多目标交互面板**:虚拟面板上可配置多个选择目标,支持自定义布局与样式 +- **反应时间记录**:精确记录从面板目标出现到食指选择的反应时长,用于交互性能分析 +- **选择有效性判定**:自动检测食指与目标区域的接触状态,判断选择有效性 +- **动态目标生成**:支持随机/序列生成面板目标,模拟复杂选择任务 + + +## 目录结构 + +``` +HCL_interaction_task/ +├── mobl_arms_index_pointing/ # 食指指向追踪模块 +│ ├── config.yaml # 指向仿真参数配置 +│ ├── simulation.xml # 指向功能模型定义 +│ ├── evaluator.py # 指向模块程序入口 +│ ├── simulator.py # 指向仿真核心逻辑 +│ ├── assets/ # 指向模块模型资源(网格/纹理) +│ └── mujoco_ros_demo/ # 指向模块ROS 2功能包 +├── mobl_arms_index_choice_reaction/ # 面板选择反应模块 +│ ├── config.yaml # 面板交互参数配置 +│ ├── simulation.xml # 面板功能模型定义 +│ ├── main.py # 面板模块程序入口 +│ ├── simulator.py # 面板仿真核心逻辑 +│ └── assets/ # 面板模块模型资源(含面板纹理) +├── README.md # 项目说明文档 +└── requirements.txt # 项目依赖清单 +``` + + +## 模型结构 + +### 食指指向模型(`mobl_arms_index_pointing/simulation.xml`) +- 核心组件:上肢骨骼结构、肌肉肌腱系统、目标标记点(小球) +- 关键定义:手指关节活动范围、肌肉附着点、目标位置约束 + +### 面板选择模型(`mobl_arms_index_choice_reaction/simulation.xml`) +- 核心组件:继承基础上肢模型,新增交互面板几何体、多目标区域标记 +- 关键定义:面板碰撞属性、目标区域触发条件、手指接触检测参数 + + +## 系统要求 + +- Ubuntu 22.04 LTS (ROS 2 Humble) +- Python 3.8+(推荐3.10.18) +- MuJoCo 2.3.0+(测试版本3.3.7) +- 依赖库:OpenCV、MediaPipe、NumPy、PyYAML +- 环境管理:Conda 环境管理器 +- 额外依赖(C++):libglfw3-dev、libyaml-cpp-dev、libeigen3-dev + + +## 安装步骤 + +### 1. 克隆仓库(如适用) + +```bash +git clone https://github.com/lbxlb/nn.git +cd HCL_interaction_task +``` + +### 2. 环境配置 + +```bash +# 创建并激活conda环境 +conda create -n mjoco_ros python=3.10 +conda activate mjoco_ros + +# 安装依赖 +pip install -r requirements.txt +# 或手动安装 +pip install mujoco mediapipe numpy opencv-python pyyaml + +# 安装ROS 2相关依赖(如使用ROS功能) +sudo apt install ros-humble-ros-base +``` + +### 3. 模型资源准备 + +1. 下载完整模型文件集(含精细STL模型和纹理): + [完整模型文件集网盘链接](链接: https://pan.baidu.com/s/1sA0BgEPRgxXTqe6ZdEm7Sg?pwd=rq8e 提取码: rq8e) + +2. 解压资源文件到对应模块的assets目录: + - 指向模块:`mobl_arms_index_pointing/assets/` + - 面板模块:`mobl_arms_index_choice_reaction/assets/` + + +## 配置说明 + +### 食指指向模块配置(`mobl_arms_index_pointing/config.yaml`) + +```yaml +dt: 0.05 # 仿真步长 +render_mode: "human" # 渲染模式("human"/"offscreen") +resolution: [1280, 960] # 窗口分辨率 +target_pos: [0.4, 0, 0.7] # 目标小球坐标 +``` + +### 面板选择模块配置(`mobl_arms_index_choice_reaction/config.yaml`) + +```yaml +dt: 0.05 # 仿真步长 +render_mode: "human" # 渲染模式 +resolution: [1280, 960] # 窗口分辨率 +panel_pos: [0.5, 0, 0.6] # 面板位置 +target_count: 3 # 面板目标数量 +target_size: 0.05 # 目标区域大小 +reaction_timeout: 5.0 # 反应超时时间(秒) +``` + + +### ROS 2功能包结构 + +``` +mujoco_ros_demo/ +├── config/ +│ ├── assets/ # 3D模型文件(STL格式) +│ ├── humanoid.xml # 机器人模型配置文件 +│ └── config.yaml # 仿真参数配置文件(C++版本) +├── launch/ +│ └── main.launch.py # ROS2启动文件(支持Python/C++节点) +├── mujoco_ros_demo/ # Python节点目录 +│ ├── __init__.py +│ ├── mujoco_publisher.py # Python版:发布关节角度数据的节点 +│ └── data_subscriber.py # Python版:订阅并处理数据的节点 +├── mujoco_demo_cpp/ # C++节点目录(新增) +│ ├── simulator.cpp/.hpp # C++版:MuJoCo仿真核心逻辑 +│ ├── mujoco_publisher.cpp # C++版:发布关节角度数据的节点 +│ ├── data_subscriber.cpp # C++版:订阅并处理数据的节点 +│ ├── data_acquire.cpp # C++版:感知数据采集节点 +│ └── perception_node.cpp # C++版:感知数据处理节点 +├── CMakeLists.txt # C++编译配置文件(新增) +├── package.xml # ROS2包配置文件(更新依赖) +├── setup.py # Python包安装配置 +``` + +## 节点说明 + +1. **MujocoPublisher** (mujoco_publisher.py/mujoco_publisher.cpp) + - 功能:加载MuJoCo模型并发布关节角度数据 + - 发布主题:/joint_angles (std_msgs/Float64MultiArray) + - 参数:model_path - 机器人模型文件路径 + +2. **DataSubscriber** (data_subscriber.py/data_subscriber.cpp) + - 功能:订阅关节角度数据并计算平均值 + - 订阅主题:/joint_angles (std_msgs/Float64MultiArray) + - 输出:终端打印关节角度及平均值 + +3. **DataAcquire** (data_acquire.py) + - 功能:采集外部传感器或设备数据(如手势追踪原始数据) + - 发布主题:/raw_sensor_data (自定义消息类型) + - 支持:MediaPipe原始数据、外部传感器输入等 +![ezgif-59ef5d64b961194d](https://github.com/user-attachments/assets/8baece71-eefd-40f8-8197-a92a7c6f9d02) +4. **PerceptionNode** (perception_node.py) + - 功能:处理感知数据,实现手势识别与解析 + - 订阅主题:/raw_sensor_data + - 发布主题:/processed_gesture (包含解析后的手势指令) +![ezgif-40d1cc7854d18dd9](https://github.com/user-attachments/assets/3510e8f7-bf61-466b-9bae-2a55a86406e0) + +6. **Main** (main.launch.py) + - 功能:启动ROS 2系统并连接各个节点 + - 启动节点:MujocoPublisher、DataSubscriber +![ezgif-7dec2645d82e4788](https://github.com/user-attachments/assets/97475b38-4876-409b-b52d-ce0e535c520b) + + + +## 使用方法 + +### 1. 基础仿真运行 + +```bash +python evaluator.py --config config.yaml --model simulation.xml +``` + +### 2. ROS 2系统运行 + +```bash +# 构建项目 +colcon build +source install/setup.bash + +# 启动ROS 2节点(python) +ros2 launch mujoco_ros_demo main.launch.py + +# 运行数据采集节点(C++) +ros2 run mujoco_ros_demo data_acquire_cpp +``` + +### 3. 查看运行状态 + +- 查看节点信息:`ros2 node list` +- 查看话题信息:`ros2 topic list` +- 可视化工具:`rqt` + +## 项目来源/参考 + +- [MuJoCo](https://github.com/deepmind/mujoco) - 高性能物理引擎 +- [ROS 2](https://github.com/ros2) - 机器人操作系统 +- [User-in-the-Box](https://github.com/User-in-the-Box/user-in-the-box) - 基础模型参考 + +## 许可证 + +本项目基于Apache-2.0许可证发布。 \ No newline at end of file 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/mobl_arms_index_pointing/README.md b/src/HCL_interaction_task/mobl_arms_index_pointing/README.md deleted file mode 100644 index b0d81733fa..0000000000 --- a/src/HCL_interaction_task/mobl_arms_index_pointing/README.md +++ /dev/null @@ -1,195 +0,0 @@ -# 生物模型交互与ROS 2集成项目 - -## 项目概述 - -本项目融合了基于MuJoCo的人体上肢物理仿真与ROS 2机器人操作系统,实现了兼具高精度食指追踪功能与分布式通信能力的综合仿真系统。项目核心包含两大部分:一是通过MediaPipe手部追踪驱动的食指指向仿真,二是基于ROS 2的关节数据发布与感知处理模块,可实现虚拟模型与外部系统的实时数据交互。 - -### 演示视频 - -- **食指指向功能演示**: -![ezgif-72b271013158aed5](https://github.com/user-attachments/assets/937c5994-4cfe-42ae-8f77-2eb0ebb1b41a) -## 主要特点 - -- **高精度骨骼模型**:包含上肢完整骨骼结构,细化手部掌骨、指骨(近节/中节/远节)的几何与物理参数 -- **增强型肌肉驱动**:新增食指相关肌肉群(FDPI、EDM等)的肌腱路径定义,提升指向动作真实性 -- **实时手势映射**:集成MediaPipe手部追踪,将真实手势实时映射到虚拟模型 -- **目标追踪功能**:支持配置目标坐标,实现食指自动指向指定位置 -- **ROS 2分布式通信**:通过ROS 2节点实现关节数据发布、感知数据处理与外部系统交互 -- **模块化设计**:保持与[User-in-the-Box](https://github.com/User-in-the-Box/user-in-the-box)原项目的兼容性,同时扩展ROS 2接口 - -## 文件说明 - -| 文件名 | 描述 | -|--------|------| -| **config.yaml** | 仿真参数配置文件,包含仿真步长(dt)、渲染模式、窗口分辨率及目标位置(target_pos)等 | -| **simulation.xml** | MuJoCo模型定义文件,包含骨骼结构、肌肉肌腱参数、标记点及关节活动范围等核心信息 | -| **evaluator.py** | 程序入口脚本,通过命令行参数接收配置文件和模型文件路径,初始化并启动仿真 | -| **simulator.py** | 仿真器核心逻辑,包含MuJoCo环境初始化、Viewer适配、仿真循环控制及手势映射功能 | -| **assets/** | 模型资源文件夹,存放网格文件(.stl)和纹理文件,定义骨骼与手部的几何形状 | -| **mujoco_ros_demo/** | ROS 2功能包目录 | - - -## 模型结构 - -仿真模型定义在`simulation.xml`中,核心组件包括: -- **骨骼结构**:详细的上肢骨骼(锁骨、尺骨、掌骨及指骨等),通过网格文件定义几何形状 -- **标记点(sites)**:用于定位肌肉附着点和关键位置(如手指关节、肌肉路径点) -- **肌腱与肌肉**:定义主要肌肉的路径(三角肌、肱二头肌等)及物理参数,实现逼真驱动 -- **关节**:定义各关节的活动范围和轴方向(如肘关节弯曲角度限制) - -## 系统要求 - -- Ubuntu 22.04 LTS (ROS 2 Humble) -- Python 3.8+(3.10.18) -- MuJoCo 2.3.0+(3.3.7) -- OpenCV -- MediaPipe -- NumPy -- PyYAML -- Conda 环境管理器 -- 额外依赖(C++):libglfw3-dev、libyaml-cpp-dev、libeigen3-dev - -## 安装步骤 - -### 1. 克隆仓库 - -```bash -# 克隆主项目 -git clone https://github.com/yourusername/mobl-arms-index-pointing.git -cd mobl-arms-index-pointing - -# 克隆依赖项目(如需要) -git clone https://github.com/User-in-the-Box/user-in-the-box.git -``` - -### 2. 安装ROS 2 Humble - -按照官方指南安装:https://docs.ros.org/en/humble/Installation.html - -### 3. 创建并配置环境 - -```bash -# 创建并激活conda环境 -conda create -n mjoco_ros python=3.10 -conda activate mjoco_ros - -# 安装核心依赖 -pip install mujoco mediapipe numpy opencv-python pyyaml - -# 安装ROS 2相关依赖(如需要) -sudo apt install ros-humble-ros-base - -# 安装项目包 -pip install -e . -``` - -### 4. 模型文件准备 - -完整模型文件集(包含40+手部精细模型文件)可通过以下链接下载: -[完整模型文件集网盘链接](https://pan.baidu.com/s/1SN5SWpyfKR7KYDbE8lzvBw?pwd=9e9u) - -下载后解压到 `mujoco_ros_demo/config/assets/` 目录 - -## 配置说明 - -### 仿真参数配置(config.yaml) - -```yaml -dt: 0.05 # 仿真步长(越小精度越高,性能消耗越大) -render_mode: "human" # 渲染模式("human"显示窗口,"offscreen"无窗口运行) -resolution: [1280, 960] # 窗口分辨率 [宽度, 高度] -target_pos: [0.4, 0, 0.7] # 食指追踪目标坐标 -``` - -### ROS 2功能包结构 - -``` -mujoco_ros_demo/ -├── config/ -│ ├── assets/ # 3D模型文件(STL格式) -│ ├── humanoid.xml # 机器人模型配置文件 -│ └── config.yaml # 仿真参数配置文件(C++版本) -├── launch/ -│ └── main.launch.py # ROS2启动文件(支持Python/C++节点) -├── mujoco_ros_demo/ # Python节点目录 -│ ├── __init__.py -│ ├── mujoco_publisher.py # Python版:发布关节角度数据的节点 -│ └── data_subscriber.py # Python版:订阅并处理数据的节点 -├── mujoco_demo_cpp/ # C++节点目录(新增) -│ ├── simulator.cpp/.hpp # C++版:MuJoCo仿真核心逻辑 -│ ├── mujoco_publisher.cpp # C++版:发布关节角度数据的节点 -│ ├── data_subscriber.cpp # C++版:订阅并处理数据的节点 -│ ├── data_acquire.cpp # C++版:感知数据采集节点 -│ └── perception_node.cpp # C++版:感知数据处理节点 -├── CMakeLists.txt # C++编译配置文件(新增) -├── package.xml # ROS2包配置文件(更新依赖) -├── setup.py # Python包安装配置 -``` - -## 节点说明 - -1. **MujocoPublisher** (mujoco_publisher.py/mujoco_publisher.cpp) - - 功能:加载MuJoCo模型并发布关节角度数据 - - 发布主题:/joint_angles (std_msgs/Float64MultiArray) - - 参数:model_path - 机器人模型文件路径 - -2. **DataSubscriber** (data_subscriber.py/data_subscriber.cpp) - - 功能:订阅关节角度数据并计算平均值 - - 订阅主题:/joint_angles (std_msgs/Float64MultiArray) - - 输出:终端打印关节角度及平均值 - -3. **DataAcquire** (data_acquire.py) - - 功能:采集外部传感器或设备数据(如手势追踪原始数据) - - 发布主题:/raw_sensor_data (自定义消息类型) - - 支持:MediaPipe原始数据、外部传感器输入等 -![ezgif-59ef5d64b961194d](https://github.com/user-attachments/assets/8baece71-eefd-40f8-8197-a92a7c6f9d02) -4. **PerceptionNode** (perception_node.py) - - 功能:处理感知数据,实现手势识别与解析 - - 订阅主题:/raw_sensor_data - - 发布主题:/processed_gesture (包含解析后的手势指令) -![ezgif-40d1cc7854d18dd9](https://github.com/user-attachments/assets/3510e8f7-bf61-466b-9bae-2a55a86406e0) - -6. **Main** (main.launch.py) - - 功能:启动ROS 2系统并连接各个节点 - - 启动节点:MujocoPublisher、DataSubscriber -![ezgif-7dec2645d82e4788](https://github.com/user-attachments/assets/97475b38-4876-409b-b52d-ce0e535c520b) - - - -## 使用方法 - -### 1. 基础仿真运行 - -```bash -python evaluator.py --config config.yaml --model simulation.xml -``` - -### 2. ROS 2系统运行 - -```bash -# 构建项目 -colcon build -source install/setup.bash - -# 启动ROS 2节点(python) -ros2 launch mujoco_ros_demo main.launch.py - -# 运行数据采集节点(C++) -ros2 run mujoco_ros_demo data_acquire_cpp -``` - -### 3. 查看运行状态 - -- 查看节点信息:`ros2 node list` -- 查看话题信息:`ros2 topic list` -- 可视化工具:`rqt` - -## 项目来源/参考 - -- [MuJoCo](https://github.com/deepmind/mujoco) - 高性能物理引擎 -- [ROS 2](https://github.com/ros2) - 机器人操作系统 -- [User-in-the-Box](https://github.com/User-in-the-Box/user-in-the-box) - 基础模型参考 - -## 许可证 - -本项目基于Apache-2.0许可证发布。 \ 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/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/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/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/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/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