diff --git a/src/MMAP-DRL-Nav/carla_environment.py b/src/MMAP-DRL-Nav/carla_environment.py index ab6cf28dfd..a61fec162d 100644 --- a/src/MMAP-DRL-Nav/carla_environment.py +++ b/src/MMAP-DRL-Nav/carla_environment.py @@ -25,7 +25,7 @@ def is_port_used(port): s.close() if not is_port_used(2000): - raise RuntimeError("❌ 2000端口未被占用，请先启动CARLA模拟器") + raise RuntimeError("2000端口未被占用，请先启动CARLA模拟器") max_retry = 3 retry_count = 0 @@ -35,10 +35,10 @@ def is_port_used(port): break except RuntimeError as e: retry_count += 1 - print(f"⚠️ 连接失败，重试第{retry_count}次...") + print(f"连接失败，重试第{retry_count}次...") time.sleep(5) else: - raise RuntimeError("❌ CARLA连接超时（3次重试失败），请检查模拟器是否正常启动") + raise RuntimeError("CARLA连接超时（3次重试失败），请检查模拟器是否启动") self.blueprint_library = self.world.get_blueprint_library() @@ -56,26 +56,45 @@ def is_port_used(port): 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 - # ========== 红绿灯奖惩核心配置（长间隔防重复） ========== - self.red_light_penalty = -10.0 # 闯红灯扣分 - self.green_light_reward = 5.0 # 绿灯加分 - self.traffic_light_cooldown = 10.0 # 延长冷却到10秒（核心修改） - self.last_traffic_light_time = 0 # 上次奖惩时间 - self.traffic_light_trigger_distance = 10.0 # 触发距离 - self.traffic_light_reset_distance = 15.0 # 离开15米才重置标记 - self.has_triggered_red = False # 红灯触发标记 - self.has_triggered_green = False # 绿灯触发标记 - - # ========== 新增：超速检测配置 ========== - self.speed_limit_urban = 30.0 # 城区限速（km/h） - self.speed_limit_highway = 50.0 # 高速/郊区限速（km/h） - self.over_speed_light_penalty = -1.0 # 轻度超速（超10km/h内）扣分/帧 - self.over_speed_heavy_penalty = -3.0 # 重度超速（超10km/h以上）扣分/帧 - self.last_over_speed_time = 0 # 上次超速扣分时间（避免刷屏） - self.over_speed_cooldown = 1.0 # 超速扣分冷却（1秒/次） - - # 出生点碰撞检测配置 + # ========== 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.spawn_retry_times = 20 self.spawn_safe_radius = 2.0 @@ -98,15 +117,9 @@ def is_port_used(port): self.view_distance = 8.0 self.z_offset = 0.5 - # ========== 核心修复：红绿灯判定（长间隔+防重复） ========== + # ========== 红绿灯判定（移除特殊符号） ========== def _check_traffic_light(self): - """ - 优化逻辑： - 1. 冷却时间延长到10秒，单次闯红灯仅扣1次分 - 2. 车辆离开15米范围才重置标记，低速行驶不重复判定 - """ current_time = time.time() - # 10秒冷却内不判定 if current_time - self.last_traffic_light_time < self.traffic_light_cooldown: return 0.0 @@ -116,50 +129,50 @@ def _check_traffic_light(self): vehicle_loc = self.vehicle.get_transform().location traffic_lights = self.world.get_actors().filter('traffic.traffic_light') reward = 0.0 - has_near_light = False # 是否有近距离灯 + 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) - # 标记：有10米内的灯 if dist <= self.traffic_light_trigger_distance: has_near_light = True light_state = light.state - # 红灯：仅首次触发扣分 - if light_state == carla.TrafficLightState.Red and not self.has_triggered_red: - reward = self.red_light_penalty - print(f"⚠️ 闯红灯！扣分{self.red_light_penalty}") - self.has_triggered_red = True - self.has_triggered_green = False - self.last_traffic_light_time = current_time + + # 红灯逻辑：停车奖励 / 闯灯惩罚 + 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}") + print(f"绿灯合规通过！加分{self.green_light_reward}") self.has_triggered_green = True - self.has_triggered_red = False self.last_traffic_light_time = current_time break - # 离开15米范围，才重置触发标记 + 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): - """ - 超速检测逻辑： - 1. 将车辆速度从m/s转换为km/h（1 m/s = 3.6 km/h） - 2. 区分城区/高速限速（默认按城区30km/h） - 3. 轻度/重度超速分级惩罚，1秒冷却避免刷屏 - """ current_time = time.time() if current_time - self.last_over_speed_time < self.over_speed_cooldown: return 0.0 @@ -167,30 +180,81 @@ def _check_over_speed(self): if not self.vehicle or not self.vehicle.is_alive: return 0.0 - # 获取车辆速度（m/s）并转换为km/h + # 速度计算（m/s → km/h） 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 - # 简化：默认按城区限速30km/h（可扩展为按车道/地图区分） - speed_limit = self.speed_limit_urban - over_speed = speed_km_h - speed_limit - + # 超速判定 + over_speed = speed_km_h - self.speed_limit_urban reward = 0.0 if over_speed > 0: - # 轻度超速（0~10km/h） if 0 < over_speed <= 10: reward = self.over_speed_light_penalty - print(f"⚠️ 轻度超速！当前{speed_km_h:.1f}km/h，限速{speed_limit}km/h，扣分{self.over_speed_light_penalty}") - # 重度超速（>10km/h） else: reward = self.over_speed_heavy_penalty - print(f"❌ 重度超速！当前{speed_km_h:.1f}km/h，限速{speed_limit}km/h，扣分{self.over_speed_heavy_penalty}") self.last_over_speed_time = current_time return reward - # ========== 安全生成车辆（保留） ========== + # ========== 车道偏离检测（3秒检测 + 10秒日志 + 无特殊符号） ========== + def _check_lane_offset(self): + current_time = time.time() + # 仅每3秒执行一次检测 + 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 + + # 获取当前车道waypoint和车辆位置 + vehicle_transform = self.vehicle.get_transform() + waypoint = self.world.get_map().get_waypoint(vehicle_transform.location, project_to_road=True) + + # 完全偏离道路（无可用waypoint） + if not waypoint: + # 仅每10秒输出一次日志 + 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)) + + # 按偏移量分级判定（3秒检测一次，10秒日志一次） + reward = 0.0 + if offset < self.lane_offset_light: + # 保持车道内：奖励（无日志） + reward = self.lane_keep_reward + elif offset < self.lane_offset_heavy: + # 轻微偏离：单次扣0.2分 + reward = self.lane_light_penalty + # 仅每10秒输出一次日志 + 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: + # 严重偏离：单次扣1.0分 + reward = self.lane_heavy_penalty + # 仅每10秒输出一次日志 + 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: @@ -206,15 +270,15 @@ def _spawn_vehicle_safely(self, vehicle_bp): try: vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) if vehicle is not None: - print(f"✅ 车辆生成成功（重试{attempt}次）") + print(f"车辆生成成功（重试{attempt}次）") return vehicle except RuntimeError as e: - print(f"⚠️ 出生点碰撞，重试第{attempt+1}次...") + print(f"出生点碰撞，重试第{attempt+1}次...") continue - raise RuntimeError("❌ 所有出生点都有碰撞，无法生成车辆！") + raise RuntimeError("所有出生点都有碰撞，无法生成车辆！") - # ========== 优化NPC生成（保留） ========== + # ========== 优化NPC生成 ========== def _spawn_small_npc(self): for v in self.npc_vehicle_list: if v.is_alive: @@ -273,7 +337,7 @@ def _spawn_small_npc(self): except: continue - # ========== 初始化碰撞传感器（保留） ========== + # ========== 初始化碰撞传感器 ========== def _init_collision_sensor(self): collision_bp = self.blueprint_library.find('sensor.other.collision') collision_transform = carla.Transform(carla.Location(x=0, y=0, z=0)) @@ -282,7 +346,19 @@ 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 + 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.blueprint_library.find('sensor.camera.rgb') camera_bp.set_attribute('image_size_x', '128') @@ -293,7 +369,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: @@ -303,13 +379,15 @@ def reset(self): 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 @@ -317,6 +395,10 @@ def reset(self): # 重置超速标记 self.last_over_speed_time = 0 + # 重置车道检测/日志标记 + self.last_lane_check_time = 0 + self.last_lane_log_time = 0 + # 生成车辆 vehicle_bp = self.blueprint_library.filter('vehicle.tesla.model3')[0] self.vehicle = self._spawn_vehicle_safely(vehicle_bp) @@ -340,21 +422,7 @@ 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 _collision_callback(self, event): - self.has_collision = True - other_actor_type = event.other_actor.type_id - if 'vehicle' in other_actor_type: - self.hit_vehicle = True - elif 'walker' in other_actor_type: - self.hit_pedestrian = True - - # ========== 相机回调（保留） ========== - 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 follow_vehicle(self): spectator = self.world.get_spectator() if not spectator or not self.vehicle or not self.vehicle.is_alive: @@ -373,11 +441,16 @@ 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()") @@ -408,23 +481,60 @@ def step(self, action): self.world.tick() self.follow_vehicle() - # 计算总奖励（基础+红绿灯+超速） + # ========== 1. 基础行驶奖励 ========== base_reward = 0.1 if throttle > 0 else (-0.1 if throttle < 0 else 0.0) + + # ========== 2. 红绿灯奖惩 ========== traffic_light_reward = self._check_traffic_light() - over_speed_reward = self._check_over_speed() # 新增：超速奖惩 - total_reward = base_reward + traffic_light_reward + over_speed_reward + + # ========== 3. 超速奖惩 ========== + over_speed_reward = self._check_over_speed() + + # ========== 4. 车道偏离奖惩（3秒检测 + 10秒日志） ========== + lane_reward = self._check_lane_offset() + + # ========== 5. 碰撞奖惩（仅扣一次分 + 移除特殊符号） ========== + collision_reward = 0.0 + 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 + + # ========== 总奖励计算 ========== + total_reward = ( + base_reward # 基础行驶 + + traffic_light_reward # 红绿灯 + + over_speed_reward # 超速 + + lane_reward # 车道偏离 + + collision_reward # 碰撞（仅一次） + ) next_state = self.get_observation() - done = self.hit_vehicle return next_state, total_reward, done, { "base_reward": base_reward, "traffic_light_reward": traffic_light_reward, - "over_speed_reward": over_speed_reward, # 新增：返回超速奖励 + "over_speed_reward": over_speed_reward, + "lane_reward": lane_reward, + "collision_reward": collision_reward, "total_reward": total_reward } - # ========== 关闭环境（保留） ========== + # ========== 关闭环境（移除特殊符号） ========== def close(self): # 清理NPC for v in self.npc_vehicle_list: @@ -442,26 +552,26 @@ def close(self): self.sync_settings.synchronous_mode = False self.world.apply_settings(self.sync_settings) except Exception as e: - print(f"⚠️ 恢复异步模式时警告：{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}") + 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}") + 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}") + print(f"销毁碰撞传感器时警告：{e}") time.sleep(0.5) - print("✅ CARLA环境已关闭（同步模式已恢复为异步）") \ No newline at end of file + print("CARLA环境已关闭（同步模式已恢复为异步）") \ No newline at end of file