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