Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
227 changes: 227 additions & 0 deletions src/airsim_control/AirSim_Maze_Navigation/run_inference.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,227 @@
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

# ==============================================================================
# 配置区域
# ==============================================================================

# 项目名称缩写:ASMN (AirSim Maze Navigation)
PROJECT_ABBR = "ASMN"

# 获取当前脚本所在目录作为项目根目录
BASE_DIR = os.path.dirname(os.path.abspath(__file__))

# 使用相对路径
MODELS_DIR = os.path.join(BASE_DIR, f"{PROJECT_ABBR}_models")
LOG_PATH = os.path.join(BASE_DIR, f"{PROJECT_ABBR}_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)

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(f" {PROJECT_ABBR} - AirSim UAV Maze Navigation ")
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"{PROJECT_ABBR}_trajectory_{timestamp}.csv")

# 使用 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(f"\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}")

# 追加数据到 CSV 文件
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(f"{PROJECT_ABBR} Dashboard", dashboard)
if cv2.waitKey(1) & 0xFF == ord('q'):
print("用户手动停止测试。")
break

except KeyboardInterrupt:
print("\n检测到键盘中断,停止测试。")
finally:
print("\n" + "=" * 50)
print(f" {PROJECT_ABBR} 测试总结报告 ")
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()
116 changes: 116 additions & 0 deletions src/airsim_control/AirSim_Maze_Navigation/train.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,116 @@
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

# 路径配置 (已改为相对路径)
# 获取当前脚本文件所在的绝对目录
SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))

# 拼接路径:在脚本同级目录下生成 models 和 logs
MODELS_DIR = os.path.join(SCRIPT_DIR, "models")
LOG_DIR = os.path.join(SCRIPT_DIR, "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(f"模型保存路径: {MODELS_DIR}")
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()
Loading
Loading