From a7690e398c9e8163b34efcd63c91f742f6a0a9eb Mon Sep 17 00:00:00 2001 From: wuuu0236 <2416234104@qq.com> Date: Thu, 18 Dec 2025 20:17:28 +0800 Subject: [PATCH 1/8] Update carla_env_multi_obs.py --- .../carla_env_multi_obs.py | 346 +++++++----------- 1 file changed, 123 insertions(+), 223 deletions(-) diff --git a/src/AutonomousDriving_RL/carla_env_multi_obs.py b/src/AutonomousDriving_RL/carla_env_multi_obs.py index 1d97813b7f..552f6eb4c5 100644 --- a/src/AutonomousDriving_RL/carla_env_multi_obs.py +++ b/src/AutonomousDriving_RL/carla_env_multi_obs.py @@ -1,282 +1,188 @@ # carla_env/carla_env_multi_obs.py -# 本文件定义了一个基于 CARLA 仿真器的自定义 Gymnasium 环境, -# 用于训练强化学习智能体(如 PPO)控制自动驾驶车辆。 -# 观测空间为 4 维连续状态(位置 + 速度),动作空间为 3 维连续控制(油门、转向、刹车)。 +import carla +import numpy as np +import random +import time +import os +import json +from gymnasium import Env, spaces -import carla # CARLA 仿真器 Python API -import numpy as np # 数值计算库 -import random # 随机数生成 -import time # 时间控制(用于延迟) -import os # 操作系统接口(文件操作) -import json # JSON 文件读写(用于保存/加载车辆ID) -from gymnasium import Env, spaces # Gymnasium 标准环境接口 - -# 定义临时文件路径,用于记录上一次运行生成的车辆ID(与脚本同目录) VEHICLE_ID_FILE = ".last_vehicle_id.json" +TRAJECTORY_LOG_FILE = "trajectory.csv" class CarlaEnvMultiObs(Env): - """ - 自定义 CARLA 强化学习环境类,继承自 gymnasium.Env。 - 支持自动清理历史车辆、多地图适配、第三人称视角跟随、安全spawn等特性。 - """ - - def __init__(self, keep_alive_after_exit=True): - """ - 初始化环境。 - :param keep_alive_after_exit: 若为 True,close() 时不销毁车辆,便于人工观察或录屏。 - """ + def __init__(self, keep_alive_after_exit=True, log_trajectory=True): super(CarlaEnvMultiObs, self).__init__() - - # CARLA 客户端与世界对象 self.client = None self.world = None - - # 车辆相关 - self.vehicle = None # 当前控制的车辆 Actor - self._current_vehicle_id = None # 本次生成的车辆 ID(用于下次清理) - - # 训练控制 - self.frame_count = 0 # 已执行的仿真步数 - self.max_frames = 1000 # 最大允许步数(用于 truncated 判定) - self.prev_x = 0.0 # 上一帧的 x 坐标(用于计算位移奖励) - - # 视角控制 - self.spectator = None # CARLA 观察者(摄像头) - - # 行为标志 + self.vehicle = None + self._current_vehicle_id = None + self.frame_count = 0 + self.max_frames = 1000 + self.spectator = None self.keep_alive = keep_alive_after_exit + self.log_trajectory = log_trajectory + self.trajectory_data = [] - # 定义观测空间:[x, y, vx, vy] —— 位置 (m) + 速度 (m/s) self.observation_space = spaces.Box( - low=-np.inf, - high=np.inf, - shape=(4,), - dtype=np.float32 + low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32 ) - - # 定义动作空间:[throttle, steer, brake] - # - throttle: [0.0, 1.0] 油门(0=松开,1=全踩) - # - steer: [-1.0, 1.0] 转向(-1=左打满,1=右打满) - # - brake: [0.0, 1.0] 刹车(0=松开,1=全刹) self.action_space = spaces.Box( low=np.array([0.0, -1.0, 0.0]), high=np.array([1.0, 1.0, 1.0]), dtype=np.float32 ) + def _connect_carla(self, max_retries=3): + """自动重试连接 CARLA""" + for attempt in range(max_retries): + try: + print(f"🔄 尝试连接 CARLA 服务器 (第 {attempt + 1} 次)...") + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(10.0) + self.world = self.client.get_world() + if self.world is not None: + print(f"✅ 成功连接到 CARLA!地图: {self.world.get_map().name}") + return True + except Exception as e: + print(f"⚠️ 连接失败: {e}") + time.sleep(2) + raise RuntimeError("❌ 无法连接到 CARLA 服务器,请确保 CARLA 已启动!") + def reset(self, seed=None, options=None): - """ - 重置环境到初始状态。 - :param seed: 随机种子(用于可复现性) - :param options: 额外选项(本实现未使用) - :return: 初始观测值 (obs), info 字典 - """ super().reset(seed=seed) - - # 设置随机种子(确保行为可复现) if seed is not None: random.seed(seed) np.random.seed(seed) - try: - # 连接 CARLA 服务器(localhost:2000) - if self.client is None: - print("🔄 尝试连接 CARLA 服务器...") - self.client = carla.Client('localhost', 2000) - self.client.set_timeout(20.0) # 超时20秒 - self.world = self.client.get_world() - if self.world is None: - raise RuntimeError("❌ 无法获取 CARLA 世界!") - print(f"✅ 成功连接到 CARLA!地图: {self.world.get_map().name}") - - # 清理上一次运行残留的车辆(通过ID文件) - self._destroy_last_run_vehicle() - - # 生成新车 - self.spawn_vehicle() - - # 同步几帧,确保车辆稳定 - for _ in range(5): - self.world.tick() - time.sleep(0.05) + self._connect_carla() + self._destroy_last_run_vehicle() + self.spawn_vehicle() - # 获取观察者并设置第三人称视角 - self.spectator = self.world.get_spectator() - self._update_spectator_view() - print("🎥 第三人称视角已激活(完整车身 + 前方道路可见)") + for _ in range(5): + self.world.tick() + time.sleep(0.05) - # 重置计数器 - self.frame_count = 0 - obs = self.get_observation() - self.prev_x = obs[0] # 记录初始x位置 - return obs, {} + self.spectator = self.world.get_spectator() + self._update_spectator_view() - except Exception as e: - print(f"❌ 初始化失败: {e}") - raise + self.trajectory_data = [] + self.frame_count = 0 + obs = self.get_observation() + return obs, {} def _destroy_last_run_vehicle(self): - """ - 安全销毁上一次运行留下的车辆。 - 即使 .last_vehicle_id.json 文件损坏、为空或不存在,也能优雅处理,不抛出异常。 - """ - # 若无记录文件,直接跳过 if not os.path.exists(VEHICLE_ID_FILE): - print("ℹ️ 无历史车辆记录,跳过清理") return - - last_id = None try: - # 安全读取 JSON 文件 with open(VEHICLE_ID_FILE, 'r') as f: - content = f.read().strip() - if not content: - print("⚠️ 车辆ID文件为空") - return - data = json.loads(content) + data = json.load(f) last_id = data.get("vehicle_id") - if last_id is None: - print("⚠️ 车辆ID字段缺失") - return - except (json.JSONDecodeError, OSError, ValueError) as e: - # 文件损坏时,尝试删除它 - print(f"⚠️ 读取车辆ID文件失败(文件可能损坏): {e}") - try: - os.remove(VEHICLE_ID_FILE) - except OSError: - pass - return - - # 验证ID类型 - if not isinstance(last_id, int): - print(f"⚠️ 车辆ID类型无效: {type(last_id)}") - return - - # 发送销毁命令 - print(f"🧹 正在销毁上一次运行的车辆 (ID: {last_id})...") - batch = [carla.command.DestroyActor(last_id)] - responses = self.client.apply_batch_sync(batch, do_tick=True) - - if responses[0].error: - print(f" - 销毁失败: {responses[0].error}") - else: - print("✅ 上次车辆已成功清理") - - # 清理后删除ID文件 + if isinstance(last_id, int): + self.client.apply_batch_sync([carla.command.DestroyActor(last_id)], do_tick=True) + except Exception: + pass try: os.remove(VEHICLE_ID_FILE) - except OSError as e: - print(f"⚠️ 删除车辆ID文件失败: {e}") + except OSError: + pass def spawn_vehicle(self): - """ - 在当前地图的安全位置生成一辆特斯拉 Model 3(若不可用则随机选车)。 - 支持 Town01/03/05/10 的预设 spawn 点,其他地图自动选择最低 z 的点。 - """ blueprint_library = self.world.get_blueprint_library() + # 优先使用特斯拉,否则随机选一个 vehicle_bp = blueprint_library.find('vehicle.tesla.model3') - if not vehicle_bp: + if not vehicle_bp or not vehicle_bp.has_attribute('number_of_wheels'): vehicle_bp = random.choice(blueprint_library.filter('vehicle.*')) - # 根据地图名称选择 spawn 位置 + # 设置颜色(可选) + if vehicle_bp.has_attribute('color'): + color = random.choice(vehicle_bp.get_attribute('color').recommended_values) + vehicle_bp.set_attribute('color', color) + map_name = self.world.get_map().name.lower() - if 'town01' in map_name: - spawn_transform = carla.Transform( - carla.Location(x=-60.0, y=20.0, z=0.3), - carla.Rotation(yaw=90.0) - ) - elif 'town03' in map_name: - spawn_transform = carla.Transform( - carla.Location(x=70.0, y=-10.0, z=0.3), - carla.Rotation(yaw=180.0) - ) - elif 'town05' in map_name: - spawn_transform = carla.Transform( - carla.Location(x=-75.0, y=16.0, z=0.3), - carla.Rotation(yaw=90.0) - ) - elif 'town10' in map_name: + spawn_transform = None + + # 针对 Town10HD_Opt 使用已知安全点 + if 'town10' in map_name: spawn_transform = carla.Transform( carla.Location(x=100.0, y=130.0, z=0.3), carla.Rotation(yaw=180.0) ) else: - # 通用 fallback:选择 z 最低的 spawn 点(更平坦安全) + # 通用 fallback:使用第一个 spawn point spawn_points = self.world.get_map().get_spawn_points() - if not spawn_points: - raise RuntimeError("❌ 地图中没有可用的 spawn points!") - spawn_transform = min(spawn_points, key=lambda t: t.location.z) + if spawn_points: + spawn_transform = spawn_points[0] + else: + # 极端 fallback:原点上方 + spawn_transform = carla.Transform(carla.Location(x=0, y=0, z=1.0), carla.Rotation()) - # 尝试生成车辆 + # 尝试主位置 self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform) + + # 如果失败,遍历所有 spawn points if self.vehicle is None: - # 若失败,遍历所有 spawn 点,增加 z 安全余量 - spawn_points = self.world.get_map().get_spawn_points() - for transform in spawn_points: - safe_z = max(transform.location.z, 0.0) + 0.3 - safe_transform = carla.Transform( - carla.Location(x=transform.location.x, y=transform.location.y, z=safe_z), - transform.rotation + print("⚠️ 主 spawn 点失败,尝试遍历所有可用点...") + all_spawn_points = self.world.get_map().get_spawn_points() + random.shuffle(all_spawn_points) # 随机顺序避免总用同一个 + for sp in all_spawn_points: + # 抬高一点防止穿地 + safe_z = max(sp.location.z, 0.0) + 0.3 + safe_sp = carla.Transform( + carla.Location(x=sp.location.x, y=sp.location.y, z=safe_z), + sp.rotation ) - self.vehicle = self.world.try_spawn_actor(vehicle_bp, safe_transform) + self.vehicle = self.world.try_spawn_actor(vehicle_bp, safe_sp) if self.vehicle is not None: + print(f"✅ 在备用点成功生成车辆: ({safe_sp.location.x:.1f}, {safe_sp.location.y:.1f})") break if self.vehicle is None: - raise RuntimeError("❌ 无法生成车辆!") + raise RuntimeError("❌ 所有 spawn 点均无法生成车辆!请检查地图或 CARLA 状态。") - # 记录车辆信息 self._current_vehicle_id = self.vehicle.id loc = self.vehicle.get_location() print( f"✅ 车辆生成成功: {self.vehicle.type_id} | ID={self._current_vehicle_id} | 位置: ({loc.x:.1f}, {loc.y:.1f}, {loc.z:.1f})") - # 原子写入车辆ID文件(防止并发写入损坏) - temp_file = VEHICLE_ID_FILE + ".tmp" try: - with open(temp_file, 'w') as f: + with open(VEHICLE_ID_FILE, 'w') as f: json.dump({"vehicle_id": self._current_vehicle_id}, f) - os.replace(temp_file, VEHICLE_ID_FILE) # 原子操作 except Exception as e: - print(f"⚠️ 保存车辆ID失败(不影响运行): {e}") + print(f"⚠️ 保存车辆ID失败: {e}") def _update_spectator_view(self): - """ - 更新 CARLA 观察者视角,使其跟随车辆(第三人称)。 - 相机位于车辆后上方,俯视前方道路。 - """ + """修复视角:温和第三人称,确保看到整车""" if not (self.vehicle and self.spectator): return - v_transform = self.vehicle.get_transform() - # 相对偏移:后方8米,上方4米 - offset = carla.Location(x=-8.0, y=0.0, z=4.0) - spectator_loc = v_transform.transform(offset) - spectator_rot = carla.Rotation( - pitch=-20.0, # 俯视角 - yaw=v_transform.rotation.yaw, # 跟随车辆朝向 - roll=0.0 - ) - self.spectator.set_transform(carla.Transform(spectator_loc, spectator_rot)) + try: + v_transform = self.vehicle.get_transform() + # 相机:后方5米,右侧1米,上方2.2米(更低更稳) + offset = carla.Location(x=-5.0, y=1.0, z=2.2) + camera_loc = v_transform.transform(offset) + # 俯角 -10°(不要太陡),yaw 跟随车辆 + spectator_rot = carla.Rotation( + pitch=-10.0, + yaw=v_transform.rotation.yaw, + roll=0.0 + ) + self.spectator.set_transform(carla.Transform(camera_loc, spectator_rot)) + except Exception: + pass # 容错 + + def _log_trajectory(self, x, y, speed): + if self.log_trajectory: + self.trajectory_data.append((x, y, speed)) def get_observation(self): - """ - 获取当前环境观测值。 - :return: np.array([x, y, vx, vy], dtype=np.float32) - """ if not self.vehicle or not self.vehicle.is_alive: - # 车辆不存在时返回零向量(避免崩溃) return np.zeros(4, dtype=np.float32) loc = self.vehicle.get_location() vel = self.vehicle.get_velocity() return np.array([loc.x, loc.y, vel.x, vel.y], dtype=np.float32) def step(self, action): - """ - 执行一步环境交互。 - :param action: [throttle, steer, brake] - :return: obs, reward, terminated, truncated, info - """ throttle, steer, brake = action control = carla.VehicleControl( throttle=float(throttle), @@ -284,56 +190,50 @@ def step(self, action): brake=float(brake) ) self.vehicle.apply_control(control) - self.world.tick() # 推进仿真 + self.world.tick() self.frame_count += 1 - self._update_spectator_view() # 更新视角 + self._update_spectator_view() - # 检查车辆是否被销毁 if not self.vehicle or not self.vehicle.is_alive: return np.zeros(4, dtype=np.float32), -10.0, True, False, {} - # 获取观测 obs = self.get_observation() x, y, vx, vy = obs speed = np.linalg.norm([vx, vy]) - # ======================== - # ✅【强力推荐】使用车辆朝向速度作为主奖励 - # ======================== vehicle_transform = self.vehicle.get_transform() - forward_vector = vehicle_transform.get_forward_vector() # 车头方向单位向量 - - # 计算速度在车头方向的投影(鼓励向前行驶) + forward_vector = vehicle_transform.get_forward_vector() forward_speed = vx * forward_vector.x + vy * forward_vector.y - - # 主奖励:只奖励正向前进(倒车不奖励) reward = 1.0 * max(forward_speed, 0.0) - - # 额外惩罚:如果几乎静止,施加较大惩罚(促进行动) if speed < 0.1: reward -= 0.5 - self.prev_x = x - terminated = False # 暂无终止条件(如碰撞) - truncated = self.frame_count >= self.max_frames # 超过最大步数则截断 + self._log_trajectory(x, y, speed) + + terminated = False + truncated = self.frame_count >= self.max_frames return obs, reward, terminated, truncated, {} def close(self): - """ - 关闭环境,释放资源。 - 若 keep_alive=True,则保留车辆供人工观察;否则销毁。 - """ + if self.log_trajectory and self.trajectory_data: + try: + with open(TRAJECTORY_LOG_FILE, 'w') as f: + f.write("x,y,speed\n") + for x, y, speed in self.trajectory_data: + f.write(f"{x:.3f},{y:.3f},{speed:.3f}\n") + print(f"📊 轨迹已保存至: {TRAJECTORY_LOG_FILE}") + except Exception as e: + print(f"⚠️ 轨迹保存失败: {e}") + if self.keep_alive: print("ℹ️ 车辆已保留(ID已记录,下次运行时将自动清理)") if self.vehicle: - # 松开所有控制,让车自然停下 self.vehicle.apply_control(carla.VehicleControl()) - for i in range(30): # 同步30帧确保停止 + for i in range(30): self.world.tick() self._update_spectator_view() time.sleep(0.1) print("✅ 现在你可以自由操作 CARLA 视角(按 F1~F4)!") else: - # 彻底清理 if self.vehicle and self.vehicle.is_alive: self.vehicle.destroy() From a69ceac5eed0aa941fc34bb6adfd839b1a46cc94 Mon Sep 17 00:00:00 2001 From: wuuu0236 <2416234104@qq.com> Date: Thu, 18 Dec 2025 22:04:58 +0800 Subject: [PATCH 2/8] Update carla_env_multi_obs.py --- .../carla_env_multi_obs.py | 310 +++++++++++++++--- 1 file changed, 263 insertions(+), 47 deletions(-) diff --git a/src/AutonomousDriving_RL/carla_env_multi_obs.py b/src/AutonomousDriving_RL/carla_env_multi_obs.py index 552f6eb4c5..1efaeef533 100644 --- a/src/AutonomousDriving_RL/carla_env_multi_obs.py +++ b/src/AutonomousDriving_RL/carla_env_multi_obs.py @@ -1,4 +1,12 @@ # carla_env/carla_env_multi_obs.py + +""" +CARLA 强化学习环境封装(支持多维观测) +- 支持两种观测模式:旧版 4 维(位置+速度) / 新版 9 维(增强感知) +- 集成碰撞检测、轨迹记录、自动车辆清理、视角跟随等功能 +- 适配 Gymnasium 接口,可直接用于 Stable-Baselines3 等 RL 库 +""" + import carla import numpy as np import random @@ -7,27 +15,73 @@ import json from gymnasium import Env, spaces +# 全局常量:用于跨运行清理上一次残留车辆 VEHICLE_ID_FILE = ".last_vehicle_id.json" +# 轨迹日志文件名(x, y, speed) TRAJECTORY_LOG_FILE = "trajectory.csv" class CarlaEnvMultiObs(Env): - def __init__(self, keep_alive_after_exit=True, log_trajectory=True): + """ + 基于 CARLA 的自动驾驶强化学习环境 + """ + + def __init__(self, keep_alive_after_exit=True, log_trajectory=True, legacy_mode=False): + """ + 初始化环境 + + 参数: + keep_alive_after_exit (bool): + 是否在环境关闭后保留车辆(便于手动观察或调试) + log_trajectory (bool): + 是否记录车辆轨迹到 CSV 文件 + legacy_mode (bool): + 是否使用旧版 4 维观测空间([x, y, vx, vy]) + 默认 False → 使用新版 9 维观测(含车道、障碍物、红灯等) + """ super(CarlaEnvMultiObs, self).__init__() + + # CARLA 客户端与世界对象 self.client = None self.world = None self.vehicle = None self._current_vehicle_id = None + + # 训练控制 self.frame_count = 0 - self.max_frames = 1000 + self.max_frames = 1000 # 单轮最大步数(防止无限运行) + + # 视角控制 self.spectator = None + + # 行为控制标志 self.keep_alive = keep_alive_after_exit self.log_trajectory = log_trajectory - self.trajectory_data = [] + self.trajectory_data = [] # 存储 (x, y, speed) 轨迹点 - self.observation_space = spaces.Box( - low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32 - ) + # 碰撞传感器 + self._collision_sensor = None + self._collision_hist = [] # 存储碰撞事件 + + # 观测模式开关 + self.legacy_mode = legacy_mode + + # 根据模式设置观测空间 + if self.legacy_mode: + # 旧版:仅位置和速度(无方向、无环境感知) + self.observation_space = spaces.Box( + low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32 + ) + else: + # 新版:9 维标准化观测 + # [speed_norm, lane_offset_norm, curvature_norm, obstacle_dist_norm, + # is_red_light, vx_norm, vy_norm, sin(yaw), cos(yaw)] + self.observation_space = spaces.Box( + low=-1.0, high=1.0, shape=(9,), dtype=np.float32 + ) + + # 动作空间:[throttle, steer, brake] + # throttle ∈ [0, 1], steer ∈ [-1, 1], brake ∈ [0, 1] self.action_space = spaces.Box( low=np.array([0.0, -1.0, 0.0]), high=np.array([1.0, 1.0, 1.0]), @@ -35,12 +89,14 @@ def __init__(self, keep_alive_after_exit=True, log_trajectory=True): ) def _connect_carla(self, max_retries=3): - """自动重试连接 CARLA""" + """ + 尝试连接本地 CARLA 服务器(localhost:2000) + """ for attempt in range(max_retries): try: print(f"🔄 尝试连接 CARLA 服务器 (第 {attempt + 1} 次)...") self.client = carla.Client('localhost', 2000) - self.client.set_timeout(10.0) + self.client.set_timeout(10.0) # 超时 10 秒 self.world = self.client.get_world() if self.world is not None: print(f"✅ 成功连接到 CARLA!地图: {self.world.get_map().name}") @@ -51,28 +107,53 @@ def _connect_carla(self, max_retries=3): raise RuntimeError("❌ 无法连接到 CARLA 服务器,请确保 CARLA 已启动!") def reset(self, seed=None, options=None): + """ + 重置环境:清理旧车、生成新车、初始化传感器 + """ super().reset(seed=seed) if seed is not None: random.seed(seed) np.random.seed(seed) + # 连接 CARLA 并销毁上次残留车辆 self._connect_carla() self._destroy_last_run_vehicle() + + # 生成新车辆 self.spawn_vehicle() + # 清空碰撞历史并创建新碰撞传感器 + self._collision_hist.clear() + if self._collision_sensor is not None: + self._collision_sensor.destroy() + self._collision_sensor = None + + bp = self.world.get_blueprint_library().find('sensor.other.collision') + self._collision_sensor = self.world.spawn_actor(bp, carla.Transform(), attach_to=self.vehicle) + self._collision_sensor.listen(lambda event: self._collision_hist.append(event)) + + # 等待几帧让物理稳定 for _ in range(5): self.world.tick() time.sleep(0.05) + # 设置第三人称视角跟随车辆 self.spectator = self.world.get_spectator() self._update_spectator_view() + # 重置轨迹与帧计数 self.trajectory_data = [] self.frame_count = 0 + + # 返回初始观测 obs = self.get_observation() return obs, {} def _destroy_last_run_vehicle(self): + """ + 从 .last_vehicle_id.json 读取上次车辆 ID 并尝试销毁 + 避免多次运行导致车辆堆积 + """ if not os.path.exists(VEHICLE_ID_FILE): return try: @@ -89,13 +170,16 @@ def _destroy_last_run_vehicle(self): pass def spawn_vehicle(self): + """ + 在地图中生成一辆车(优先 Tesla Model 3) + - Town10HD_Opt 使用固定 spawn 点 + - 其他地图使用第一个可用 spawn 点 + - 若失败则遍历所有点尝试 + """ blueprint_library = self.world.get_blueprint_library() - # 优先使用特斯拉,否则随机选一个 vehicle_bp = blueprint_library.find('vehicle.tesla.model3') if not vehicle_bp or not vehicle_bp.has_attribute('number_of_wheels'): vehicle_bp = random.choice(blueprint_library.filter('vehicle.*')) - - # 设置颜色(可选) if vehicle_bp.has_attribute('color'): color = random.choice(vehicle_bp.get_attribute('color').recommended_values) vehicle_bp.set_attribute('color', color) @@ -103,39 +187,32 @@ def spawn_vehicle(self): map_name = self.world.get_map().name.lower() spawn_transform = None - # 针对 Town10HD_Opt 使用已知安全点 + # Town10HD 特定 spawn 点(避免出生在空中/水里) if 'town10' in map_name: spawn_transform = carla.Transform( carla.Location(x=100.0, y=130.0, z=0.3), carla.Rotation(yaw=180.0) ) else: - # 通用 fallback:使用第一个 spawn point spawn_points = self.world.get_map().get_spawn_points() if spawn_points: spawn_transform = spawn_points[0] else: - # 极端 fallback:原点上方 spawn_transform = carla.Transform(carla.Location(x=0, y=0, z=1.0), carla.Rotation()) - # 尝试主位置 self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform) - - # 如果失败,遍历所有 spawn points if self.vehicle is None: print("⚠️ 主 spawn 点失败,尝试遍历所有可用点...") all_spawn_points = self.world.get_map().get_spawn_points() - random.shuffle(all_spawn_points) # 随机顺序避免总用同一个 + random.shuffle(all_spawn_points) for sp in all_spawn_points: - # 抬高一点防止穿地 - safe_z = max(sp.location.z, 0.0) + 0.3 + safe_z = max(sp.location.z, 0.0) + 0.3 # 抬高一点避免穿模 safe_sp = carla.Transform( carla.Location(x=sp.location.x, y=sp.location.y, z=safe_z), sp.rotation ) self.vehicle = self.world.try_spawn_actor(vehicle_bp, safe_sp) if self.vehicle is not None: - print(f"✅ 在备用点成功生成车辆: ({safe_sp.location.x:.1f}, {safe_sp.location.y:.1f})") break if self.vehicle is None: @@ -146,6 +223,7 @@ def spawn_vehicle(self): print( f"✅ 车辆生成成功: {self.vehicle.type_id} | ID={self._current_vehicle_id} | 位置: ({loc.x:.1f}, {loc.y:.1f}, {loc.z:.1f})") + # 保存 ID 供下次清理 try: with open(VEHICLE_ID_FILE, 'w') as f: json.dump({"vehicle_id": self._current_vehicle_id}, f) @@ -153,36 +231,138 @@ def spawn_vehicle(self): print(f"⚠️ 保存车辆ID失败: {e}") def _update_spectator_view(self): - """修复视角:温和第三人称,确保看到整车""" + """ + 更新第三人称摄像机视角,跟随车辆后上方 + """ if not (self.vehicle and self.spectator): return try: v_transform = self.vehicle.get_transform() - # 相机:后方5米,右侧1米,上方2.2米(更低更稳) - offset = carla.Location(x=-5.0, y=1.0, z=2.2) + offset = carla.Location(x=-5.0, y=1.0, z=2.2) # 相对偏移 camera_loc = v_transform.transform(offset) - # 俯角 -10°(不要太陡),yaw 跟随车辆 - spectator_rot = carla.Rotation( - pitch=-10.0, - yaw=v_transform.rotation.yaw, - roll=0.0 - ) + spectator_rot = carla.Rotation(pitch=-10.0, yaw=v_transform.rotation.yaw, roll=0.0) self.spectator.set_transform(carla.Transform(camera_loc, spectator_rot)) except Exception: - pass # 容错 + pass + + def _traffic_light_ahead(self, dist=15.0): + """ + 检测前方 dist 米内是否有红灯 + 利用向量点积判断是否在车辆前方 + """ + if not self.vehicle: + return False + lights = self.world.get_actors().filter('traffic.traffic_light*') + vehicle_transform = self.vehicle.get_transform() + forward = vehicle_transform.get_forward_vector() + for light in lights: + delta = light.get_transform().location - vehicle_transform.location + dot = delta.x * forward.x + delta.y * forward.y # 点积 + if 0 < dot < dist and delta.distance(vehicle_transform.location) < dist: + if light.state == carla.TrafficLightState.Red: + return True + return False def _log_trajectory(self, x, y, speed): + """ + 记录当前帧的轨迹点(若启用) + """ if self.log_trajectory: self.trajectory_data.append((x, y, speed)) def get_observation(self): + """ + 获取当前环境观测值 + + 返回: + np.ndarray: 形状为 (4,) 或 (9,) 的浮点数组 + """ if not self.vehicle or not self.vehicle.is_alive: - return np.zeros(4, dtype=np.float32) - loc = self.vehicle.get_location() - vel = self.vehicle.get_velocity() - return np.array([loc.x, loc.y, vel.x, vel.y], dtype=np.float32) + dim = 4 if self.legacy_mode else 9 + return np.zeros(dim, dtype=np.float32) + + if self.legacy_mode: + # 旧模式:仅位置和速度 + loc = self.vehicle.get_location() + vel = self.vehicle.get_velocity() + return np.array([loc.x, loc.y, vel.x, vel.y], dtype=np.float32) + + # 新模式:9 维增强观测 + transform = self.vehicle.get_transform() + velocity = self.vehicle.get_velocity() + speed = np.linalg.norm([velocity.x, velocity.y]) # 2D 速度大小 + forward_vec = transform.get_forward_vector() + + # 车道信息(通过 Waypoint 获取) + try: + waypoint = self.world.get_map().get_waypoint(transform.location, project_to_road=True) + lane_offset = transform.location.distance(waypoint.transform.location) # 到车道中心距离 + next_wp_list = waypoint.next(5.0) + next_wp = next_wp_list[0] if next_wp_list else waypoint + # 曲率:下一 waypoint 与当前航向的偏转角(反映弯道程度) + curvature = abs(next_wp.transform.rotation.yaw - waypoint.transform.rotation.yaw) / 5.0 + except: + lane_offset, curvature = 5.0, 0.5 # 异常时设为最差值 + + # 障碍物检测:向前发射射线(ray-cast) + obstacle_dist = 50.0 + try: + start = transform.location + carla.Location(z=0.5) + end = start + forward_vec * 20.0 + hits = self.world.cast_ray(start, end) + if hits: + obstacle_dist = min(h.distance for h in hits) + except: + pass + + # 红灯检测 + is_red_light = self._traffic_light_ahead() + + # 构建 9 维观测(全部归一化到 [-1, 1] 或 [0, 1]) + obs = np.array([ + speed / 30.0, # 速度归一化(假设 max=30 m/s) + min(lane_offset, 3.0) / 3.0, # 车道偏移(最大 3 米) + min(curvature, 10.0) / 10.0, # 曲率(最大 10 度/米) + min(obstacle_dist, 50.0) / 50.0, # 障碍物距离(最大 50 米) + float(is_red_light), # 红灯(0 或 1) + np.clip(velocity.x / 30.0, -1, 1), # vx 归一化 + np.clip(velocity.y / 30.0, -1, 1), # vy 归一化 + np.sin(np.radians(transform.rotation.yaw)), # 航向角正弦 + np.cos(np.radians(transform.rotation.yaw)) # 航向角余弦 + ], dtype=np.float32) + + return np.clip(obs, -1.0, 1.0) + + def _compute_reward(self, speed, lane_offset, obstacle_dist, is_red_light, action): + """ + 计算每一步的奖励(当前实现较基础,可优化) + """ + reward = 0.0 + target_speed = 10.0 + reward += -abs(speed - target_speed) * 0.1 # 速度惩罚 + + if lane_offset < 1.0: + reward += (1.0 - lane_offset) * 0.5 # 车道内奖励 + else: + reward -= 1.0 # 车道外惩罚 + + if obstacle_dist < 5.0: + reward -= (5.0 - obstacle_dist) * 2.0 + if obstacle_dist < 2.0: + reward -= 10.0 # 极近重罚 + + if is_red_light and speed > 1.0: + reward -= 5.0 # 闯红灯惩罚 + + throttle, steer, brake = action + reward -= (abs(steer) * 0.1 + abs(brake) * 0.05) # 控制平滑性 + + return reward def step(self, action): + """ + 执行一步动作,返回 (obs, reward, terminated, truncated, info) + """ throttle, steer, brake = action control = carla.VehicleControl( throttle=float(throttle), @@ -194,27 +374,57 @@ def step(self, action): self.frame_count += 1 self._update_spectator_view() + # 车辆死亡(如被销毁) if not self.vehicle or not self.vehicle.is_alive: - return np.zeros(4, dtype=np.float32), -10.0, True, False, {} + dim = 4 if self.legacy_mode else 9 + obs = np.zeros(dim, dtype=np.float32) + return obs, -10.0, True, False, {} - obs = self.get_observation() - x, y, vx, vy = obs - speed = np.linalg.norm([vx, vy]) + # 获取状态 + transform = self.vehicle.get_transform() + velocity = self.vehicle.get_velocity() + speed = np.linalg.norm([velocity.x, velocity.y]) - vehicle_transform = self.vehicle.get_transform() - forward_vector = vehicle_transform.get_forward_vector() - forward_speed = vx * forward_vector.x + vy * forward_vector.y - reward = 1.0 * max(forward_speed, 0.0) - if speed < 0.1: - reward -= 0.5 + try: + waypoint = self.world.get_map().get_waypoint(transform.location, project_to_road=True) + lane_offset = transform.location.distance(waypoint.transform.location) + except: + lane_offset = 5.0 + + obstacle_dist = 50.0 + try: + forward_vec = transform.get_forward_vector() + start = transform.location + carla.Location(z=0.5) + end = start + forward_vec * 20.0 + hits = self.world.cast_ray(start, end) + if hits: + obstacle_dist = min(h.distance for h in hits) + except: + pass - self._log_trajectory(x, y, speed) + is_red_light = self._traffic_light_ahead() + reward = self._compute_reward(speed, lane_offset, obstacle_dist, is_red_light, action) - terminated = False + # 终止条件:碰撞 + terminated = len(self._collision_hist) > 0 + if terminated: + reward -= 50.0 + + # 截断条件:超时 truncated = self.frame_count >= self.max_frames + + # 记录轨迹 + self._log_trajectory(transform.location.x, transform.location.y, speed) + + # 获取新观测 + obs = self.get_observation() return obs, reward, terminated, truncated, {} def close(self): + """ + 关闭环境:保存轨迹、清理传感器和车辆 + """ + # 保存轨迹 if self.log_trajectory and self.trajectory_data: try: with open(TRAJECTORY_LOG_FILE, 'w') as f: @@ -225,6 +435,12 @@ def close(self): except Exception as e: print(f"⚠️ 轨迹保存失败: {e}") + # 销毁传感器 + if self._collision_sensor is not None: + self._collision_sensor.destroy() + self._collision_sensor = None + + # 车辆处理 if self.keep_alive: print("ℹ️ 车辆已保留(ID已记录,下次运行时将自动清理)") if self.vehicle: From a7c155142305546956f080ed76602d1b4b01a387 Mon Sep 17 00:00:00 2001 From: wuuu0236 <2416234104@qq.com> Date: Fri, 19 Dec 2025 17:17:51 +0800 Subject: [PATCH 3/8] Update carla_env_multi_obs.py --- .../carla_env_multi_obs.py | 441 ++++++------------ 1 file changed, 149 insertions(+), 292 deletions(-) diff --git a/src/AutonomousDriving_RL/carla_env_multi_obs.py b/src/AutonomousDriving_RL/carla_env_multi_obs.py index 1efaeef533..a8605e1b08 100644 --- a/src/AutonomousDriving_RL/carla_env_multi_obs.py +++ b/src/AutonomousDriving_RL/carla_env_multi_obs.py @@ -1,10 +1,9 @@ # carla_env/carla_env_multi_obs.py - """ -CARLA 强化学习环境封装(支持多维观测) -- 支持两种观测模式:旧版 4 维(位置+速度) / 新版 9 维(增强感知) -- 集成碰撞检测、轨迹记录、自动车辆清理、视角跟随等功能 -- 适配 Gymnasium 接口,可直接用于 Stable-Baselines3 等 RL 库 +CARLA 强化学习环境(4D 观测增强版) +- 观测: [x, y, vx, vy] +- 动作: [throttle, steer, brake] +- 新增: 车道保持奖励、合理速度区间、轨迹日志、参数化配置、抗崩溃机制 """ import carla @@ -15,145 +14,118 @@ import json from gymnasium import Env, spaces -# 全局常量:用于跨运行清理上一次残留车辆 VEHICLE_ID_FILE = ".last_vehicle_id.json" -# 轨迹日志文件名(x, y, speed) -TRAJECTORY_LOG_FILE = "trajectory.csv" class CarlaEnvMultiObs(Env): - """ - 基于 CARLA 的自动驾驶强化学习环境 - """ - - def __init__(self, keep_alive_after_exit=True, log_trajectory=True, legacy_mode=False): - """ - 初始化环境 - - 参数: - keep_alive_after_exit (bool): - 是否在环境关闭后保留车辆(便于手动观察或调试) - log_trajectory (bool): - 是否记录车辆轨迹到 CSV 文件 - legacy_mode (bool): - 是否使用旧版 4 维观测空间([x, y, vx, vy]) - 默认 False → 使用新版 9 维观测(含车道、障碍物、红灯等) - """ - super(CarlaEnvMultiObs, self).__init__() - - # CARLA 客户端与世界对象 + def __init__( + self, + keep_alive_after_exit=False, + log_trajectory=True, + trajectory_log_file="trajectory.csv", + target_speed=8.0, # 目标速度 (m/s) + max_episode_steps=1000, # 最大步数 + map_name=None, # 指定地图(如 'Town10HD') + spawn_point_index=0, # spawn 点索引 + random_spawn=False, # 是否随机 spawn + reward_weights=None # 奖励权重配置 + ): + super().__init__() self.client = None self.world = None self.vehicle = None self._current_vehicle_id = None - - # 训练控制 self.frame_count = 0 - self.max_frames = 1000 # 单轮最大步数(防止无限运行) - - # 视角控制 + self.max_episode_steps = max_episode_steps self.spectator = None - - # 行为控制标志 self.keep_alive = keep_alive_after_exit self.log_trajectory = log_trajectory - self.trajectory_data = [] # 存储 (x, y, speed) 轨迹点 - - # 碰撞传感器 + self.trajectory_log_file = trajectory_log_file + self.trajectory_data = [] self._collision_sensor = None - self._collision_hist = [] # 存储碰撞事件 - - # 观测模式开关 - self.legacy_mode = legacy_mode - - # 根据模式设置观测空间 - if self.legacy_mode: - # 旧版:仅位置和速度(无方向、无环境感知) - self.observation_space = spaces.Box( - low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32 - ) - else: - # 新版:9 维标准化观测 - # [speed_norm, lane_offset_norm, curvature_norm, obstacle_dist_norm, - # is_red_light, vx_norm, vy_norm, sin(yaw), cos(yaw)] - self.observation_space = spaces.Box( - low=-1.0, high=1.0, shape=(9,), dtype=np.float32 - ) - - # 动作空间:[throttle, steer, brake] - # throttle ∈ [0, 1], steer ∈ [-1, 1], brake ∈ [0, 1] + self._collision_hist = [] + + # 奖励配置 + self.target_speed = target_speed + self.reward_weights = { + 'forward': 0.1, + 'speed_match': 0.5, + 'lane_center': 1.0, + 'steer_smooth': 0.05, + 'brake_penalty': 0.1, + 'collision': -50.0, + 'time_bonus': 0.01 + } + if reward_weights: + self.reward_weights.update(reward_weights) + + # 地图与 spawn 配置 + self.map_name = map_name + self.spawn_point_index = spawn_point_index + self.random_spawn = random_spawn + + # 固定 4D 观测空间 + self.observation_space = spaces.Box( + low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32 + ) self.action_space = spaces.Box( low=np.array([0.0, -1.0, 0.0]), high=np.array([1.0, 1.0, 1.0]), dtype=np.float32 ) - def _connect_carla(self, max_retries=3): - """ - 尝试连接本地 CARLA 服务器(localhost:2000) - """ + def _connect_carla(self, max_retries=3, timeout=10.0): for attempt in range(max_retries): try: print(f"🔄 尝试连接 CARLA 服务器 (第 {attempt + 1} 次)...") self.client = carla.Client('localhost', 2000) - self.client.set_timeout(10.0) # 超时 10 秒 + self.client.set_timeout(timeout) self.world = self.client.get_world() - if self.world is not None: - print(f"✅ 成功连接到 CARLA!地图: {self.world.get_map().name}") - return True + if self.map_name and self.map_name not in self.world.get_map().name: + print(f"🔄 加载指定地图: {self.map_name}") + self.world = self.client.load_world(self.map_name) + print(f"✅ 成功连接!地图: {self.world.get_map().name}") + return True except Exception as e: print(f"⚠️ 连接失败: {e}") time.sleep(2) - raise RuntimeError("❌ 无法连接到 CARLA 服务器,请确保 CARLA 已启动!") + raise RuntimeError("❌ 无法连接 CARLA,请确保已启动 `CarlaUE4.sh`") def reset(self, seed=None, options=None): - """ - 重置环境:清理旧车、生成新车、初始化传感器 - """ super().reset(seed=seed) if seed is not None: random.seed(seed) np.random.seed(seed) - # 连接 CARLA 并销毁上次残留车辆 self._connect_carla() self._destroy_last_run_vehicle() - - # 生成新车辆 self.spawn_vehicle() - # 清空碰撞历史并创建新碰撞传感器 + # 初始化传感器 self._collision_hist.clear() - if self._collision_sensor is not None: + if self._collision_sensor: self._collision_sensor.destroy() - self._collision_sensor = None - bp = self.world.get_blueprint_library().find('sensor.other.collision') self._collision_sensor = self.world.spawn_actor(bp, carla.Transform(), attach_to=self.vehicle) self._collision_sensor.listen(lambda event: self._collision_hist.append(event)) - # 等待几帧让物理稳定 + # 稳定物理 for _ in range(5): self.world.tick() time.sleep(0.05) - # 设置第三人称视角跟随车辆 + # 设置视角 self.spectator = self.world.get_spectator() self._update_spectator_view() - # 重置轨迹与帧计数 + # 重置状态 self.trajectory_data = [] self.frame_count = 0 - # 返回初始观测 obs = self.get_observation() return obs, {} def _destroy_last_run_vehicle(self): - """ - 从 .last_vehicle_id.json 读取上次车辆 ID 并尝试销毁 - 避免多次运行导致车辆堆积 - """ if not os.path.exists(VEHICLE_ID_FILE): return try: @@ -170,12 +142,6 @@ def _destroy_last_run_vehicle(self): pass def spawn_vehicle(self): - """ - 在地图中生成一辆车(优先 Tesla Model 3) - - Town10HD_Opt 使用固定 spawn 点 - - 其他地图使用第一个可用 spawn 点 - - 若失败则遍历所有点尝试 - """ blueprint_library = self.world.get_blueprint_library() vehicle_bp = blueprint_library.find('vehicle.tesla.model3') if not vehicle_bp or not vehicle_bp.has_attribute('number_of_wheels'): @@ -184,46 +150,35 @@ def spawn_vehicle(self): color = random.choice(vehicle_bp.get_attribute('color').recommended_values) vehicle_bp.set_attribute('color', color) - map_name = self.world.get_map().name.lower() - spawn_transform = None + spawn_points = self.world.get_map().get_spawn_points() + if not spawn_points: + raise RuntimeError("❌ 地图无可用 spawn 点!") - # Town10HD 特定 spawn 点(避免出生在空中/水里) - if 'town10' in map_name: - spawn_transform = carla.Transform( - carla.Location(x=100.0, y=130.0, z=0.3), - carla.Rotation(yaw=180.0) - ) + if self.random_spawn: + spawn_transform = random.choice(spawn_points) + elif self.spawn_point_index < len(spawn_points): + spawn_transform = spawn_points[self.spawn_point_index] else: - spawn_points = self.world.get_map().get_spawn_points() - if spawn_points: - spawn_transform = spawn_points[0] - else: - spawn_transform = carla.Transform(carla.Location(x=0, y=0, z=1.0), carla.Rotation()) + spawn_transform = spawn_points[0] + # 尝试生成 self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform) if self.vehicle is None: - print("⚠️ 主 spawn 点失败,尝试遍历所有可用点...") - all_spawn_points = self.world.get_map().get_spawn_points() - random.shuffle(all_spawn_points) - for sp in all_spawn_points: - safe_z = max(sp.location.z, 0.0) + 0.3 # 抬高一点避免穿模 - safe_sp = carla.Transform( - carla.Location(x=sp.location.x, y=sp.location.y, z=safe_z), - sp.rotation - ) - self.vehicle = self.world.try_spawn_actor(vehicle_bp, safe_sp) - if self.vehicle is not None: - break + # 备用:抬高 Z 轴 + sp = spawn_transform + safe_sp = carla.Transform( + carla.Location(x=sp.location.x, y=sp.location.y, z=max(sp.location.z, 0.0) + 0.5), + sp.rotation + ) + self.vehicle = self.world.try_spawn_actor(vehicle_bp, safe_sp) if self.vehicle is None: - raise RuntimeError("❌ 所有 spawn 点均无法生成车辆!请检查地图或 CARLA 状态。") + raise RuntimeError("❌ 所有 spawn 方式均失败!") self._current_vehicle_id = self.vehicle.id loc = self.vehicle.get_location() - print( - f"✅ 车辆生成成功: {self.vehicle.type_id} | ID={self._current_vehicle_id} | 位置: ({loc.x:.1f}, {loc.y:.1f}, {loc.z:.1f})") + print(f"✅ 车辆生成成功 | ID={self._current_vehicle_id} | ({loc.x:.1f}, {loc.y:.1f})") - # 保存 ID 供下次清理 try: with open(VEHICLE_ID_FILE, 'w') as f: json.dump({"vehicle_id": self._current_vehicle_id}, f) @@ -231,225 +186,127 @@ def spawn_vehicle(self): print(f"⚠️ 保存车辆ID失败: {e}") def _update_spectator_view(self): - """ - 更新第三人称摄像机视角,跟随车辆后上方 - """ if not (self.vehicle and self.spectator): return try: v_transform = self.vehicle.get_transform() - offset = carla.Location(x=-5.0, y=1.0, z=2.2) # 相对偏移 + offset = carla.Location(x=-6.0, y=0.0, z=2.5) camera_loc = v_transform.transform(offset) - spectator_rot = carla.Rotation(pitch=-10.0, yaw=v_transform.rotation.yaw, roll=0.0) - self.spectator.set_transform(carla.Transform(camera_loc, spectator_rot)) + rot = carla.Rotation(pitch=-15.0, yaw=v_transform.rotation.yaw) + self.spectator.set_transform(carla.Transform(camera_loc, rot)) except Exception: pass - def _traffic_light_ahead(self, dist=15.0): - """ - 检测前方 dist 米内是否有红灯 - 利用向量点积判断是否在车辆前方 - """ - if not self.vehicle: - return False - lights = self.world.get_actors().filter('traffic.traffic_light*') - vehicle_transform = self.vehicle.get_transform() - forward = vehicle_transform.get_forward_vector() - for light in lights: - delta = light.get_transform().location - vehicle_transform.location - dot = delta.x * forward.x + delta.y * forward.y # 点积 - if 0 < dot < dist and delta.distance(vehicle_transform.location) < dist: - if light.state == carla.TrafficLightState.Red: - return True - return False - - def _log_trajectory(self, x, y, speed): - """ - 记录当前帧的轨迹点(若启用) - """ - if self.log_trajectory: - self.trajectory_data.append((x, y, speed)) - def get_observation(self): - """ - 获取当前环境观测值 - - 返回: - np.ndarray: 形状为 (4,) 或 (9,) 的浮点数组 - """ if not self.vehicle or not self.vehicle.is_alive: - dim = 4 if self.legacy_mode else 9 - return np.zeros(dim, dtype=np.float32) - - if self.legacy_mode: - # 旧模式:仅位置和速度 - loc = self.vehicle.get_location() - vel = self.vehicle.get_velocity() - return np.array([loc.x, loc.y, vel.x, vel.y], dtype=np.float32) - - # 新模式:9 维增强观测 - transform = self.vehicle.get_transform() - velocity = self.vehicle.get_velocity() - speed = np.linalg.norm([velocity.x, velocity.y]) # 2D 速度大小 - forward_vec = transform.get_forward_vector() - - # 车道信息(通过 Waypoint 获取) - try: - waypoint = self.world.get_map().get_waypoint(transform.location, project_to_road=True) - lane_offset = transform.location.distance(waypoint.transform.location) # 到车道中心距离 - next_wp_list = waypoint.next(5.0) - next_wp = next_wp_list[0] if next_wp_list else waypoint - # 曲率:下一 waypoint 与当前航向的偏转角(反映弯道程度) - curvature = abs(next_wp.transform.rotation.yaw - waypoint.transform.rotation.yaw) / 5.0 - except: - lane_offset, curvature = 5.0, 0.5 # 异常时设为最差值 - - # 障碍物检测:向前发射射线(ray-cast) - obstacle_dist = 50.0 + return np.zeros(4, dtype=np.float32) + loc = self.vehicle.get_location() + vel = self.vehicle.get_velocity() + # 防止 NaN + x = float(loc.x) if np.isfinite(loc.x) else 0.0 + y = float(loc.y) if np.isfinite(loc.y) else 0.0 + vx = float(vel.x) if np.isfinite(vel.x) else 0.0 + vy = float(vel.y) if np.isfinite(vel.y) else 0.0 + return np.array([x, y, vx, vy], dtype=np.float32) + + def _get_lane_offset(self): + """计算到最近车道中心的距离(仅用于奖励,不放入 obs)""" try: - start = transform.location + carla.Location(z=0.5) - end = start + forward_vec * 20.0 - hits = self.world.cast_ray(start, end) - if hits: - obstacle_dist = min(h.distance for h in hits) + waypoint = self.world.get_map().get_waypoint( + self.vehicle.get_location(), project_to_road=True + ) + return self.vehicle.get_location().distance(waypoint.transform.location) except: - pass + return 5.0 # 默认远离车道 - # 红灯检测 - is_red_light = self._traffic_light_ahead() - - # 构建 9 维观测(全部归一化到 [-1, 1] 或 [0, 1]) - obs = np.array([ - speed / 30.0, # 速度归一化(假设 max=30 m/s) - min(lane_offset, 3.0) / 3.0, # 车道偏移(最大 3 米) - min(curvature, 10.0) / 10.0, # 曲率(最大 10 度/米) - min(obstacle_dist, 50.0) / 50.0, # 障碍物距离(最大 50 米) - float(is_red_light), # 红灯(0 或 1) - np.clip(velocity.x / 30.0, -1, 1), # vx 归一化 - np.clip(velocity.y / 30.0, -1, 1), # vy 归一化 - np.sin(np.radians(transform.rotation.yaw)), # 航向角正弦 - np.cos(np.radians(transform.rotation.yaw)) # 航向角余弦 - ], dtype=np.float32) - - return np.clip(obs, -1.0, 1.0) - - def _compute_reward(self, speed, lane_offset, obstacle_dist, is_red_light, action): - """ - 计算每一步的奖励(当前实现较基础,可优化) - """ + def _compute_reward(self, speed, lane_offset, action): + w = self.reward_weights reward = 0.0 - target_speed = 10.0 - reward += -abs(speed - target_speed) * 0.1 # 速度惩罚 - if lane_offset < 1.0: - reward += (1.0 - lane_offset) * 0.5 # 车道内奖励 + # 前进奖励 + if speed > 0.1: + reward += w['forward'] * speed + + # 速度匹配 + speed_diff = abs(speed - self.target_speed) + if speed_diff < 1.0: + reward += w['speed_match'] else: - reward -= 1.0 # 车道外惩罚 + reward -= speed_diff * 0.05 - if obstacle_dist < 5.0: - reward -= (5.0 - obstacle_dist) * 2.0 - if obstacle_dist < 2.0: - reward -= 10.0 # 极近重罚 + # 车道中心奖励(即使 4D 也鼓励 stay in lane) + if lane_offset < 1.0: + reward += w['lane_center'] * (1.0 - lane_offset) + else: + reward -= (lane_offset - 1.0) * 0.5 - if is_red_light and speed > 1.0: - reward -= 5.0 # 闯红灯惩罚 + # 控制平滑 + _, steer, brake = action + reward -= w['steer_smooth'] * abs(steer) + reward -= w['brake_penalty'] * brake - throttle, steer, brake = action - reward -= (abs(steer) * 0.1 + abs(brake) * 0.05) # 控制平滑性 + # 时间奖励(鼓励存活) + reward += w['time_bonus'] return reward def step(self, action): - """ - 执行一步动作,返回 (obs, reward, terminated, truncated, info) - """ - throttle, steer, brake = action - control = carla.VehicleControl( - throttle=float(throttle), - steer=float(steer), - brake=float(brake) - ) + # 安全钳位 + throttle = np.clip(action[0], 0.0, 1.0) + steer = np.clip(action[1], -1.0, 1.0) + brake = np.clip(action[2], 0.0, 1.0) + control = carla.VehicleControl(throttle=float(throttle), steer=float(steer), brake=float(brake)) self.vehicle.apply_control(control) self.world.tick() self.frame_count += 1 self._update_spectator_view() - # 车辆死亡(如被销毁) + # 车辆死亡 if not self.vehicle or not self.vehicle.is_alive: - dim = 4 if self.legacy_mode else 9 - obs = np.zeros(dim, dtype=np.float32) - return obs, -10.0, True, False, {} + obs = np.zeros(4, dtype=np.float32) + return obs, self.reward_weights['collision'], True, False, {} - # 获取状态 - transform = self.vehicle.get_transform() + # 状态 velocity = self.vehicle.get_velocity() - speed = np.linalg.norm([velocity.x, velocity.y]) + speed = np.sqrt(max(0.0, velocity.x**2 + velocity.y**2)) + lane_offset = self._get_lane_offset() + reward = self._compute_reward(speed, lane_offset, [throttle, steer, brake]) - try: - waypoint = self.world.get_map().get_waypoint(transform.location, project_to_road=True) - lane_offset = transform.location.distance(waypoint.transform.location) - except: - lane_offset = 5.0 - - obstacle_dist = 50.0 - try: - forward_vec = transform.get_forward_vector() - start = transform.location + carla.Location(z=0.5) - end = start + forward_vec * 20.0 - hits = self.world.cast_ray(start, end) - if hits: - obstacle_dist = min(h.distance for h in hits) - except: - pass - - is_red_light = self._traffic_light_ahead() - reward = self._compute_reward(speed, lane_offset, obstacle_dist, is_red_light, action) - - # 终止条件:碰撞 + # 终止条件 terminated = len(self._collision_hist) > 0 if terminated: - reward -= 50.0 + reward = self.reward_weights['collision'] - # 截断条件:超时 - truncated = self.frame_count >= self.max_frames + truncated = self.frame_count >= self.max_episode_steps # 记录轨迹 - self._log_trajectory(transform.location.x, transform.location.y, speed) + if self.log_trajectory: + loc = self.vehicle.get_location() + self.trajectory_data.append((float(loc.x), float(loc.y), float(speed))) - # 获取新观测 obs = self.get_observation() - return obs, reward, terminated, truncated, {} + return obs, reward, terminated, truncated, { + "speed": speed, + "lane_offset": lane_offset, + "collision": terminated + } def close(self): - """ - 关闭环境:保存轨迹、清理传感器和车辆 - """ # 保存轨迹 if self.log_trajectory and self.trajectory_data: try: - with open(TRAJECTORY_LOG_FILE, 'w') as f: + with open(self.trajectory_log_file, 'w') as f: f.write("x,y,speed\n") for x, y, speed in self.trajectory_data: f.write(f"{x:.3f},{y:.3f},{speed:.3f}\n") - print(f"📊 轨迹已保存至: {TRAJECTORY_LOG_FILE}") + print(f"📊 轨迹已保存至: {self.trajectory_log_file}") except Exception as e: print(f"⚠️ 轨迹保存失败: {e}") - # 销毁传感器 - if self._collision_sensor is not None: + # 清理 + if self._collision_sensor: self._collision_sensor.destroy() - self._collision_sensor = None - - # 车辆处理 - if self.keep_alive: - print("ℹ️ 车辆已保留(ID已记录,下次运行时将自动清理)") - if self.vehicle: - self.vehicle.apply_control(carla.VehicleControl()) - for i in range(30): - self.world.tick() - self._update_spectator_view() - time.sleep(0.1) - print("✅ 现在你可以自由操作 CARLA 视角(按 F1~F4)!") - else: - if self.vehicle and self.vehicle.is_alive: - self.vehicle.destroy() + if not self.keep_alive and self.vehicle and self.vehicle.is_alive: + self.vehicle.destroy() + elif self.keep_alive: + print("ℹ️ 车辆已保留(下次运行将自动清理)") From 8edd78829a121d98bd1b2cbda1fc0fb808c26bc8 Mon Sep 17 00:00:00 2001 From: wuuu0236 <2416234104@qq.com> Date: Fri, 19 Dec 2025 17:18:11 +0800 Subject: [PATCH 4/8] Update eval_agent.py --- src/AutonomousDriving_RL/eval_agent.py | 96 +++++++++++++++++++------- 1 file changed, 71 insertions(+), 25 deletions(-) diff --git a/src/AutonomousDriving_RL/eval_agent.py b/src/AutonomousDriving_RL/eval_agent.py index 712343cbde..a4233e0cbd 100644 --- a/src/AutonomousDriving_RL/eval_agent.py +++ b/src/AutonomousDriving_RL/eval_agent.py @@ -1,46 +1,92 @@ # eval_agent.py """ -加载训练好的模型,在 CARLA 中演示智能体驾驶行为 +CARLA 智能体评估脚本(4D 环境专用) +- 不使用 Monitor / DummyVecEnv +- 支持多轮评估、轨迹分离、种子复现 +- 输出平均奖励与标准差 """ import argparse +import os import numpy as np from stable_baselines3 import PPO from carla_env.carla_env_multi_obs import CarlaEnvMultiObs def main(): - parser = argparse.ArgumentParser() - parser.add_argument("--model_path", type=str, default="./checkpoints/best_model.zip", help="模型路径") - parser.add_argument("--steps", type=int, default=200, help="演示步数") + 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") args = parser.parse_args() - print("🔄 加载环境与模型...") - env = CarlaEnvMultiObs(keep_alive_after_exit=True) # 保留车辆便于观察 - model = PPO.load(args.model_path, env=env) + if not os.path.exists(args.model_path): + raise FileNotFoundError(f"❌ Model file not found: {args.model_path}") - print("▶️ 开始驾驶演示(运行 {} 步)...".format(args.steps)) - obs, _ = env.reset() - total_reward = 0.0 + print(f"🎯 Loading model from: {args.model_path}") + model = PPO.load(args.model_path) # 注意:不传 env! - for step in range(args.steps): - action, _ = model.predict(obs, deterministic=True) - obs, reward, terminated, truncated, info = env.step(action) - total_reward += reward + rewards = [] + for ep in range(args.episodes): + print(f"\n▶️ Starting evaluation episode {ep + 1}/{args.episodes} ...") - # 每 50 步打印状态 - if step % 50 == 0 or step == args.steps - 1: - x, y, vx, vy = obs - speed = np.linalg.norm([vx, vy]) - print(f" Step {step}: 位置=({x:.1f}, {y:.1f}), 速度={speed:.2f} m/s") + # 创建独立环境实例(每轮新环境) + 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 + ) - if terminated or truncated: - break + try: + obs, _ = env.reset(seed=args.seed + ep) + total_reward = 0.0 - print(f"✅ 演示完成!总奖励: {total_reward:.2f}") - input("🛑 准备好后,请回到本窗口按 Enter 键退出...") - env.close() + for step in range(args.steps_per_ep): + 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}, ...") if __name__ == "__main__": - main() \ No newline at end of file + main() From 1fbc1a53994eb10e9c4e9e4bfb32a52e14c1bb55 Mon Sep 17 00:00:00 2001 From: wuuu0236 <2416234104@qq.com> Date: Sat, 20 Dec 2025 21:34:20 +0800 Subject: [PATCH 5/8] Update carla_env_multi_obs.py --- .../carla_env_multi_obs.py | 42 +++++++++++++++++++ 1 file changed, 42 insertions(+) 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: From d9ecfaaaa8732e7a4b2a65f7058f086b9b93cd84 Mon Sep 17 00:00:00 2001 From: wuuu0236 <2416234104@qq.com> Date: Sat, 20 Dec 2025 21:34:42 +0800 Subject: [PATCH 6/8] Update eval_agent.py --- src/AutonomousDriving_RL/eval_agent.py | 229 +++++++++++++++++-------- 1 file changed, 157 insertions(+), 72 deletions(-) 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__": From 4a5d4c1f3860c72702f6238508b4c1ee1241351a Mon Sep 17 00:00:00 2001 From: wuuu0236 <2416234104@qq.com> Date: Sun, 21 Dec 2025 20:46:07 +0800 Subject: [PATCH 7/8] Update eval_agent.py --- src/AutonomousDriving_RL/eval_agent.py | 212 ++++++++++++------------- 1 file changed, 105 insertions(+), 107 deletions(-) diff --git a/src/AutonomousDriving_RL/eval_agent.py b/src/AutonomousDriving_RL/eval_agent.py index e90a6e9545..8492afb7ee 100644 --- a/src/AutonomousDriving_RL/eval_agent.py +++ b/src/AutonomousDriving_RL/eval_agent.py @@ -1,9 +1,9 @@ # eval_agent.py """ -评估已训练的 PPO 自动驾驶智能体 -支持两种模式: -1. 默认:沿车道中心自动前进(使用 get_forward_waypoint) -2. 指定目标点:导航到 (target_x, target_y) +增强版 CARLA 智能体评估器 +- 支持单目标 / 多目标导航 +- 路径可视化 + 平滑转向 + 动态调速 +- 无需重新训练模型 """ import argparse @@ -11,166 +11,164 @@ import carla from stable_baselines3 import PPO from carla_env.carla_env_multi_obs import CarlaEnvMultiObs +import time + + +def draw_path(world, points, life_time=60.0): + """在 CARLA 中绘制路径(绿色线)""" + for i in range(len(points) - 1): + world.debug.draw_line( + points[i], + points[i + 1], + thickness=0.1, + color=carla.Color(0, 255, 0), + life_time=life_time + ) + + +def parse_targets(target_str): + """解析目标点字符串: 'x1,y1;x2,y2;...' → [Location(...), ...]""" + if not target_str: + return [] + targets = [] + for pair in target_str.split(";"): + x, y = map(float, pair.split(",")) + targets.append(carla.Location(x=x, y=y, z=0.0)) + return targets def main(): - 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" - ) + parser = argparse.ArgumentParser(description="增强版 CARLA 导航评估") + parser.add_argument("--model_path", type=str, default="./checkpoints/best_model.zip") + parser.add_argument("--steps", type=int, default=1000) + parser.add_argument("--targets", type=str, default=None, + help='目标点序列,格式: "x1,y1;x2,y2;..."') + parser.add_argument("--target_x", type=float, default=None) + parser.add_argument("--target_y", type=float, default=None) + parser.add_argument("--waypoint_dist", type=float, default=4.0) + parser.add_argument("--steer_gain", type=float, default=1.8, help="转向增益") + parser.add_argument("--arrival_radius", type=float, default=1.0, help="到达判定半径(米)") + parser.add_argument("--visualize_path", action="store_true", help="在CARLA中绘制路径") args = parser.parse_args() - print("🚀 正在启动评估环境...") - print("💡 请确保 CARLA 仿真器(CarlaUE4.exe)已在后台运行!\n") + # 解析目标点 + targets = parse_targets(args.targets) + if args.target_x is not None and args.target_y is not None: + targets.insert(0, carla.Location(x=args.target_x, y=args.target_y, z=0.0)) + print("🚀 启动增强版导航评估器...") + print(f"🎯 目标点数量: {len(targets)}") + if targets: + for i, t in enumerate(targets): + print(f" {i + 1}. ({t.x:.1f}, {t.y:.1f})") + + env = None 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 + current_target_idx = 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("🛣️ 未指定目标点,将沿车道自动前进...") + # 可视化路径 + if args.visualize_path and targets: + draw_path(env.world, [env.vehicle.get_location()] + targets, life_time=120.0) 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 + print("⚠️ 车辆丢失,尝试重置...") + obs, _ = env.reset() + continue + + # 获取当前目标 + current_target = None + if targets: + if current_target_idx < len(targets): + current_target = targets[current_target_idx] + dist_to_target = vehicle_tf.location.distance(current_target) + if dist_to_target < args.arrival_radius: + print(f"✅ 到达第 {current_target_idx + 1} 个目标点!") + current_target_idx += 1 + if current_target_idx >= len(targets): + print("🏁 所有目标点已到达!") + break + else: + break # 所有目标完成 - if global_target is not None: - # --- 模式1:朝向全局目标点 --- + # 计算局部目标点 + if current_target: to_target = np.array([ - global_target.x - vehicle_tf.location.x, - global_target.y - vehicle_tf.location.y + current_target.x - vehicle_tf.location.x, + current_target.y - vehicle_tf.location.y ]) - dist_to_target = np.linalg.norm(to_target) - - if dist_to_target < 1.0: - print("🏁 已到达目标点!") - break - - # 计算单位方向向量 - direction = to_target / (dist_to_target + 1e-6) + direction = to_target / (np.linalg.norm(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: - # 计算期望转向角(基于局部目标) + # 计算转向 + steer = 0.0 + if local_target and vehicle_tf: 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: - # 计算夹角(使用叉积判断左右) + if norm_fw > 1e-3 and norm_wp > 1e-3: 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, π] - - # 叉积符号决定转向方向 + angle = np.arccos(np.clip(cos_angle, -1.0, 1.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) # 比例增益可调 + steer = np.clip(angle * np.sign(cross) * args.steer_gain, -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)) + # 动态调速:弯道或接近目标时减速 + 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) + if current_target: + dist = vehicle_tf.location.distance(current_target) + if dist < 5.0: # 接近目标 + throttle *= (dist / 5.0) # 线性减速 + if abs(steer) > 0.6: # 急转弯 + throttle *= 0.7 - # 执行动作 + action = np.array([throttle, steer, brake]) obs, reward, terminated, truncated, info = env.step(action) total_reward += reward - # 定期打印状态 - if step % 50 == 0 or step == args.steps - 1: + # 日志 + if step % 50 == 0: 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") + target_info = f" → 目标{current_target_idx + 1}" if current_target else "" + print(f" Step {step:4d}: ({x:6.1f}, {y:6.1f}) @ {speed:4.1f} m/s{target_info}") - # 终止条件 if terminated or truncated: reason = "碰撞" if terminated else "超时" - print(f"⏹️ 演示结束(原因: {reason})") + print(f"⏹️ 终止: {reason}") break - print(f"\n✅ 演示完成!总奖励: {total_reward:.2f}") - print("ℹ️ 车辆已保留在 CARLA 中,可自由观察。") - input("\n🛑 按 Enter 键退出并销毁车辆...") + print(f"\n✅ 演示结束 | 总奖励: {total_reward:.2f}") + input("\n🛑 按 Enter 退出...") + except KeyboardInterrupt: + print("\n⚠️ 用户中断") except Exception as e: - print(f"\n❌ 运行出错: {e}") + print(f"\n❌ 错误: {e}") import traceback traceback.print_exc() finally: - try: + if env: env.close() - except: - pass if __name__ == "__main__": From 796e30c876474381a6c412bc2416819b61881a6e Mon Sep 17 00:00:00 2001 From: wuuu0236 <2416234104@qq.com> Date: Mon, 22 Dec 2025 23:24:31 +0800 Subject: [PATCH 8/8] Add files via upload --- .../catkin_ws/src/carla_rl_ros/CMakeLists.txt | 14 + .../src/carla_rl_ros/launch/eval.launch | 6 + .../wu/catkin_ws/src/carla_rl_ros/package.xml | 11 + .../scripts/carla_env_multi_obs.py | 354 ++++++++++++++++++ .../src/carla_rl_ros/scripts/eval_agent.py | 175 +++++++++ .../src/carla_rl_ros/scripts/ros_eval_node.py | 79 ++++ .../carla_rl_ros/scripts/ros_train_node.py | 43 +++ .../src/carla_rl_ros/scripts/train_agent.py | 59 +++ 8 files changed, 741 insertions(+) create mode 100644 src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/CMakeLists.txt create mode 100644 src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/launch/eval.launch create mode 100644 src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/package.xml create mode 100644 src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/carla_env_multi_obs.py create mode 100644 src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/eval_agent.py create mode 100644 src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/ros_eval_node.py create mode 100644 src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/ros_train_node.py create mode 100644 src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/train_agent.py diff --git a/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/CMakeLists.txt b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/CMakeLists.txt new file mode 100644 index 0000000000..3d8b02050b --- /dev/null +++ b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.0.2) +project(carla_rl_ros) + +find_package(catkin REQUIRED COMPONENTS + rospy +) + +catkin_package() + +catkin_install_python(PROGRAMS + scripts/ros_eval_node.py + scripts/ros_train_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/launch/eval.launch b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/launch/eval.launch new file mode 100644 index 0000000000..0563fcb1ca --- /dev/null +++ b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/launch/eval.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/package.xml b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/package.xml new file mode 100644 index 0000000000..10c6f3ef58 --- /dev/null +++ b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/package.xml @@ -0,0 +1,11 @@ + + + carla_rl_ros + 0.1.0 + ROS wrapper for CARLA RL (0.9.14) + Your Name + MIT + + catkin + rospy + diff --git a/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/carla_env_multi_obs.py b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/carla_env_multi_obs.py new file mode 100644 index 0000000000..8405f7dbf4 --- /dev/null +++ b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/carla_env_multi_obs.py @@ -0,0 +1,354 @@ +# carla_env/carla_env_multi_obs.py +""" +CARLA 强化学习环境(4D 观测增强版) +- 观测: [x, y, vx, vy] +- 动作: [throttle, steer, brake] +- 新增: 车道保持奖励、合理速度区间、轨迹日志、参数化配置、抗崩溃机制 +- 【本次更新】新增 get_forward_waypoint() 用于高层导航 +""" + +import carla +import numpy as np +import random +import time +import os +import json +from gymnasium import Env, spaces + +VEHICLE_ID_FILE = ".last_vehicle_id.json" + + +class CarlaEnvMultiObs(Env): + def __init__( + self, + keep_alive_after_exit=False, + log_trajectory=True, + trajectory_log_file="trajectory.csv", + target_speed=8.0, # 目标速度 (m/s) + max_episode_steps=1000, # 最大步数 + map_name=None, # 指定地图(如 'Town10HD') + spawn_point_index=0, # spawn 点索引 + random_spawn=False, # 是否随机 spawn + reward_weights=None # 奖励权重配置 + ): + super().__init__() + self.client = None + self.world = None + self.vehicle = None + self._current_vehicle_id = None + self.frame_count = 0 + self.max_episode_steps = max_episode_steps + self.spectator = None + self.keep_alive = keep_alive_after_exit + self.log_trajectory = log_trajectory + self.trajectory_log_file = trajectory_log_file + self.trajectory_data = [] + self._collision_sensor = None + self._collision_hist = [] + + # 奖励配置 + self.target_speed = target_speed + self.reward_weights = { + 'forward': 0.1, + 'speed_match': 0.5, + 'lane_center': 1.0, + 'steer_smooth': 0.05, + 'brake_penalty': 0.1, + 'collision': -50.0, + 'time_bonus': 0.01 + } + if reward_weights: + self.reward_weights.update(reward_weights) + + # 地图与 spawn 配置 + self.map_name = map_name + self.spawn_point_index = spawn_point_index + self.random_spawn = random_spawn + + # 固定 4D 观测空间 + self.observation_space = spaces.Box( + low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32 + ) + self.action_space = spaces.Box( + low=np.array([0.0, -1.0, 0.0]), + high=np.array([1.0, 1.0, 1.0]), + dtype=np.float32 + ) + + def _connect_carla(self, max_retries=3, timeout=10.0): + for attempt in range(max_retries): + try: + print(f"🔄 尝试连接 CARLA 服务器 (第 {attempt + 1} 次)...") + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(timeout) + self.world = self.client.get_world() + if self.map_name and self.map_name not in self.world.get_map().name: + print(f"🔄 加载指定地图: {self.map_name}") + self.world = self.client.load_world(self.map_name) + print(f"✅ 成功连接!地图: {self.world.get_map().name}") + return True + except Exception as e: + print(f"⚠️ 连接失败: {e}") + time.sleep(2) + raise RuntimeError("❌ 无法连接 CARLA,请确保已启动 `CarlaUE4.sh`") + + def reset(self, seed=None, options=None): + super().reset(seed=seed) + if seed is not None: + random.seed(seed) + np.random.seed(seed) + + self._connect_carla() + self._destroy_last_run_vehicle() + self.spawn_vehicle() + + # 初始化传感器 + self._collision_hist.clear() + if self._collision_sensor: + self._collision_sensor.destroy() + bp = self.world.get_blueprint_library().find('sensor.other.collision') + self._collision_sensor = self.world.spawn_actor(bp, carla.Transform(), attach_to=self.vehicle) + self._collision_sensor.listen(lambda event: self._collision_hist.append(event)) + + # 稳定物理 + for _ in range(5): + self.world.tick() + time.sleep(0.05) + + # 设置视角 + self.spectator = self.world.get_spectator() + self._update_spectator_view() + + # 重置状态 + self.trajectory_data = [] + self.frame_count = 0 + + obs = self.get_observation() + return obs, {} + + def _destroy_last_run_vehicle(self): + if not os.path.exists(VEHICLE_ID_FILE): + return + try: + with open(VEHICLE_ID_FILE, 'r') as f: + data = json.load(f) + last_id = data.get("vehicle_id") + if isinstance(last_id, int): + self.client.apply_batch_sync([carla.command.DestroyActor(last_id)], do_tick=True) + except Exception: + pass + try: + os.remove(VEHICLE_ID_FILE) + except OSError: + pass + + def spawn_vehicle(self): + blueprint_library = self.world.get_blueprint_library() + vehicle_bp = blueprint_library.find('vehicle.tesla.model3') + if not vehicle_bp or not vehicle_bp.has_attribute('number_of_wheels'): + vehicle_bp = random.choice(blueprint_library.filter('vehicle.*')) + if vehicle_bp.has_attribute('color'): + color = random.choice(vehicle_bp.get_attribute('color').recommended_values) + vehicle_bp.set_attribute('color', color) + + spawn_points = self.world.get_map().get_spawn_points() + if not spawn_points: + raise RuntimeError("❌ 地图无可用 spawn 点!") + + if self.random_spawn: + spawn_transform = random.choice(spawn_points) + elif self.spawn_point_index < len(spawn_points): + spawn_transform = spawn_points[self.spawn_point_index] + else: + spawn_transform = spawn_points[0] + + # 尝试生成 + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_transform) + if self.vehicle is None: + # 备用:抬高 Z 轴 + sp = spawn_transform + safe_sp = carla.Transform( + carla.Location(x=sp.location.x, y=sp.location.y, z=max(sp.location.z, 0.0) + 0.5), + sp.rotation + ) + self.vehicle = self.world.try_spawn_actor(vehicle_bp, safe_sp) + + if self.vehicle is None: + raise RuntimeError("❌ 所有 spawn 方式均失败!") + + self._current_vehicle_id = self.vehicle.id + loc = self.vehicle.get_location() + print(f"✅ 车辆生成成功 | ID={self._current_vehicle_id} | ({loc.x:.1f}, {loc.y:.1f})") + + try: + with open(VEHICLE_ID_FILE, 'w') as f: + json.dump({"vehicle_id": self._current_vehicle_id}, f) + except Exception as e: + print(f"⚠️ 保存车辆ID失败: {e}") + + def _update_spectator_view(self): + if not (self.vehicle and self.spectator): + return + try: + v_transform = self.vehicle.get_transform() + offset = carla.Location(x=-6.0, y=0.0, z=2.5) + camera_loc = v_transform.transform(offset) + rot = carla.Rotation(pitch=-15.0, yaw=v_transform.rotation.yaw) + self.spectator.set_transform(carla.Transform(camera_loc, rot)) + except Exception: + pass + + def get_observation(self): + if not self.vehicle or not self.vehicle.is_alive: + return np.zeros(4, dtype=np.float32) + loc = self.vehicle.get_location() + vel = self.vehicle.get_velocity() + # 防止 NaN + x = float(loc.x) if np.isfinite(loc.x) else 0.0 + y = float(loc.y) if np.isfinite(loc.y) else 0.0 + vx = float(vel.x) if np.isfinite(vel.x) else 0.0 + vy = float(vel.y) if np.isfinite(vel.y) else 0.0 + return np.array([x, y, vx, vy], dtype=np.float32) + + def _get_lane_offset(self): + """计算到最近车道中心的距离(仅用于奖励,不放入 obs)""" + try: + waypoint = self.world.get_map().get_waypoint( + self.vehicle.get_location(), project_to_road=True + ) + return self.vehicle.get_location().distance(waypoint.transform.location) + except: + return 5.0 # 默认远离车道 + + def _compute_reward(self, speed, lane_offset, action): + w = self.reward_weights + reward = 0.0 + + # 前进奖励 + if speed > 0.1: + reward += w['forward'] * speed + + # 速度匹配 + speed_diff = abs(speed - self.target_speed) + if speed_diff < 1.0: + reward += w['speed_match'] + else: + reward -= speed_diff * 0.05 + + # 车道中心奖励(即使 4D 也鼓励 stay in lane) + if lane_offset < 1.0: + reward += w['lane_center'] * (1.0 - lane_offset) + else: + reward -= (lane_offset - 1.0) * 0.5 + + # 控制平滑 + _, steer, brake = action + reward -= w['steer_smooth'] * abs(steer) + reward -= w['brake_penalty'] * brake + + # 时间奖励(鼓励存活) + reward += w['time_bonus'] + + return reward + + def step(self, action): + # 安全钳位 + throttle = np.clip(action[0], 0.0, 1.0) + steer = np.clip(action[1], -1.0, 1.0) + brake = np.clip(action[2], 0.0, 1.0) + control = carla.VehicleControl(throttle=float(throttle), steer=float(steer), brake=float(brake)) + self.vehicle.apply_control(control) + self.world.tick() + self.frame_count += 1 + self._update_spectator_view() + + # 车辆死亡 + if not self.vehicle or not self.vehicle.is_alive: + obs = np.zeros(4, dtype=np.float32) + return obs, self.reward_weights['collision'], True, False, {} + + # 状态 + velocity = self.vehicle.get_velocity() + speed = np.sqrt(max(0.0, velocity.x**2 + velocity.y**2)) + lane_offset = self._get_lane_offset() + reward = self._compute_reward(speed, lane_offset, [throttle, steer, brake]) + + # 终止条件 + terminated = len(self._collision_hist) > 0 + if terminated: + reward = self.reward_weights['collision'] + + truncated = self.frame_count >= self.max_episode_steps + + # 记录轨迹 + if self.log_trajectory: + loc = self.vehicle.get_location() + self.trajectory_data.append((float(loc.x), float(loc.y), float(speed))) + + obs = self.get_observation() + return obs, reward, terminated, truncated, { + "speed": speed, + "lane_offset": lane_offset, + "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: + try: + with open(self.trajectory_log_file, 'w') as f: + f.write("x,y,speed\n") + for x, y, speed in self.trajectory_data: + f.write(f"{x:.3f},{y:.3f},{speed:.3f}\n") + print(f"📊 轨迹已保存至: {self.trajectory_log_file}") + except Exception as e: + print(f"⚠️ 轨迹保存失败: {e}") + + # 清理 + if self._collision_sensor: + self._collision_sensor.destroy() + if not self.keep_alive and self.vehicle and self.vehicle.is_alive: + self.vehicle.destroy() + elif self.keep_alive: + print("ℹ️ 车辆已保留(下次运行将自动清理)") \ No newline at end of file diff --git a/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/eval_agent.py b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/eval_agent.py new file mode 100644 index 0000000000..ac73dc7a3d --- /dev/null +++ b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/eval_agent.py @@ -0,0 +1,175 @@ +# eval_agent.py +""" +增强版 CARLA 智能体评估器 +- 支持单目标 / 多目标导航 +- 路径可视化 + 平滑转向 + 动态调速 +- 无需重新训练模型 +""" + +import argparse +import numpy as np +import carla +from stable_baselines3 import PPO +from carla_env_multi_obs import CarlaEnvMultiObs +import time + + +def draw_path(world, points, life_time=60.0): + """在 CARLA 中绘制路径(绿色线)""" + for i in range(len(points) - 1): + world.debug.draw_line( + points[i], + points[i + 1], + thickness=0.1, + color=carla.Color(0, 255, 0), + life_time=life_time + ) + + +def parse_targets(target_str): + """解析目标点字符串: 'x1,y1;x2,y2;...' → [Location(...), ...]""" + if not target_str: + return [] + targets = [] + for pair in target_str.split(";"): + x, y = map(float, pair.split(",")) + targets.append(carla.Location(x=x, y=y, z=0.0)) + return targets + + +def main(): + parser = argparse.ArgumentParser(description="增强版 CARLA 导航评估") + parser.add_argument("--model_path", type=str, default="./checkpoints/best_model.zip") + parser.add_argument("--steps", type=int, default=1000) + parser.add_argument("--targets", type=str, default=None, + help='目标点序列,格式: "x1,y1;x2,y2;..."') + parser.add_argument("--target_x", type=float, default=None) + parser.add_argument("--target_y", type=float, default=None) + parser.add_argument("--waypoint_dist", type=float, default=4.0) + parser.add_argument("--steer_gain", type=float, default=1.8, help="转向增益") + parser.add_argument("--arrival_radius", type=float, default=1.0, help="到达判定半径(米)") + parser.add_argument("--visualize_path", action="store_true", help="在CARLA中绘制路径") + args = parser.parse_args() + + # 解析目标点 + targets = parse_targets(args.targets) + if args.target_x is not None and args.target_y is not None: + targets.insert(0, carla.Location(x=args.target_x, y=args.target_y, z=0.0)) + + print("🚀 启动增强版导航评估器...") + print(f"🎯 目标点数量: {len(targets)}") + if targets: + for i, t in enumerate(targets): + print(f" {i + 1}. ({t.x:.1f}, {t.y:.1f})") + + env = None + try: + env = CarlaEnvMultiObs() + model = PPO.load(args.model_path) + obs, _ = env.reset() + total_reward = 0.0 + current_target_idx = 0 + + # 可视化路径 + if args.visualize_path and targets: + draw_path(env.world, [env.vehicle.get_location()] + targets, life_time=120.0) + + print("\n▶️ 开始驾驶演示...\n") + + for step in range(args.steps): + vehicle_tf = env.get_vehicle_transform() + if vehicle_tf is None: + print("⚠️ 车辆丢失,尝试重置...") + obs, _ = env.reset() + continue + + # 获取当前目标 + current_target = None + if targets: + if current_target_idx < len(targets): + current_target = targets[current_target_idx] + dist_to_target = vehicle_tf.location.distance(current_target) + if dist_to_target < args.arrival_radius: + print(f"✅ 到达第 {current_target_idx + 1} 个目标点!") + current_target_idx += 1 + if current_target_idx >= len(targets): + print("🏁 所有目标点已到达!") + break + else: + break # 所有目标完成 + + # 计算局部目标点 + if current_target: + to_target = np.array([ + current_target.x - vehicle_tf.location.x, + current_target.y - vehicle_tf.location.y + ]) + direction = to_target / (np.linalg.norm(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: + local_target = env.get_forward_waypoint(distance=args.waypoint_dist) + + # 计算转向 + steer = 0.0 + if local_target and vehicle_tf: + 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 and norm_wp > 1e-3: + cos_angle = (forward.x * to_waypoint[0] + forward.y * to_waypoint[1]) / (norm_fw * norm_wp) + angle = np.arccos(np.clip(cos_angle, -1.0, 1.0)) + cross = forward.x * to_waypoint[1] - forward.y * to_waypoint[0] + steer = np.clip(angle * np.sign(cross) * args.steer_gain, -1.0, 1.0) + + # 动态调速:弯道或接近目标时减速 + 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)) + + if current_target: + dist = vehicle_tf.location.distance(current_target) + if dist < 5.0: # 接近目标 + throttle *= (dist / 5.0) # 线性减速 + if abs(steer) > 0.6: # 急转弯 + throttle *= 0.7 + + action = np.array([throttle, steer, brake]) + obs, reward, terminated, truncated, info = env.step(action) + total_reward += reward + + # 日志 + if step % 50 == 0: + x, y, vx, vy = obs + speed = np.linalg.norm([vx, vy]) + target_info = f" → 目标{current_target_idx + 1}" if current_target else "" + print(f" Step {step:4d}: ({x:6.1f}, {y:6.1f}) @ {speed:4.1f} m/s{target_info}") + + if terminated or truncated: + reason = "碰撞" if terminated else "超时" + print(f"⏹️ 终止: {reason}") + break + + print(f"\n✅ 演示结束 | 总奖励: {total_reward:.2f}") + input("\n🛑 按 Enter 退出...") + + except KeyboardInterrupt: + print("\n⚠️ 用户中断") + except Exception as e: + print(f"\n❌ 错误: {e}") + import traceback + traceback.print_exc() + finally: + if env: + env.close() + + +if __name__ == "__main__": + main() diff --git a/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/ros_eval_node.py b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/ros_eval_node.py new file mode 100644 index 0000000000..11327b42ed --- /dev/null +++ b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/ros_eval_node.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 +""" +ROS wrapper for eval_agent.py +Patches CarlaEnv to support remote CARLA server via ROS params. +""" + +import rospy +import sys +import os +import carla +# 添加当前目录到 Python 路径 +sys.path.append(os.path.dirname(__file__)) + +# Patch CarlaEnv to accept remote CARLA host +import carla_env_multi_obs + +_original_init = carla_env_multi_obs.CarlaEnvMultiObs.__init__ + +def _patched_init(self): + carla_host = rospy.get_param('~carla_host', 'localhost') + carla_port = rospy.get_param('~carla_port', 2000) + for attempt in range(3): + try: + rospy.loginfo(f"Connecting to CARLA at {carla_host}:{carla_port}") + self.client = carla.Client(carla_host, carla_port) + self.client.set_timeout(10.0) + self.world = self.client.get_world() + break + except Exception as e: + rospy.logwarn(f"Connection failed: {e}") + if attempt == 2: + raise Exception("Failed to connect to CARLA after 3 attempts") + + # 调用原始 __init__ 完成其余初始化 + _original_init(self) + +# 替换原方法 +carla_env_multi_obs.CarlaEnvMultiObs.__init__ = _patched_init + +if __name__ == '__main__': + rospy.init_node('carla_rl_eval', anonymous=True) + + # 读取 ROS 参数 + model_path = rospy.get_param('~model_path', './checkpoints/best_model.zip') + steps = rospy.get_param('~steps', 1000) + targets_str = rospy.get_param('~targets', '') + target_x = rospy.get_param('~target_x', None) + target_y = rospy.get_param('~target_y', None) + waypoint_dist = rospy.get_param('~waypoint_dist', 4.0) + steer_gain = rospy.get_param('~steer_gain', 1.8) + arrival_radius = rospy.get_param('~arrival_radius', 1.0) + visualize_path = rospy.get_param('~visualize_path', False) + + # 构造命令行参数 + argv = ['ros_eval_node.py', '--model_path', model_path, '--steps', str(steps)] + if targets_str: + argv.extend(['--targets', targets_str]) + if target_x is not None and target_y is not None: + argv.extend(['--target_x', str(target_x), '--target_y', str(target_y)]) + argv.extend([ + '--waypoint_dist', str(waypoint_dist), + '--steer_gain', str(steer_gain), + '--arrival_radius', str(arrival_radius) + ]) + if visualize_path: + argv.append('--visualize_path') + + # 临时替换 sys.argv 并运行 main + original_argv = sys.argv + sys.argv = argv + + try: + from eval_agent import main as eval_main + eval_main() + except Exception as e: + rospy.logerr(f"Error in evaluation: {e}") + raise + finally: + sys.argv = original_argv diff --git a/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/ros_train_node.py b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/ros_train_node.py new file mode 100644 index 0000000000..d1ce3ac128 --- /dev/null +++ b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/ros_train_node.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +import rospy +import sys +import os +import time +sys.path.append(os.path.dirname(__file__)) + +import carla_env_multi_obs + +_original_init = carla_env_multi_obs.CarlaEnvMultiObs.__init__ + +def _patched_init(self): + carla_host = rospy.get_param('~carla_host', 'localhost') + carla_port = rospy.get_param('~carla_port', 2000) + for attempt in range(3): + try: + rospy.loginfo(f"🔄 Connecting to CARLA at {carla_host}:{carla_port}") + self.client = carla.Client(carla_host, carla_port) + self.client.set_timeout(10.0) + self.world = self.client.get_world() + break + except Exception as e: + rospy.logwarn(f"⚠️ Retry {attempt+1}: {e}") + time.sleep(2) + else: + raise RuntimeError("❌ CARLA connection failed") + _original_init(self) + +carla_env_multi_obs.CarlaEnvMultiObs.__init__ = _patched_init + +from train_agent import main as train_main + +if __name__ == '__main__': + rospy.init_node('carla_rl_train', anonymous=True) + rospy.loginfo("🚀 Starting training via ROS...") + try: + train_main() + except KeyboardInterrupt: + rospy.loginfo("Training stopped by user.") + except Exception as e: + rospy.logerr(f"Training error: {e}") + raise + diff --git a/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/train_agent.py b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/train_agent.py new file mode 100644 index 0000000000..f56abf0a76 --- /dev/null +++ b/src/AutonomousDriving_RL/home/wu/catkin_ws/src/carla_rl_ros/scripts/train_agent.py @@ -0,0 +1,59 @@ +# train_ppo.py +""" +使用新奖励函数训练 PPO 模型 +""" + +import os +from stable_baselines3 import PPO +from stable_baselines3.common.callbacks import CheckpointCallback +from carla_env.carla_env_multi_obs import CarlaEnvMultiObs + +if __name__ == "__main__": + # 创建环境 + env = CarlaEnvMultiObs( + random_spawn=True, + max_episode_steps=1000, + debug=False + ) + + # 日志和模型保存路径 + log_dir = "./logs/" + checkpoint_dir = "./checkpoints/" + os.makedirs(log_dir, exist_ok=True) + os.makedirs(checkpoint_dir, exist_ok=True) + + # 回调:每 10k 步保存一次 + checkpoint_callback = CheckpointCallback( + save_freq=10000, + save_path=checkpoint_dir, + name_prefix="ppo_carla" + ) + + # 创建 PPO 模型 + model = PPO( + "MlpPolicy", + env, + verbose=1, + tensorboard_log=log_dir, + learning_rate=3e-4, + n_steps=2048, + batch_size=64, + n_epochs=10, + gamma=0.99, + gae_lambda=0.95, + clip_range=0.2, + ent_coef=0.01 + ) + + # 开始训练 + print("🚀 开始训练 PPO 模型...") + model.learn( + total_timesteps=500_000, + callback=checkpoint_callback, + tb_log_name="ppo_run" + ) + + # 保存最终模型 + model.save(os.path.join(checkpoint_dir, "best_model.zip")) + print("✅ 训练完成,模型已保存至 ./checkpoints/best_model.zip") + env.close() \ No newline at end of file