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