Repository navigation
完成ros封装 #4338
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Closed
Closed
完成ros封装 #4338
Changes from all commits
Commits
Show all changes
4 commits
Select commit
Hold shift + click to select a range
File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,221 @@ | ||
| import glob | ||
| import os | ||
| import time | ||
| import numpy as np | ||
| import cv2 | ||
| import csv | ||
| from datetime import datetime | ||
| from stable_baselines3 import PPO | ||
| from custom_env import AirSimMazeEnv | ||
| import math | ||
|
|
||
| # ============================================================================== | ||
| # 配置区域 | ||
| # ============================================================================== | ||
| MODELS_DIR = r"D:\Others\MyAirsimprojects\models" | ||
| LOG_PATH = r"D:\Others\MyAirsimprojects\inference_logs" # 轨迹保存路径 | ||
| os.makedirs(LOG_PATH, exist_ok=True) | ||
|
|
||
| # 可视化配置 | ||
| SHOW_DASHBOARD = True # 是否显示 OpenCV 仪表盘 | ||
| DASHBOARD_SIZE = (800, 400) # 宽, 高 | ||
|
|
||
|
|
||
| # ============================================================================== | ||
| # 辅助函数 | ||
| # ============================================================================== | ||
|
|
||
| def get_all_models(path_dir): | ||
| """获取所有模型并按时间排序""" | ||
| files = glob.glob(os.path.join(path_dir, '*.zip')) | ||
| # 按修改时间倒序排列 (最新的在前) | ||
| files.sort(key=os.path.getctime, reverse=True) | ||
| return files | ||
|
|
||
|
|
||
| def draw_dashboard(obs, action, reward, step_count, last_info): | ||
| """绘制实时仪表盘""" | ||
| # 1. 创建黑色背景画布 | ||
| canvas = np.zeros((DASHBOARD_SIZE[1], DASHBOARD_SIZE[0], 3), dtype=np.uint8) | ||
|
|
||
| # --- 左侧: 深度摄像头画面 --- | ||
| if 'image' in obs: | ||
| depth_img = obs['image'] | ||
| depth_img = cv2.cvtColor(depth_img, cv2.COLOR_GRAY2BGR) | ||
| depth_img = cv2.resize(depth_img, (350, 350), interpolation=cv2.INTER_NEAREST) | ||
| h, w, _ = depth_img.shape | ||
| canvas[25:25 + h, 25:25 + w] = depth_img | ||
| cv2.putText(canvas, "Depth Camera", (25, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) | ||
|
|
||
| # --- 右侧: 数据与 Lidar --- | ||
| x_start = 400 | ||
| y_start = 50 | ||
| line_height = 30 | ||
|
|
||
| infos = [ | ||
| f"Step: {step_count}", | ||
| f"Fwd Vel: {action[0]:.2f} (x5.0 m/s)", | ||
| f"Yaw Rate: {action[1]:.2f} (x60 deg/s)", | ||
| f"Reward: {reward:.3f}", | ||
| f"Status: {last_info}" | ||
| ] | ||
|
|
||
| for i, text in enumerate(infos): | ||
| color = (0, 255, 0) | ||
| if "Reward" in text and reward < 0: | ||
| color = (0, 0, 255) | ||
| if "Status" in text and "撞墙" in text: | ||
| color = (0, 0, 255) | ||
|
|
||
| # 这里需要注意: OpenCV putText 不支持 emoji,如果 last_info 包含 emoji 可能会显示乱码或问号 | ||
| # 但不会报错崩溃。崩溃是在写入 CSV 时发生的。 | ||
| cv2.putText(canvas, text, (x_start, y_start + i * line_height), | ||
| cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2) | ||
|
|
||
| # --- 右下角: Lidar 雷达图 --- | ||
| lidar_center = (600, 300) | ||
| lidar_radius = 80 | ||
| cv2.circle(canvas, lidar_center, 2, (0, 255, 255), -1) | ||
| cv2.circle(canvas, lidar_center, lidar_radius, (50, 50, 50), 1) | ||
|
|
||
| if 'lidar' in obs: | ||
| lidar_data = obs['lidar'] | ||
| for i in range(0, 180, 2): | ||
| dist = lidar_data[i] | ||
| if dist < 20: | ||
| angle_deg = -90 + i | ||
| angle_rad = math.radians(angle_deg - 90) | ||
| r_pixel = (dist / 20.0) * lidar_radius | ||
| pt_x = int(lidar_center[0] + r_pixel * math.cos(angle_rad)) | ||
| pt_y = int(lidar_center[1] + r_pixel * math.sin(angle_rad)) | ||
| pt_color = (0, 255, 0) if dist > 5 else (0, 0, 255) | ||
| cv2.circle(canvas, (pt_x, pt_y), 2, pt_color, -1) | ||
|
|
||
| return canvas | ||
|
|
||
|
|
||
| # ============================================================================== | ||
| # 主逻辑 | ||
| # ============================================================================== | ||
|
|
||
| def main(): | ||
| print("==================================================") | ||
| print(" AirSim 无人机智能导航 - 推理与评估系统 ") | ||
| print("==================================================") | ||
|
|
||
| # 1. 模型选择 | ||
| models = get_all_models(MODELS_DIR) | ||
| if not models: | ||
| print("错误: 未找到任何模型文件!请先运行 train.py。") | ||
| return | ||
|
|
||
| print(f"发现 {len(models)} 个模型存档。") | ||
| print(f"默认加载最新的: {os.path.basename(models[0])}") | ||
| model_path = models[0] | ||
|
|
||
| # 2. 加载环境与模型 | ||
| print("正在初始化环境...") | ||
| env = AirSimMazeEnv() | ||
|
|
||
| print(f"正在加载神经网络: {model_path} ...") | ||
| model = PPO.load(model_path) | ||
|
|
||
| # 3. 初始化统计数据 | ||
| stats = { | ||
| "episodes": 0, | ||
| "success": 0, | ||
| "collision": 0, | ||
| "out_of_bounds": 0, | ||
| "timeout": 0 | ||
| } | ||
|
|
||
| # 4. 创建轨迹日志文件 | ||
| timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") | ||
| csv_filename = os.path.join(LOG_PATH, f"trajectory_{timestamp}.csv") | ||
|
|
||
| # 【修复点 1】增加 encoding='utf-8-sig' | ||
| with open(csv_filename, 'w', newline='', encoding='utf-8-sig') as f: | ||
| writer = csv.writer(f) | ||
| writer.writerow(["Episode", "Step", "X", "Y", "Z", "Reward", "DoneType"]) | ||
|
|
||
| print("\n>>> 开始推理 (按 'q' 退出 OpenCV 窗口或 Ctrl+C 停止) <<<\n") | ||
|
|
||
| try: | ||
| obs, _ = env.reset() | ||
| episode_reward = 0 | ||
| step_count = 0 | ||
| current_traj = [] | ||
| done_reason = "Running" | ||
|
|
||
| while True: | ||
| action, _ = model.predict(obs, deterministic=True) | ||
| obs, reward, done, truncated, info = env.step(action) | ||
| episode_reward += reward | ||
| step_count += 1 | ||
|
|
||
| pos = env.client.getMultirotorState().kinematics_estimated.position | ||
| current_traj.append([stats['episodes'], step_count, pos.x_val, pos.y_val, pos.z_val, reward]) | ||
|
|
||
| if done: | ||
| stats['episodes'] += 1 | ||
| if reward >= 50: | ||
| stats['success'] += 1 | ||
| done_reason = "✅ SUCCESS" | ||
| elif reward <= -50: | ||
| stats['collision'] += 1 | ||
| done_reason = "❌ COLLISION" | ||
| elif reward == -20: | ||
| stats['out_of_bounds'] += 1 | ||
| done_reason = "⚠️ OUT OF BOUNDS" | ||
| else: | ||
| stats['timeout'] += 1 | ||
| done_reason = "⏳ TIMEOUT/OTHER" | ||
|
|
||
| print( | ||
| f"Episode {stats['episodes']} 结束 | 原因: {done_reason} | 总分: {episode_reward:.2f} | 步数: {step_count}") | ||
|
|
||
| # 【修复点 2】增加 encoding='utf-8-sig' | ||
| with open(csv_filename, 'a', newline='', encoding='utf-8-sig') as f: | ||
| writer = csv.writer(f) | ||
| for row in current_traj: | ||
| writer.writerow(row + [done_reason]) | ||
|
|
||
| obs, _ = env.reset() | ||
| episode_reward = 0 | ||
| step_count = 0 | ||
| current_traj = [] | ||
| if SHOW_DASHBOARD: | ||
| time.sleep(0.5) | ||
| else: | ||
| done_reason = "Flying..." | ||
|
|
||
| if SHOW_DASHBOARD: | ||
| dashboard = draw_dashboard(obs, action, reward, step_count, done_reason) | ||
| cv2.imshow("AirSim AI Dashboard", dashboard) | ||
| if cv2.waitKey(1) & 0xFF == ord('q'): | ||
| print("用户手动停止测试。") | ||
| break | ||
|
|
||
| except KeyboardInterrupt: | ||
| print("\n检测到键盘中断,停止测试。") | ||
| finally: | ||
| print("\n" + "=" * 50) | ||
| print(" 测试总结报告 ") | ||
| print("=" * 50) | ||
| total = stats['episodes'] | ||
| if total > 0: | ||
| print(f"总回合数: {total}") | ||
| print(f"成功次数: {stats['success']} ({stats['success'] / total * 100:.1f}%)") | ||
| print(f"撞墙次数: {stats['collision']} ({stats['collision'] / total * 100:.1f}%)") | ||
| print(f"越界次数: {stats['out_of_bounds']} ({stats['out_of_bounds'] / total * 100:.1f}%)") | ||
| print(f"轨迹数据已保存至: {csv_filename}") | ||
| else: | ||
| print("未完成任何完整回合。") | ||
| print("=" * 50) | ||
|
|
||
| env.close() | ||
| cv2.destroyAllWindows() | ||
|
|
||
|
|
||
| if __name__ == "__main__": | ||
| main() | ||
File renamed without changes.
File renamed without changes.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,111 @@ | ||
| import glob | ||
| import os | ||
| import torch as th | ||
| from stable_baselines3 import PPO | ||
| from stable_baselines3.common.callbacks import CheckpointCallback | ||
| from stable_baselines3.common.utils import set_random_seed | ||
| from stable_baselines3.common.vec_env import DummyVecEnv, VecMonitor | ||
| from custom_env import AirSimMazeEnv | ||
| from typing import Callable | ||
|
|
||
| # === 路径配置 === | ||
| MODELS_DIR = r"D:\Others\MyAirsimprojects\models" | ||
| LOG_DIR = r"D:\Others\MyAirsimprojects\airsim_logs" | ||
|
|
||
| os.makedirs(MODELS_DIR, exist_ok=True) | ||
| os.makedirs(LOG_DIR, exist_ok=True) | ||
|
|
||
|
|
||
| # === 优化 1: 线性学习率调度器 === | ||
| def linear_schedule(initial_value: float) -> Callable[[float], float]: | ||
| """ | ||
| 线性学习率调度函数。 | ||
| :param initial_value: 初始学习率 | ||
| :return: 当前步数的学习率 | ||
| """ | ||
|
|
||
| def func(progress_remaining: float) -> float: | ||
| # progress_remaining 会从 1.0 (开始) 线性减少到 0.0 (结束) | ||
| return progress_remaining * initial_value | ||
|
|
||
| return func | ||
|
|
||
|
|
||
| def get_latest_model_path(path_dir): | ||
| list_of_files = glob.glob(os.path.join(path_dir, '*.zip')) | ||
| if not list_of_files: | ||
| return None | ||
| return max(list_of_files, key=os.path.getctime) | ||
|
|
||
|
|
||
| def main(): | ||
| set_random_seed(42) # 固定随机种子,方便复现 | ||
|
|
||
| # 实例化环境 | ||
| # 使用 DummyVecEnv 包装,虽然只有一个环境,但这是标准做法,方便未来扩展 | ||
| env = DummyVecEnv([lambda: AirSimMazeEnv()]) | ||
| env = VecMonitor(env) # 添加 Monitor 以记录详细日志 | ||
|
|
||
| latest_model_path = get_latest_model_path(MODELS_DIR) | ||
|
|
||
| # === 优化 2: 自定义网络架构 === | ||
| # net_arch=[256, 256]: 使用两个 256 神经元的隐藏层 (比默认的 64 强大很多) | ||
| # activation_fn: 使用 Tanh 或 ReLU | ||
| policy_kwargs = dict( | ||
| activation_fn=th.nn.Tanh, | ||
| net_arch=dict(pi=[256, 256], vf=[256, 256]) | ||
| ) | ||
|
|
||
| if latest_model_path: | ||
| print(f"--- 发现存档: {latest_model_path},继续训练 ---") | ||
| # 加载旧模型 | ||
| # 注意:加载旧模型时,learning_rate 会被覆盖为旧模型的设置 | ||
| # 如果你想在旧模型上强制使用新参数,需要手动修改 model.learning_rate | ||
| model = PPO.load(latest_model_path, env=env, tensorboard_log=LOG_DIR) | ||
|
|
||
| # 强制更新学习率策略 (可选,如果你想让旧模型也享受线性衰减) | ||
| # model.lr_schedule = linear_schedule(0.0003) | ||
|
|
||
| reset_timesteps = False | ||
| else: | ||
| print(f"--- 未发现存档,开始【从头训练】(优化版) ---") | ||
| model = PPO( | ||
| "MultiInputPolicy", | ||
| env, | ||
| verbose=1, | ||
| tensorboard_log=LOG_DIR, | ||
|
|
||
| # --- 核心超参数优化 --- | ||
| learning_rate=linear_schedule(0.0003), # 动态学习率 | ||
| batch_size=256, # 大批量,更稳 | ||
| n_steps=2048, # 每次更新前的采样步数 | ||
| gamma=0.99, # 折扣因子 (长远利益) | ||
| gae_lambda=0.95, # 优势估计 | ||
| clip_range=0.2, # PPO 修剪范围 | ||
| ent_coef=0.01, # 优化 3: 熵系数,鼓励探索,防止过早死板 | ||
| policy_kwargs=policy_kwargs, # 应用更大的网络 | ||
| device="auto" # 自动使用 GPU | ||
| ) | ||
| reset_timesteps = True | ||
|
|
||
| checkpoint_callback = CheckpointCallback( | ||
| save_freq=20000, # 每 2万步保存一次,不用太频繁 | ||
| save_path=MODELS_DIR, | ||
| name_prefix='drone_maze_opt' # 改个名字区分一下 | ||
| ) | ||
|
|
||
| print("🚀 优化版训练引擎启动...") | ||
| print("配置: Linear LR, Net=[256,256], Ent=0.01") | ||
|
|
||
| model.learn( | ||
| total_timesteps=1000000, # 建议跑 100万步 | ||
| callback=checkpoint_callback, | ||
| reset_num_timesteps=reset_timesteps | ||
| ) | ||
|
|
||
| model.save(os.path.join(MODELS_DIR, "drone_maze_final_opt")) | ||
| print("训练结束。") | ||
|
|
||
|
|
||
| if __name__ == "__main__": | ||
| main() |
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
不能使用绝对路径