diff --git a/src/AutonomousDriving_RL/carla_env_multi_obs.py b/src/AutonomousDriving_RL/carla_env_multi_obs.py index a8605e1b08..af73e7528c 100644 --- a/src/AutonomousDriving_RL/carla_env_multi_obs.py +++ b/src/AutonomousDriving_RL/carla_env_multi_obs.py @@ -4,6 +4,7 @@ - 观测: [x, y, vx, vy] - 动作: [throttle, steer, brake] - 新增: 车道保持奖励、合理速度区间、轨迹日志、参数化配置、抗崩溃机制 +- 【本次更新】新增 get_forward_waypoint() 用于高层导航 """ import carla @@ -291,6 +292,47 @@ def step(self, action): "collision": terminated } + # ================================ + # 【新增功能】用于高层导航 + # ================================ + + def get_vehicle_transform(self): + """安全获取车辆当前位姿(Transform)""" + if not self.vehicle or not self.vehicle.is_alive: + return None + try: + return self.vehicle.get_transform() + except: + return None + + def get_forward_waypoint(self, distance=3.0): + """ + 获取车辆前方指定距离的车道中心点(世界坐标) + :param distance: 前瞻距离(米),建议 2.0~5.0 + :return: carla.Location 对象,若失败返回 None + """ + try: + vehicle_tf = self.get_vehicle_transform() + if vehicle_tf is None: + return None + # 沿车头方向前进 + forward = vehicle_tf.get_forward_vector() + target_loc = vehicle_tf.location + carla.Location( + x=forward.x * distance, + y=forward.y * distance, + z=0.0 + ) + # 投影到最近可行驶车道中心 + waypoint = self.world.get_map().get_waypoint( + target_loc, + project_to_road=True, + lane_type=carla.LaneType.Driving + ) + return waypoint.transform.location if waypoint else None + except Exception as e: + print(f"⚠️ get_forward_waypoint 失败: {e}") + return None + def close(self): # 保存轨迹 if self.log_trajectory and self.trajectory_data: diff --git a/src/AutonomousDriving_RL/eval_agent.py b/src/AutonomousDriving_RL/eval_agent.py index a4233e0cbd..e90a6e9545 100644 --- a/src/AutonomousDriving_RL/eval_agent.py +++ b/src/AutonomousDriving_RL/eval_agent.py @@ -1,91 +1,176 @@ # eval_agent.py """ -CARLA 智能体评估脚本(4D 环境专用) -- 不使用 Monitor / DummyVecEnv -- 支持多轮评估、轨迹分离、种子复现 -- 输出平均奖励与标准差 +评估已训练的 PPO 自动驾驶智能体 +支持两种模式: +1. 默认:沿车道中心自动前进(使用 get_forward_waypoint) +2. 指定目标点:导航到 (target_x, target_y) """ import argparse -import os import numpy as np +import carla from stable_baselines3 import PPO from carla_env.carla_env_multi_obs import CarlaEnvMultiObs def main(): - parser = argparse.ArgumentParser(description="Evaluate a trained PPO agent in CARLA (4D mode)") - parser.add_argument("--model-path", type=str, default="models/ppo_carla.zip", - help="Path to the trained model (.zip file)") - parser.add_argument("--episodes", type=int, default=3, - help="Number of evaluation episodes") - parser.add_argument("--steps-per-ep", type=int, default=500, - help="Max steps per episode") - parser.add_argument("--keep-alive", action="store_true", - help="Keep last vehicle alive after evaluation") - parser.add_argument("--log-file", type=str, default="eval_trajectory.csv", - help="Base name for trajectory logs") - parser.add_argument("--seed", type=int, default=42, - help="Base random seed for reproducibility") + parser = argparse.ArgumentParser(description="评估 CARLA PPO 自动驾驶智能体") + parser.add_argument( + "--model_path", + type=str, + default="./checkpoints/best_model.zip", + help="已训练模型路径(.zip 文件)" + ) + parser.add_argument( + "--steps", + type=int, + default=500, + help="最大运行步数" + ) + parser.add_argument( + "--target_x", + type=float, + default=None, + help="全局目标点 x 坐标(世界坐标系,单位:米)" + ) + parser.add_argument( + "--target_y", + type=float, + default=None, + help="全局目标点 y 坐标(世界坐标系,单位:米)" + ) + parser.add_argument( + "--waypoint_dist", + type=float, + default=4.0, + help="局部目标点前瞻距离(米),建议 2.0~5.0" + ) args = parser.parse_args() - if not os.path.exists(args.model_path): - raise FileNotFoundError(f"❌ Model file not found: {args.model_path}") - - print(f"🎯 Loading model from: {args.model_path}") - model = PPO.load(args.model_path) # 注意:不传 env! - - rewards = [] - for ep in range(args.episodes): - print(f"\n▶️ Starting evaluation episode {ep + 1}/{args.episodes} ...") - - # 创建独立环境实例(每轮新环境) - env = CarlaEnvMultiObs( - keep_alive_after_exit=(args.keep_alive and ep == args.episodes - 1), - log_trajectory=True, - trajectory_log_file=f"ep{ep + 1}_{args.log_file}", - max_episode_steps=args.steps_per_ep - ) - - try: - obs, _ = env.reset(seed=args.seed + ep) - total_reward = 0.0 + print("🚀 正在启动评估环境...") + print("💡 请确保 CARLA 仿真器(CarlaUE4.exe)已在后台运行!\n") + + try: + # 创建环境(保留车辆以便观察) + env = CarlaEnvMultiObs(keep_alive_after_exit=True, max_episode_steps=args.steps) + + # 加载模型(仅用于底层控制:油门/刹车) + print(f"📂 加载模型: {args.model_path}") + model = PPO.load(args.model_path) + + # 初始化环境 + print("🔄 重置环境并生成车辆...") + obs, _ = env.reset() + total_reward = 0.0 + + # 设置全局目标点(如果提供) + global_target = None + if args.target_x is not None and args.target_y is not None: + global_target = carla.Location(x=args.target_x, y=args.target_y, z=0.0) + print(f"🎯 全局目标点: ({args.target_x:.1f}, {args.target_y:.1f})") + else: + print("🛣️ 未指定目标点,将沿车道自动前进...") + + print("\n▶️ 开始驾驶演示...\n") + + for step in range(args.steps): + # ===== 高层导航逻辑:计算局部目标点 ===== + local_target = None + vehicle_tf = env.get_vehicle_transform() + + if vehicle_tf is None: + print("⚠️ 车辆状态异常,终止演示") + break + + if global_target is not None: + # --- 模式1:朝向全局目标点 --- + to_target = np.array([ + global_target.x - vehicle_tf.location.x, + global_target.y - vehicle_tf.location.y + ]) + dist_to_target = np.linalg.norm(to_target) + + if dist_to_target < 1.0: + print("🏁 已到达目标点!") + break - for step in range(args.steps_per_ep): + # 计算单位方向向量 + direction = to_target / (dist_to_target + 1e-6) + local_target = carla.Location( + x=vehicle_tf.location.x + direction[0] * args.waypoint_dist, + y=vehicle_tf.location.y + direction[1] * args.waypoint_dist, + z=vehicle_tf.location.z + ) + else: + # --- 模式2:沿车道中心前进 --- + local_target = env.get_forward_waypoint(distance=args.waypoint_dist) + if local_target is None: + print("⚠️ 无法获取前方路点,使用原始策略") + local_target = None + + # ===== 底层控制:结合 PPO 与转向决策 ===== + if local_target is not None: + # 计算期望转向角(基于局部目标) + forward = vehicle_tf.get_forward_vector() + to_waypoint = np.array([ + local_target.x - vehicle_tf.location.x, + local_target.y - vehicle_tf.location.y + ]) + + # 避免除零 + norm_fw = np.linalg.norm([forward.x, forward.y]) + norm_wp = np.linalg.norm(to_waypoint) + if norm_fw < 1e-3 or norm_wp < 1e-3: + steer = 0.0 + else: + # 计算夹角(使用叉积判断左右) + cos_angle = (forward.x * to_waypoint[0] + forward.y * to_waypoint[1]) / (norm_fw * norm_wp) + cos_angle = np.clip(cos_angle, -1.0, 1.0) + angle = np.arccos(cos_angle) # [0, π] + + # 叉积符号决定转向方向 + cross = forward.x * to_waypoint[1] - forward.y * to_waypoint[0] + steer = np.clip(angle * np.sign(cross) * 1.5, -1.0, 1.0) # 比例增益可调 + + # 使用 PPO 决定油门和刹车(输入仍为原始 4D 观测) + throttle_brake_action, _ = model.predict(obs, deterministic=True) + throttle = float(np.clip(throttle_brake_action[0], 0.0, 1.0)) + brake = float(np.clip(throttle_brake_action[2], 0.0, 1.0)) + + action = np.array([throttle, steer, brake]) + else: + # 回退到纯 PPO 策略 action, _ = model.predict(obs, deterministic=True) - obs, reward, terminated, truncated, info = env.step(action) - total_reward += reward - - if terminated or truncated: - print(f" ⏹️ Episode ended early at step {step + 1}") - break - rewards.append(total_reward) - print(f" ✅ Episode {ep + 1} finished | Total Reward: {total_reward:.2f} | Steps: {step + 1}") - - except Exception as e: - print(f" ❌ Episode {ep + 1} failed: {e}") - rewards.append(-1e6) # 记录失败 - finally: - env.close() # 确保清理资源 - - # 统计结果 - valid_rewards = [r for r in rewards if r > -1e5] - if not valid_rewards: - print("💥 All episodes failed!") - return - - mean_reward = np.mean(valid_rewards) - std_reward = np.std(valid_rewards) if len(valid_rewards) > 1 else 0.0 - - print("\n" + "="*50) - print(f"📊 Evaluation Results ({len(valid_rewards)} successful episodes):") - print(f" Mean Reward: {mean_reward:.2f}") - print(f" Std Dev: {std_reward:.2f}") - print(f" Min: {min(valid_rewards):.2f}") - print(f" Max: {max(valid_rewards):.2f}") - print("="*50) - print(f"📁 Trajectory files saved as: ep1_{args.log_file}, ep2_{args.log_file}, ...") + # 执行动作 + obs, reward, terminated, truncated, info = env.step(action) + total_reward += reward + + # 定期打印状态 + if step % 50 == 0 or step == args.steps - 1: + x, y, vx, vy = obs + speed = np.linalg.norm([vx, vy]) + print(f" Step {step:3d}: 位置=({x:6.1f}, {y:6.1f}), 速度={speed:5.2f} m/s") + + # 终止条件 + if terminated or truncated: + reason = "碰撞" if terminated else "超时" + print(f"⏹️ 演示结束(原因: {reason})") + break + + print(f"\n✅ 演示完成!总奖励: {total_reward:.2f}") + print("ℹ️ 车辆已保留在 CARLA 中,可自由观察。") + input("\n🛑 按 Enter 键退出并销毁车辆...") + + except Exception as e: + print(f"\n❌ 运行出错: {e}") + import traceback + traceback.print_exc() + finally: + try: + env.close() + except: + pass if __name__ == "__main__":