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