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