From cfdc7b48181a5dc9636812513de4a61a7e8614e0 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Thu, 18 Dec 2025 19:53:52 +0800 Subject: [PATCH 1/4] updata --- src/MMAP-DRL-Nav/carla_environment.py | 122 ++++++++++++++++++++++++-- 1 file changed, 113 insertions(+), 9 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index 6e218282e5..cbd7071750 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -40,6 +40,7 @@ def is_port_used(port): else: raise RuntimeError("CARLA连接超时(3次重试失败),请检查模拟器是否启动") + # ========== 修复核心错误:从World获取蓝图库(而非Map) ========== self.blueprint_library = self.world.get_blueprint_library() # ========== 同步模式配置 ========== @@ -123,6 +124,20 @@ def is_port_used(port): self.max_steps = 800 # 单回合最大步数(调整为800) self.current_step = 0 # 当前回合已执行步数 + # ========== 新增:档位检测核心配置 ========== + self.gear_config = { + # 车速区间(km/h) : 推荐档位 + 0: 1, # 0-10km/h → 1挡 + 10: 2, # 10-20km/h → 2挡 + 20: 3, # 20-30km/h → 3挡 + 30: 4, # 30-40km/h → 4挡 + 40: 5 # ≥40km/h → 5挡 + } + self.gear_correct_reward = 0.1 # 档位正确加分 + self.last_speed_range = 0 # 记录上一次的车速区间(用于判断是否跨档位) + self.last_gear = 1 # 记录上一步档位 + self.is_initial_gear_logged = False # 标记初始档位日志是否已输出 + # 基础配置(完全保留) self.spawn_retry_times = 20 self.spawn_safe_radius = 2.0 @@ -146,6 +161,62 @@ def is_port_used(port): self.view_distance = 8.0 self.z_offset = 0.5 + # ========== 新增:档位奖励计算(初始输出一次 + 跨区间输出) ========== + def _calculate_gear_reward(self): + if not self.vehicle or not self.vehicle.is_alive: + return 0.0 + + # 1. 计算当前车速(m/s → km/h) + velocity = self.vehicle.get_velocity() + speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + speed_kmh = round(speed_m_s * 3.6, 1) + + # 2. 判断当前车速区间 + current_speed_range = 0 + if speed_kmh < 0: # 倒车 + current_speed_range = -1 + recommended_gear = 'R' + else: + # 匹配前进车速区间 + for range_val in sorted(self.gear_config.keys(), reverse=True): + if speed_kmh >= range_val: + current_speed_range = range_val + recommended_gear = self.gear_config[range_val] + break + + # 3. 获取当前档位 + control = self.vehicle.get_control() + current_gear = control.gear + if control.reverse or speed_kmh < 0: # 倒车挡 + current_gear = 'R' + + gear_reward = 0.0 + # 4. 初始启动时输出一次(仅首次) + if not self.is_initial_gear_logged: + is_correct = (current_gear == recommended_gear) + if is_correct: + gear_reward = self.gear_correct_reward + if speed_kmh < 0: + print(f"初始倒车档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前R挡)") + else: + print(f"初始档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前{current_gear}挡,对应{current_speed_range}-{current_speed_range+10}km/h区间)") + self.is_initial_gear_logged = True # 标记初始日志已输出 + # 5. 跨档位区间时输出(初始之后) + elif current_speed_range != self.last_speed_range: + # 档位正确判断 + is_correct = (current_gear == recommended_gear) + if is_correct: + gear_reward = self.gear_correct_reward + # 仅档位正确时输出日志 + if speed_kmh < 0: + print(f"倒车档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前R挡)") + else: + print(f"车速进入{current_speed_range}-{current_speed_range+10}km/h区间,档位{current_gear}正确加分:{self.gear_correct_reward}") + # 更新上次车速区间 + self.last_speed_range = current_speed_range + + return gear_reward + # ========== 天气切换:输出中文名称 ========== def switch_weather(self): available_weathers = [w for w in self.preset_weathers if w != self.current_weather] @@ -325,7 +396,7 @@ def _spawn_small_npc(self): p.destroy() self.npc_pedestrian_list.clear() - vehicle_bps = self.blueprint_library.filter('vehicle.*') + vehicle_bps = self.world.get_blueprint_library().filter('vehicle.*') spawn_points = self.world.get_map().get_spawn_points() if len(spawn_points) < 50: @@ -355,7 +426,7 @@ def _spawn_small_npc(self): except: continue - pedestrian_bps = self.blueprint_library.filter('walker.pedestrian.*') + pedestrian_bps = self.world.get_blueprint_library().filter('walker.pedestrian.*') for _ in range(5): if self.vehicle is not None: hero_loc = self.vehicle.get_transform().location @@ -375,7 +446,7 @@ def _spawn_small_npc(self): # ========== 初始化碰撞传感器(完全保留原始代码) ========== def _init_collision_sensor(self): - collision_bp = self.blueprint_library.find('sensor.other.collision') + collision_bp = self.world.get_blueprint_library().find('sensor.other.collision') collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) self.collision_sensor = self.world.spawn_actor( collision_bp, collision_transform, attach_to=self.vehicle @@ -396,7 +467,7 @@ def _collision_callback(self, event): # ========== 初始化相机(完全保留原始代码) ========== def _init_camera(self): - camera_bp = self.blueprint_library.find('sensor.camera.rgb') + camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '128') camera_bp.set_attribute('image_size_y', '128') camera_bp.set_attribute('fov', '90') @@ -431,12 +502,17 @@ def reset(self): # ========== 新增:重置步数 ========== self.current_step = 0 + + # ========== 新增:重置档位相关状态 ========== + self.last_speed_range = 0 + self.last_gear = 1 + self.is_initial_gear_logged = False # 重置初始档位日志标记 # ========== 切换天气(会自动输出中文名称) ========== self.switch_weather() # 生成车辆 - vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0] + vehicle_bp = self.world.get_blueprint_library().filter('vehicle.tesla.model3')[0] self.vehicle = self._spawn_vehicle_safely(vehicle_bp) # ========== 新增:生成导航目标点 ========== @@ -486,7 +562,7 @@ def _camera_callback(self, image): array = np.frombuffer(image.raw_data, dtype=np.uint8) self.image_data = array.reshape((image.height, image.width, 4))[:, :, :3] - # ========== 核心:奖励函数(集成导航+步数限制) ========== + # ========== 核心:奖励函数(集成导航+步数限制 + 新增档位奖励) ========== def step(self, action): if self.vehicle is None or not self.vehicle.is_alive: raise RuntimeError("车辆未初始化/已销毁,请先调用reset()") @@ -508,12 +584,35 @@ def step(self, action): elif action == 3: throttle = -0.2 + # ========== 优化:根据车速自动匹配合理档位 ========== + # 计算当前车速 + velocity = self.vehicle.get_velocity() + speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + current_speed = speed_m_s * 3.6 + + # 匹配档位 + gear = 1 + if throttle < 0: # 倒车 + gear = -1 + else: + # 根据车速匹配前进档位 + if 0 <= current_speed < 10: + gear = 1 + elif 10 <= current_speed < 20: + gear = 2 + elif 20 <= current_speed < 30: + gear = 3 + elif 30 <= current_speed < 40: + gear = 4 + else: + gear = 5 + self.vehicle.apply_control(carla.VehicleControl( throttle=throttle, steer=steer, hand_brake=False, reverse=(throttle < 0), - gear=1, + gear=gear, manual_gear_shift=True )) @@ -526,6 +625,10 @@ def step(self, action): over_speed_reward = self._check_over_speed() lane_reward = self._check_lane_offset() collision_reward = 0.0 + + # ========== 新增:计算档位奖励 ========== + gear_reward = self._calculate_gear_reward() + done = False if self.has_collision and not self.collision_penalty_applied: @@ -565,8 +668,8 @@ def step(self, action): print(f"达到最大步数{self.max_steps},终止训练(当前距离目标点:{dist_to_target:.2f}米)") done = True - # 总奖励 - total_reward = base_reward + traffic_light_reward + over_speed_reward + lane_reward + collision_reward + nav_reward + # 总奖励(新增档位奖励) + total_reward = base_reward + traffic_light_reward + over_speed_reward + lane_reward + collision_reward + nav_reward + gear_reward next_state = self.get_observation() return next_state, total_reward, done, { @@ -576,6 +679,7 @@ def step(self, action): "lane_reward": lane_reward, "collision_reward": collision_reward, "nav_reward": nav_reward, + "gear_reward": gear_reward, # 新增:返回档位奖励 "total_reward": total_reward, "current_step": self.current_step, "dist_to_target": dist_to_target, From dd1cacd65a37e40a879e58b62c0087af4f05ed6d Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Thu, 18 Dec 2025 21:05:29 +0800 Subject: [PATCH 2/4] updata --- src/MMAP-DRL-Nav/carla_environment.py | 224 ++++++++++++++------------ 1 file changed, 117 insertions(+), 107 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index cbd7071750..6539d17b35 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -95,103 +95,147 @@ def is_port_used(port): self.collision_pedestrian_penalty = -100.0 # 碰撞行人惩罚 self.collision_static_penalty = -15.0 # 碰撞静态物体惩罚 - # ========== 天气系统:新增中文名称映射 + 移除冗余日志 ========== + # ========== 天气系统:仅保留白天/夜晚切换 ========== self.weather_mode = "dynamic" - # 核心修改:天气枚举值 → 中文名称映射 + # 简化:只保留白天(ClearNoon)和夜晚(ClearNight)两种天气 self.weather_name_map = { - carla.WeatherParameters.ClearNoon: "晴天", - carla.WeatherParameters.CloudyNoon: "多云", - carla.WeatherParameters.WetNoon: "小雨", - carla.WeatherParameters.WetCloudyNoon: "多云转小雨", - carla.WeatherParameters.MidRainyNoon: "中雨", - carla.WeatherParameters.HardRainNoon: "大雨", - carla.WeatherParameters.SoftRainNoon: "细雨" + "Day": "晴天(白天)", + "Night": "晴天(夜晚)" } - self.preset_weathers = list(self.weather_name_map.keys()) # 从映射中获取枚举值 - self.current_weather = random.choice(self.preset_weathers) - self.world.set_weather(self.current_weather) - # 可选:打印初始天气名称(仅易读名称,无冗余日志) - print(f"初始天气:{self.weather_name_map[self.current_weather]}") - - # ========== 新增:导航系统配置 ========== - self.target_location = None # 导航目标点 - self.target_radius = 5.0 # 到达终点的判定半径(米) - self.nav_reward_per_meter = 0.02 # 每靠近目标点1米的奖励 - self.goal_completion_reward = 100.0 # 到达终点的高额奖励 - self.last_dist_to_target = 0.0 # 上一步与目标点的距离 - - # ========== 新增:步数限制配置 ========== - self.max_steps = 800 # 单回合最大步数(调整为800) - self.current_step = 0 # 当前回合已执行步数 - - # ========== 新增:档位检测核心配置 ========== + # 预设天气列表:仅白天/夜晚 + self.all_weather_ids = ["Day", "Night"] + # 随机选择初始天气 + self.current_weather_id = random.choice(self.all_weather_ids) + self._set_weather_by_id(self.current_weather_id) + print(f"初始天气:{self.weather_name_map[self.current_weather_id]}") + + # ========== 仅保留大灯核心功能:夜晚开灯加分 ========== + self.headlight_required_weathers = ["Night"] # 仅夜晚需要开大灯 + self.headlight_on_reward = 1.0 # 夜晚开灯加分(提高奖励值) + self.headlight_check_cooldown = 2.0 # 检测间隔缩短为2秒 + self.last_headlight_check_time = 0 + self.vehicle_headlights_on = False + + # ========== 其他配置完全保留 ========== + self.target_location = None + self.target_radius = 5.0 + self.nav_reward_per_meter = 0.02 + self.goal_completion_reward = 100.0 + self.last_dist_to_target = 0.0 + + self.max_steps = 800 + self.current_step = 0 + self.gear_config = { - # 车速区间(km/h) : 推荐档位 - 0: 1, # 0-10km/h → 1挡 - 10: 2, # 10-20km/h → 2挡 - 20: 3, # 20-30km/h → 3挡 - 30: 4, # 30-40km/h → 4挡 - 40: 5 # ≥40km/h → 5挡 + 0: 1, + 10: 2, + 20: 3, + 30: 4, + 40: 5 } - self.gear_correct_reward = 0.1 # 档位正确加分 - self.last_speed_range = 0 # 记录上一次的车速区间(用于判断是否跨档位) - self.last_gear = 1 # 记录上一步档位 - self.is_initial_gear_logged = False # 标记初始档位日志是否已输出 + self.gear_correct_reward = 0.1 + self.last_speed_range = 0 + self.last_gear = 1 + self.is_initial_gear_logged = False - # 基础配置(完全保留) self.spawn_retry_times = 20 self.spawn_safe_radius = 2.0 - # 动作/观测空间(完全保留) self.action_space = gym.spaces.Discrete(4) self.observation_space = gym.spaces.Box( low=0, high=255, shape=(128, 128, 3), dtype=np.uint8 ) - # 核心对象(完全保留) self.vehicle = None self.camera = None self.collision_sensor = None self.image_data = None self.has_collision = False - # 视角参数(完全保留) self.view_height = 6.0 self.view_pitch = -15.0 self.view_distance = 8.0 self.z_offset = 0.5 - # ========== 新增:档位奖励计算(初始输出一次 + 跨区间输出) ========== + # ========== 简化:仅实现白天/夜晚天气切换 ========== + def _set_weather_by_id(self, weather_id): + """仅设置白天/夜晚两种天气""" + weather = carla.WeatherParameters() + if weather_id == "Day": + # 白天:标准晴天 + weather.sun_altitude_angle = 90.0 + weather.sun_azimuth_angle = 0.0 + weather.cloudiness = 0.0 + weather.precipitation = 0.0 + weather.brightness = 1.0 + weather.directionald_light_intensity = 1.0 + elif weather_id == "Night": + # 夜晚:极低光照+月光 + weather.sun_altitude_angle = -90.0 # 太阳在地平线下 + weather.moon_altitude_angle = 30.0 + weather.moon_intensity = 1.0 + weather.brightness = 0.1 + weather.directionald_light_intensity = 0.01 + weather.ambient_occlusion_intensity = 0.2 + self.world.set_weather(weather) + + # ========== 简化:仅保留夜晚开灯加分功能 ========== + def _check_headlight_status(self): + current_time = time.time() + if current_time - self.last_headlight_check_time < self.headlight_check_cooldown: + return 0.0 + + self.last_headlight_check_time = current_time + + if not self.vehicle or not self.vehicle.is_alive: + return 0.0 + + # 仅夜晚检测大灯 + need_headlight = self.current_weather_id == "Night" + if not need_headlight: + return 0.0 + + # 获取大灯状态 + try: + vehicle_light_state = self.vehicle.get_light_state() + self.vehicle_headlights_on = vehicle_light_state == carla.VehicleLightState.HeadlightOn + except: + self.vehicle_headlights_on = False + + reward = 0.0 + if need_headlight and self.vehicle_headlights_on: + # 夜晚开灯就加分 + reward = self.headlight_on_reward + print(f"[夜晚] 大灯已开启,加分{self.headlight_on_reward}") + + return reward + + # ========== 以下所有方法完全保留原有逻辑,未做修改 ========== def _calculate_gear_reward(self): if not self.vehicle or not self.vehicle.is_alive: return 0.0 - # 1. 计算当前车速(m/s → km/h) velocity = self.vehicle.get_velocity() speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) speed_kmh = round(speed_m_s * 3.6, 1) - # 2. 判断当前车速区间 current_speed_range = 0 - if speed_kmh < 0: # 倒车 + if speed_kmh < 0: current_speed_range = -1 recommended_gear = 'R' else: - # 匹配前进车速区间 for range_val in sorted(self.gear_config.keys(), reverse=True): if speed_kmh >= range_val: current_speed_range = range_val recommended_gear = self.gear_config[range_val] break - # 3. 获取当前档位 control = self.vehicle.get_control() current_gear = control.gear - if control.reverse or speed_kmh < 0: # 倒车挡 + if control.reverse or speed_kmh < 0: current_gear = 'R' gear_reward = 0.0 - # 4. 初始启动时输出一次(仅首次) if not self.is_initial_gear_logged: is_correct = (current_gear == recommended_gear) if is_correct: @@ -200,35 +244,26 @@ def _calculate_gear_reward(self): print(f"初始倒车档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前R挡)") else: print(f"初始档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前{current_gear}挡,对应{current_speed_range}-{current_speed_range+10}km/h区间)") - self.is_initial_gear_logged = True # 标记初始日志已输出 - # 5. 跨档位区间时输出(初始之后) + self.is_initial_gear_logged = True elif current_speed_range != self.last_speed_range: - # 档位正确判断 is_correct = (current_gear == recommended_gear) if is_correct: gear_reward = self.gear_correct_reward - # 仅档位正确时输出日志 if speed_kmh < 0: print(f"倒车档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前R挡)") else: print(f"车速进入{current_speed_range}-{current_speed_range+10}km/h区间,档位{current_gear}正确加分:{self.gear_correct_reward}") - # 更新上次车速区间 self.last_speed_range = current_speed_range return gear_reward - # ========== 天气切换:输出中文名称 ========== def switch_weather(self): - available_weathers = [w for w in self.preset_weathers if w != self.current_weather] - new_weather = random.choice(available_weathers) - self.world.set_weather(new_weather) - self.current_weather = new_weather - # 输出易读的天气名称(替代原有枚举值日志) - print(f"天气已切换为:{self.weather_name_map[self.current_weather]}") - - # ========== 新增:生成导航目标点 ========== + available_weathers = [w for w in self.all_weather_ids if w != self.current_weather_id] + self.current_weather_id = random.choice(available_weathers) + self._set_weather_by_id(self.current_weather_id) + print(f"天气已切换为:{self.weather_name_map[self.current_weather_id]}") + def _generate_random_target(self): - """随机生成导航目标点(基于地图合法出生点)""" spawn_points = self.world.get_map().get_spawn_points() if not spawn_points: hero_loc = self.vehicle.get_transform().location if self.vehicle else carla.Location(x=0, y=0, z=0.5) @@ -242,7 +277,6 @@ def _generate_random_target(self): self.last_dist_to_target = self.vehicle.get_transform().location.distance(self.target_location) print(f"生成导航目标点:({self.target_location.x:.2f}, {self.target_location.y:.2f}),初始距离:{self.last_dist_to_target:.2f}米") - # ========== 红绿灯判定(完全保留原始代码) ========== def _check_traffic_light(self): current_time = time.time() if current_time - self.last_traffic_light_time < self.traffic_light_cooldown: @@ -293,7 +327,6 @@ def _check_traffic_light(self): return reward - # ========== 超速检测(完全保留原始代码) ========== def _check_over_speed(self): current_time = time.time() if current_time - self.last_over_speed_time < self.over_speed_cooldown: @@ -317,7 +350,6 @@ def _check_over_speed(self): return reward - # ========== 车道偏离检测(完全保留原始代码) ========== def _check_lane_offset(self): current_time = time.time() if current_time - self.last_lane_check_time < self.lane_check_interval: @@ -361,7 +393,6 @@ def _check_lane_offset(self): return reward - # ========== 安全生成车辆(完全保留原始代码) ========== def _spawn_vehicle_safely(self, vehicle_bp): spawn_points = self.world.get_map().get_spawn_points() if not spawn_points: @@ -385,7 +416,6 @@ def _spawn_vehicle_safely(self, vehicle_bp): raise RuntimeError("所有出生点都有碰撞,无法生成车辆!") - # ========== 优化NPC生成(完全保留原始代码) ========== def _spawn_small_npc(self): for v in self.npc_vehicle_list: if v.is_alive: @@ -444,7 +474,6 @@ def _spawn_small_npc(self): except: continue - # ========== 初始化碰撞传感器(完全保留原始代码) ========== def _init_collision_sensor(self): collision_bp = self.world.get_blueprint_library().find('sensor.other.collision') collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) @@ -453,7 +482,6 @@ def _init_collision_sensor(self): ) self.collision_sensor.listen(lambda event: self._collision_callback(event)) - # ========== 碰撞回调(完全保留原始代码) ========== def _collision_callback(self, event): self.has_collision = True other_actor = event.other_actor @@ -465,7 +493,6 @@ def _collision_callback(self, event): elif 'static' in other_actor_type or 'building' in other_actor_type or 'guardrail' in other_actor_type: self.hit_static = True - # ========== 初始化相机(完全保留原始代码) ========== def _init_camera(self): camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '128') @@ -476,9 +503,7 @@ def _init_camera(self): self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) self.camera.listen(lambda img: self._camera_callback(img)) - # ========== 重置环境(新增导航+步数重置 + 移除天气日志) ========== def reset(self): - # 清理旧资源 if self.vehicle is not None and self.vehicle.is_alive: self.vehicle.destroy() if self.camera is not None and self.camera.is_alive: @@ -487,7 +512,6 @@ def reset(self): self.collision_sensor.destroy() self.image_data = None - # 重置标记 self.has_collision = False self.hit_vehicle = False self.hit_pedestrian = False @@ -500,30 +524,32 @@ def reset(self): self.last_lane_check_time = 0 self.last_lane_log_time = 0 - # ========== 新增:重置步数 ========== self.current_step = 0 - - # ========== 新增:重置档位相关状态 ========== self.last_speed_range = 0 self.last_gear = 1 - self.is_initial_gear_logged = False # 重置初始档位日志标记 + self.is_initial_gear_logged = False + self.last_headlight_check_time = 0 + self.vehicle_headlights_on = False - # ========== 切换天气(会自动输出中文名称) ========== self.switch_weather() - # 生成车辆 vehicle_bp = self.world.get_blueprint_library().filter('vehicle.tesla.model3')[0] self.vehicle = self._spawn_vehicle_safely(vehicle_bp) - # ========== 新增:生成导航目标点 ========== - self._generate_random_target() + # 夜晚自动开灯 + if self.current_weather_id == "Night": + try: + self.vehicle.set_light_state(carla.VehicleLightState.HeadlightOn) + self.vehicle_headlights_on = True + print(f"当前为夜晚,已自动开启大灯") + except: + self.vehicle_headlights_on = False - # 初始化传感器 + self._generate_random_target() self._init_camera() self._init_collision_sensor() self._spawn_small_npc() - # 等待传感器就绪 timeout = 0 while self.image_data is None and timeout < 30: self.world.tick() @@ -534,7 +560,6 @@ def reset(self): self.world.tick() return self.image_data.copy() if self.image_data is not None else np.zeros((128,128,3), dtype=np.uint8) - # ========== 视角跟随(完全保留原始代码) ========== def follow_vehicle(self): spectator = self.world.get_spectator() if not spectator or not self.vehicle or not self.vehicle.is_alive: @@ -553,24 +578,19 @@ def follow_vehicle(self): carla.Rotation(pitch=self.view_pitch, yaw=vehicle_tf.rotation.yaw, roll=0.0) )) - # ========== 获取观测(完全保留原始代码) ========== def get_observation(self): return self.image_data.copy() if self.image_data is not None else np.zeros((128, 128, 3), dtype=np.uint8) - # ========== 相机回调(完全保留原始代码) ========== def _camera_callback(self, image): array = np.frombuffer(image.raw_data, dtype=np.uint8) self.image_data = array.reshape((image.height, image.width, 4))[:, :, :3] - # ========== 核心:奖励函数(集成导航+步数限制 + 新增档位奖励) ========== def step(self, action): if self.vehicle is None or not self.vehicle.is_alive: raise RuntimeError("车辆未初始化/已销毁,请先调用reset()") - # ========== 新增:步数+1 ========== self.current_step += 1 - # 车辆控制逻辑 throttle = 0.0 steer = 0.0 if action == 0: @@ -584,18 +604,14 @@ def step(self, action): elif action == 3: throttle = -0.2 - # ========== 优化:根据车速自动匹配合理档位 ========== - # 计算当前车速 velocity = self.vehicle.get_velocity() speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) current_speed = speed_m_s * 3.6 - # 匹配档位 gear = 1 - if throttle < 0: # 倒车 + if throttle < 0: gear = -1 else: - # 根据车速匹配前进档位 if 0 <= current_speed < 10: gear = 1 elif 10 <= current_speed < 20: @@ -619,15 +635,13 @@ def step(self, action): self.world.tick() self.follow_vehicle() - # 奖励计算 base_reward = 0.1 if throttle > 0 else (-0.1 if throttle < 0 else 0.0) traffic_light_reward = self._check_traffic_light() over_speed_reward = self._check_over_speed() lane_reward = self._check_lane_offset() collision_reward = 0.0 - - # ========== 新增:计算档位奖励 ========== gear_reward = self._calculate_gear_reward() + headlight_reward = self._check_headlight_status() done = False @@ -646,7 +660,6 @@ def step(self, action): done = False self.collision_penalty_applied = True - # ========== 导航奖励计算 ========== nav_reward = 0.0 dist_to_target = -1.0 if self.target_location is not None and self.vehicle.is_alive: @@ -663,13 +676,11 @@ def step(self, action): nav_reward = max(nav_reward, -0.01) self.last_dist_to_target = dist_to_target - # ========== 步数超限判定 ========== if self.current_step >= self.max_steps and not done: print(f"达到最大步数{self.max_steps},终止训练(当前距离目标点:{dist_to_target:.2f}米)") done = True - # 总奖励(新增档位奖励) - total_reward = base_reward + traffic_light_reward + over_speed_reward + lane_reward + collision_reward + nav_reward + gear_reward + total_reward = base_reward + traffic_light_reward + over_speed_reward + lane_reward + collision_reward + nav_reward + gear_reward + headlight_reward next_state = self.get_observation() return next_state, total_reward, done, { @@ -679,15 +690,14 @@ def step(self, action): "lane_reward": lane_reward, "collision_reward": collision_reward, "nav_reward": nav_reward, - "gear_reward": gear_reward, # 新增:返回档位奖励 + "gear_reward": gear_reward, + "headlight_reward": headlight_reward, "total_reward": total_reward, "current_step": self.current_step, "dist_to_target": dist_to_target, - # 可选:在返回的info中添加当前天气名称 - "current_weather": self.weather_name_map[self.current_weather] + "current_weather": self.weather_name_map[self.current_weather_id] } - # ========== 关闭环境(完全保留原始代码) ========== def close(self): for v in self.npc_vehicle_list: if v.is_alive: From 00c6828ecf657cabf56f166f5d969cd87079bb22 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Thu, 18 Dec 2025 21:15:25 +0800 Subject: [PATCH 3/4] updata --- src/MMAP-DRL-Nav/carla_environment.py | 156 +++++++++++++++++++------- 1 file changed, 113 insertions(+), 43 deletions(-) diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index 6539d17b35..82ffbac062 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -95,24 +95,47 @@ def is_port_used(port): self.collision_pedestrian_penalty = -100.0 # 碰撞行人惩罚 self.collision_static_penalty = -15.0 # 碰撞静态物体惩罚 - # ========== 天气系统:仅保留白天/夜晚切换 ========== + # ========== 天气系统:恢复所有原始天气 + 昼夜切换 ========== self.weather_mode = "dynamic" - # 简化:只保留白天(ClearNoon)和夜晚(ClearNight)两种天气 + # 恢复所有原始天气类型 + 自定义昼夜天气 self.weather_name_map = { - "Day": "晴天(白天)", - "Night": "晴天(夜晚)" + # 白天天气(原有) + carla.WeatherParameters.ClearNoon: "晴天(中午)", + carla.WeatherParameters.CloudyNoon: "多云(中午)", + carla.WeatherParameters.WetNoon: "小雨(中午)", + carla.WeatherParameters.WetCloudyNoon: "多云转小雨(中午)", + carla.WeatherParameters.MidRainyNoon: "中雨(中午)", + carla.WeatherParameters.HardRainNoon: "大雨(中午)", + carla.WeatherParameters.SoftRainNoon: "细雨(中午)", + # 自定义夜晚天气(补充) + "ClearNight": "晴天(夜晚)", + "CloudyNight": "多云(夜晚)", + "WetNight": "小雨(夜晚)" } - # 预设天气列表:仅白天/夜晚 - self.all_weather_ids = ["Day", "Night"] + + # 预设天气列表:恢复所有原有 + 自定义夜晚 + self.builtin_weathers = [ + carla.WeatherParameters.ClearNoon, + carla.WeatherParameters.CloudyNoon, + carla.WeatherParameters.WetNoon, + carla.WeatherParameters.WetCloudyNoon, + carla.WeatherParameters.MidRainyNoon, + carla.WeatherParameters.HardRainNoon, + carla.WeatherParameters.SoftRainNoon + ] + self.custom_night_weathers = ["ClearNight", "CloudyNight", "WetNight"] + self.all_weather_ids = self.builtin_weathers + self.custom_night_weathers + # 随机选择初始天气 self.current_weather_id = random.choice(self.all_weather_ids) self._set_weather_by_id(self.current_weather_id) print(f"初始天气:{self.weather_name_map[self.current_weather_id]}") - # ========== 仅保留大灯核心功能:夜晚开灯加分 ========== - self.headlight_required_weathers = ["Night"] # 仅夜晚需要开大灯 - self.headlight_on_reward = 1.0 # 夜晚开灯加分(提高奖励值) - self.headlight_check_cooldown = 2.0 # 检测间隔缩短为2秒 + # ========== 大灯奖惩配置:夜晚开灯奖励 + 未开灯轻微惩罚 ========== + self.headlight_required_weathers = ["ClearNight", "CloudyNight", "WetNight"] # 所有夜晚天气需要开灯 + self.headlight_on_reward = 0.5 # 夜晚开灯奖励(适中) + self.headlight_off_penalty = -0.2 # 夜晚未开灯轻微惩罚(和轻微车道偏离一致) + self.headlight_check_cooldown = 2.0 # 检测间隔2秒 self.last_headlight_check_time = 0 self.vehicle_headlights_on = False @@ -157,31 +180,52 @@ def is_port_used(port): self.view_distance = 8.0 self.z_offset = 0.5 - # ========== 简化:仅实现白天/夜晚天气切换 ========== + # ========== 恢复原始天气设置 + 完善夜晚天气参数 ========== def _set_weather_by_id(self, weather_id): - """仅设置白天/夜晚两种天气""" - weather = carla.WeatherParameters() - if weather_id == "Day": - # 白天:标准晴天 - weather.sun_altitude_angle = 90.0 - weather.sun_azimuth_angle = 0.0 - weather.cloudiness = 0.0 - weather.precipitation = 0.0 - weather.brightness = 1.0 - weather.directionald_light_intensity = 1.0 - elif weather_id == "Night": - # 夜晚:极低光照+月光 - weather.sun_altitude_angle = -90.0 # 太阳在地平线下 - weather.moon_altitude_angle = 30.0 - weather.moon_intensity = 1.0 - weather.brightness = 0.1 - weather.directionald_light_intensity = 0.01 - weather.ambient_occlusion_intensity = 0.2 - self.world.set_weather(weather) - - # ========== 简化:仅保留夜晚开灯加分功能 ========== + """兼容内置天气枚举和自定义夜晚天气""" + if isinstance(weather_id, carla.WeatherParameters): + # 原有白天天气:直接使用CARLA内置枚举 + self.world.set_weather(weather_id) + else: + # 自定义夜晚天气:基于对应白天天气调整光照 + weather = carla.WeatherParameters() + if weather_id == "ClearNight": + # 晴天夜晚:极低光照+月光 + weather.sun_altitude_angle = -90.0 + weather.moon_altitude_angle = 30.0 + weather.moon_intensity = 1.0 + weather.cloudiness = 0.0 + weather.precipitation = 0.0 + weather.brightness = 0.1 + weather.directionald_light_intensity = 0.01 + weather.ambient_occlusion_intensity = 0.2 + elif weather_id == "CloudyNight": + # 多云夜晚 + weather.sun_altitude_angle = -90.0 + weather.moon_altitude_angle = 25.0 + weather.moon_intensity = 0.7 + weather.cloudiness = 80.0 + weather.precipitation = 0.0 + weather.brightness = 0.08 + weather.directionald_light_intensity = 0.008 + weather.ambient_occlusion_intensity = 0.25 + elif weather_id == "WetNight": + # 小雨夜晚 + weather.sun_altitude_angle = -90.0 + weather.moon_altitude_angle = 20.0 + weather.moon_intensity = 0.5 + weather.cloudiness = 90.0 + weather.precipitation = 20.0 + weather.precipitation_deposits = 10.0 + weather.brightness = 0.07 + weather.directionald_light_intensity = 0.005 + weather.ambient_occlusion_intensity = 0.3 + self.world.set_weather(weather) + + # ========== 大灯奖惩:夜晚开灯奖励 + 未开灯轻微惩罚 ========== def _check_headlight_status(self): current_time = time.time() + # 冷却时间内不检测 if current_time - self.last_headlight_check_time < self.headlight_check_cooldown: return 0.0 @@ -190,8 +234,8 @@ def _check_headlight_status(self): if not self.vehicle or not self.vehicle.is_alive: return 0.0 - # 仅夜晚检测大灯 - need_headlight = self.current_weather_id == "Night" + # 仅夜晚天气检测大灯 + need_headlight = self.current_weather_id in self.headlight_required_weathers if not need_headlight: return 0.0 @@ -199,18 +243,24 @@ def _check_headlight_status(self): try: vehicle_light_state = self.vehicle.get_light_state() self.vehicle_headlights_on = vehicle_light_state == carla.VehicleLightState.HeadlightOn - except: + except Exception as e: self.vehicle_headlights_on = False + print(f"检测大灯状态失败:{e}") reward = 0.0 - if need_headlight and self.vehicle_headlights_on: - # 夜晚开灯就加分 - reward = self.headlight_on_reward - print(f"[夜晚] 大灯已开启,加分{self.headlight_on_reward}") + if need_headlight: + if self.vehicle_headlights_on: + # 夜晚开灯:奖励 + reward = self.headlight_on_reward + print(f"[{self.weather_name_map[self.current_weather_id]}] 大灯已开启,加分{self.headlight_on_reward}") + else: + # 夜晚未开灯:轻微惩罚 + reward = self.headlight_off_penalty + print(f"[{self.weather_name_map[self.current_weather_id]}] 大灯未开启,扣分{abs(self.headlight_off_penalty)}") return reward - # ========== 以下所有方法完全保留原有逻辑,未做修改 ========== + # ========== 以下所有方法完全保留原始代码,未做任何修改 ========== def _calculate_gear_reward(self): if not self.vehicle or not self.vehicle.is_alive: return 0.0 @@ -504,6 +554,7 @@ def _init_camera(self): self.camera.listen(lambda img: self._camera_callback(img)) def reset(self): + # 清理资源 if self.vehicle is not None and self.vehicle.is_alive: self.vehicle.destroy() if self.camera is not None and self.camera.is_alive: @@ -512,6 +563,7 @@ def reset(self): self.collision_sensor.destroy() self.image_data = None + # 重置状态 self.has_collision = False self.hit_vehicle = False self.hit_pedestrian = False @@ -531,33 +583,40 @@ def reset(self): self.last_headlight_check_time = 0 self.vehicle_headlights_on = False + # 切换天气 self.switch_weather() + # 生成车辆 vehicle_bp = self.world.get_blueprint_library().filter('vehicle.tesla.model3')[0] self.vehicle = self._spawn_vehicle_safely(vehicle_bp) # 夜晚自动开灯 - if self.current_weather_id == "Night": + if self.current_weather_id in self.headlight_required_weathers: try: self.vehicle.set_light_state(carla.VehicleLightState.HeadlightOn) self.vehicle_headlights_on = True - print(f"当前为夜晚,已自动开启大灯") - except: + print(f"当前为{self.weather_name_map[self.current_weather_id]},已自动开启大灯") + except Exception as e: self.vehicle_headlights_on = False + print(f"自动开启大灯失败:{e}") + # 生成目标点 + 初始化传感器 + 生成NPC self._generate_random_target() self._init_camera() self._init_collision_sensor() self._spawn_small_npc() + # 等待相机数据 timeout = 0 while self.image_data is None and timeout < 30: self.world.tick() time.sleep(0.001) timeout += 1 + # 跟随车辆 self.follow_vehicle() self.world.tick() + return self.image_data.copy() if self.image_data is not None else np.zeros((128,128,3), dtype=np.uint8) def follow_vehicle(self): @@ -591,6 +650,7 @@ def step(self, action): self.current_step += 1 + # 解析动作 throttle = 0.0 steer = 0.0 if action == 0: @@ -604,6 +664,7 @@ def step(self, action): elif action == 3: throttle = -0.2 + # 计算车速和档位 velocity = self.vehicle.get_velocity() speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) current_speed = speed_m_s * 3.6 @@ -623,6 +684,7 @@ def step(self, action): else: gear = 5 + # 控制车辆 self.vehicle.apply_control(carla.VehicleControl( throttle=throttle, steer=steer, @@ -635,6 +697,7 @@ def step(self, action): self.world.tick() self.follow_vehicle() + # 计算各项奖励 base_reward = 0.1 if throttle > 0 else (-0.1 if throttle < 0 else 0.0) traffic_light_reward = self._check_traffic_light() over_speed_reward = self._check_over_speed() @@ -645,6 +708,7 @@ def step(self, action): done = False + # 碰撞惩罚 if self.has_collision and not self.collision_penalty_applied: if self.hit_pedestrian: collision_reward = self.collision_pedestrian_penalty @@ -660,6 +724,7 @@ def step(self, action): done = False self.collision_penalty_applied = True + # 导航奖励 nav_reward = 0.0 dist_to_target = -1.0 if self.target_location is not None and self.vehicle.is_alive: @@ -676,10 +741,12 @@ def step(self, action): nav_reward = max(nav_reward, -0.01) self.last_dist_to_target = dist_to_target + # 步数超限 if self.current_step >= self.max_steps and not done: print(f"达到最大步数{self.max_steps},终止训练(当前距离目标点:{dist_to_target:.2f}米)") done = True + # 总奖励 total_reward = base_reward + traffic_light_reward + over_speed_reward + lane_reward + collision_reward + nav_reward + gear_reward + headlight_reward next_state = self.get_observation() @@ -699,6 +766,7 @@ def step(self, action): } def close(self): + # 清理NPC for v in self.npc_vehicle_list: if v.is_alive: v.destroy() @@ -709,12 +777,14 @@ def close(self): self.npc_pedestrian_list.clear() self.traffic_manager.set_synchronous_mode(False) + # 恢复同步模式 try: self.sync_settings.synchronous_mode = False self.world.apply_settings(self.sync_settings) except Exception as e: print(f"恢复异步模式时警告:{e}") + # 销毁车辆和传感器 try: if self.vehicle is not None and self.vehicle.is_alive: self.vehicle.destroy() From 30f297261d75cec272724910a273963d8054ee90 Mon Sep 17 00:00:00 2001 From: timetravler123 <2094033771@qq.com> Date: Sun, 21 Dec 2025 21:03:45 +0800 Subject: [PATCH 4/4] updata --- src/MMAP-DRL-Nav/carla_pkg/CMakeLists.txt | 206 +++++ .../carla_pkg/launch/carla_launch.launch | 9 + src/MMAP-DRL-Nav/carla_pkg/package.xml | 71 ++ .../carla_pkg/scripts/attention_module.py | 48 ++ .../carla_pkg/scripts/carla_environment.py | 807 ++++++++++++++++++ .../carla_pkg/scripts/decision_module.py | 25 + .../carla_pkg/scripts/dqn_agent.py | 152 ++++ src/MMAP-DRL-Nav/carla_pkg/scripts/main.py | 276 ++++++ src/MMAP-DRL-Nav/carla_pkg/scripts/pruning.py | 34 + .../carla_pkg/scripts/quantization.py | 7 + .../carla_pkg/scripts/run_simulation.py | 141 +++ src/MMAP-DRL-Nav/carla_pkg/scripts/train.py | 144 ++++ 12 files changed, 1920 insertions(+) create mode 100644 src/MMAP-DRL-Nav/carla_pkg/CMakeLists.txt create mode 100644 src/MMAP-DRL-Nav/carla_pkg/launch/carla_launch.launch create mode 100644 src/MMAP-DRL-Nav/carla_pkg/package.xml create mode 100644 src/MMAP-DRL-Nav/carla_pkg/scripts/attention_module.py create mode 100644 src/MMAP-DRL-Nav/carla_pkg/scripts/carla_environment.py create mode 100644 src/MMAP-DRL-Nav/carla_pkg/scripts/decision_module.py create mode 100644 src/MMAP-DRL-Nav/carla_pkg/scripts/dqn_agent.py create mode 100644 src/MMAP-DRL-Nav/carla_pkg/scripts/main.py create mode 100644 src/MMAP-DRL-Nav/carla_pkg/scripts/pruning.py create mode 100644 src/MMAP-DRL-Nav/carla_pkg/scripts/quantization.py create mode 100644 src/MMAP-DRL-Nav/carla_pkg/scripts/run_simulation.py create mode 100644 src/MMAP-DRL-Nav/carla_pkg/scripts/train.py diff --git a/src/MMAP-DRL-Nav/carla_pkg/CMakeLists.txt b/src/MMAP-DRL-Nav/carla_pkg/CMakeLists.txt new file mode 100644 index 0000000000..ecf508f804 --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 2.8.3) +project(carla_pkg) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + sensor_msgs + cv_bridge +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + CATKIN_DEPENDS rospy std_msgs sensor_msgs cv_bridge +# INCLUDE_DIRS include +# LIBRARIES carla_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/carla_pkg.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/carla_pkg_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +catkin_install_python(PROGRAMS scripts/main.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_carla_pkg.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/MMAP-DRL-Nav/carla_pkg/launch/carla_launch.launch b/src/MMAP-DRL-Nav/carla_pkg/launch/carla_launch.launch new file mode 100644 index 0000000000..118f6d0426 --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/launch/carla_launch.launch @@ -0,0 +1,9 @@ + + + + pkg="carla_pkg" + type="main.py" + output="screen" + respawn="false" /> + diff --git a/src/MMAP-DRL-Nav/carla_pkg/package.xml b/src/MMAP-DRL-Nav/carla_pkg/package.xml new file mode 100644 index 0000000000..08f5db0783 --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/package.xml @@ -0,0 +1,71 @@ + + + carla_pkg + 0.0.0 + The carla_pkg package + + + + + xxw + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + cv_bridge + rospy + sensor_msgs + std_msgs + cv_bridge + rospy + sensor_msgs + std_msgs + cv_bridge + rospy + sensor_msgs + std_msgs + + + + + + + + diff --git a/src/MMAP-DRL-Nav/carla_pkg/scripts/attention_module.py b/src/MMAP-DRL-Nav/carla_pkg/scripts/attention_module.py new file mode 100644 index 0000000000..8a80987e4c --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/scripts/attention_module.py @@ -0,0 +1,48 @@ +import torch +import torch.nn as nn + +class CrossDomainAttention(nn.Module): + def __init__(self, input_dims, num_blocks=6): + super(CrossDomainAttention, self).__init__() + # 多头注意力模块(embed_dim 需与投影后的维度一致,即256) + self.attention_blocks = nn.ModuleList([ + nn.MultiheadAttention(embed_dim=256, num_heads=8) for _ in range(num_blocks) + ]) + # 投影层:将每个输入特征映射到256维 + self.proj_layers = nn.ModuleList([ + nn.Linear(dim, 256) for dim in input_dims + ]) + # 可选:层归一化,稳定训练 + self.norm_layers = nn.ModuleList([ + nn.LayerNorm(256) for _ in range(num_blocks) + ]) + + def forward(self, *inputs): + processed_inputs = [] + for i, x in enumerate(inputs): + # 1. 处理高维特征(如 segmentation 的 (B, 256, H, W)) + if x.dim() > 2: + # 对空间维度求平均,降为 (B, C),其中 C=256(与 input_dims 对应) + x = x.mean(dim=tuple(range(2, x.dim()))) # 保留前2维 (B, C) + + # 2. 确保输入线性层的是2维张量 (B, D) + assert x.dim() == 2, f"特征 {i} 经处理后必须是2维张量,当前维度:{x.dim()}" + + # 3. 投影到256维,并调整为 MultiheadAttention 要求的格式 (seq_len=1, B, 256) + # (因为每个特征是全局描述,序列长度设为1) + x = self.proj_layers[i](x) # (B, 256) + x = x.unsqueeze(0) # (1, B, 256),seq_len=1 + processed_inputs.append(x) + + # 4. 拼接所有跨域特征,形成序列 (seq_len=N, B, 256),其中 N=5(5个特征) + x = torch.cat(processed_inputs, dim=0) # (5, B, 256) + + # 5. 经过多个注意力块 + for i, block in enumerate(self.attention_blocks): + # 自注意力计算 + attn_output, _ = block(x, x, x) # (5, B, 256) + # 残差连接 + 层归一化 + x = self.norm_layers[i](x + attn_output) + + # 6. 对序列维度求平均,得到融合后的特征 (B, 256) + return x.mean(dim=0) # (B, 256) \ No newline at end of file diff --git a/src/MMAP-DRL-Nav/carla_pkg/scripts/carla_environment.py b/src/MMAP-DRL-Nav/carla_pkg/scripts/carla_environment.py new file mode 100644 index 0000000000..82ffbac062 --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/scripts/carla_environment.py @@ -0,0 +1,807 @@ +import carla +import numpy as np +import gym +import time +import socket +import random + +class CarlaEnvironment(gym.Env): + def __init__(self): + super(CarlaEnvironment, self).__init__() + # 初始化CARLA客户端 + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(20.0) + + # ========== 1. 端口检测 + 重试连接 ========== + def is_port_used(port): + """检测端口是否被占用""" + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + try: + s.connect(('localhost', port)) + return True + except: + return False + finally: + s.close() + + if not is_port_used(2000): + raise RuntimeError("2000端口未被占用,请先启动CARLA模拟器") + + max_retry = 3 + retry_count = 0 + while retry_count < max_retry: + try: + self.world = self.client.get_world() + break + except RuntimeError as e: + retry_count += 1 + print(f"连接失败,重试第{retry_count}次...") + time.sleep(5) + else: + raise RuntimeError("CARLA连接超时(3次重试失败),请检查模拟器是否启动") + + # ========== 修复核心错误:从World获取蓝图库(而非Map) ========== + self.blueprint_library = self.world.get_blueprint_library() + + # ========== 同步模式配置 ========== + self.sync_settings = self.world.get_settings() + self.sync_settings.synchronous_mode = True + self.sync_settings.fixed_delta_seconds = 1.0 / 30 + self.sync_settings.no_rendering_mode = False + self.world.apply_settings(self.sync_settings) + + # ========== NPC配置 ========== + self.traffic_manager = self.client.get_trafficmanager(8000) + self.traffic_manager.set_synchronous_mode(True) + self.npc_vehicle_list = [] + self.npc_pedestrian_list = [] + self.hit_vehicle = False + self.hit_pedestrian = False + self.hit_static = False # 碰撞静态物体标记 + self.collision_penalty_applied = False # 碰撞惩罚是否已执行 + + # ========== 1. 红绿灯奖惩配置 ========== + self.red_light_penalty = -8.0 # 闯红灯一次性重罚 + self.green_light_reward = 10.0 # 绿灯通过高奖励 + self.red_light_stop_reward = 0.3 # 红灯停车奖励(高于前进奖励) + self.traffic_light_cooldown = 10.0 + self.last_traffic_light_time = 0 + self.traffic_light_trigger_distance = 10.0 + self.traffic_light_reset_distance = 15.0 + self.has_triggered_red = False + self.has_triggered_green = False + + # ========== 2. 超速奖惩配置 ========== + self.speed_limit_urban = 30.0 # 城区限速30km/h + self.over_speed_light_penalty = -1.0 # 轻度超速(30-40km/h)/秒 + self.over_speed_heavy_penalty = -4.0 # 重度超速(>40km/h)/秒 + self.over_speed_cooldown = 1.0 + self.last_over_speed_time = 0 + + # ========== 3. 车道偏离奖惩配置(核心调整) ========== + self.lane_keep_reward = 0.05 # 保持车道内奖励/帧 + self.lane_light_penalty = -0.2 # 轻微偏离单次扣分 + self.lane_heavy_penalty = -1.0 # 严重偏离单次扣分 + self.lane_offset_light = 0.2 # 轻微偏离阈值(米) + self.lane_offset_heavy = 0.4 # 严重偏离阈值(米) + self.lane_check_interval = 3.0 # 车道检测间隔:3秒 + self.lane_log_interval = 10.0 # 车道日志输出间隔:10秒 + self.last_lane_check_time = 0 # 上次车道检测时间戳 + self.last_lane_log_time = 0 # 上次车道日志输出时间戳 + self.enable_lane_log = True # 开启车道偏离日志 + + # ========== 4. 碰撞奖惩配置 ========== + self.collision_vehicle_penalty = -50.0 # 碰撞车辆惩罚 + self.collision_pedestrian_penalty = -100.0 # 碰撞行人惩罚 + self.collision_static_penalty = -15.0 # 碰撞静态物体惩罚 + + # ========== 天气系统:恢复所有原始天气 + 昼夜切换 ========== + self.weather_mode = "dynamic" + # 恢复所有原始天气类型 + 自定义昼夜天气 + self.weather_name_map = { + # 白天天气(原有) + carla.WeatherParameters.ClearNoon: "晴天(中午)", + carla.WeatherParameters.CloudyNoon: "多云(中午)", + carla.WeatherParameters.WetNoon: "小雨(中午)", + carla.WeatherParameters.WetCloudyNoon: "多云转小雨(中午)", + carla.WeatherParameters.MidRainyNoon: "中雨(中午)", + carla.WeatherParameters.HardRainNoon: "大雨(中午)", + carla.WeatherParameters.SoftRainNoon: "细雨(中午)", + # 自定义夜晚天气(补充) + "ClearNight": "晴天(夜晚)", + "CloudyNight": "多云(夜晚)", + "WetNight": "小雨(夜晚)" + } + + # 预设天气列表:恢复所有原有 + 自定义夜晚 + self.builtin_weathers = [ + carla.WeatherParameters.ClearNoon, + carla.WeatherParameters.CloudyNoon, + carla.WeatherParameters.WetNoon, + carla.WeatherParameters.WetCloudyNoon, + carla.WeatherParameters.MidRainyNoon, + carla.WeatherParameters.HardRainNoon, + carla.WeatherParameters.SoftRainNoon + ] + self.custom_night_weathers = ["ClearNight", "CloudyNight", "WetNight"] + self.all_weather_ids = self.builtin_weathers + self.custom_night_weathers + + # 随机选择初始天气 + self.current_weather_id = random.choice(self.all_weather_ids) + self._set_weather_by_id(self.current_weather_id) + print(f"初始天气:{self.weather_name_map[self.current_weather_id]}") + + # ========== 大灯奖惩配置:夜晚开灯奖励 + 未开灯轻微惩罚 ========== + self.headlight_required_weathers = ["ClearNight", "CloudyNight", "WetNight"] # 所有夜晚天气需要开灯 + self.headlight_on_reward = 0.5 # 夜晚开灯奖励(适中) + self.headlight_off_penalty = -0.2 # 夜晚未开灯轻微惩罚(和轻微车道偏离一致) + self.headlight_check_cooldown = 2.0 # 检测间隔2秒 + self.last_headlight_check_time = 0 + self.vehicle_headlights_on = False + + # ========== 其他配置完全保留 ========== + self.target_location = None + self.target_radius = 5.0 + self.nav_reward_per_meter = 0.02 + self.goal_completion_reward = 100.0 + self.last_dist_to_target = 0.0 + + self.max_steps = 800 + self.current_step = 0 + + self.gear_config = { + 0: 1, + 10: 2, + 20: 3, + 30: 4, + 40: 5 + } + self.gear_correct_reward = 0.1 + self.last_speed_range = 0 + self.last_gear = 1 + self.is_initial_gear_logged = False + + self.spawn_retry_times = 20 + self.spawn_safe_radius = 2.0 + + self.action_space = gym.spaces.Discrete(4) + self.observation_space = gym.spaces.Box( + low=0, high=255, shape=(128, 128, 3), dtype=np.uint8 + ) + + self.vehicle = None + self.camera = None + self.collision_sensor = None + self.image_data = None + self.has_collision = False + + self.view_height = 6.0 + self.view_pitch = -15.0 + self.view_distance = 8.0 + self.z_offset = 0.5 + + # ========== 恢复原始天气设置 + 完善夜晚天气参数 ========== + def _set_weather_by_id(self, weather_id): + """兼容内置天气枚举和自定义夜晚天气""" + if isinstance(weather_id, carla.WeatherParameters): + # 原有白天天气:直接使用CARLA内置枚举 + self.world.set_weather(weather_id) + else: + # 自定义夜晚天气:基于对应白天天气调整光照 + weather = carla.WeatherParameters() + if weather_id == "ClearNight": + # 晴天夜晚:极低光照+月光 + weather.sun_altitude_angle = -90.0 + weather.moon_altitude_angle = 30.0 + weather.moon_intensity = 1.0 + weather.cloudiness = 0.0 + weather.precipitation = 0.0 + weather.brightness = 0.1 + weather.directionald_light_intensity = 0.01 + weather.ambient_occlusion_intensity = 0.2 + elif weather_id == "CloudyNight": + # 多云夜晚 + weather.sun_altitude_angle = -90.0 + weather.moon_altitude_angle = 25.0 + weather.moon_intensity = 0.7 + weather.cloudiness = 80.0 + weather.precipitation = 0.0 + weather.brightness = 0.08 + weather.directionald_light_intensity = 0.008 + weather.ambient_occlusion_intensity = 0.25 + elif weather_id == "WetNight": + # 小雨夜晚 + weather.sun_altitude_angle = -90.0 + weather.moon_altitude_angle = 20.0 + weather.moon_intensity = 0.5 + weather.cloudiness = 90.0 + weather.precipitation = 20.0 + weather.precipitation_deposits = 10.0 + weather.brightness = 0.07 + weather.directionald_light_intensity = 0.005 + weather.ambient_occlusion_intensity = 0.3 + self.world.set_weather(weather) + + # ========== 大灯奖惩:夜晚开灯奖励 + 未开灯轻微惩罚 ========== + def _check_headlight_status(self): + current_time = time.time() + # 冷却时间内不检测 + if current_time - self.last_headlight_check_time < self.headlight_check_cooldown: + return 0.0 + + self.last_headlight_check_time = current_time + + if not self.vehicle or not self.vehicle.is_alive: + return 0.0 + + # 仅夜晚天气检测大灯 + need_headlight = self.current_weather_id in self.headlight_required_weathers + if not need_headlight: + return 0.0 + + # 获取大灯状态 + try: + vehicle_light_state = self.vehicle.get_light_state() + self.vehicle_headlights_on = vehicle_light_state == carla.VehicleLightState.HeadlightOn + except Exception as e: + self.vehicle_headlights_on = False + print(f"检测大灯状态失败:{e}") + + reward = 0.0 + if need_headlight: + if self.vehicle_headlights_on: + # 夜晚开灯:奖励 + reward = self.headlight_on_reward + print(f"[{self.weather_name_map[self.current_weather_id]}] 大灯已开启,加分{self.headlight_on_reward}") + else: + # 夜晚未开灯:轻微惩罚 + reward = self.headlight_off_penalty + print(f"[{self.weather_name_map[self.current_weather_id]}] 大灯未开启,扣分{abs(self.headlight_off_penalty)}") + + return reward + + # ========== 以下所有方法完全保留原始代码,未做任何修改 ========== + def _calculate_gear_reward(self): + if not self.vehicle or not self.vehicle.is_alive: + return 0.0 + + velocity = self.vehicle.get_velocity() + speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + speed_kmh = round(speed_m_s * 3.6, 1) + + current_speed_range = 0 + if speed_kmh < 0: + current_speed_range = -1 + recommended_gear = 'R' + else: + for range_val in sorted(self.gear_config.keys(), reverse=True): + if speed_kmh >= range_val: + current_speed_range = range_val + recommended_gear = self.gear_config[range_val] + break + + control = self.vehicle.get_control() + current_gear = control.gear + if control.reverse or speed_kmh < 0: + current_gear = 'R' + + gear_reward = 0.0 + if not self.is_initial_gear_logged: + is_correct = (current_gear == recommended_gear) + if is_correct: + gear_reward = self.gear_correct_reward + if speed_kmh < 0: + print(f"初始倒车档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前R挡)") + else: + print(f"初始档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前{current_gear}挡,对应{current_speed_range}-{current_speed_range+10}km/h区间)") + self.is_initial_gear_logged = True + elif current_speed_range != self.last_speed_range: + is_correct = (current_gear == recommended_gear) + if is_correct: + gear_reward = self.gear_correct_reward + if speed_kmh < 0: + print(f"倒车档位正确加分:{self.gear_correct_reward}(车速{speed_kmh}km/h,当前R挡)") + else: + print(f"车速进入{current_speed_range}-{current_speed_range+10}km/h区间,档位{current_gear}正确加分:{self.gear_correct_reward}") + self.last_speed_range = current_speed_range + + return gear_reward + + def switch_weather(self): + available_weathers = [w for w in self.all_weather_ids if w != self.current_weather_id] + self.current_weather_id = random.choice(available_weathers) + self._set_weather_by_id(self.current_weather_id) + print(f"天气已切换为:{self.weather_name_map[self.current_weather_id]}") + + def _generate_random_target(self): + spawn_points = self.world.get_map().get_spawn_points() + if not spawn_points: + hero_loc = self.vehicle.get_transform().location if self.vehicle else carla.Location(x=0, y=0, z=0.5) + random_x = hero_loc.x + random.uniform(-200, 200) + random_y = hero_loc.y + random.uniform(-200, 200) + self.target_location = carla.Location(x=random_x, y=random_y, z=0.5) + else: + target_spawn = random.choice(spawn_points) + self.target_location = target_spawn.location + if self.vehicle and self.vehicle.is_alive: + self.last_dist_to_target = self.vehicle.get_transform().location.distance(self.target_location) + print(f"生成导航目标点:({self.target_location.x:.2f}, {self.target_location.y:.2f}),初始距离:{self.last_dist_to_target:.2f}米") + + def _check_traffic_light(self): + current_time = time.time() + if current_time - self.last_traffic_light_time < self.traffic_light_cooldown: + return 0.0 + + if not self.vehicle or not self.vehicle.is_alive: + return 0.0 + + vehicle_loc = self.vehicle.get_transform().location + traffic_lights = self.world.get_actors().filter('traffic.traffic_light') + reward = 0.0 + has_near_light = False + + velocity = self.vehicle.get_velocity() + speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + is_stopped = speed_m_s < 0.1 + + for light in traffic_lights: + dist = vehicle_loc.distance(light.get_transform().location) + if dist <= self.traffic_light_trigger_distance: + has_near_light = True + light_state = light.state + + if light_state == carla.TrafficLightState.Red: + if not is_stopped and not self.has_triggered_red: + reward = self.red_light_penalty + print(f"闯红灯!扣分{self.red_light_penalty}") + self.has_triggered_red = True + self.last_traffic_light_time = current_time + elif is_stopped: + reward = self.red_light_stop_reward + break + + elif light_state == carla.TrafficLightState.Green and not self.has_triggered_green: + reward = self.green_light_reward + print(f"绿灯合规通过!加分{self.green_light_reward}") + self.has_triggered_green = True + self.last_traffic_light_time = current_time + break + + elif dist > self.traffic_light_reset_distance: + self.has_triggered_red = False + self.has_triggered_green = False + + if not has_near_light: + self.has_triggered_red = False + self.has_triggered_green = False + + return reward + + def _check_over_speed(self): + current_time = time.time() + if current_time - self.last_over_speed_time < self.over_speed_cooldown: + return 0.0 + + if not self.vehicle or not self.vehicle.is_alive: + return 0.0 + + velocity = self.vehicle.get_velocity() + speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + speed_km_h = speed_m_s * 3.6 + + over_speed = speed_km_h - self.speed_limit_urban + reward = 0.0 + if over_speed > 0: + if 0 < over_speed <= 10: + reward = self.over_speed_light_penalty + else: + reward = self.over_speed_heavy_penalty + self.last_over_speed_time = current_time + + return reward + + def _check_lane_offset(self): + current_time = time.time() + if current_time - self.last_lane_check_time < self.lane_check_interval: + return 0.0 + + self.last_lane_check_time = current_time + + if not self.vehicle or not self.vehicle.is_alive: + return 0.0 + + vehicle_transform = self.vehicle.get_transform() + waypoint = self.world.get_map().get_waypoint(vehicle_transform.location, project_to_road=True) + + if not waypoint: + if self.enable_lane_log and current_time - self.last_lane_log_time >= self.lane_log_interval: + print(f"完全偏离道路!扣分{self.lane_heavy_penalty}") + self.last_lane_log_time = current_time + return self.lane_heavy_penalty + + lane_center = waypoint.transform.location + vehicle_loc = vehicle_transform.location + yaw_rad = np.radians(waypoint.transform.rotation.yaw) + + offset_x = vehicle_loc.x - lane_center.x + offset_y = vehicle_loc.y - lane_center.y + offset = np.abs(offset_x * np.sin(yaw_rad) - offset_y * np.cos(yaw_rad)) + + reward = 0.0 + if offset < self.lane_offset_light: + reward = self.lane_keep_reward + elif offset < self.lane_offset_heavy: + reward = self.lane_light_penalty + if self.enable_lane_log and current_time - self.last_lane_log_time >= self.lane_log_interval: + print(f"轻微偏离车道(偏移{offset:.2f}m)!扣分{self.lane_light_penalty}") + self.last_lane_log_time = current_time + else: + reward = self.lane_heavy_penalty + if self.enable_lane_log and current_time - self.last_lane_log_time >= self.lane_log_interval: + print(f"严重偏离车道(偏移{offset:.2f}m)!扣分{self.lane_heavy_penalty}") + self.last_lane_log_time = current_time + + return reward + + def _spawn_vehicle_safely(self, vehicle_bp): + spawn_points = self.world.get_map().get_spawn_points() + if not spawn_points: + spawn_points = [carla.Transform( + carla.Location(x=random.uniform(-50, 50), y=random.uniform(-50, 50), z=0.5), + carla.Rotation(yaw=random.uniform(0, 360)) + )] + + for attempt in range(self.spawn_retry_times): + spawn_point = random.choice(spawn_points) + spawn_point.location.z += 0.2 + + try: + vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) + if vehicle is not None: + print(f"车辆生成成功(重试{attempt}次)") + return vehicle + except RuntimeError as e: + print(f"出生点碰撞,重试第{attempt+1}次...") + continue + + raise RuntimeError("所有出生点都有碰撞,无法生成车辆!") + + def _spawn_small_npc(self): + for v in self.npc_vehicle_list: + if v.is_alive: + v.destroy() + self.npc_vehicle_list.clear() + for p in self.npc_pedestrian_list: + if p.is_alive: + p.destroy() + self.npc_pedestrian_list.clear() + + vehicle_bps = self.world.get_blueprint_library().filter('vehicle.*') + spawn_points = self.world.get_map().get_spawn_points() + + if len(spawn_points) < 50: + for _ in range(50 - len(spawn_points)): + random_x = np.random.uniform(-200, 200) + random_y = np.random.uniform(-200, 200) + spawn_points.append(carla.Transform(carla.Location(x=random_x, y=random_y, z=0.5))) + + if self.vehicle is not None: + hero_loc = self.vehicle.get_transform().location + valid_spawn = [] + for sp in spawn_points: + dist = np.linalg.norm([sp.location.x - hero_loc.x, sp.location.y - hero_loc.y]) + if dist > 15.0: + valid_spawn.append(sp) + spawn_points = valid_spawn[:50] + else: + spawn_points = spawn_points[:50] + + for sp in spawn_points: + try: + sp.location.z += 0.1 + npc_vehicle = self.world.try_spawn_actor(random.choice(vehicle_bps), sp) + if npc_vehicle is not None: + self.npc_vehicle_list.append(npc_vehicle) + npc_vehicle.set_autopilot(True, self.traffic_manager.get_port()) + except: + continue + + pedestrian_bps = self.world.get_blueprint_library().filter('walker.pedestrian.*') + for _ in range(5): + if self.vehicle is not None: + hero_loc = self.vehicle.get_transform().location + random_x = hero_loc.x + random.uniform(20, 50) * (1 if random.random()>0.5 else -1) + random_y = hero_loc.y + random.uniform(20, 50) * (1 if random.random()>0.5 else -1) + else: + random_x = np.random.uniform(-100, 100) + random_y = np.random.uniform(-100, 100) + + loc = carla.Location(x=random_x, y=random_y, z=0.1) + try: + pedestrian = self.world.try_spawn_actor(random.choice(pedestrian_bps), carla.Transform(loc)) + if pedestrian: + self.npc_pedestrian_list.append(pedestrian) + except: + continue + + def _init_collision_sensor(self): + collision_bp = self.world.get_blueprint_library().find('sensor.other.collision') + collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) + self.collision_sensor = self.world.spawn_actor( + collision_bp, collision_transform, attach_to=self.vehicle + ) + self.collision_sensor.listen(lambda event: self._collision_callback(event)) + + def _collision_callback(self, event): + self.has_collision = True + other_actor = event.other_actor + other_actor_type = other_actor.type_id + if 'vehicle' in other_actor_type: + self.hit_vehicle = True + elif 'walker' in other_actor_type: + self.hit_pedestrian = True + elif 'static' in other_actor_type or 'building' in other_actor_type or 'guardrail' in other_actor_type: + self.hit_static = True + + def _init_camera(self): + camera_bp = self.world.get_blueprint_library().find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', '128') + camera_bp.set_attribute('image_size_y', '128') + camera_bp.set_attribute('fov', '90') + camera_bp.set_attribute('sensor_tick', '0.0') + camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) + self.camera = self.world.spawn_actor(camera_bp, camera_transform, attach_to=self.vehicle) + self.camera.listen(lambda img: self._camera_callback(img)) + + def reset(self): + # 清理资源 + if self.vehicle is not None and self.vehicle.is_alive: + self.vehicle.destroy() + if self.camera is not None and self.camera.is_alive: + self.camera.destroy() + if self.collision_sensor is not None and self.collision_sensor.is_alive: + self.collision_sensor.destroy() + self.image_data = None + + # 重置状态 + self.has_collision = False + self.hit_vehicle = False + self.hit_pedestrian = False + self.hit_static = False + self.collision_penalty_applied = False + self.last_traffic_light_time = 0 + self.has_triggered_red = False + self.has_triggered_green = False + self.last_over_speed_time = 0 + self.last_lane_check_time = 0 + self.last_lane_log_time = 0 + + self.current_step = 0 + self.last_speed_range = 0 + self.last_gear = 1 + self.is_initial_gear_logged = False + self.last_headlight_check_time = 0 + self.vehicle_headlights_on = False + + # 切换天气 + self.switch_weather() + + # 生成车辆 + vehicle_bp = self.world.get_blueprint_library().filter('vehicle.tesla.model3')[0] + self.vehicle = self._spawn_vehicle_safely(vehicle_bp) + + # 夜晚自动开灯 + if self.current_weather_id in self.headlight_required_weathers: + try: + self.vehicle.set_light_state(carla.VehicleLightState.HeadlightOn) + self.vehicle_headlights_on = True + print(f"当前为{self.weather_name_map[self.current_weather_id]},已自动开启大灯") + except Exception as e: + self.vehicle_headlights_on = False + print(f"自动开启大灯失败:{e}") + + # 生成目标点 + 初始化传感器 + 生成NPC + self._generate_random_target() + self._init_camera() + self._init_collision_sensor() + self._spawn_small_npc() + + # 等待相机数据 + timeout = 0 + while self.image_data is None and timeout < 30: + self.world.tick() + time.sleep(0.001) + timeout += 1 + + # 跟随车辆 + self.follow_vehicle() + self.world.tick() + + return self.image_data.copy() if self.image_data is not None else np.zeros((128,128,3), dtype=np.uint8) + + def follow_vehicle(self): + spectator = self.world.get_spectator() + if not spectator or not self.vehicle or not self.vehicle.is_alive: + return + + vehicle_tf = self.vehicle.get_transform() + vehicle_loc = vehicle_tf.location + yaw_rad = np.radians(vehicle_tf.rotation.yaw) + + cam_x = vehicle_loc.x - (np.cos(yaw_rad) * self.view_distance) + cam_y = vehicle_loc.y - (np.sin(yaw_rad) * self.view_distance) + cam_z = vehicle_loc.z + self.view_height + self.z_offset + + spectator.set_transform(carla.Transform( + carla.Location(x=cam_x, y=cam_y, z=cam_z), + carla.Rotation(pitch=self.view_pitch, yaw=vehicle_tf.rotation.yaw, roll=0.0) + )) + + def get_observation(self): + return self.image_data.copy() if self.image_data is not None else np.zeros((128, 128, 3), dtype=np.uint8) + + def _camera_callback(self, image): + array = np.frombuffer(image.raw_data, dtype=np.uint8) + self.image_data = array.reshape((image.height, image.width, 4))[:, :, :3] + + def step(self, action): + if self.vehicle is None or not self.vehicle.is_alive: + raise RuntimeError("车辆未初始化/已销毁,请先调用reset()") + + self.current_step += 1 + + # 解析动作 + throttle = 0.0 + steer = 0.0 + if action == 0: + throttle = 0.5 + elif action == 1: + throttle = 0.4 + steer = -0.1 + elif action == 2: + throttle = 0.4 + steer = 0.1 + elif action == 3: + throttle = -0.2 + + # 计算车速和档位 + velocity = self.vehicle.get_velocity() + speed_m_s = np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2) + current_speed = speed_m_s * 3.6 + + gear = 1 + if throttle < 0: + gear = -1 + else: + if 0 <= current_speed < 10: + gear = 1 + elif 10 <= current_speed < 20: + gear = 2 + elif 20 <= current_speed < 30: + gear = 3 + elif 30 <= current_speed < 40: + gear = 4 + else: + gear = 5 + + # 控制车辆 + self.vehicle.apply_control(carla.VehicleControl( + throttle=throttle, + steer=steer, + hand_brake=False, + reverse=(throttle < 0), + gear=gear, + manual_gear_shift=True + )) + + self.world.tick() + self.follow_vehicle() + + # 计算各项奖励 + base_reward = 0.1 if throttle > 0 else (-0.1 if throttle < 0 else 0.0) + traffic_light_reward = self._check_traffic_light() + over_speed_reward = self._check_over_speed() + lane_reward = self._check_lane_offset() + collision_reward = 0.0 + gear_reward = self._calculate_gear_reward() + headlight_reward = self._check_headlight_status() + + done = False + + # 碰撞惩罚 + if self.has_collision and not self.collision_penalty_applied: + if self.hit_pedestrian: + collision_reward = self.collision_pedestrian_penalty + print(f"碰撞行人!扣分{self.collision_pedestrian_penalty},终止训练") + done = True + elif self.hit_vehicle: + collision_reward = self.collision_vehicle_penalty + print(f"碰撞车辆!扣分{self.collision_vehicle_penalty},终止训练") + done = True + elif self.hit_static: + collision_reward = self.collision_static_penalty + print(f"碰撞静态物体!扣分{self.collision_static_penalty}") + done = False + self.collision_penalty_applied = True + + # 导航奖励 + nav_reward = 0.0 + dist_to_target = -1.0 + if self.target_location is not None and self.vehicle.is_alive: + vehicle_loc = self.vehicle.get_transform().location + dist_to_target = vehicle_loc.distance(self.target_location) + + if dist_to_target < self.target_radius: + nav_reward = self.goal_completion_reward + print(f"到达目标点!奖励{self.goal_completion_reward},终止训练") + done = True + else: + dist_diff = self.last_dist_to_target - dist_to_target + nav_reward = dist_diff * self.nav_reward_per_meter + nav_reward = max(nav_reward, -0.01) + self.last_dist_to_target = dist_to_target + + # 步数超限 + if self.current_step >= self.max_steps and not done: + print(f"达到最大步数{self.max_steps},终止训练(当前距离目标点:{dist_to_target:.2f}米)") + done = True + + # 总奖励 + total_reward = base_reward + traffic_light_reward + over_speed_reward + lane_reward + collision_reward + nav_reward + gear_reward + headlight_reward + next_state = self.get_observation() + + return next_state, total_reward, done, { + "base_reward": base_reward, + "traffic_light_reward": traffic_light_reward, + "over_speed_reward": over_speed_reward, + "lane_reward": lane_reward, + "collision_reward": collision_reward, + "nav_reward": nav_reward, + "gear_reward": gear_reward, + "headlight_reward": headlight_reward, + "total_reward": total_reward, + "current_step": self.current_step, + "dist_to_target": dist_to_target, + "current_weather": self.weather_name_map[self.current_weather_id] + } + + def close(self): + # 清理NPC + for v in self.npc_vehicle_list: + if v.is_alive: + v.destroy() + self.npc_vehicle_list.clear() + for p in self.npc_pedestrian_list: + if p.is_alive: + p.destroy() + self.npc_pedestrian_list.clear() + self.traffic_manager.set_synchronous_mode(False) + + # 恢复同步模式 + try: + self.sync_settings.synchronous_mode = False + self.world.apply_settings(self.sync_settings) + except Exception as e: + print(f"恢复异步模式时警告:{e}") + + # 销毁车辆和传感器 + try: + if self.vehicle is not None and self.vehicle.is_alive: + self.vehicle.destroy() + except Exception as e: + print(f"销毁车辆时警告:{e}") + + try: + if self.camera is not None and self.camera.is_alive: + self.camera.destroy() + except Exception as e: + print(f"销毁相机时警告:{e}") + + try: + if self.collision_sensor is not None and self.collision_sensor.is_alive: + self.collision_sensor.destroy() + except Exception as e: + print(f"销毁碰撞传感器时警告:{e}") + + time.sleep(0.5) + print("CARLA环境已关闭(同步模式已恢复为异步)") \ No newline at end of file diff --git a/src/MMAP-DRL-Nav/carla_pkg/scripts/decision_module.py b/src/MMAP-DRL-Nav/carla_pkg/scripts/decision_module.py new file mode 100644 index 0000000000..7695d2d4aa --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/scripts/decision_module.py @@ -0,0 +1,25 @@ +import torch +import torch.nn as nn + +class DecisionModule(nn.Module): + def __init__(self): + super(DecisionModule, self).__init__() + self.policy_net = nn.Sequential( + nn.Linear(256, 128), + nn.ReLU(), + nn.Linear(128, 64), + nn.ReLU(), + nn.Linear(64, 2) # 输出:转向角和油门 + ) + self.value_net = nn.Sequential( + nn.Linear(256, 128), + nn.ReLU(), + nn.Linear(128, 64), + nn.ReLU(), + nn.Linear(64, 1) # 值函数 + ) + + def forward(self, fused_features): + policy = self.policy_net(fused_features) + value = self.value_net(fused_features) + return policy, value diff --git a/src/MMAP-DRL-Nav/carla_pkg/scripts/dqn_agent.py b/src/MMAP-DRL-Nav/carla_pkg/scripts/dqn_agent.py new file mode 100644 index 0000000000..b0975981ba --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/scripts/dqn_agent.py @@ -0,0 +1,152 @@ +import torch +import torch.nn as nn +import numpy as np +import random +from collections import deque # 高效内存操作 +from .pruning import prune_model +from .quantization import quantize_model + +class DQNAgent: + def __init__(self, state_shape, action_size, config): + """ + 初始化DQN智能体(适配CARLA图像输入,修复卷积维度计算错误) + :param state_shape: 图像形状 (128, 128, 3) + :param action_size: 动作维度(4:前进/左转/右转/后退) + :param config: 配置字典 + """ + self.state_shape = state_shape # (128, 128, 3) + self.action_size = action_size + self.memory = deque(maxlen=config.get('agent', {}).get('memory_capacity', 10000)) # 经验池 + self.gamma = 0.95 # 折扣因子 + self.epsilon = 1.0 # 初始探索率 + self.epsilon_decay = config['agent']['epsilon_decay'] # 探索率衰减 + self.epsilon_min = config['agent']['epsilon_min'] # 最小探索率 + self.learning_rate = config['train']['learning_rate'] # 学习率 + self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") # 设备 + + # 构建CNN模型(自适应池化解决维度计算错误)并移到指定设备 + self.model = self._build_model().to(self.device) + self.optimizer = torch.optim.Adam(self.model.parameters(), lr=self.learning_rate) # 优化器 + self.loss_fn = nn.MSELoss() # 损失函数 + + # 模型输入输出校验(新增:验证维度匹配) + self._validate_model_dim() + + def _build_model(self): + """构建适配128×128×3图像的CNN模型(自适应池化,无需手动计算卷积维度)""" + return nn.Sequential( + # 卷积层1:提取低级视觉特征 (3,128,128) → (32,31,31) + nn.Conv2d(in_channels=3, out_channels=32, kernel_size=8, stride=4, padding=1), + nn.ReLU(), + # 卷积层2:提取中级特征 (32,31,31) → (64,15,15) + nn.Conv2d(32, 64, kernel_size=4, stride=2, padding=1), + nn.ReLU(), + # 卷积层3:提取高级特征 (64,15,15) → (64,15,15) + nn.Conv2d(64, 64, kernel_size=3, stride=1, padding=1), + nn.ReLU(), + # 自适应池化:强制输出8×8,彻底避免维度计算错误 (64,15,15) → (64,8,8) + nn.AdaptiveAvgPool2d((8, 8)), + # 展平特征图(64*8*8=4096,固定维度) + nn.Flatten(), + # 全连接层:映射到动作空间 + nn.Linear(64 * 8 * 8, 512), + nn.ReLU(), + nn.Linear(512, self.action_size) # 输出4个动作的Q值 + ) + + def _validate_model_dim(self): + """验证模型输入输出维度是否匹配(新增)""" + try: + # 构建测试输入:(batch, 3, H, W) + dummy_input = torch.randn(1, 3, self.state_shape[0], self.state_shape[1]).to(self.device) + with torch.no_grad(): + dummy_output = self.model(dummy_input) + print(f"✅ 模型维度校验通过 | 输入维度:{dummy_input.shape} | 输出维度:{dummy_output.shape}") + except Exception as e: + raise ValueError(f"❌ 模型维度校验失败:{e}") + + def remember(self, state, action, reward, next_state, done): + """存储经验到回放池(标准化数据格式)""" + state = np.array(state, dtype=np.float32) + next_state = np.array(next_state, dtype=np.float32) + reward = np.array(reward, dtype=np.float32) + self.memory.append((state, action, reward, next_state, done)) + + def act(self, state): + """ε-贪心策略选择动作(适配图像输入)""" + # 探索阶段:随机选动作 + if np.random.rand() <= self.epsilon: + return np.random.choice(self.action_size) + + # 利用阶段:模型预测最优动作 + # 维度转换:HWC(128,128,3) → CHW(3,128,128) + batch维度 + 归一化 + state_tensor = torch.FloatTensor(state).permute(2, 0, 1).unsqueeze(0).to(self.device) / 255.0 + # 模型推理(无梯度) + with torch.no_grad(): + q_values = self.model(state_tensor) + # 返回Q值最大的动作 + return np.argmax(q_values.cpu().detach().numpy()[0]) + + def replay(self, batch_size): + """批量经验回放(GPU加速,替换逐样本更新)""" + if len(self.memory) < batch_size: + return + + # 随机采样批次经验 + minibatch = random.sample(self.memory, batch_size) + # 拆分批次数据 + states = np.array([exp[0] for exp in minibatch]) # (batch, 128, 128, 3) + actions = np.array([exp[1] for exp in minibatch]) + rewards = np.array([exp[2] for exp in minibatch]) + next_states = np.array([exp[3] for exp in minibatch]) # (batch, 128, 128, 3) + dones = np.array([exp[4] for exp in minibatch]) + + # 维度转换:HWC → CHW + 移到设备 + 归一化 + states_tensor = torch.FloatTensor(states).permute(0, 3, 1, 2).to(self.device) / 255.0 # (batch, 3, 128, 128) + next_states_tensor = torch.FloatTensor(next_states).permute(0, 3, 1, 2).to(self.device) / 255.0 + actions_tensor = torch.LongTensor(actions).to(self.device) + rewards_tensor = torch.FloatTensor(rewards).to(self.device) + dones_tensor = torch.FloatTensor(dones).to(self.device) + + # 计算当前Q值(仅选执行动作的Q值) + current_q = self.model(states_tensor).gather(1, actions_tensor.unsqueeze(1)).squeeze(1) + + # 计算目标Q值(Bellman方程) + with torch.no_grad(): + next_q = self.model(next_states_tensor).max(1)[0] # 下一个状态的最大Q值 + target_q = rewards_tensor + self.gamma * next_q * (1 - dones_tensor) # 目标Q值 + + # 梯度下降更新 + self.optimizer.zero_grad() + loss = self.loss_fn(current_q, target_q) + loss.backward() + self.optimizer.step() + + # 衰减探索率 + if self.epsilon > self.epsilon_min: + self.epsilon *= self.epsilon_decay + + def calculate_reward(self, current_position, target_position, road_position, done): + """奖励函数(适配CARLA环境)""" + distance_to_target = np.linalg.norm(current_position - target_position) + distance_to_road = np.linalg.norm(current_position - road_position) + + if done: + return 100.0 + elif distance_to_target < 1.0: + return 10.0 + elif distance_to_road > 1.0: + return -5.0 + elif distance_to_target < 5.0: + return 1.0 + else: + return -1.0 + + def get_state(self, position, orientation, target_position, road_position): + """备用:低维状态提取(实际训练用图像)""" + state = np.array([ + position[0], position[1], orientation, + target_position[0], target_position[1], + road_position[0], road_position[1] + ], dtype=np.float32) + return state diff --git a/src/MMAP-DRL-Nav/carla_pkg/scripts/main.py b/src/MMAP-DRL-Nav/carla_pkg/scripts/main.py new file mode 100644 index 0000000000..7dd5bd9068 --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/scripts/main.py @@ -0,0 +1,276 @@ +#!/usr/bin/env python +# coding=utf-8 +# ROS1节点版:CARLA DQN训练/测试脚本 +import torch +import torch.nn as nn +import torch.optim as optim +import numpy as np +import argparse +import rospy # 新增:ROS1核心库 +from std_msgs.msg import Float32MultiArray # 新增:发布控制指令 +from sensor_msgs.msg import Image # 新增:发布相机图像 +from cv_bridge import CvBridge # 新增:图像格式转换 +# 注意:修改导入路径(因为文件都在同一目录,去掉models/envs前缀) +from dqn_agent import DQNAgent +from pruning import ModelPruner +from quantization import quantize_model +from carla_environment import CarlaEnvironment +import yaml + +# -------------------------- +# 新增:ROS节点核心类(封装CARLA+ROS通信) +# -------------------------- +class CarlaDQNROSNode: + def __init__(self): + # 1. 初始化ROS节点 + rospy.init_node('carla_dqn_node', anonymous=False) + rospy.loginfo("【ROS】CARLA DQN节点启动成功!") + + # 2. 创建ROS发布者 + self.control_pub = rospy.Publisher('/carla/control', Float32MultiArray, queue_size=10) + self.image_pub = rospy.Publisher('/carla/camera', Image, queue_size=10) + self.bridge = CvBridge() + + # 3. 初始化配置(默认加载config.yaml,可通过ROS参数覆盖) + self.config_path = rospy.get_param("~config_path", "configs/config.yaml") + self.config = load_config(self.config_path) + rospy.loginfo(f"【ROS】加载配置文件:{self.config_path}") + + # 4. 初始化CARLA环境(替换原有环境初始化) + self.env = CarlaEnvironment() + # 重写CARLA环境的相机回调,发布到ROS + self.env.camera.listen(lambda data: self._camera_callback(data)) + rospy.loginfo("【ROS】CARLA环境初始化成功!") + + # 5. DQN智能体(后续在train/test中初始化) + self.agent = None + self.state_shape = self.env.observation_space.shape + self.action_size = self.env.action_space.n + rospy.loginfo(f"【ROS】状态形状:{self.state_shape},动作维度:{self.action_size}") + + # 6. ROS循环频率 + self.rate = rospy.Rate(5) # 5Hz,适配虚拟机性能 + + # 新增:相机回调→发布ROS图像 + def _camera_callback(self, carla_image): + img_array = np.frombuffer(carla_image.raw_data, dtype=np.uint8) + img_array = img_array.reshape((carla_image.height, carla_image.width, 4))[:, :, :3] + ros_img = self.bridge.cv2_to_imgmsg(img_array, encoding='bgr8') + self.image_pub.publish(ros_img) + + # 新增:发布控制指令到ROS + def publish_control(self, throttle, steer): + control_msg = Float32MultiArray() + control_msg.data = [throttle, steer] + self.control_pub.publish(control_msg) + rospy.loginfo(f"【ROS】发布控制指令:油门={throttle:.2f},转向={steer:.2f}") + +# -------------------------- +# 原有逻辑:解析命令行参数(兼容ROS) +# -------------------------- +def parse_args(): + parser = argparse.ArgumentParser(description='CARLA DQN 训练/测试脚本(ROS版)') + parser.add_argument('--mode', type=str, required=False, choices=['train', 'test'], + help='运行模式:train(训练)/ test(测试),可通过ROS参数覆盖') + parser.add_argument('--config', type=str, default='configs/config.yaml', + help='配置文件路径,可通过ROS参数覆盖') + return parser.parse_args() + +def load_config(config_path='configs/config.yaml'): + try: + with open(config_path, 'r') as file: + config = yaml.safe_load(file) + rospy.loginfo(f"成功加载配置文件:{config_path}") # 替换print为rospy.loginfo + return config + except Exception as e: + rospy.logerr(f"加载配置文件失败:{e}") # 替换print为rospy.logerr + raise + +# -------------------------- +# 原有逻辑:训练函数(适配ROS) +# -------------------------- +def train_model(node, config): + rospy.loginfo("=== 开始DQN训练(ROS版)===") + try: + # 初始化DQN智能体 + node.agent = DQNAgent(state_shape=node.state_shape, action_size=node.action_size, config=config) + rospy.loginfo("DQN智能体初始化成功") + + episodes = config['train']['episodes'] + rospy.loginfo(f"开始训练:共{episodes}轮Episode") + for e in range(episodes): + # ROS节点关闭时停止训练 + if rospy.is_shutdown(): + rospy.loginfo("【ROS】节点已关闭,停止训练") + break + + state = node.env.reset() + state = state.astype(np.float32) / 255.0 + done = False + total_reward = 0 + step = 0 + + while not done and step < 500 and not rospy.is_shutdown(): + step += 1 + action = node.agent.act(state) + next_state, reward, done, _ = node.env.step(action) + + # 数据预处理 + next_state = next_state.astype(np.float32) / 255.0 + reward = np.clip(reward, -10, 10) + + # 记忆存储 + node.agent.remember(state, action, reward, next_state, done) + state = next_state + total_reward += reward + + # 经验回放 + if len(node.agent.memory) > config['train']['batch_size']: + node.agent.replay(config['train']['batch_size']) + + # 发布控制指令到ROS(从action解析油门/转向,需匹配你的action映射) + # 替换下面的映射规则为你实际的action→控制指令逻辑 + if action == 0: + throttle, steer = 0.4, 0.0 + elif action == 1: + throttle, steer = 0.3, -0.2 + elif action == 2: + throttle, steer = 0.3, 0.2 + else: + throttle, steer = 0.0, 0.0 + node.publish_control(throttle, steer) + + # ROS频率控制 + node.rate.sleep() + + # 打印训练日志(每5轮一次) + if (e + 1) % 5 == 0: + rospy.loginfo(f"Episode {e+1:4d}/{episodes}, Total Reward: {total_reward:6.1f}, 探索率: {node.agent.epsilon:.4f}") + + # 模型优化(剪枝+量化) + rospy.loginfo("开始模型剪枝...") + pruner = ModelPruner(node.agent.model) + pruner.prune_model(amount=0.2) + rospy.loginfo("模型剪枝完成(移除20%权重)") + + rospy.loginfo("开始模型量化...") + node.agent.model = quantize_model(node.agent.model) + rospy.loginfo("模型量化完成") + + # 导出ONNX模型 + rospy.loginfo("导出模型为ONNX格式...") + export_to_onnx(node.agent.model, node.state_shape, config.get('model', {}).get('onnx_path', 'model.onnx')) + rospy.loginfo("模型导出成功!") + + # 保存模型权重 + torch.save(node.agent.model.state_dict(), "dqn_carla_final.pth") + rospy.loginfo("模型权重已保存:dqn_carla_final.pth") + + except Exception as e: + rospy.logerr(f"训练过程出错:{e}") + raise + +# -------------------------- +# 原有逻辑:ONNX导出(保留) +# -------------------------- +def export_to_onnx(model, state_shape, file_path='model.onnx'): + dummy_input = torch.randn(1, 3, state_shape[0], state_shape[1]).to(next(model.parameters()).device) + try: + torch.onnx.export( + model, + dummy_input, + file_path, + opset_version=12, + input_names=["input_image"], + output_names=["action_q_values"], + dynamic_axes={"input_image": {0: "batch_size"}, "action_q_values": {0: "batch_size"}} + ) + except Exception as e: + rospy.logerr(f"ONNX导出失败:{e}") + raise + +# -------------------------- +# 原有逻辑:测试函数(适配ROS) +# -------------------------- +def test_model(node, config): + rospy.loginfo("=== 开始测试(ROS版)===") + try: + node.agent = DQNAgent(state_shape=node.state_shape, action_size=node.action_size, config=config) + # 加载训练好的模型 + node.agent.model.load_state_dict(torch.load("dqn_carla_final.pth")) + node.agent.model.eval() # 评估模式 + rospy.loginfo("模型加载成功,进入评估模式") + + rospy.loginfo("开始测试(10轮)...") + for e in range(10): + if rospy.is_shutdown(): + rospy.loginfo("【ROS】节点已关闭,停止测试") + break + + state = node.env.reset() + state = state.astype(np.float32) / 255.0 + done = False + total_reward = 0 + step = 0 + while not done and step < 500 and not rospy.is_shutdown(): + step += 1 + action = node.agent.act(state) + next_state, reward, done, _ = node.env.step(action) + next_state = next_state.astype(np.float32) / 255.0 + state = next_state + total_reward += reward + + # 发布控制指令到ROS + if action == 0: + throttle, steer = 0.4, 0.0 + elif action == 1: + throttle, steer = 0.3, -0.2 + elif action == 2: + throttle, steer = 0.3, 0.2 + else: + throttle, steer = 0.0, 0.0 + node.publish_control(throttle, steer) + + node.rate.sleep() + rospy.loginfo(f"Test Episode {e+1}, Total Reward: {total_reward:.1f}") + node.env.close() + except Exception as e: + rospy.logerr(f"测试过程出错:{e}") + raise + +# -------------------------- +# 程序入口(ROS适配版) +# -------------------------- +if __name__ == "__main__": + try: + # 1. 解析命令行参数 + args = parse_args() + + # 2. 创建ROS节点实例 + node = CarlaDQNROSNode() + + # 3. 优先使用ROS参数,其次命令行参数 + mode = rospy.get_param("~mode", args.mode) + if not mode: + rospy.logerr("请指定运行模式:--mode train/test 或设置ROS参数 ~mode") + exit(1) + + rospy.loginfo(f"当前运行模式:{mode}") + + # 4. 执行训练/测试 + if mode == 'train': + train_model(node, node.config) + elif mode == 'test': + test_model(node, node.config) + else: + rospy.logerr(f"无效模式:{mode},仅支持 train / test") + + # 5. ROS保持运行(直到手动关闭) + rospy.spin() + except rospy.ROSInterruptException: + rospy.loginfo("【ROS】节点已手动停止!") + except Exception as e: + rospy.logerr(f"\n程序异常退出:{e}") + import traceback + traceback.print_exc() + exit(1) diff --git a/src/MMAP-DRL-Nav/carla_pkg/scripts/pruning.py b/src/MMAP-DRL-Nav/carla_pkg/scripts/pruning.py new file mode 100644 index 0000000000..d9215cd453 --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/scripts/pruning.py @@ -0,0 +1,34 @@ +import torch +import torch.nn as nn +import torch.nn.utils.prune as prune + +class ModelPruner: + def __init__(self, model): + + + self.model = model + + def prune_model(self, amount=0.2): + + for layer in self.model.modules(): + if isinstance(layer, nn.Linear): + prune.ln_structured(layer, name='weight', amount=amount, n=2, dim=0) + print(f"Pruned {amount*100:.1f}% of {layer} weights.") + return self.model + + def remove_pruning(self): + + for layer in self.model.modules(): + if isinstance(layer, nn.Linear): + prune.remove(layer, 'weight') + print(f"Removed pruning from {layer}.") + + def print_pruning_summary(self): + + for layer in self.model.modules(): + if isinstance(layer, nn.Linear): + pruning_amount = 1.0 - torch.count_nonzero(layer.weight) / layer.weight.numel() + print(f"{layer}: {pruning_amount*100:.1f}% pruned.") + + + diff --git a/src/MMAP-DRL-Nav/carla_pkg/scripts/quantization.py b/src/MMAP-DRL-Nav/carla_pkg/scripts/quantization.py new file mode 100644 index 0000000000..68c6776239 --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/scripts/quantization.py @@ -0,0 +1,7 @@ +import torch + +def quantize_model(model): #对模型进行动态量化 + model.eval() + with torch.no_grad(): + quantized_model = torch.quantization.quantize_dynamic(model, {nn.Linear}, dtype=torch.qint8) + return quantized_model diff --git a/src/MMAP-DRL-Nav/carla_pkg/scripts/run_simulation.py b/src/MMAP-DRL-Nav/carla_pkg/scripts/run_simulation.py new file mode 100644 index 0000000000..a5ebd56d33 --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/scripts/run_simulation.py @@ -0,0 +1,141 @@ +# 1. 导入模块(新增 cv2 和 os 用于图像保存) +import torch +import time +import numpy as np +import cv2 # 用于图像保存 +import os # 用于创建目录 +from models.perception_module import PerceptionModule +from models.attention_module import CrossDomainAttention +from models.decision_module import DecisionModule +from models.dqn_agent import DQNAgent +from envs.carla_environment import CarlaEnvironment +import carla + +# 2. 定义 IntegratedSystem 类 +class IntegratedSystem: + def __init__(self, device='cpu'): + self.device = device + self.perception = PerceptionModule().to(self.device) + # 补充 input_dims 参数(与感知模块输出维度匹配) + self.attention = CrossDomainAttention( + num_blocks=6, + input_dims=[256, 256, 6, 256, 256] + ).to(self.device) + self.decision = DecisionModule().to(self.device) + + def forward(self, image, lidar_data, imu_data): + scene_info, segmentation, odometry, obstacles, boundary = self.perception(imu_data, image, lidar_data) + fused_features = self.attention(scene_info, segmentation, odometry, obstacles, boundary) + policy, value = self.decision(fused_features) + return policy, value + +# 3. 定义传感器数据适配函数(桥接CARLA和模型) +def adapt_sensor_data(env, system): + """ + 从CARLA环境获取真实图像,转换为模型输入格式 + (LiDAR/IMU暂用模拟数据,后续可扩展为真实传感器) + """ + # 1. 获取CARLA真实相机图像 (128, 128, 3) → 适配模型输入 + raw_image = env.get_observation() # 真实RGB图像 + # 转换格式:HWC(128,128,3) → CHW(3,128,128) → 缩放至256×256(匹配模型输入) + image = torch.FloatTensor(raw_image).permute(2, 0, 1).unsqueeze(0) / 255.0 # 归一化到[0,1] + image = torch.nn.functional.interpolate(image, size=(256, 256), mode='bilinear') # 缩放至256×256 + image = image.to(system.device) + + # 2. 模拟LiDAR数据(后续需添加CARLA LiDAR传感器) + lidar_data = torch.randn(1, 256, 256).unsqueeze(0).to(system.device) + + # 3. 模拟IMU数据(后续需添加CARLA IMU传感器) + imu_data = torch.randn(1, 6).to(system.device) + + return image, lidar_data, imu_data, raw_image # 新增返回原始图像 + +# 4. 定义图像保存函数 +def save_camera_image(raw_image, step, save_dir="carla_camera_images"): + """ + 保存CARLA相机原始图像到本地 + :param raw_image: 原始RGB图像 (128, 128, 3) + :param step: 仿真步数 + :param save_dir: 保存目录 + """ + # 创建保存目录(不存在则创建) + if not os.path.exists(save_dir): + os.makedirs(save_dir) + + # 转换RGB格式为OpenCV的BGR格式(OpenCV默认BGR) + image_bgr = cv2.cvtColor(raw_image, cv2.COLOR_RGB2BGR) + + # 定义保存路径 + save_path = os.path.join(save_dir, f"camera_step_{step:03d}.png") # 03d 补零,如 001, 002 + + # 保存图像 + cv2.imwrite(save_path, image_bgr) + + # 打印保存日志 + print(f"📸 第 {step} 步相机图像已保存:{save_path}") + +# 5. 定义 run_simulation 函数 +def run_simulation(): + # 初始化CARLA环境 + env = CarlaEnvironment() + # 关键:调用reset()生成车辆和相机,初始化self.vehicle + env.reset() + + # 校验车辆是否生成成功 + if env.vehicle is None: + raise RuntimeError("❌ 车辆生成失败!请检查:\n1. CARLA模拟器是否启动\n2. 端口是否为2000\n3. 地图是否加载完成") + + # 初始化集成系统 + system = IntegratedSystem(device='cuda' if torch.cuda.is_available() else 'cpu') + + print("✅ 仿真开始,运行100步...") + # 控制保存频率:比如每5步保存一张,或只保存前10张,避免文件过多 + save_frequency = 5 # 每5步保存一次 + max_save_images = 10 # 最多保存10张图像 + + saved_count = 0 + for step in range(100): + try: + # 获取适配后的传感器数据 + 原始图像 + image, lidar_data, imu_data, raw_image = adapt_sensor_data(env, system) + + # 保存相机图像(按频率保存,且不超过最大数量) + if (step + 1) % save_frequency == 0 and saved_count < max_save_images: + save_camera_image(raw_image, step + 1) + saved_count += 1 + + # 前向传播得到策略 + policy, value = system.forward(image, lidar_data, imu_data) + + # 转换为CARLA控制信号(限制范围避免异常) + throttle = float(torch.clamp(policy[0][0], 0, 1)) # 油门范围[0,1] + steer = float(torch.clamp(policy[0][1], -1, 1)) # 转向范围[-1,1] + control = carla.VehicleControl(throttle=throttle, steer=steer) + + # 应用控制指令到车辆 + env.vehicle.apply_control(control) + + # 打印运行日志(方便调试) + print(f"第 {step+1:3d} 步 | 油门:{throttle:.2f} | 转向:{steer:.2f} | 相机图像像素范围:{raw_image.min()}~{raw_image.max()}") + + time.sleep(0.1) # 模拟时间间隔 + + except Exception as e: + print(f"❌ 第 {step+1} 步出错:{str(e)}") + break + + # 仿真结束,清理环境 + env.close() + print("✅ 仿真结束,环境已清理") + print(f"📂 共保存 {saved_count} 张相机图像到:carla_camera_images/ 目录") + +# 6. 程序入口(放在最后) +if __name__ == "__main__": + # 检查OpenCV是否安装 + try: + import cv2 + except ImportError: + print("❌ 未安装OpenCV,无法保存图像!请执行:pip install opencv-python") + exit(1) + + run_simulation() \ No newline at end of file diff --git a/src/MMAP-DRL-Nav/carla_pkg/scripts/train.py b/src/MMAP-DRL-Nav/carla_pkg/scripts/train.py new file mode 100644 index 0000000000..8ed9b49d73 --- /dev/null +++ b/src/MMAP-DRL-Nav/carla_pkg/scripts/train.py @@ -0,0 +1,144 @@ +# 强制打印脚本标识,确认运行的是新版本 +print("=====================================") +print("✅ 运行的是最终版训练脚本(train_final.py)") +print("=====================================") + +import torch +import torch.nn as nn +import torch.optim as optim +import numpy as np +import yaml +from models.dqn_agent import DQNAgent +from models.pruning import ModelPruner +from models.quantization import quantize_model +from envs.carla_environment import CarlaEnvironment + +def load_config(config_path='configs/config.yaml'): + """加载配置文件""" + try: + with open(config_path, 'r') as file: + config = yaml.safe_load(file) + print(f"✅ 成功加载配置文件:{config_path}") + return config + except Exception as e: + raise ValueError(f"❌ 加载配置文件失败:{e}") + +def train_model(config): + """训练DQN模型(适配CARLA真实图像输入)""" + # 1. 初始化设备(GPU/CPU) + device = torch.device("cuda" if torch.cuda.is_available() else "cpu") + print(f"✅ 使用设备:{device}") + + # 2. 初始化CARLA环境 + print("🔧 初始化CARLA环境...") + env = CarlaEnvironment() + state_shape = env.observation_space.shape # (128, 128, 3) 完整图像形状 + action_size = env.action_space.n + print(f"✅ CARLA环境初始化成功 | 状态形状:{state_shape} | 动作维度:{action_size}") + + # 3. 初始化DQN智能体(仅传state_shape,绝对不含state_size) + print("🔧 初始化DQN智能体...") + agent = DQNAgent( + state_shape=state_shape, # 唯一正确的参数名 + action_size=action_size, + config=config + ) + print("✅ DQN智能体初始化成功") + + # 4. 训练参数 + episodes = config['train']['episodes'] + batch_size = config['train']['batch_size'] + reward_history = [] # 记录奖励历史 + + # 5. 开始训练 + print(f"🚀 开始训练:共{episodes}轮Episode") + for e in range(episodes): + # 重置环境,获取初始状态 + state = env.reset() + # 图像归一化(0-255 → 0-1) + state = state.astype(np.float32) / 255.0 + done = False + total_reward = 0 + step = 0 + + while not done: + step += 1 + # 选择动作 + action = agent.act(state) + # 执行动作,获取环境反馈 + next_state, reward, done, _ = env.step(action) + + # 数据预处理 + next_state = next_state.astype(np.float32) / 255.0 # 归一化 + reward = np.clip(reward, -10, 10) # 奖励裁剪,避免极端值 + + # 存储经验 + agent.remember(state, action, reward, next_state, done) + # 更新状态 + state = next_state + total_reward += reward + + # 经验回放(批量更新) + if len(agent.memory) > batch_size: + agent.replay(batch_size) + + # 防止单轮步数过多 + if step > 500: + done = True + + # 记录奖励,打印训练日志 + reward_history.append(total_reward) + avg_reward = np.mean(reward_history[-10:]) if len(reward_history) >= 10 else total_reward + print(f"📊 Episode {e+1}/{episodes} | 总奖励:{total_reward:.2f} | 最近10轮平均:{avg_reward:.2f} | 探索率:{agent.epsilon:.4f}") + + # 6. 模型优化(剪枝+量化) + print("\n🔧 开始模型优化(剪枝+量化)...") + try: + pruner = ModelPruner(agent.model) + pruner.prune_model(amount=0.2) # 剪枝20%参数 + agent.model = quantize_model(agent.model) # 量化模型 + print("✅ 模型优化完成") + except Exception as e: + print(f"⚠️ 模型优化失败(可忽略):{e}") + + # 7. 导出ONNX模型 + try: + export_to_onnx(agent.model, state_shape, device) + except Exception as e: + print(f"⚠️ ONNX导出失败(可忽略):{e}") + + # 8. 保存模型权重 + torch.save(agent.model.state_dict(), "dqn_carla_model_final.pth") + print("✅ 模型权重已保存:dqn_carla_model_final.pth") + + # 9. 清理环境 + env.close() + print("\n🎉 训练完成!") + +def export_to_onnx(model, state_shape, device, file_path='model_final.onnx'): + """导出ONNX模型(适配CNN图像输入)""" + # 构建dummy input:(1, 3, 128, 128) + dummy_input = torch.randn(1, 3, state_shape[0], state_shape[1]).to(device) + # 导出ONNX + torch.onnx.export( + model, + dummy_input, + file_path, + opset_version=12, + input_names=["input_image"], + output_names=["action_q_values"], + dynamic_axes={"input_image": {0: "batch_size"}, "action_q_values": {0: "batch_size"}} + ) + print(f"✅ ONNX模型已导出:{file_path}") + +if __name__ == "__main__": + # 加载配置 + config = load_config() + # 启动训练 + try: + train_model(config) + except Exception as e: + print(f"❌ 训练过程出错:{e}") + import traceback + traceback.print_exc() # 打印详细错误栈 + raise \ No newline at end of file