diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/README.md b/src/Autonomous_vehicle_navigation_using_deep_learning_master/README.md
index 4940205df8..dc80010bc9 100644
--- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/README.md
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/README.md
@@ -1,119 +1,101 @@
# Autonomous Vehicle Navigation Using Deep Learning
-
-本项目基于深度学习实现自动驾驶汽车在CARLA仿真环境中的导航系统,支持自定义轨迹规划和行人动态模拟。
-
-## 快速开始
+基于深度学习的自动驾驶汽车导航系统,支持CARLA仿真环境。
+## 🚀 快速开始
### 环境要求
-- **操作系统**: Ubuntu 20.04
-- **仿真环境**: CARLA 0.9.13
-- **Python**: 3.7
-- **包管理**: Conda虚拟环境
+- Ubuntu 20.04
+- CARLA 0.9.13
+- Python 3.7
+- ROS Noetic
-### 安装步骤
+## 📁 项目结构
+```bash
+text
+├── main/ # 主程序目录
+├── models/ # 预训练模型
+├── test/ # 测试脚本
+└── carla_ros_ws/ # ROS版本(可选)
+```
-1. **安装依赖包**:
+### 安装依赖
```bash
-conda create -n carla-env python=3.7
-conda activate carla-env
+# 创建Python环境
+conda create -n carla-ros python=3.7
+conda activate carla-ros
pip install -r requirements.txt
```
-2. **启动CARLA仿真器**:
+## 🏗️ ROS工作空间构建
+
+### 1. 构建ROS包
```bash
-./CarlaUE4.sh
+cd carla_ros_ws
+catkin_make
+source devel/setup.bash
```
-3. **运行主程序**:
+### 2. 安装ROS依赖
```bash
-cd main
-python main.py
+cd src/carla_autonomous/utils
+./install.sh
```
-4. **运行测试程序**:
+## 🎮 运行方法
+
+### 一键启动
```bash
-cd test
-python test_driving.py
+cd carla_ros_ws/src/carla_autonomous/utils
+./run_carla.sh
```
-## 项目结构
-```
-│ README.md
-│ requirements.txt
-├─agents
-│ │ __init__.py
-│ ├─navigation
-│ │ basic_agent.py
-│ │ behavior_agent.py
-│ │ behavior_types.py
-│ │ controller.py
-│ │ global_route_planner.py
-│ │ local_planner.py
-│ │ __init__.py
-│ └─tools
-│ misc.py
-│ __init__.py
-├─main
-│ car_env.py
-│ config.py
-│ config_manager.py
-│ get_location.py
-│ main.py
-│ model_manager.py
-│ route_visualizer.py
-│ traffic_manager.py
-│ trajectory_manager.py
-│ vehicle_tracker.py
-├─models
-│ Braking___282.00max__282.00avg__282.00min__1679121006.model
-│ Driving__6030.00max_6030.00avg_6030.00min__1679109656.model
-└─test
- braking_dqn.py
- driving_dqn.py
- pedestrians_1.py
- pedestrians_2.py
- test_braking.py
- test_driving.py
+### 手动启动
+```bash
+# 终端1:启动CARLA
+./CarlaUE4.sh
+
+# 终端2:启动ROS节点
+cd carla_ros_ws
+source devel/setup.bash
+roslaunch carla_autonomous carla_autonomous.launch
+
+# 终端3:控制客户端
+cd carla_ros_ws
+source devel/setup.bash
+python src/carla_autonomous/scripts/carla_control_client.py
```
-## 核心功能
+## 📖 基础版本(无ROS)
-### 1. 自定义轨迹规划
-使用 `get_location.py` 获取当前摄像头坐标,配置到 `config.py`:
+1. 启动CARLA仿真器
-```python
-TRAJECTORIES = {
- "custom_trajectory": {
- "start": [x, y, z, yaw], # 起点坐标和朝向
- "end": [x, y, z], # 终点坐标
- "description": "自定义轨迹 - 城镇道路"
- }
-}
+```bash
+./CarlaUE4.sh
```
+2. 配置轨迹点
-### 2. 模型测试
-- **刹车测试**: `test_braking.py` - 验证紧急制动性能
-- **驾驶测试**: `test_driving.py` - 评估导航准确性
-### 3. 行人模拟
-- `pedestrians_1.py` - 随机行人生成(模式1)
-- `pedestrians_2.py` - 随机行人生成(模式2)
-## 配置说明
+python main/get_location.py # 获取当前坐标
-### 关键配置文件
-`config.py` 包含所有可调整参数:
-- 轨迹起点/终点坐标
-- 深度学习模型参数
-- 仿真环境设置
+编辑 main/config.py 中的 TRAJECTORIES 配置
-## 参考项目
-本项目参考自: [varunpratap222/Autonomous-Vehicle-Navigation-Using-Deep-Learning](https://github.com/varunpratap222/Autonomous-Vehicle-Navigation-Using-Deep-Learning.git)
+```bash
+# 运行主程序
+python main/main.py
+```
+
+## ⚡ 快速测试
+```bash
+# 刹车测试
+python test/test_braking.py
-## 📝 注意事项
-1. 确保CARLA仿真器已正确启动
-2. 建议在独立的Conda环境中运行
-3. 行人模拟模块需要额外计算资源
+# 驾驶测试
+python test/test_driving.py
+```
----
+## ⚠️ 注意事项
+1. 必须先构建ROS工作空间:`catkin_make`
+2. 每次使用前需要source:`source devel/setup.bash`
+3. 确保CARLA版本为0.9.13
-**温馨提示**: 运行前请确认CARLA版本为0.9.13,Python版本为3.7,以避免兼容性问题。
\ No newline at end of file
+## 📖 参考
+参考项目:varunpratap222/Autonomous-Vehicle-Navigation-Using-Deep-Learning
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/CMakeLists.txt b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/CMakeLists.txt
new file mode 100644
index 0000000000..115ffb8cf7
Binary files /dev/null and b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/CMakeLists.txt differ
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/CMakeLists.txt b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/CMakeLists.txt
new file mode 100644
index 0000000000..9db848299e
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/CMakeLists.txt
@@ -0,0 +1,77 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(carla_autonomous)
+
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ std_msgs
+ geometry_msgs
+ sensor_msgs
+ nav_msgs
+ visualization_msgs
+ tf
+ cv_bridge
+ message_generation
+)
+
+add_service_files(
+ FILES
+ Reset.srv
+ StartEpisode.srv
+ Stop.srv
+)
+
+generate_messages(
+ DEPENDENCIES
+ std_msgs
+ geometry_msgs
+)
+
+catkin_package(
+ CATKIN_DEPENDS
+ rospy
+ std_msgs
+ geometry_msgs
+ sensor_msgs
+ nav_msgs
+ visualization_msgs
+ tf
+ cv_bridge
+ message_runtime
+)
+
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+)
+
+# 安装Python脚本
+catkin_install_python(PROGRAMS
+ scripts/carla_autonomous_node.py
+ scripts/carla_control_client.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# 安装其他文件
+install(DIRECTORY launch/
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+
+install(DIRECTORY config/
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
+)
+
+install(DIRECTORY src/
+ DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
+ PATTERN "*.py"
+)
+
+install(DIRECTORY models/
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/models
+)
+
+install(DIRECTORY pics/
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/pics
+)
+
+install(DIRECTORY utils/
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/utils
+)
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/__init__.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/__init__.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/__init__.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/__init__.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/__init__.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/__init__.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/__init__.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/__init__.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/basic_agent.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/basic_agent.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/basic_agent.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/basic_agent.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/behavior_agent.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/behavior_agent.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/behavior_agent.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/behavior_agent.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/behavior_types.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/behavior_types.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/behavior_types.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/behavior_types.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/controller.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/controller.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/controller.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/controller.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/global_route_planner.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/global_route_planner.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/global_route_planner.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/global_route_planner.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/local_planner.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/local_planner.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/navigation/local_planner.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/navigation/local_planner.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/tools/__init__.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/tools/__init__.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/tools/__init__.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/tools/__init__.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/tools/misc.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/tools/misc.py
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/agents/tools/misc.py
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/agents/tools/misc.py
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/config/carla.rviz b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/config/carla.rviz
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/config/ros_config.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/config/ros_config.py
new file mode 100644
index 0000000000..40466783ec
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/config/ros_config.py
@@ -0,0 +1,38 @@
+"""
+ROS特定的配置参数
+"""
+
+# ROS主题名称
+ROS_TOPICS = {
+ 'vehicle_state': '/carla/vehicle_state',
+ 'vehicle_control': '/carla/vehicle_control',
+ 'camera_image': '/carla/camera/image',
+ 'segmentation_image': '/carla/camera/segmentation',
+ 'planned_path': '/carla/planned_path',
+ 'visualization': '/carla/visualization',
+ 'status': '/carla/status',
+ 'reward': '/carla/reward'
+}
+
+# ROS服务名称
+ROS_SERVICES = {
+ 'reset': '/carla/reset',
+ 'start_episode': '/carla/start_episode',
+ 'stop': '/carla/stop'
+}
+
+# ROS参数
+ROS_PARAMS = {
+ 'node_name': 'carla_autonomous_node',
+ 'rate_hz': 30.0,
+ 'queue_size': 10,
+ 'latch': False
+}
+
+# 坐标框架
+ROS_FRAMES = {
+ 'map': 'map',
+ 'base_link': 'base_link',
+ 'camera': 'camera_link',
+ 'world': 'world'
+}
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/config/ros_params.yaml b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/config/ros_params.yaml
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/launch/carla_autonomous.launch b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/launch/carla_autonomous.launch
new file mode 100644
index 0000000000..fb957762db
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/launch/carla_autonomous.launch
@@ -0,0 +1,45 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/launch/carla_manual.launch b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/launch/carla_manual.launch
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/launch/carla_with_rviz.launch b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/launch/carla_with_rviz.launch
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Braking___282.model b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/models/Braking___282.model
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Braking___282.model
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/models/Braking___282.model
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Driving__6030.model b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/models/Driving__6030.model
similarity index 100%
rename from src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Driving__6030.model
rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/models/Driving__6030.model
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/package.xml b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/package.xml
new file mode 100644
index 0000000000..75261c32a2
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/package.xml
@@ -0,0 +1,34 @@
+
+
+ carla_autonomous
+ 0.1.0
+ CARLA Autonomous Driving ROS Interface
+
+ Your Name
+ MIT
+
+ catkin
+
+ message_generation
+ std_msgs
+ geometry_msgs
+ sensor_msgs
+ nav_msgs
+ visualization_msgs
+ rospy
+ tf
+ cv_bridge
+ rospy
+ std_msgs
+ geometry_msgs
+ sensor_msgs
+ nav_msgs
+ visualization_msgs
+ tf
+ cv_bridge
+ message_runtime
+
+
+
+
+
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_autonomous_node.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_autonomous_node.py
new file mode 100644
index 0000000000..3b3140a43e
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_autonomous_node.py
@@ -0,0 +1,499 @@
+#!/usr/bin/env python3
+"""
+CARLA自动驾驶ROS节点
+"""
+
+import rospy
+import os
+import sys
+import time
+import threading
+import numpy as np
+
+# 禁用TensorFlow警告
+os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
+os.environ['CUDA_VISIBLE_DEVICES'] = '-1' # 使用CPU
+
+# ROS消息
+from std_msgs.msg import Header, Float32, Bool, Int32, String
+from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
+from sensor_msgs.msg import Image
+from nav_msgs.msg import Odometry, Path
+from visualization_msgs.msg import Marker, MarkerArray
+import tf
+from cv_bridge import CvBridge
+from carla_autonomous.srv import StartEpisode, Stop, Reset
+# 导入项目模块
+project_dir = os.path.join(os.path.dirname(__file__), '..', 'src')
+sys.path.insert(0, os.path.abspath(project_dir))
+from car_env import CarEnv
+from route_visualizer import RouteVisualizer
+from vehicle_tracker import VehicleTracker
+from traffic_manager import TrafficManager
+from model_manager import ModelManager
+import config as cfg
+
+class CarlaAutonomousROS:
+ """CARLA自动驾驶ROS节点"""
+
+ def __init__(self):
+ # 初始化ROS节点
+ rospy.init_node('carla_autonomous_node', anonymous=True)
+
+ # ROS参数
+ self.rate = rospy.Rate(cfg.FPS_LIMIT if cfg.FPS_LIMIT > 0 else 30)
+ self.cv_bridge = CvBridge()
+
+ # 加载配置
+ self.trajectory = cfg.get_current_trajectory()
+ if not self.trajectory:
+ rospy.logerr("无法加载轨迹配置")
+ sys.exit(1)
+
+ # ROS发布器
+ self.vehicle_state_pub = rospy.Publisher('/carla/vehicle_state', Odometry, queue_size=10)
+ self.vehicle_control_pub = rospy.Publisher('/carla/vehicle_control', Twist, queue_size=10)
+ self.camera_image_pub = rospy.Publisher('/carla/camera/image', Image, queue_size=10)
+ self.seg_image_pub = rospy.Publisher('/carla/camera/segmentation', Image, queue_size=10)
+ self.path_pub = rospy.Publisher('/carla/planned_path', Path, queue_size=10)
+ self.marker_pub = rospy.Publisher('/carla/visualization', MarkerArray, queue_size=10)
+ self.status_pub = rospy.Publisher('/carla/status', String, queue_size=10)
+ self.reward_pub = rospy.Publisher('/carla/reward', Float32, queue_size=10)
+
+ # ROS服务
+ self.reset_srv = rospy.Service('/carla/reset', Reset, self.handle_reset)
+ self.start_srv = rospy.Service('/carla/start_episode', StartEpisode, self.handle_start_episode)
+ self.stop_srv = rospy.Service('/carla/stop', Stop, self.handle_stop)
+
+ # ROS订阅器
+ rospy.Subscriber('/carla/control_cmd', Twist, self.control_callback)
+
+ # 初始化变量
+ self.env = None
+ self.model_manager = None
+ self.visualizer = None
+ self.tracker = None
+ self.traffic_mgr = None
+
+ # 控制标志
+ self.running = False
+ self.current_episode = 0
+ self.total_reward = 0.0
+ self.step_count = 0
+
+ # 线程锁
+ self.lock = threading.Lock()
+
+ # 初始化CARLA环境
+ self.init_carla_environment()
+
+ rospy.loginfo("CARLA自动驾驶ROS节点初始化完成")
+
+ def init_carla_environment(self):
+ """初始化CARLA环境"""
+ try:
+ # 创建环境
+ self.env = CarEnv(self.trajectory['start'], self.trajectory['end'])
+
+ # 创建管理器
+ self.model_manager = ModelManager()
+ self.visualizer = RouteVisualizer(self.env.world)
+ self.tracker = VehicleTracker(self.env.world)
+
+ # 加载模型
+ if not self.model_manager.load_models():
+ rospy.logerr("模型加载失败")
+ return False
+
+ # 生成交通
+ if cfg.ENABLE_TRAFFIC:
+ self.traffic_mgr = TrafficManager(self.env.client, self.env.world)
+ self.traffic_mgr.generate_traffic(
+ num_vehicles=cfg.TRAFFIC_VEHICLES,
+ num_walkers=cfg.TRAFFIC_WALKERS,
+ safe_mode=cfg.TRAFFIC_SAFE_MODE
+ )
+
+ rospy.loginfo("CARLA环境初始化成功")
+ return True
+
+ except Exception as e:
+ rospy.logerr(f"初始化CARLA环境失败: {e}")
+ return False
+
+ def control_callback(self, msg):
+ """控制命令回调"""
+ if not self.env or not self.env.vehicle:
+ return
+
+ try:
+ # 将ROS Twist消息转换为CARLA控制
+ control = carla.VehicleControl()
+ control.throttle = max(0.0, min(1.0, msg.linear.x))
+ control.steer = max(-1.0, min(1.0, msg.angular.z))
+ control.brake = max(0.0, min(1.0, msg.linear.z))
+
+ # 应用控制
+ self.env.vehicle.apply_control(control)
+
+ except Exception as e:
+ rospy.logwarn(f"控制命令处理失败: {e}")
+
+ def publish_vehicle_state(self):
+ """发布车辆状态"""
+ if not self.env or not self.env.vehicle:
+ return
+
+ try:
+ # 获取车辆状态
+ vehicle = self.env.vehicle
+ transform = vehicle.get_transform()
+ velocity = vehicle.get_velocity()
+
+ # 创建Odometry消息
+ odom = Odometry()
+ odom.header = Header()
+ odom.header.stamp = rospy.Time.now()
+ odom.header.frame_id = "map"
+ odom.child_frame_id = "base_link"
+
+ # 位置
+ odom.pose.pose.position = Point(
+ x=transform.location.x,
+ y=transform.location.y,
+ z=transform.location.z
+ )
+
+ # 方向(四元数)
+ roll = transform.rotation.roll * np.pi / 180
+ pitch = transform.rotation.pitch * np.pi / 180
+ yaw = transform.rotation.yaw * np.pi / 180
+ quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
+ odom.pose.pose.orientation = Quaternion(*quat)
+
+ # 速度
+ odom.twist.twist.linear = Vector3(
+ x=velocity.x,
+ y=velocity.y,
+ z=velocity.z
+ )
+
+ # 发布
+ self.vehicle_state_pub.publish(odom)
+
+ except Exception as e:
+ rospy.logwarn(f"发布车辆状态失败: {e}")
+
+ def publish_camera_images(self):
+ """发布相机图像"""
+ if not self.env or not hasattr(self.env, 'cam') or self.env.cam is None:
+ return
+
+ try:
+ # RGB图像
+ cv_image = np.frombuffer(self.env.cam.raw_data, dtype=np.uint8)
+ cv_image = cv_image.reshape((self.env.cam.height, self.env.cam.width, 4))
+ cv_image = cv_image[:, :, :3] # 移除alpha通道
+
+ image_msg = self.cv_bridge.cv2_to_imgmsg(cv_image, encoding="bgr8")
+ image_msg.header.stamp = rospy.Time.now()
+ self.camera_image_pub.publish(image_msg)
+
+ # 分割图像
+ if hasattr(self.env, 'seg_array') and self.env.seg_array is not None:
+ seg_msg = self.cv_bridge.cv2_to_imgmsg(self.env.seg_array, encoding="bgr8")
+ seg_msg.header.stamp = rospy.Time.now()
+ self.seg_image_pub.publish(seg_msg)
+
+ except Exception as e:
+ rospy.logwarn(f"发布图像失败: {e}")
+
+ def publish_planned_path(self):
+ """发布规划路径"""
+ if not self.env or not hasattr(self.env, 'path'):
+ return
+
+ try:
+ path_msg = Path()
+ path_msg.header = Header()
+ path_msg.header.stamp = rospy.Time.now()
+ path_msg.header.frame_id = "map"
+
+ for waypoint in self.env.path:
+ pose = Pose()
+ pose.position = Point(
+ x=waypoint.transform.location.x,
+ y=waypoint.transform.location.y,
+ z=waypoint.transform.location.z
+ )
+ path_msg.poses.append(pose)
+
+ self.path_pub.publish(path_msg)
+
+ except Exception as e:
+ rospy.logwarn(f"发布规划路径失败: {e}")
+
+ def publish_visualization_markers(self):
+ """发布可视化标记"""
+ if not self.env or not self.env.vehicle:
+ return
+
+ try:
+ marker_array = MarkerArray()
+
+ # 车辆标记
+ vehicle_marker = Marker()
+ vehicle_marker.header.frame_id = "map"
+ vehicle_marker.header.stamp = rospy.Time.now()
+ vehicle_marker.ns = "vehicle"
+ vehicle_marker.id = 0
+ vehicle_marker.type = Marker.CUBE
+ vehicle_marker.action = Marker.ADD
+
+ transform = self.env.vehicle.get_transform()
+ vehicle_marker.pose.position = Point(
+ x=transform.location.x,
+ y=transform.location.y,
+ z=transform.location.z + 0.5
+ )
+
+ quat = tf.transformations.quaternion_from_euler(
+ 0, 0, transform.rotation.yaw * np.pi / 180
+ )
+ vehicle_marker.pose.orientation = Quaternion(*quat)
+
+ vehicle_marker.scale.x = 2.0
+ vehicle_marker.scale.y = 1.0
+ vehicle_marker.scale.z = 1.5
+ vehicle_marker.color.r = 0.0
+ vehicle_marker.color.g = 1.0
+ vehicle_marker.color.b = 0.0
+ vehicle_marker.color.a = 0.8
+ vehicle_marker.lifetime = rospy.Duration(0.1)
+
+ marker_array.markers.append(vehicle_marker)
+
+ # 目标点标记
+ goal_marker = Marker()
+ goal_marker.header.frame_id = "map"
+ goal_marker.header.stamp = rospy.Time.now()
+ goal_marker.ns = "goal"
+ goal_marker.id = 1
+ goal_marker.type = Marker.SPHERE
+ goal_marker.action = Marker.ADD
+
+ goal_marker.pose.position = Point(
+ x=self.trajectory['end'][0],
+ y=self.trajectory['end'][1],
+ z=self.trajectory['end'][2] + 1.0
+ )
+
+ goal_marker.scale.x = 2.0
+ goal_marker.scale.y = 2.0
+ goal_marker.scale.z = 2.0
+ goal_marker.color.r = 1.0
+ goal_marker.color.g = 0.0
+ goal_marker.color.b = 0.0
+ goal_marker.color.a = 0.8
+ goal_marker.lifetime = rospy.Duration(0.1)
+
+ marker_array.markers.append(goal_marker)
+
+ self.marker_pub.publish(marker_array)
+
+ except Exception as e:
+ rospy.logwarn(f"发布可视化标记失败: {e}")
+
+ def publish_status(self, status_msg):
+ """发布状态信息"""
+ try:
+ status = String()
+ status.data = status_msg
+ self.status_pub.publish(status)
+ except Exception as e:
+ rospy.logwarn(f"发布状态失败: {e}")
+
+ def handle_reset(self, req):
+ """重置环境服务处理"""
+ with self.lock:
+ try:
+ rospy.loginfo("收到重置环境请求")
+
+ # 重置环境
+ if self.env:
+ current_state = self.env.reset()
+ self.publish_status("环境重置成功")
+
+ # 返回当前状态
+ return ResetResponse(current_state)
+ else:
+ rospy.logerr("环境未初始化")
+ return ResetResponse([])
+
+ except Exception as e:
+ rospy.logerr(f"重置环境失败: {e}")
+ return ResetResponse([])
+
+ def handle_start_episode(self, req):
+ """开始episode服务处理"""
+ with self.lock:
+ if self.running:
+ rospy.logwarn("已经有episode在运行")
+ return StartEpisodeResponse(False, "已有episode在运行")
+
+ try:
+ self.running = True
+ self.current_episode += 1
+ self.total_reward = 0.0
+ self.step_count = 0
+
+ rospy.loginfo(f"开始Episode {self.current_episode}")
+
+ # 在新线程中运行episode
+ episode_thread = threading.Thread(target=self.run_episode)
+ episode_thread.start()
+
+ return StartEpisodeResponse(True, f"Episode {self.current_episode} 已开始")
+
+ except Exception as e:
+ rospy.logerr(f"开始episode失败: {e}")
+ self.running = False
+ return StartEpisodeResponse(False, str(e))
+
+ def handle_stop(self, req):
+ """停止服务处理"""
+ with self.lock:
+ rospy.loginfo("收到停止请求")
+ self.running = False
+ return StopResponse(True, "已停止")
+
+ def run_episode(self):
+ """运行一个episode"""
+ rospy.loginfo(f"开始运行Episode {self.current_episode}")
+
+ try:
+ # 重置环境
+ current_state = self.env.reset()
+
+ # 设置车辆跟踪
+ self.tracker.set_follow_view(self.env.vehicle)
+
+ # 主循环
+ done = False
+ while not done and not rospy.is_shutdown() and self.running:
+ if self.step_count >= cfg.MAX_STEPS_PER_EPISODE:
+ rospy.loginfo("达到最大步数")
+ break
+
+ # 模型预测动作
+ action = self.model_manager.predict_action(current_state)
+
+ # 执行动作
+ new_state, reward, done, _ = self.env.step(action, current_state)
+
+ # 更新统计
+ self.total_reward += reward
+ self.step_count += 1
+
+ # 发布奖励
+ reward_msg = Float32()
+ reward_msg.data = reward
+ self.reward_pub.publish(reward_msg)
+
+ # 更新状态
+ current_state = new_state
+
+ # 发布数据
+ self.publish_all_data()
+
+ # 检查ROS是否正常
+ if rospy.is_shutdown():
+ break
+
+ # 控制循环频率
+ self.rate.sleep()
+
+ # 发布最终状态
+ if done:
+ status_msg = f"Episode {self.current_episode} 完成,总奖励: {self.total_reward:.2f}"
+ else:
+ status_msg = f"Episode {self.current_episode} 中断,步数: {self.step_count}"
+
+ self.publish_status(status_msg)
+ rospy.loginfo(status_msg)
+
+ # 重置运行标志
+ self.running = False
+
+ except Exception as e:
+ rospy.logerr(f"运行episode时出错: {e}")
+ self.running = False
+
+ def publish_all_data(self):
+ """发布所有数据"""
+ try:
+ self.publish_vehicle_state()
+ self.publish_camera_images()
+ self.publish_planned_path()
+ self.publish_visualization_markers()
+ except Exception as e:
+ rospy.logwarn(f"发布数据时出错: {e}")
+
+ def run(self):
+ """主运行循环"""
+ rospy.loginfo("CARLA自动驾驶ROS节点开始运行")
+
+ # 等待CARLA服务器
+ time.sleep(2.0)
+
+ # 发布初始状态
+ self.publish_status("节点就绪")
+
+ # 主循环
+ while not rospy.is_shutdown():
+ try:
+ if self.running and self.env and self.env.vehicle:
+ # 发布数据
+ self.publish_all_data()
+
+ # 控制循环频率
+ self.rate.sleep()
+
+ except KeyboardInterrupt:
+ break
+ except Exception as e:
+ rospy.logerr(f"主循环出错: {e}")
+ time.sleep(1.0)
+
+ # 清理
+ self.cleanup()
+ rospy.loginfo("CARLA自动驾驶ROS节点已停止")
+
+ def cleanup(self):
+ """清理资源"""
+ rospy.loginfo("正在清理资源...")
+
+ try:
+ # 停止车辆跟踪
+ if self.tracker:
+ self.tracker.cleanup()
+
+ # 清理交通
+ if self.traffic_mgr:
+ self.traffic_mgr.cleanup()
+
+ # 清理环境
+ if self.env:
+ self.env.cleanup()
+
+ except Exception as e:
+ rospy.logwarn(f"清理资源时出错: {e}")
+
+if __name__ == '__main__':
+ try:
+ node = CarlaAutonomousROS()
+ node.run()
+ except rospy.ROSInterruptException:
+ pass
+ except Exception as e:
+ rospy.logerr(f"节点启动失败: {e}")
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_autonomous_services.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_autonomous_services.py
new file mode 100644
index 0000000000..2e3fc5733a
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_autonomous_services.py
@@ -0,0 +1,36 @@
+#!/usr/bin/env python3
+"""
+CARLA自动驾驶ROS服务定义
+"""
+
+from rospy import Service, ServiceProxy
+from std_srvs.srv import Empty, EmptyResponse
+from std_msgs.msg import Float32MultiArray
+
+# 自定义服务
+from carla_autonomous.srv import *
+
+class Reset(Service):
+ """重置环境服务"""
+ def __init__(self):
+ super().__init__('reset', Reset, self.callback)
+
+ def callback(self, req):
+ # 返回状态数组
+ return ResetResponse(Float32MultiArray())
+
+class StartEpisode(Service):
+ """开始episode服务"""
+ def __init__(self):
+ super().__init__('start_episode', StartEpisode, self.callback)
+
+ def callback(self, req):
+ return StartEpisodeResponse(True, "Episode started")
+
+class Stop(Service):
+ """停止服务"""
+ def __init__(self):
+ super().__init__('stop', Stop, self.callback)
+
+ def callback(self, req):
+ return StopResponse(True, "Stopped")
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_control_client.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_control_client.py
new file mode 100644
index 0000000000..0b6b82a93c
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/scripts/carla_control_client.py
@@ -0,0 +1,140 @@
+#!/usr/bin/env python3
+"""
+CARLA控制客户端 - 用于手动控制或外部控制
+"""
+
+import rospy
+from std_msgs.msg import String, Float32
+from geometry_msgs.msg import Twist
+from carla_autonomous.srv import StartEpisode, Reset, Stop
+
+class CarlaControlClient:
+ def __init__(self):
+ rospy.init_node('carla_control_client')
+
+ # 服务代理
+ self.start_episode = rospy.ServiceProxy('/carla/start_episode', StartEpisode)
+ self.reset = rospy.ServiceProxy('/carla/reset', Reset)
+ self.stop = rospy.ServiceProxy('/carla/stop', Stop)
+
+ # 控制发布器
+ self.control_pub = rospy.Publisher('/carla/control_cmd', Twist, queue_size=10)
+
+ rospy.loginfo("CARLA控制客户端已启动")
+
+ def manual_control(self):
+ """手动控制模式"""
+ rospy.loginfo("进入手动控制模式")
+ rospy.loginfo("使用键盘控制车辆:")
+ rospy.loginfo(" W/S: 前进/后退")
+ rospy.loginfo(" A/D: 左转/右转")
+ rospy.loginfo(" Space: 刹车")
+ rospy.loginfo(" Q: 退出")
+
+ try:
+ import curses
+ stdscr = curses.initscr()
+ curses.noecho()
+ curses.cbreak()
+ stdscr.keypad(True)
+
+ while not rospy.is_shutdown():
+ key = stdscr.getch()
+
+ control_msg = Twist()
+
+ if key == ord('w'):
+ control_msg.linear.x = 0.5 # 前进
+ elif key == ord('s'):
+ control_msg.linear.x = -0.3 # 后退
+ elif key == ord('a'):
+ control_msg.angular.z = 0.5 # 左转
+ elif key == ord('d'):
+ control_msg.angular.z = -0.5 # 右转
+ elif key == ord(' '):
+ control_msg.linear.z = 1.0 # 刹车
+ elif key == ord('q'):
+ break
+ else:
+ continue
+
+ self.control_pub.publish(control_msg)
+
+ except ImportError:
+ rospy.logwarn("curses模块未安装,无法使用键盘控制")
+ except Exception as e:
+ rospy.logerr(f"手动控制出错: {e}")
+ finally:
+ try:
+ curses.nocbreak()
+ stdscr.keypad(False)
+ curses.echo()
+ curses.endwin()
+ except:
+ pass
+
+ def start_autonomous_episode(self):
+ """启动自主驾驶episode"""
+ try:
+ response = self.start_episode()
+ if response.success:
+ rospy.loginfo(f"启动成功: {response.message}")
+ else:
+ rospy.logwarn(f"启动失败: {response.message}")
+ except rospy.ServiceException as e:
+ rospy.logerr(f"服务调用失败: {e}")
+
+ def reset_environment(self):
+ """重置环境"""
+ try:
+ response = self.reset()
+ rospy.loginfo("环境重置成功")
+ except rospy.ServiceException as e:
+ rospy.logerr(f"重置服务调用失败: {e}")
+
+ def stop_all(self):
+ """停止所有"""
+ try:
+ response = self.stop()
+ rospy.loginfo("停止成功")
+ except rospy.ServiceException as e:
+ rospy.logerr(f"停止服务调用失败: {e}")
+
+ def run(self):
+ """运行客户端"""
+ rospy.loginfo("控制命令:")
+ rospy.loginfo(" 1: 启动自主驾驶")
+ rospy.loginfo(" 2: 手动控制")
+ rospy.loginfo(" 3: 重置环境")
+ rospy.loginfo(" 4: 停止")
+ rospy.loginfo(" 0: 退出")
+
+ try:
+ while not rospy.is_shutdown():
+ command = input("请输入命令: ")
+
+ if command == '1':
+ self.start_autonomous_episode()
+ elif command == '2':
+ self.manual_control()
+ elif command == '3':
+ self.reset_environment()
+ elif command == '4':
+ self.stop_all()
+ elif command == '0':
+ rospy.loginfo("退出客户端")
+ break
+ else:
+ rospy.logwarn("未知命令")
+
+ except KeyboardInterrupt:
+ rospy.loginfo("客户端已停止")
+ except Exception as e:
+ rospy.logerr(f"客户端运行出错: {e}")
+
+if __name__ == '__main__':
+ try:
+ client = CarlaControlClient()
+ client.run()
+ except rospy.ROSInterruptException:
+ pass
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/car_env.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/car_env.py
new file mode 100644
index 0000000000..230a4e6cf1
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/car_env.py
@@ -0,0 +1,598 @@
+from __future__ import print_function
+
+import glob
+import os
+import sys
+import random
+
+try:
+ sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
+ sys.version_info.major,
+ sys.version_info.minor,
+ 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+except IndexError:
+ pass
+
+# 获取当前文件的目录
+current_dir = os.path.dirname(os.path.abspath(__file__))
+# 获取父目录
+parent_dir = os.path.dirname(current_dir)
+# 将父目录添加到系统路径
+sys.path.append(parent_dir)
+from agents.navigation.global_route_planner import GlobalRoutePlanner
+
+import carla
+
+from carla import ColorConverter as cc
+from carla import Transform
+from carla import Location
+from carla import Rotation
+
+from PIL import Image
+
+import keras
+import tensorflow as tf
+
+import argparse
+import collections
+import datetime
+import logging
+import math
+import random
+import re
+import weakref
+import time
+import numpy as np
+import cv2
+from collections import deque
+from keras.applications.xception import Xception
+from tensorflow.keras.optimizers import Adam
+from keras.models import Model
+from keras.callbacks import TensorBoard
+
+from keras.models import Sequential, Model, load_model
+from keras.layers import AveragePooling2D, Conv2D, Activation, Flatten, GlobalAveragePooling2D, Dense, Concatenate, Input
+
+import tensorflow as tf
+import keras.backend.tensorflow_backend as backend
+from threading import Thread
+from tensorflow.keras import regularizers
+import matplotlib.pyplot as plt
+
+from tqdm import tqdm
+
+SHOW_PREVIEW = False
+IM_WIDTH = 640
+IM_HEIGHT = 480
+SECONDS_PER_EPISODE = 20
+REPLAY_MEMORY_SIZE = 5_000
+MIN_REPLAY_MEMORY_SIZE = 1_000
+MINIBATCH_SIZE = 16
+PREDICTION_BATCH_SIZE = 1
+TRAINING_BATCH_SIZE = MINIBATCH_SIZE // 4
+UPDATE_TARGET_EVERY = 2
+MODEL_NAME = "Braking"
+
+MEMORY_FRACTION = 0.8
+MIN_REWARD = 0
+
+EPISODES = 20
+DISCOUNT = 0.99
+epsilon = 0.99
+EPSILON_DECAY = 0.95
+MIN_EPSILON = 0.01
+
+AGGREGATE_STATS_EVERY = 10
+
+class CarEnv:
+
+ SHOW_CAM = SHOW_PREVIEW
+ STEER_AMT = 1.0 # actions that the agent can take [-1, 0, 1] --> [turn left, go straight, turn right]
+ im_width = IM_WIDTH
+ im_height = IM_HEIGHT
+ front_camera = None
+
+ def __init__(self, start, end):
+ # to initialize
+ self.client = carla.Client("localhost", 2000)
+ self.client.set_timeout(20.0)
+ self.world = self.client.get_world()
+ self.blueprint_library = self.world.get_blueprint_library()
+ self.model_3 = self.blueprint_library.find("vehicle.tesla.model3")
+ self.front_model3 = self.blueprint_library.find("vehicle.tesla.model3")
+ self.via = 2
+ self.crossing = 0
+ self.reached = 0
+ self.final_destination = end
+ self.initial_pos = start
+ self.distance = 0
+ self.cam = None
+ self.seg = None
+ self.actor_list = []
+
+ # 移除环境清理,保留周围环境
+ # self.cleanup_environment()
+
+ def cleanup_environment(self):
+ """清理环境中的车辆和行人"""
+ try:
+ actors = self.world.get_actors()
+ destroyed_count = 0
+
+ for actor in actors:
+ if actor.type_id.startswith('vehicle.') or actor.type_id.startswith('walker.'):
+ try:
+ actor.destroy()
+ destroyed_count += 1
+ except:
+ pass
+
+ if destroyed_count > 0:
+ print(f"清理了 {destroyed_count} 个现有演员")
+
+ time.sleep(2.0)
+
+ except Exception as e:
+ print(f"环境清理时出错: {e}")
+
+ def cleanup(self):
+ """清理所有演员"""
+ print("清理环境中的演员...")
+
+ try:
+ # 首先销毁我们创建的演员
+ for actor in self.actor_list:
+ try:
+ if actor.is_alive:
+ actor.destroy()
+ except Exception as e:
+ print(f"销毁演员失败: {e}")
+
+ self.actor_list = []
+ time.sleep(1.0) # 等待销毁完成
+
+ except Exception as e:
+ print(f"清理时出错: {e}")
+
+ def reset(self):
+ # 只清理我们自己创建的演员,不清理环境
+ self.cleanup_self_actors()
+
+ # store any collision detected
+ self.collision_history = []
+ # to store all the actors that are present in the environment
+ self.actor_list = []
+ # store the number of times the vehicles crosses the lane marking
+ self.lanecrossing_history = []
+
+ # 先计算轨迹,以便获取正确的朝向
+ traj = self.trajectory()
+ self.path = []
+ for el in traj:
+ self.path.append(el[0])
+
+ # 使用路径上的第一个点来确定正确的朝向
+ if len(self.path) > 0:
+ first_waypoint = self.path[0]
+ correct_yaw = first_waypoint.transform.rotation.yaw
+ # 确保朝向正确(与路径方向一致)
+ print(f"路径朝向: {correct_yaw}°")
+ else:
+ correct_yaw = self.initial_pos[3]
+
+ self.transform = Transform(
+ Location(x=self.initial_pos[0], y=self.initial_pos[1], z=self.initial_pos[2]),
+ Rotation(yaw=correct_yaw) # 使用路径的朝向
+ )
+
+ # 生成车辆
+ self.vehicle = self.world.try_spawn_actor(self.model_3, self.transform)
+
+ if self.vehicle is None:
+ # 如果生成失败,尝试附近的位置
+ for i in range(5):
+ offset_x = random.uniform(-2.0, 2.0)
+ offset_y = random.uniform(-2.0, 2.0)
+ temp_transform = Transform(
+ Location(x=self.initial_pos[0] + offset_x, y=self.initial_pos[1] + offset_y, z=self.initial_pos[2]),
+ Rotation(yaw=correct_yaw)
+ )
+ self.vehicle = self.world.try_spawn_actor(self.model_3, temp_transform)
+ if self.vehicle is not None:
+ break
+
+ if self.vehicle is None:
+ raise RuntimeError("无法生成车辆")
+
+ self.actor_list.append(self.vehicle)
+
+ print("Spawning my agent.....")
+ print(f"车辆位置: ({self.initial_pos[0]:.2f}, {self.initial_pos[1]:.2f}, {self.initial_pos[2]:.2f})")
+ print(f"车辆朝向: {-correct_yaw}°")
+
+ # to use the RGB camera
+ self.depth_camera = self.blueprint_library.find("sensor.camera.depth")
+ self.depth_camera.set_attribute("image_size_x", f"{IM_WIDTH}")
+ self.depth_camera.set_attribute("image_size_y", f"{IM_HEIGHT}")
+ self.depth_camera.set_attribute("fov", f"40")
+
+ self.camera_spawn_point = carla.Transform(carla.Location(x=2, y = 0, z=1.4), Rotation(yaw=0))
+
+ # to spawn the camera
+ self.camera_sensor = self.world.spawn_actor(self.depth_camera, self.camera_spawn_point, attach_to = self.vehicle)
+ self.actor_list.append(self.camera_sensor)
+
+ # to record the data from the camera sensor
+ self.camera_sensor.listen(lambda data: self.image_dep(data))
+
+ '''
+ To spawn the SEGMENTATION camera
+ '''
+ self.seg_camera = self.blueprint_library.find("sensor.camera.semantic_segmentation")
+ self.seg_camera.set_attribute("image_size_x", f"{IM_WIDTH}")
+ self.seg_camera.set_attribute("image_size_y", f"{IM_HEIGHT}")
+ self.seg_camera.set_attribute("fov", f"40")
+
+ # to spawn the segmentation camera exactly in between the 2 depth cameras
+ self.seg_camera_spawn_point = carla.Transform(carla.Location(x=2, y = 0, z=1.4), Rotation(yaw=0))
+
+ # to spawn the camera
+ self.seg_camera_sensor = self.world.spawn_actor(self.seg_camera, self.seg_camera_spawn_point, attach_to = self.vehicle)
+ self.actor_list.append(self.seg_camera_sensor)
+
+ self.seg_camera_sensor.listen(lambda data: self.image_seg(data))
+
+ # 不要初始化车辆控制,保持静止
+ self.vehicle.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0))
+ time.sleep(2) # 等待传感器初始化
+
+ # to introduce the collision sensor to detect what type of collision is happening
+ col_sensor = self.blueprint_library.find("sensor.other.collision")
+
+ # keeping the location of the sensor to be same as that of the RGB camera
+ self.collision_sensor = self.world.spawn_actor(col_sensor, self.camera_spawn_point, attach_to = self.vehicle)
+ self.actor_list.append(self.collision_sensor)
+
+ # to record the data from the collision sensor
+ self.collision_sensor.listen(lambda event: self.collision_data(event))
+
+ # to introduce the lanecrossing sensor to identify vehicles trajectory
+ lane_crossing_sensor = self.blueprint_library.find("sensor.other.lane_invasion")
+
+ # keeping the location of the sensor to be same as that of RGM Camera
+ self.lanecrossing_sensor = self.world.spawn_actor(lane_crossing_sensor, self.camera_spawn_point, attach_to = self.vehicle)
+ self.actor_list.append(self.lanecrossing_sensor)
+
+ # to record the data from the lanecrossing_sensor
+ self.lanecrossing_sensor.listen(lambda event: self.lanecrossing_data(event))
+
+ # 等待传感器数据
+ while self.cam is None or self.seg is None:
+ time.sleep(0.01)
+
+ self.process_images()
+
+ # episode计时
+ self.episode_start = time.time()
+
+ # 返回初始状态,车辆保持静止
+ return [(self.distance-300)/300, -1, 0, 0]
+ # 其余方法保持不变...
+ def collision_data(self, event):
+ self.collision_history.append(event)
+
+
+ # to record the lane crossing data
+ def lanecrossing_data(self, event):
+ self.lanecrossing_history.append(event)
+ print("Lane crossing history: ", event)
+
+
+ def image_dep(self, image):
+ self.cam = image
+
+
+ def image_seg(self, image):
+ self.seg = image
+
+
+ # to process the image
+ def process_images(self):
+ # Convert depth image to array of depth values
+ depth_array1 = np.frombuffer(self.cam.raw_data, dtype=np.dtype("uint8"))
+ depth_array1 = np.reshape(depth_array1, (self.cam.height, self.cam.width, 4))
+ depth_array1 = depth_array1.astype(np.int32)
+
+ # Using this formula to get the distances
+ depth_map = (depth_array1[:, :, 0]*255*255 + depth_array1[:, :, 1]*255 + depth_array1[:, :, 2])/1000
+
+ # Making the sky at 0 distance
+ x = np.where(depth_map >= 16646.655)
+ depth_map[x] = 0
+
+ # Showing the initial depth image
+ #cv2.imshow("Initial: ", np.array(depth_array1, dtype = np.uint8))
+
+ # Calculate distance from camera to each point in world coordinates
+ distances = depth_map
+
+
+ # uncomment the code below to get the distance map
+ # # Plot the distance map
+ #fig, ax = plt.subplots()
+ #cmap = plt.cm.jet
+ #cmap.set_bad(color='black')
+ #im = ax.imshow(depth_array, cmap=cmap, vmin=0, vmax=50)#int(distances[int(cy),int(cx)]*2))
+ #ax.set_title('Distance Map')
+ #ax.set_xlabel('Pixel X')
+ #ax.set_ylabel('Pixel Y')
+ #cbar = ax.figure.colorbar(im, ax=ax)
+ #cbar.ax.set_ylabel('Distance (m)', rotation=-90, va="bottom")
+ #plt.savefig("pics/"+str(int(time.time()*100))+".jpg")
+
+ image_array = np.frombuffer(self.seg.raw_data, dtype=np.dtype("uint8"))
+ image_array = np.reshape(image_array, (self.seg.height, self.seg.width, 4))
+
+ # removing the alpha channel
+ image_array = image_array[:, :, :3]
+ self.seg_array = image_array
+
+ colors = {
+ 0: [0, 0, 0], # None
+ 1: [70, 70, 70], # Buildings
+ 2: [190, 153, 153], # Fences
+ 3: [72, 0, 90], # Other
+ 4: [220, 20, 60], # Pedestrians
+ 5: [153, 153, 153], # Poles
+ 6: [157, 234, 50], # RoadLines
+ 7: [128, 64, 128], # Roads
+ 8: [244, 35, 232], # Sidewalks
+ 9: [107, 142, 35], # Vegetation
+ 10: [0, 0, 255], # Vehicles
+ 11: [102, 102, 156], # Walls
+ 12: [220, 220, 0], # TrafficSigns
+ }
+
+ # to store the vehicle indices only
+ lane = np.where((self.seg_array == [0, 0, 6]).all(axis = 2))
+
+ copy_seg_img = np.copy(self.seg_array)
+
+
+ if False:
+ #sidewalk = np.where((copy_seg_img == [0, 0, 8]).all(axis = 2))
+ #p2 = np.polyfit(sidewalk[0], sidewalk[1], 1)
+
+ # Fit a polynomial of degree 2
+ p = np.polyfit(lane[0], lane[1], 2)
+
+ # Create a new set of x-values to plot the fitted curve
+ x_fit = np.linspace(0, 479, 480)
+
+ # Evaluate the fitted polynomial at the new x-values
+ y_fit = np.polyval(p, x_fit)
+
+ # Evaluate the fitted polynomial at the new x-values
+ #y_fit2 = np.polyval(p2, x_fit)
+
+ # Plot the data and the fitted curve
+ #plt.plot(x, y, 'o', label='data')
+ #plt.plot(x_fit, y_fit, '-', label='fit')
+ #plt.legend()
+ #plt.show()
+
+ for i in range(480):
+ for j in range(640):
+ if j < y_fit[i]:
+ copy_seg_img[i,j] = [0,0,0]
+ distances[i,j] = 0
+
+
+ # to store the vehicle indices only
+ self.vehicle_indices = np.where((copy_seg_img == [0, 0, 10]).all(axis = 2))
+
+ # to store the pedestrian indices only
+ self.pedestrian_indices = np.where((copy_seg_img == [0, 0, 4]).all(axis = 2))
+
+ if len(self.vehicle_indices[0]) != 0:
+ dis = np.sum(distances[self.vehicle_indices])/len(self.vehicle_indices[0])
+ else:
+ dis = 10000
+
+ if len(self.pedestrian_indices[0]) != 0:
+ dis_ped = np.sum(distances[self.pedestrian_indices])/len(self.pedestrian_indices[0])
+ else:
+ dis_ped = 10000
+
+ copy_seg_img2 = np.copy(copy_seg_img)
+ for key in colors:
+ copy_seg_img2[np.where((copy_seg_img2 == [0, 0, key]).all(axis = 2))] = colors[key]
+
+ # to save the image
+ cv2.imwrite("pics/seg/seg_"+str(int(time.time()*100))+".jpg", copy_seg_img2)
+
+
+ self.distance = min(dis, dis_ped)
+
+ return dis
+
+
+ def step(self, action, current_state):
+ '''
+ To take 6 actions; brake, go straight, turn left, turn right, turn slightly left, turn slightly right
+ '''
+ # 应用动作控制
+ if action == 0:
+ self.vehicle.apply_control(carla.VehicleControl(throttle=0, brake=1.0))
+ print("执行动作: 刹车")
+ elif action == 1:
+ self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer= 0*self.STEER_AMT))
+ print("执行动作: 直行")
+ elif action == 2:
+ self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=-0.3*self.STEER_AMT))
+ print("执行动作: 左转")
+ elif action == 3:
+ self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer= 0.3*self.STEER_AMT))
+ print("执行动作: 右转")
+ elif action == 4:
+ self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=-0.1*self.STEER_AMT))
+ print("执行动作: 微左")
+ elif action == 5:
+ self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer= 0.1*self.STEER_AMT))
+ print("执行动作: 微右")
+
+ # 处理图像
+ self.process_images()
+
+ # initialize a reward for a single action
+ reward = 0
+
+ # to calculate the kmh of the vehicle
+ v = self.vehicle.get_velocity()
+ kmh = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2))
+
+ # to get the position and orientation of the car
+ pos = self.vehicle.get_transform().location
+ rot = self.vehicle.get_transform().rotation
+
+ # 获取最近的路径点
+ waypoint_ind = self.get_closest_waypoint(self.path, self.vehicle.get_transform())
+ current_waypoint = self.path[waypoint_ind]
+
+ # 计算到终点的距离
+ dist_from_goal = np.sqrt((pos.x - self.final_destination[0])**2 + (pos.y-self.final_destination[1])**2)
+
+ done = False
+
+ # 计算角度差
+ waypoint_rot = current_waypoint.transform.rotation
+ orientation_diff = waypoint_rot.yaw - rot.yaw
+ phi = orientation_diff % 360 - 360 * (orientation_diff % 360 > 180)
+
+ # 计算横向偏差
+ if waypoint_ind < len(self.path) - 1:
+ next_waypoint = self.path[waypoint_ind + 1]
+ waypoint_loc = current_waypoint.transform.location
+ next_waypoint_loc = next_waypoint.transform.location
+
+ u = [waypoint_loc.x - next_waypoint_loc.x, waypoint_loc.y - next_waypoint_loc.y]
+ v = [pos.x - waypoint_loc.x, pos.y - waypoint_loc.y]
+
+ if np.linalg.norm(u) > 0.1 and np.linalg.norm(v) > 0.1:
+ signed_dis = np.linalg.norm(v) * np.sin(np.sign(np.cross(u,v)) * np.arccos(np.dot(u,v)/(np.linalg.norm(u)*np.linalg.norm(v))))
+ else:
+ signed_dis = 0
+ else:
+ signed_dis = 0
+
+ current_state[3] = current_state[3]/15
+
+ print(f"角度差: {phi:.2f}°, 横向偏差: {signed_dis:.2f}m")
+ print(f"距离障碍物: {current_state[0]*300+300:.2f}m")
+ print(f"速度: {(current_state[1]+current_state[0])*30+30:.2f}km/h")
+
+ # [原有的奖励计算代码保持不变...]
+
+ # 检查是否完成
+ if len(self.collision_history) != 0:
+ done = True
+ reward = -200
+ print("❌ 发生碰撞!")
+
+ if abs(phi) > 100:
+ done = True
+ reward = -200
+ print("❌ 方向偏差过大!")
+
+ if abs(signed_dis) > 2:
+ reward = -50
+ print("⚠️ 横向偏差过大")
+
+ # 到达终点
+ if dist_from_goal < 10:
+ self.reached = 1
+ done = True
+ reward = 1000
+ print("✅ 成功到达终点!")
+
+ # 超时检查
+ if self.episode_start + 200 < time.time():
+ done = True
+ print("⏰ 超时")
+
+ print(f"奖励: {reward}")
+
+ # 返回新的状态、奖励、完成标志和当前路径点
+ return [(self.distance-300)/300, (kmh-30)/30-(self.distance-300)/300, phi, signed_dis*15], reward, done, current_waypoint
+ def trajectory(self, draw = False):
+
+ amap = self.world.get_map()
+ sampling_resolution = 0.5
+ # dao = GlobalRoutePlannerDAO(amap, sampling_resolution)
+ grp = GlobalRoutePlanner(amap, sampling_resolution)
+ # grp.setup()
+
+ #start_location = self.vehicle.get_transform().location
+ start_location = carla.Location(x=self.initial_pos[0], y=self.initial_pos[1], z=0)
+ end_location = carla.Location(x=self.final_destination[0], y=self.final_destination[1], z=0)
+ a = amap.get_waypoint(start_location, project_to_road=True)
+ b = amap.get_waypoint(end_location, project_to_road=True)
+ spawn_points = self.world.get_map().get_spawn_points()
+ #print(spawn_points)
+ a = a.transform.location
+ b = b.transform.location
+ w1 = grp.trace_route(a, b) # there are other funcations can be used to generate a route in GlobalRoutePlanner.
+ i = 0
+ if draw:
+ for w in w1:
+ if i % 10 == 0:
+ self.world.debug.draw_string(w[0].transform.location, 'O', draw_shadow=False,
+ color=carla.Color(r=255, g=0, b=0), life_time=120.0,
+ persistent_lines=True)
+ else:
+ self.world.debug.draw_string(w[0].transform.location, 'O', draw_shadow=False,
+ color = carla.Color(r=0, g=0, b=255), life_time=1000.0,
+ persistent_lines=True)
+ i += 1
+ return w1
+ def cleanup_self_actors(self):
+ """只清理我们自己创建的演员,不清理环境中的其他车辆和行人"""
+ print("清理自己创建的演员...")
+
+ try:
+ # 只清理我们自己的actor_list中的演员
+ for actor in self.actor_list:
+ try:
+ if actor.is_alive:
+ actor.destroy()
+ except Exception as e:
+ print(f"销毁演员失败: {e}")
+
+ self.actor_list = []
+ time.sleep(1.0) # 等待销毁完成
+
+ except Exception as e:
+ print(f"清理时出错: {e}")
+
+
+ def get_closest_waypoint(self, waypoint_list, vehicle_transform):
+ """获取最近的路径点"""
+ closest_waypoint = 0
+ closest_distance = float('inf')
+
+ vehicle_location = vehicle_transform.location
+
+ for i, waypoint in enumerate(waypoint_list):
+ distance = math.sqrt(
+ (waypoint.transform.location.x - vehicle_location.x)**2 +
+ (waypoint.transform.location.y - vehicle_location.y)**2
+ )
+ if distance < closest_distance:
+ closest_waypoint = i
+ closest_distance = distance
+
+ # 确保不会返回最后一个点(除非非常接近终点)
+ if closest_waypoint < len(waypoint_list) - 1:
+ return closest_waypoint
+ else:
+ return len(waypoint_list) - 1
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/config.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/config.py
new file mode 100644
index 0000000000..cd89a466af
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/config.py
@@ -0,0 +1,164 @@
+"""
+配置文件 - 存储所有配置参数
+"""
+import os
+# ==================== 轨迹配置 ====================
+TRAJECTORIES = {
+ "custom_trajectory": {
+ "start": [108.91605377197266,101.3561782836914,0.6136533617973328, 0],
+ "end": [74.64781951904297,-56.014076232910156,0.40768954157829285],
+ "description": "自定义轨迹 - 城镇道路"
+ },
+ "test_trajectory": {
+ "start": [98.25761413574219,99.15800476074219,0.4944169223308563, 0],
+ "end": [74.64781951904297,-56.014076232910156,0.40768954157829285],
+ "description": "测试轨迹 - 直线道路"
+ }
+}
+
+# 当前使用的轨迹
+CURRENT_TRAJECTORY = "test_trajectory"
+
+def get_current_trajectory():
+ """获取当前轨迹配置"""
+ if CURRENT_TRAJECTORY in TRAJECTORIES:
+ return TRAJECTORIES[CURRENT_TRAJECTORY]
+ else:
+ print(f"❌ 轨迹 '{CURRENT_TRAJECTORY}' 不存在")
+ return None
+
+
+# ==================== 模型配置 ====================
+# 设置基础路径
+current_dir = os.path.dirname(os.path.abspath(__file__))
+base_dir = os.path.dirname(current_dir) # 上一级目录
+MODELS_DIR = os.path.join(base_dir, 'models')
+braking = os.path.join(MODELS_DIR, 'Braking___282.model')
+driving = os.path.join(MODELS_DIR, 'Driving__6030.model')
+MODEL_PATHS = {
+ 'braking': braking ,
+ 'driving': driving
+}
+
+# ==================== 动作配置 ====================
+ACTION_NAMES = ["刹车", "直行", "左转", "右转", "微左", "微右"]
+
+# ==================== 训练配置 ====================
+TOTAL_EPISODES = 3 # 总共运行的episode数
+MAX_STEPS_PER_EPISODE = 2000 # 每个episode最大步数
+EPISODE_INTERVAL = 2.0 # episode之间的间隔秒数
+
+# ==================== 交通配置 ====================
+ENABLE_TRAFFIC = False # 是否启用交通流(暂时关闭,避免干扰)
+TRAFFIC_VEHICLES = 30 # 交通车辆数量
+TRAFFIC_WALKERS = 50 # 交通行人数量
+TRAFFIC_SAFE_MODE = True # 交通安全模式
+TRAFFIC_HYBRID_MODE = True # 混合物理模式
+TRAFFIC_SYNC_MODE = False # 交通同步模式
+TRAFFIC_RESPAWN = False # 是否重生休眠车辆
+
+# ==================== 可视化配置 ====================
+SHOW_ARROW = True # 是否显示车辆方向箭头
+SHOW_HISTORY_PATH = True # 是否显示历史行驶路线
+SHOW_PLANNED_ROUTE = True # 是否显示规划路线
+SHOW_VEHICLE_MARKER = False # 是否显示车辆标记
+
+# 箭头设置
+ARROW_LENGTH = 5.0 # 箭头长度(米)
+ARROW_THICKNESS = 0.2 # 箭头粗细
+ARROW_COLOR = (0, 255, 0) # 箭头颜色(绿色)
+ARROW_BRIGHTNESS = 1.0 # 箭头亮度 (0.0-1.0)
+
+# 历史路线设置
+HISTORY_PATH_COLOR = (0, 100, 255) # 历史路线颜色(蓝色)
+HISTORY_PATH_THICKNESS = 0.1 # 历史路线粗细
+HISTORY_PATH_MAX_POINTS = 500 # 历史路线最大点数
+HISTORY_PATH_FADE_OUT = True # 是否渐隐效果
+HISTORY_PATH_BRIGHTNESS = 0.8 # 历史路线亮度 (0.0-1.0)
+
+# 规划路线设置
+PLANNED_ROUTE_COLOR = (255, 0, 0) # 规划路线颜色(红色)
+PLANNED_ROUTE_THICKNESS = 0.15 # 规划路线粗细
+PLANNED_ROUTE_BRIGHTNESS = 1.0 # 规划路线亮度 (0.0-1.0)
+
+# 车辆标记设置
+VEHICLE_MARKER_COLOR = (0, 255, 0) # 车辆标记颜色(绿色)
+VEHICLE_MARKER_SIZE = 0.4 # 车辆标记大小
+VEHICLE_MARKER_BRIGHTNESS = 1.0 # 车辆标记亮度 (0.0-1.0)
+
+# 显示高度(保持不变)
+ROUTE_HEIGHT = 0.3 # 路线显示高度
+PATH_HEIGHT = 0.2 # 路径显示高度
+VEHICLE_HEIGHT = 0.25 # 车辆显示高度
+# ==================== 视角配置 ====================
+TOP_DOWN_HEIGHT = 5.0 # 俯视视角高度(提高)
+TOP_DOWN_PITCH = -30.0 # 俯视角 (几乎垂直向下,-90是完全垂直)
+SMOOTH_FOLLOW_FACTOR = 0.01 # 视角平滑系数 (0-1,越小越平滑)
+MIN_SMOOTH_FACTOR = 0.001 # 最小平滑系数
+MAX_SMOOTH_FACTOR = 0.3 # 最大平滑系数
+SMOOTH_FACTOR_ADAPTIVE = True # 是否自适应平滑系数
+DISTANCE_THRESHOLD = 1.0 # 距离阈值,超过此值加大平滑系数
+FOLLOW_DISTANCE= 8.0 # 跟随距离(米)
+FOLLOW_HEIGHT = 3.0 # 跟随高度(米)
+FOLLOW_PITCH = -20.0 # 俯视角(度)
+VIEW_UPDATE_ENABLED = True # 是否启用视角更新
+VIEW_UPDATE_FPS = 60 # 视角更新频率 (Hz)
+VIEW_UPDATE_THREADED = True # 是否使用独立线程更新视角
+# ==================== 性能配置 ====================
+DEBUG_MODE = True # 调试模式
+FPS_LIMIT = 60 # FPS限制 (0为无限制,提高性能)
+UPDATE_RATE = 0.5 # 更新率 (1.0 = 每步更新)
+USE_SMOOTH_INTERPOLATION = True # 使用平滑插值
+INTERPOLATION_STEPS = 5 # 插值步数
+MAX_FRAME_SKIP = 1 # 最大跳帧数
+
+# ==================== CARLA配置 ====================
+CARLA_HOST = "localhost"
+CARLA_PORT = 2000
+CARLA_TIMEOUT = 20.0
+
+# ==================== 仿真配置 ====================
+FIXED_DELTA_SECONDS = 0.0166 # 固定时间步长 (~60 FPS)
+SYNCHRONOUS_MODE = False # 同步模式(影响性能,暂不启用)
+NO_RENDERING_MODE = False # 无渲染模式
+
+# ==================== 传感器配置 ====================
+CAMERA_WIDTH = 640
+CAMERA_HEIGHT = 480
+CAMERA_FOV = 40
+
+def print_config():
+ """打印当前配置"""
+ print("\n" + "="*60)
+ print("当前配置")
+ print("="*60)
+
+ trajectory = get_current_trajectory()
+ if trajectory:
+ print(f"轨迹: {trajectory['description']}")
+ print(f"起点: {trajectory['start']}")
+ print(f"终点: {trajectory['end']}")
+
+ print(f"\n模型配置:")
+ print(f" 刹车模型: {MODEL_PATHS['braking']}")
+ print(f" 驾驶模型: {MODEL_PATHS['driving']}")
+
+ print(f"\n训练配置:")
+ print(f" Episodes: {TOTAL_EPISODES}")
+ print(f" 最大步数/Episode: {MAX_STEPS_PER_EPISODE}")
+ print(f" Episode间隔: {EPISODE_INTERVAL}s")
+
+ print(f"\n视角配置:")
+ print(f" 俯视高度: {TOP_DOWN_HEIGHT}m")
+ print(f" 俯视角: {TOP_DOWN_PITCH}°")
+ print(f" 平滑系数: {SMOOTH_FOLLOW_FACTOR}")
+ print(f" 自适应平滑: {'开启' if SMOOTH_FACTOR_ADAPTIVE else '关闭'}")
+ print(f" 插值步数: {INTERPOLATION_STEPS}")
+
+ print(f"\n性能配置:")
+ print(f" 调试模式: {'开启' if DEBUG_MODE else '关闭'}")
+ print(f" FPS限制: {FPS_LIMIT if FPS_LIMIT > 0 else '无限制'}")
+ print(f" 固定时间步长: {FIXED_DELTA_SECONDS}s")
+ print(f" 同步模式: {'开启' if SYNCHRONOUS_MODE else '关闭'}")
+
+ print("="*60)
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/config_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/config_manager.py
new file mode 100644
index 0000000000..6cb8308231
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/config_manager.py
@@ -0,0 +1,374 @@
+"""
+配置管理器 - 封装config.py功能,管理CARLA模拟器配置
+"""
+
+import sys
+import os
+import glob
+import re
+import socket
+import textwrap
+import datetime
+import time
+# 添加CARLA路径
+try:
+ sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
+ sys.version_info.major,
+ sys.version_info.minor,
+ 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+except IndexError:
+ pass
+
+import carla
+
+class ConfigManager:
+ """配置管理器 - 负责CARLA模拟器的配置"""
+
+ def __init__(self, client=None, host='localhost', port=2000):
+ """
+ 初始化配置管理器
+
+ Args:
+ client: 可选的CARLA客户端对象
+ host: CARLA服务器主机
+ port: CARLA服务器端口
+ """
+ if client:
+ self.client = client
+ self.world = client.get_world()
+ else:
+ self.client = carla.Client(host, port)
+ self.client.set_timeout(10.0)
+ self.world = self.client.get_world()
+
+ print("⚙️ 配置管理器初始化完成")
+
+ def get_available_maps(self):
+ """获取可用地图列表"""
+ maps = [m.replace('/Game/Carla/Maps/', '') for m in self.client.get_available_maps()]
+ return sorted(maps)
+
+ def get_weather_presets(self):
+ """获取天气预设列表"""
+ presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)]
+ return [(getattr(carla.WeatherParameters, x), x) for x in presets]
+
+ def get_available_blueprints(self, filter_pattern='*'):
+ """获取可用蓝图列表"""
+ blueprint_library = self.world.get_blueprint_library()
+ blueprints = [bp.id for bp in blueprint_library.filter(filter_pattern)]
+ return sorted(blueprints)
+
+ def load_map(self, map_name):
+ """
+ 加载地图
+
+ Args:
+ map_name: 地图名称
+
+ Returns:
+ bool: 是否成功
+ """
+ try:
+ available_maps = self.get_available_maps()
+
+ if map_name not in available_maps:
+ print(f"❌ 地图 '{map_name}' 不存在")
+ print(f"可用地图: {', '.join(available_maps)}")
+ return False
+
+ print(f"🗺️ 加载地图: {map_name}")
+ self.world = self.client.load_world(map_name)
+
+ # 等待地图加载完成
+ time.sleep(2.0)
+
+ print(f"✅ 地图加载成功")
+ return True
+
+ except Exception as e:
+ print(f"❌ 加载地图失败: {e}")
+ return False
+
+ def set_weather(self, weather_preset):
+ """
+ 设置天气
+
+ Args:
+ weather_preset: 天气预设名称
+
+ Returns:
+ bool: 是否成功
+ """
+ try:
+ if not hasattr(carla.WeatherParameters, weather_preset):
+ print(f"❌ 天气预设 '{weather_preset}' 不存在")
+ return False
+
+ weather = getattr(carla.WeatherParameters, weather_preset)
+ self.world.set_weather(weather)
+
+ print(f"☀️ 设置天气: {weather_preset}")
+ return True
+
+ except Exception as e:
+ print(f"❌ 设置天气失败: {e}")
+ return False
+
+ def set_weather_custom(self,
+ cloudiness=0.0,
+ precipitation=0.0,
+ precipitation_deposits=0.0,
+ wind_intensity=0.0,
+ sun_azimuth_angle=0.0,
+ sun_altitude_angle=75.0,
+ fog_density=0.0,
+ fog_distance=0.0,
+ wetness=0.0):
+ """
+ 设置自定义天气
+
+ Args:
+ 各种天气参数
+
+ Returns:
+ bool: 是否成功
+ """
+ try:
+ weather = carla.WeatherParameters(
+ cloudiness=cloudiness,
+ precipitation=precipitation,
+ precipitation_deposits=precipitation_deposits,
+ wind_intensity=wind_intensity,
+ sun_azimuth_angle=sun_azimuth_angle,
+ sun_altitude_angle=sun_altitude_angle,
+ fog_density=fog_density,
+ fog_distance=fog_distance,
+ wetness=wetness
+ )
+
+ self.world.set_weather(weather)
+
+ print(f"🌤️ 设置自定义天气")
+ print(f" 云量: {cloudiness}%")
+ print(f" 降水量: {precipitation}%")
+ print(f" 雾密度: {fog_density}")
+
+ return True
+
+ except Exception as e:
+ print(f"❌ 设置自定义天气失败: {e}")
+ return False
+
+ def set_fixed_fps(self, fps=20.0):
+ """
+ 设置固定帧率
+
+ Args:
+ fps: 帧率 (0表示可变帧率)
+
+ Returns:
+ bool: 是否成功
+ """
+ try:
+ settings = self.world.get_settings()
+
+ if fps > 0:
+ settings.fixed_delta_seconds = 1.0 / fps
+ print(f"📊 设置固定帧率: {fps} FPS")
+ else:
+ settings.fixed_delta_seconds = None
+ print("📊 设置可变帧率")
+
+ self.world.apply_settings(settings)
+ return True
+
+ except Exception as e:
+ print(f"❌ 设置帧率失败: {e}")
+ return False
+
+ def set_synchronous_mode(self, enabled=True, fixed_delta_seconds=0.05):
+ """
+ 设置同步模式
+
+ Args:
+ enabled: 是否启用同步模式
+ fixed_delta_seconds: 固定时间步长
+
+ Returns:
+ bool: 是否成功
+ """
+ try:
+ settings = self.world.get_settings()
+ settings.synchronous_mode = enabled
+
+ if enabled:
+ settings.fixed_delta_seconds = fixed_delta_seconds
+ print(f"⏱️ 启用同步模式,时间步长: {fixed_delta_seconds}s")
+ else:
+ print("⏱️ 禁用同步模式")
+
+ self.world.apply_settings(settings)
+ return True
+
+ except Exception as e:
+ print(f"❌ 设置同步模式失败: {e}")
+ return False
+
+ def set_rendering_mode(self, enabled=True):
+ """
+ 设置渲染模式
+
+ Args:
+ enabled: 是否启用渲染
+
+ Returns:
+ bool: 是否成功
+ """
+ try:
+ settings = self.world.get_settings()
+ settings.no_rendering_mode = not enabled
+ self.world.apply_settings(settings)
+
+ print(f"🎨 渲染模式: {'启用' if enabled else '禁用'}")
+ return True
+
+ except Exception as e:
+ print(f"❌ 设置渲染模式失败: {e}")
+ return False
+
+ def set_streaming_distance(self, tile_distance=300.0, actor_distance=100.0):
+ """
+ 设置流式加载距离
+
+ Args:
+ tile_distance: 贴图流式距离
+ actor_distance: 演员活跃距离
+
+ Returns:
+ bool: 是否成功
+ """
+ try:
+ settings = self.world.get_settings()
+ settings.tile_stream_distance = tile_distance
+ settings.actor_active_distance = actor_distance
+ self.world.apply_settings(settings)
+
+ print(f"📡 设置流式距离: 贴图={tile_distance}m, 演员={actor_distance}m")
+ return True
+
+ except Exception as e:
+ print(f"❌ 设置流式距离失败: {e}")
+ return False
+
+ def inspect_simulation(self):
+ """检查模拟器状态"""
+ try:
+ address = f'{self.client.host}:{self.client.port}'
+ elapsed_time = self.world.get_snapshot().timestamp.elapsed_seconds
+ elapsed_time = datetime.timedelta(seconds=int(elapsed_time))
+
+ actors = self.world.get_actors()
+ settings = self.world.get_settings()
+
+ # 获取当前天气
+ weather = 'Custom'
+ current_weather = self.world.get_weather()
+ for preset, name in self.get_weather_presets():
+ if current_weather == preset:
+ weather = name
+
+ # 获取帧率
+ if settings.fixed_delta_seconds is None:
+ frame_rate = 'variable'
+ else:
+ fps = 1.0 / settings.fixed_delta_seconds
+ frame_rate = f'{settings.fixed_delta_seconds*1000:.2f} ms ({fps:.0f} FPS)'
+
+ # 打印信息
+ print("\n" + "="*60)
+ print("CARLA模拟器状态检查")
+ print("="*60)
+ print(f"地址: {address:>30}")
+ print(f"版本: {self.client.get_server_version():>30}")
+ print(f"地图: {self.world.get_map().name:>30}")
+ print(f"天气: {weather:>30}")
+ print(f"运行时间: {elapsed_time:>30}")
+ print(f"帧率: {frame_rate:>30}")
+ print(f"渲染: {'禁用' if settings.no_rendering_mode else '启用':>30}")
+ print(f"同步模式: {'禁用' if not settings.synchronous_mode else '启用':>30}")
+ print(f"\n演员统计:")
+ print(f" 总演员数: {len(actors):>25}")
+ print(f" 观察者: {len(actors.filter('spectator')):>25}")
+ print(f" 静态物体: {len(actors.filter('static.*')):>25}")
+ print(f" 交通标志: {len(actors.filter('traffic.*')):>25}")
+ print(f" 车辆: {len(actors.filter('vehicle.*')):>25}")
+ print(f" 行人: {len(actors.filter('walker.*')):>25}")
+ print("="*60)
+
+ return True
+
+ except Exception as e:
+ print(f"❌ 检查模拟器状态失败: {e}")
+ return False
+
+ def apply_default_settings(self):
+ """应用默认设置"""
+ print("\n⚙️ 应用默认设置...")
+
+ settings_applied = []
+
+ # 启用渲染
+ if self.set_rendering_mode(enabled=True):
+ settings_applied.append("渲染")
+
+ # 禁用同步模式(提高性能)
+ if self.set_synchronous_mode(enabled=False):
+ settings_applied.append("同步模式")
+
+ # 设置默认天气
+ if self.set_weather("ClearNoon"):
+ settings_applied.append("天气")
+
+ # 设置固定时间步长
+ try:
+ import config as cfg
+ if self.set_fixed_fps(fps=1/cfg.FIXED_DELTA_SECONDS):
+ settings_applied.append(f"帧率({1/cfg.FIXED_DELTA_SECONDS:.1f}FPS)")
+ except:
+ if self.set_fixed_fps(fps=0):
+ settings_applied.append("帧率(可变)")
+
+ # 设置流式距离
+ if self.set_streaming_distance(tile_distance=300.0, actor_distance=100.0):
+ settings_applied.append("流式距离")
+
+ if settings_applied:
+ print(f"✅ 已应用设置: {', '.join(settings_applied)}")
+ else:
+ print("⚠️ 未应用任何设置")
+
+ return len(settings_applied) > 0
+
+ def get_current_settings(self):
+ """获取当前设置"""
+ settings = self.world.get_settings()
+
+ current_weather = self.world.get_weather()
+ weather_name = 'Custom'
+
+ for preset, name in self.get_weather_presets():
+ if current_weather == preset:
+ weather_name = name
+ break
+
+ return {
+ 'map': self.world.get_map().name,
+ 'weather': weather_name,
+ 'synchronous_mode': settings.synchronous_mode,
+ 'no_rendering': settings.no_rendering_mode,
+ 'fixed_delta_seconds': settings.fixed_delta_seconds,
+ 'fps': 1.0 / settings.fixed_delta_seconds if settings.fixed_delta_seconds else 0,
+ 'tile_stream_distance': settings.tile_stream_distance,
+ 'actor_active_distance': settings.actor_active_distance
+ }
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/get_location.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/get_location.py
new file mode 100644
index 0000000000..4faf20c628
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/get_location.py
@@ -0,0 +1,45 @@
+import glob
+import os
+import sys
+import time
+
+try:
+ sys.path.append(glob.glob('./PythonAPI/carla/dist/carla-*%d.%d-%s.egg' % (
+ sys.version_info.major,
+ sys.version_info.minor,
+ 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+except IndexError:
+ pass
+
+# ==============================================================================
+# -- imports -------------------------------------------------------------------
+# ==============================================================================
+
+#MINIBATCH_SIZE
+import carla
+
+_HOST_ = '127.0.0.1'
+_PORT_ = 2000
+_SLEEP_TIME_ = 1
+
+
+def main():
+ client = carla.Client(_HOST_, _PORT_)
+ client.set_timeout(2.0)
+ world = client.get_world()
+
+ # print(help(t))
+ # print("(x,y,z) = ({},{},{})".format(t.location.x, t.location.y,t.location.z))
+
+
+ while(True):
+ t = world.get_spectator().get_transform()
+ # coordinate_str = "(x,y) = ({},{})".format(t.location.x, t.location.y)
+ coordinate_str = "(x,y,z) = ({},{},{})".format(t.location.x, t.location.y,t.location.z)
+ print (coordinate_str)
+ time.sleep(_SLEEP_TIME_)
+
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/main.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/main.py
new file mode 100644
index 0000000000..5dab272650
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/main.py
@@ -0,0 +1,223 @@
+#!/usr/bin/env python3
+"""
+主程序 - 简化版
+"""
+
+import os
+os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
+import time
+import sys
+import glob
+
+# 导入CARLA
+try:
+ sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
+ sys.version_info.major,
+ sys.version_info.minor,
+ 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+except IndexError:
+ pass
+
+import carla
+import numpy as np
+from collections import deque
+
+from car_env import CarEnv
+from route_visualizer import RouteVisualizer
+from vehicle_tracker import VehicleTracker
+from traffic_manager import TrafficManager
+import config as cfg
+
+def setup_environment():
+ """设置环境"""
+ print("启动CARLA自动驾驶系统")
+
+ # 获取轨迹
+ trajectory = cfg.get_current_trajectory()
+ if not trajectory:
+ print("轨迹配置错误")
+ return None, None, None, None, None
+
+ print(f"使用轨迹: {trajectory['description']}")
+
+ # 创建环境
+ try:
+ env = CarEnv(trajectory['start'], trajectory['end'])
+ print("CARLA环境创建成功")
+ except Exception as e:
+ print(f"创建环境失败: {e}")
+ return None, None, None, None, None
+
+ # 设置仿真参数
+ settings = env.world.get_settings()
+ settings.fixed_delta_seconds = cfg.FIXED_DELTA_SECONDS
+ settings.synchronous_mode = False
+ settings.no_rendering_mode = False
+ env.world.apply_settings(settings)
+
+ # 设置天气
+ env.world.set_weather(carla.WeatherParameters.ClearNoon)
+
+ # 创建交通管理器
+ traffic_mgr = TrafficManager(env.client, env.world)
+
+ # 生成交通
+ if cfg.ENABLE_TRAFFIC:
+ print("生成交通流...")
+ traffic_mgr.generate_traffic(
+ num_vehicles=cfg.TRAFFIC_VEHICLES,
+ num_walkers=cfg.TRAFFIC_WALKERS,
+ safe_mode=cfg.TRAFFIC_SAFE_MODE
+ )
+
+ # 创建其他管理器
+ visualizer = RouteVisualizer(env.world)
+ tracker = VehicleTracker(env.world)
+
+ return env, traffic_mgr, visualizer, tracker, trajectory
+
+def load_models():
+ """加载模型"""
+ try:
+ from tensorflow.keras.models import load_model
+ import tensorflow as tf
+
+ braking_model = load_model(cfg.MODEL_PATHS['braking'])
+ driving_model = load_model(cfg.MODEL_PATHS['driving'])
+
+ print("模型加载成功")
+ return braking_model, driving_model
+ except Exception as e:
+ print(f"模型加载失败: {e}")
+ return None, None
+
+def predict_action(braking_model, driving_model, current_state, vehicle_state=None):
+ """预测动作"""
+ if not braking_model or not driving_model:
+ return 0
+
+ # 刹车模型预测
+ braking_state = np.array(current_state[:2]).reshape(1, -1)
+ braking_qs = braking_model.predict(braking_state, verbose=0)[0]
+ braking_action = np.argmax(braking_qs)
+
+ # 如果安全,使用驾驶模型
+ if braking_action == 1:
+ driving_state = np.array(current_state[2:]).reshape(1, -1)
+ driving_qs = driving_model.predict(driving_state, verbose=0)[0]
+ return np.argmax(driving_qs) + 1
+
+ return 0
+
+def run_episode(env, traffic_mgr, visualizer, tracker, episode_num):
+ """运行一个episode"""
+ print(f"\nEpisode {episode_num}")
+
+ # 加载模型
+ braking_model, driving_model = load_models()
+ if not braking_model or not driving_model:
+ print("无法加载模型,退出")
+ return False
+
+ # 重置环境
+ try:
+ current_state = env.reset()
+ print("环境重置成功")
+ except Exception as e:
+ print(f"环境重置失败: {e}")
+ return False
+
+ # 获取车辆
+ ego_vehicle = env.vehicle
+ if not ego_vehicle:
+ print("未找到车辆")
+ return False
+
+ # 设置后方跟随视角(启动独立线程)
+ tracker.set_follow_view(ego_vehicle)
+
+ # 绘制路线
+ if hasattr(env, 'path') and env.path:
+ route_points = []
+ for waypoint in env.path:
+ location = waypoint.transform.location
+ route_points.append((location.x, location.y, location.z))
+ visualizer.draw_planned_route(route_points)
+
+ # 运行循环 - 不再在主循环中更新视角
+ step_count = 0
+ done = False
+ fps_counter = deque(maxlen=30)
+
+ while not done and step_count < cfg.MAX_STEPS_PER_EPISODE:
+ step_count += 1
+ step_start = time.time()
+
+ # 注意:不再调用 tracker.smooth_follow_vehicle(ego_vehicle)
+ # 视角更新由独立线程处理
+
+ # 更新车辆显示
+ vehicle_state = tracker.get_vehicle_state(ego_vehicle)
+ if vehicle_state:
+ visualizer.update_vehicle_display(
+ vehicle_state['x'],
+ vehicle_state['y'],
+ vehicle_state['heading']
+ )
+
+ # 预测并执行动作
+ action = predict_action(braking_model, driving_model, current_state, vehicle_state)
+
+ try:
+ new_state, reward, done, _ = env.step(action, current_state)
+ current_state = new_state
+ except Exception as e:
+ print(f"执行动作失败: {e}")
+ done = True
+
+ # 计算FPS
+ frame_time = time.time() - step_start
+ fps_counter.append(frame_time)
+
+ if cfg.DEBUG_MODE and step_count % 50 == 0:
+ fps = len(fps_counter) / sum(fps_counter) if fps_counter else 0
+ vehicle_speed = vehicle_state.get('speed_2d', 0) if vehicle_state else 0
+ print(f"步骤 {step_count}, FPS: {fps:.1f}, 速度: {vehicle_speed:.1f}m/s, 动作: {cfg.ACTION_NAMES[action]}")
+
+ if done:
+ print(f"Episode {episode_num} 完成,步数: {step_count}")
+ break
+
+ # 清理跟踪器资源
+ tracker.cleanup()
+
+ return True
+
+def main():
+ """主函数"""
+ # 设置环境
+ result = setup_environment()
+ if not result[0]:
+ return
+
+ env, traffic_mgr, visualizer, tracker, trajectory = result
+
+ try:
+ # 运行episode
+ for episode in range(cfg.TOTAL_EPISODES):
+ success = run_episode(env, traffic_mgr, visualizer, tracker, episode + 1)
+
+ if episode < cfg.TOTAL_EPISODES - 1:
+ print(f"等待 {cfg.EPISODE_INTERVAL} 秒...")
+ time.sleep(cfg.EPISODE_INTERVAL)
+ finally:
+ # 确保清理
+ if tracker:
+ tracker.cleanup()
+ if traffic_mgr:
+ traffic_mgr.cleanup()
+
+ print("\n程序结束")
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/model_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/model_manager.py
new file mode 100644
index 0000000000..7e2df5ae2b
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/model_manager.py
@@ -0,0 +1,153 @@
+"""
+模型管理器 - 加载和管理自动驾驶模型
+"""
+
+import os
+os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
+import numpy as np
+import tensorflow as tf
+from tensorflow.keras.models import load_model
+import carla
+import config as cfg
+
+class ModelManager:
+ """模型管理器"""
+
+ def __init__(self):
+ self.braking_model = None
+ self.driving_model = None
+ self.models_loaded = False
+
+ # 设置TensorFlow
+ self._setup_tensorflow()
+
+ def _setup_tensorflow(self):
+ """设置TensorFlow配置"""
+ print(f"TensorFlow版本: {tf.__version__}")
+
+ # GPU配置
+ gpus = tf.config.list_physical_devices('GPU')
+ if gpus:
+ try:
+ for gpu in gpus:
+ tf.config.experimental.set_memory_growth(gpu, True)
+ print(f"✅ 找到 {len(gpus)} 个GPU,已启用内存增长")
+ except RuntimeError as e:
+ print(f"⚠️ GPU设置错误: {e}")
+ os.environ['CUDA_VISIBLE_DEVICES'] = '-1'
+ print("使用CPU运行")
+ else:
+ print("ℹ️ 未找到GPU,使用CPU运行")
+
+ def load_models(self):
+ """加载所有模型"""
+ print("\n" + "="*40)
+ print("加载自动驾驶模型")
+ print("="*40)
+
+ # 加载刹车模型
+ self.braking_model = self._load_single_model(
+ cfg.MODEL_PATHS['braking'],
+ "刹车模型"
+ )
+
+ # 加载驾驶模型
+ self.driving_model = self._load_single_model(
+ cfg.MODEL_PATHS['driving'],
+ "驾驶模型"
+ )
+
+ self.models_loaded = self.braking_model is not None and self.driving_model is not None
+
+ if self.models_loaded:
+ print("✅ 所有模型加载成功")
+ else:
+ print("❌ 模型加载失败")
+
+ return self.models_loaded
+
+ def _load_single_model(self, model_path, model_name):
+ """加载单个模型"""
+ if not os.path.exists(model_path):
+ print(f"❌ {model_name}文件不存在: {model_path}")
+ return None
+
+ try:
+ model = load_model(model_path)
+ print(f"✅ {model_name}加载成功: {os.path.basename(model_path)}")
+ return model
+ except Exception as e:
+ print(f"❌ {model_name}加载失败: {e}")
+ return None
+
+ def predict_action(self, current_state, vehicle_state=None):
+ """预测动作"""
+ if not self.models_loaded:
+ print("⚠️ 模型未加载,使用默认动作")
+ return 0 # 默认刹车
+
+ try:
+ # 预处理状态数据
+ braking_state = self._preprocess_state(current_state, "braking")
+ driving_state = self._preprocess_state(current_state, "driving")
+
+ # 首先检查是否需要刹车
+ braking_qs = self.braking_model.predict(braking_state, verbose=0)[0]
+ braking_action = np.argmax(braking_qs)
+
+ # 如果刹车模型判断为安全,再使用驾驶模型
+ if braking_action == 1: # 安全,可以行驶
+ # 检查交通灯
+ if vehicle_state and self._check_traffic_light(vehicle_state):
+ print("🚦 红灯 - 停车")
+ return 0
+
+ # 使用驾驶模型选择具体动作
+ driving_qs = self.driving_model.predict(driving_state, verbose=0)[0]
+ driving_action = np.argmax(driving_qs)
+
+ # 驾驶模型输出0-4,对应动作1-5
+ return driving_action + 1
+ else:
+ # 刹车
+ return 0
+
+ except Exception as e:
+ print(f"❌ 预测错误: {e}")
+ return 0 # 出错时刹车
+
+ def _preprocess_state(self, state_data, model_type):
+ """预处理状态数据"""
+ try:
+ if model_type == "braking":
+ # 刹车模型使用前两个状态
+ state_array = np.array(state_data[:2])
+ else:
+ # 驾驶模型使用后两个状态
+ state_array = np.array(state_data[2:])
+
+ # 确保是二维数组
+ if len(state_array.shape) == 1:
+ state_array = state_array.reshape(1, -1)
+
+ return state_array
+ except Exception as e:
+ print(f"状态预处理错误: {e}")
+ return np.array([[0, 0]])
+
+ def _check_traffic_light(self, vehicle_state):
+ """检查交通灯状态(简化版本)"""
+ # 这里可以扩展为实际的交通灯检测
+ # 目前返回False表示没有红灯
+ return False
+
+ def get_model_info(self):
+ """获取模型信息"""
+ info = {
+ 'braking_model_loaded': self.braking_model is not None,
+ 'driving_model_loaded': self.driving_model is not None,
+ 'models_loaded': self.models_loaded,
+ 'braking_model_path': cfg.MODEL_PATHS['braking'] if self.braking_model else None,
+ 'driving_model_path': cfg.MODEL_PATHS['driving'] if self.driving_model else None
+ }
+ return info
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/ros_config.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/ros_config.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/ros_wrapper.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/ros_wrapper.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/route_visualizer.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/route_visualizer.py
new file mode 100644
index 0000000000..53acf3be4b
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/route_visualizer.py
@@ -0,0 +1,346 @@
+"""
+路线可视化模块 - 在CARLA世界中绘制路线、车辆和路径
+"""
+
+import carla
+import math
+import time
+import config as cfg
+
+class RouteVisualizer:
+ """路线可视化器"""
+
+ def __init__(self, world):
+ self.world = world
+ self.vehicle_history = [] # 存储车辆历史位置
+
+ # 使用配置文件中的颜色定义
+ self.route_color = carla.Color(
+ int(cfg.PLANNED_ROUTE_COLOR[0] * cfg.PLANNED_ROUTE_BRIGHTNESS),
+ int(cfg.PLANNED_ROUTE_COLOR[1] * cfg.PLANNED_ROUTE_BRIGHTNESS),
+ int(cfg.PLANNED_ROUTE_COLOR[2] * cfg.PLANNED_ROUTE_BRIGHTNESS)
+ )
+ self.path_color = carla.Color(
+ int(cfg.HISTORY_PATH_COLOR[0] * cfg.HISTORY_PATH_BRIGHTNESS),
+ int(cfg.HISTORY_PATH_COLOR[1] * cfg.HISTORY_PATH_BRIGHTNESS),
+ int(cfg.HISTORY_PATH_COLOR[2] * cfg.HISTORY_PATH_BRIGHTNESS)
+ )
+ self.vehicle_color = carla.Color(
+ int(cfg.VEHICLE_MARKER_COLOR[0] * cfg.VEHICLE_MARKER_BRIGHTNESS),
+ int(cfg.VEHICLE_MARKER_COLOR[1] * cfg.VEHICLE_MARKER_BRIGHTNESS),
+ int(cfg.VEHICLE_MARKER_COLOR[2] * cfg.VEHICLE_MARKER_BRIGHTNESS)
+ )
+ self.arrow_color = carla.Color(
+ int(cfg.ARROW_COLOR[0] * cfg.ARROW_BRIGHTNESS),
+ int(cfg.ARROW_COLOR[1] * cfg.ARROW_BRIGHTNESS),
+ int(cfg.ARROW_COLOR[2] * cfg.ARROW_BRIGHTNESS)
+ )
+
+ # 显示高度
+ self.route_height = cfg.ROUTE_HEIGHT
+ self.path_height = cfg.PATH_HEIGHT
+ self.vehicle_height = cfg.VEHICLE_HEIGHT
+
+ # 存储绘制对象
+ self.route_lines = []
+ self.start_marker = None
+ self.end_marker = None
+
+ def draw_planned_route(self, route_points):
+ """绘制规划路线(常亮显示)"""
+ if not cfg.SHOW_PLANNED_ROUTE:
+ return False
+
+ # 清除之前的路线
+ self.clear_route()
+
+ if len(route_points) < 2:
+ return False
+
+ print(f"📏 绘制规划路线,共 {len(route_points)} 个点")
+
+ # 绘制整条路线
+ for i in range(len(route_points) - 1):
+ start = self._create_location(route_points[i], self.route_height)
+ end = self._create_location(route_points[i+1], self.route_height)
+
+ # 绘制线段,使用长生命时间保证常亮
+ line = self.world.debug.draw_line(
+ start, end,
+ thickness=cfg.PLANNED_ROUTE_THICKNESS,
+ color=self.route_color,
+ life_time=1000.0,
+ persistent_lines=True
+ )
+ self.route_lines.append(line)
+
+ # 绘制起点和终点标记
+ self._draw_start_end_points(route_points)
+
+ return True
+
+ def _draw_start_end_points(self, route_points):
+ """绘制起点和终点标记"""
+ if len(route_points) == 0:
+ return
+
+ # 起点标记
+ start_point = route_points[0]
+ start_loc = self._create_location(start_point, self.route_height + 0.1)
+
+ self.start_marker = self.world.debug.draw_point(
+ start_loc,
+ size=0.5,
+ color=carla.Color(255, 165, 0), # 橙色
+ life_time=1000.0,
+ persistent_lines=True
+ )
+
+ # 起点文字
+ self.world.debug.draw_string(
+ self._create_location(start_point, self.route_height + 1.0),
+ 'START',
+ draw_shadow=True,
+ color=carla.Color(255, 255, 255),
+ life_time=1000.0
+ )
+
+ # 终点标记
+ end_point = route_points[-1]
+ end_loc = self._create_location(end_point, self.route_height + 0.1)
+
+ self.end_marker = self.world.debug.draw_point(
+ end_loc,
+ size=0.5,
+ color=carla.Color(255, 0, 255), # 洋红色
+ life_time=1000.0,
+ persistent_lines=True
+ )
+
+ # 终点文字
+ self.world.debug.draw_string(
+ self._create_location(end_point, self.route_height + 1.0),
+ 'GOAL',
+ draw_shadow=True,
+ color=carla.Color(255, 255, 255),
+ life_time=1000.0
+ )
+
+ def update_vehicle_display(self, x, y, heading):
+ """更新车辆显示(位置和朝向)"""
+ # 保存历史位置
+ self.vehicle_history.append((x, y, heading, time.time()))
+
+ # 保持历史点数量
+ if len(self.vehicle_history) > cfg.HISTORY_PATH_MAX_POINTS:
+ self.vehicle_history = self.vehicle_history[-cfg.HISTORY_PATH_MAX_POINTS:]
+
+ # 绘制车辆当前位置和朝向
+ if cfg.SHOW_VEHICLE_MARKER:
+ self._draw_vehicle_current(x, y, heading)
+
+ # 绘制历史路径
+ if cfg.SHOW_HISTORY_PATH:
+ self._draw_vehicle_history()
+
+ # 更新信息显示
+ self._update_info_display(x, y, heading)
+
+ def _draw_vehicle_current(self, x, y, heading):
+ """绘制车辆当前位置和朝向"""
+ # 车辆位置点
+ vehicle_loc = self._create_location((x, y, 0), self.vehicle_height)
+
+ self.world.debug.draw_point(
+ vehicle_loc,
+ size=cfg.VEHICLE_MARKER_SIZE,
+ color=self.vehicle_color,
+ life_time=0.5, # 稍微延长显示时间
+ persistent_lines=False
+ )
+
+ # 车辆朝向箭头
+ if cfg.SHOW_ARROW:
+ arrow_length = cfg.ARROW_LENGTH
+ angle_rad = math.radians(heading)
+ end_x = x + arrow_length * math.cos(angle_rad)
+ end_y = y + arrow_length * math.sin(angle_rad)
+
+ self.world.debug.draw_arrow(
+ vehicle_loc,
+ self._create_location((end_x, end_y, 0), self.vehicle_height),
+ thickness=cfg.ARROW_THICKNESS,
+ arrow_size=0.6,
+ color=self.arrow_color,
+ life_time=0.5,
+ persistent_lines=False
+ )
+
+ # 车辆轮廓(三角形)
+ self._draw_vehicle_outline(x, y, heading)
+
+ def _draw_vehicle_outline(self, x, y, heading):
+ """绘制车辆轮廓三角形"""
+ size = 1.2
+ angle_rad = math.radians(heading)
+
+ # 前顶点
+ front_x = x + size * math.cos(angle_rad)
+ front_y = y + size * math.sin(angle_rad)
+
+ # 左后顶点
+ left_x = x + size * 0.7 * math.cos(angle_rad + math.radians(140))
+ left_y = y + size * 0.7 * math.sin(angle_rad + math.radians(140))
+
+ # 右后顶点
+ right_x = x + size * 0.7 * math.cos(angle_rad - math.radians(140))
+ right_y = y + size * 0.7 * math.sin(angle_rad - math.radians(140))
+
+ # 连接成三角形
+ points = [
+ self._create_location((front_x, front_y, 0), self.vehicle_height),
+ self._create_location((left_x, left_y, 0), self.vehicle_height),
+ self._create_location((right_x, right_y, 0), self.vehicle_height),
+ self._create_location((front_x, front_y, 0), self.vehicle_height) # 闭合
+ ]
+
+ for i in range(len(points) - 1):
+ self.world.debug.draw_line(
+ points[i], points[i+1],
+ thickness=0.01,
+ color=carla.Color(50, 255, 50), # 亮绿色轮廓
+ life_time=0.5,
+ persistent_lines=False
+ )
+
+ def _draw_vehicle_history(self):
+ """绘制车辆历史路径"""
+ if len(self.vehicle_history) < 2:
+ return
+
+ # 确定要绘制的点范围
+ if cfg.HISTORY_PATH_MAX_POINTS > 0:
+ start_idx = max(0, len(self.vehicle_history) - cfg.HISTORY_PATH_MAX_POINTS)
+ else:
+ start_idx = 0
+
+ for i in range(start_idx, len(self.vehicle_history) - 1):
+ x1, y1, _, t1 = self.vehicle_history[i]
+ x2, y2, _, t2 = self.vehicle_history[i+1]
+
+ start = self._create_location((x1, y1, 0), self.path_height)
+ end = self._create_location((x2, y2, 0), self.path_height)
+
+ # 根据时间远近调整透明度
+ if cfg.HISTORY_PATH_FADE_OUT:
+ time_diff = t2 - t1
+ if time_diff > 0:
+ # 计算衰减因子:越新的点越亮
+ age = len(self.vehicle_history) - i - 1
+ alpha = max(0.1, 1.0 / (1.0 + age * 0.05))
+ else:
+ alpha = 0.5
+ else:
+ alpha = 1.0
+
+ # 应用亮度配置
+ brightness_factor = cfg.HISTORY_PATH_BRIGHTNESS * alpha
+
+ color = carla.Color(
+ int(self.path_color.r * brightness_factor),
+ int(self.path_color.g * brightness_factor),
+ int(self.path_color.b * brightness_factor)
+ )
+
+ self.world.debug.draw_line(
+ start, end,
+ thickness=cfg.HISTORY_PATH_THICKNESS,
+ color=color,
+ life_time=0.5,
+ persistent_lines=False
+ )
+
+ def _update_info_display(self, x, y, heading):
+ """更新信息显示"""
+ info_height = 10.0
+
+ # 计算路径长度
+ path_length = self.calculate_path_length()
+
+ # 显示车辆信息
+ info_text = f"Vehicle: ({x:.1f}, {y:.1f}) | Heading: {heading:.1f}°"
+ self.world.debug.draw_string(
+ carla.Location(-30, 3, info_height),
+ info_text,
+ draw_shadow=True,
+ color=carla.Color(255, 255, 255),
+ life_time=0.3
+ )
+
+ # 显示路径长度
+ path_text = f"Path Length: {path_length:.1f}m"
+ self.world.debug.draw_string(
+ carla.Location(-30, 2, info_height),
+ path_text,
+ draw_shadow=True,
+ color=carla.Color(200, 200, 255),
+ life_time=0.3
+ )
+
+ # 显示历史点数
+ history_text = f"History Points: {len(self.vehicle_history)}/{cfg.HISTORY_PATH_MAX_POINTS}"
+ self.world.debug.draw_string(
+ carla.Location(-30, 1, info_height),
+ history_text,
+ draw_shadow=True,
+ color=carla.Color(255, 200, 200),
+ life_time=0.3
+ )
+
+ # 显示可视化状态
+ viz_status = f"Viz: Arrow={cfg.SHOW_ARROW}, History={cfg.SHOW_HISTORY_PATH}, Route={cfg.SHOW_PLANNED_ROUTE}"
+ self.world.debug.draw_string(
+ carla.Location(-30, 0, info_height),
+ viz_status,
+ draw_shadow=True,
+ color=carla.Color(200, 255, 200),
+ life_time=0.3
+ )
+
+ def calculate_path_length(self):
+ """计算已行驶路径长度"""
+ if len(self.vehicle_history) < 2:
+ return 0.0
+
+ total_length = 0.0
+ for i in range(1, len(self.vehicle_history)):
+ x1, y1, _, _ = self.vehicle_history[i-1]
+ x2, y2, _, _ = self.vehicle_history[i]
+ dx = x2 - x1
+ dy = y2 - y1
+ total_length += math.sqrt(dx*dx + dy*dy)
+
+ return total_length
+
+ def reset_history(self):
+ """重置历史记录"""
+ self.vehicle_history = []
+ print("🔄 车辆历史记录已重置")
+
+ def clear_route(self):
+ """清除路线绘制"""
+ # CARLA会自动清理过期的debug绘制
+ self.route_lines = []
+ self.start_marker = None
+ self.end_marker = None
+
+ def _create_location(self, point, z_offset=0):
+ """创建Location对象"""
+ if len(point) >= 3:
+ return carla.Location(point[0], point[1], point[2] + z_offset)
+ else:
+ return carla.Location(point[0], point[1], z_offset)
+
+ def get_vehicle_history(self):
+ """获取车辆历史记录"""
+ return self.vehicle_history
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/traffic_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/traffic_manager.py
new file mode 100644
index 0000000000..4ecf949f90
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/traffic_manager.py
@@ -0,0 +1,184 @@
+
+"""
+交通管理器-生成行人和车辆
+"""
+
+import sys
+import os
+import glob
+
+try:
+ sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
+ sys.version_info.major,
+ sys.version_info.minor,
+ 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
+except IndexError:
+ pass
+
+import carla
+from numpy import random
+
+class TrafficManager:
+ """交通管理器"""
+
+ def __init__(self, client, world):
+ self.client = client
+ self.world = world
+ self.tm_port = 8000
+ self.vehicles_list = []
+ self.walkers_list = []
+ self.all_id = []
+
+ def generate_traffic(self, num_vehicles=15, num_walkers=20, safe_mode=True):
+ """生成交通流"""
+ print(f"生成交通: {num_vehicles}辆车, {num_walkers}个行人")
+
+ # 获取交通管理器
+ traffic_manager = self.client.get_trafficmanager(self.tm_port)
+ traffic_manager.set_global_distance_to_leading_vehicle(2.5)
+
+ # 生成车辆
+ self._spawn_vehicles(traffic_manager, num_vehicles, safe_mode)
+
+ # 生成行人
+ if num_walkers > 0:
+ self._spawn_walkers_simple(num_walkers)
+
+ print(f"完成: {len(self.vehicles_list)}辆车, {len(self.walkers_list)}个行人")
+ return True
+
+ def _spawn_vehicles(self, traffic_manager, num_vehicles, safe_mode):
+ """生成车辆"""
+ blueprints = self.world.get_blueprint_library().filter('vehicle.*')
+
+ if safe_mode:
+ # 过滤危险车辆
+ safe_vehicles = [
+ 'vehicle.audi.a2',
+ 'vehicle.audi.tt',
+ 'vehicle.bmw.grandtourer',
+ 'vehicle.dodge.charger_police',
+ 'vehicle.jeep.wrangler_rubicon',
+ 'vehicle.lincoln.mkz2017',
+ 'vehicle.mercedes-benz.coupe',
+ 'vehicle.mini.cooperst',
+ 'vehicle.nissan.micra',
+ 'vehicle.nissan.patrol',
+ 'vehicle.seat.leon',
+ 'vehicle.tesla.model3',
+ 'vehicle.toyota.prius',
+ 'vehicle.volkswagen.t2'
+ ]
+ blueprints = [bp for bp in blueprints if any(safe in bp.id for safe in safe_vehicles)]
+
+ spawn_points = self.world.get_map().get_spawn_points()
+ random.shuffle(spawn_points)
+
+ batch = []
+ for i in range(min(num_vehicles, len(spawn_points))):
+ blueprint = random.choice(blueprints)
+ batch.append(
+ carla.command.SpawnActor(blueprint, spawn_points[i])
+ .then(carla.command.SetAutopilot(carla.command.FutureActor, True, traffic_manager.get_port()))
+ )
+
+ # 批量生成
+ for response in self.client.apply_batch_sync(batch):
+ if not response.error:
+ self.vehicles_list.append(response.actor_id)
+
+ def _spawn_walkers_simple(self, num_walkers):
+ """简化的行人生成"""
+ walker_bps = self.world.get_blueprint_library().filter('walker.pedestrian.*')
+
+ # 生成行人
+ batch = []
+ walker_speeds = []
+
+ for i in range(num_walkers):
+ spawn_location = self.world.get_random_location_from_navigation()
+ if spawn_location and spawn_location.z < 5: # 确保位置合理
+ walker_bp = random.choice(walker_bps)
+
+ # 设置为非无敌
+ if walker_bp.has_attribute('is_invincible'):
+ walker_bp.set_attribute('is_invincible', 'false')
+
+ # 设置速度
+ speed = 1.0 # 默认速度
+ if walker_bp.has_attribute('speed'):
+ speed = walker_bp.get_attribute('speed').recommended_values[1]
+
+ walker_speeds.append(speed)
+ batch.append(carla.command.SpawnActor(walker_bp, carla.Transform(spawn_location)))
+
+ # 生成行人实体
+ walker_ids = []
+ results = self.client.apply_batch_sync(batch)
+
+ for i, response in enumerate(results):
+ if not response.error:
+ walker_id = response.actor_id
+ walker_ids.append(walker_id)
+ self.walkers_list.append({
+ "id": walker_id,
+ "speed": walker_speeds[i] if i < len(walker_speeds) else 1.0
+ })
+
+ # 生成行人控制器
+ batch = []
+ controller_bp = self.world.get_blueprint_library().find('controller.ai.walker')
+
+ for walker_info in self.walkers_list:
+ batch.append(carla.command.SpawnActor(controller_bp, carla.Transform(), walker_info["id"]))
+
+ results = self.client.apply_batch_sync(batch)
+
+ for i, response in enumerate(results):
+ if not response.error and i < len(self.walkers_list):
+ self.walkers_list[i]["con"] = response.actor_id
+ self.all_id.append(response.actor_id)
+ self.all_id.append(self.walkers_list[i]["id"])
+
+ # 初始化控制器
+ all_actors = self.world.get_actors(self.all_id)
+
+ for i in range(0, len(self.all_id), 2):
+ if i < len(all_actors):
+ controller = all_actors[i]
+ walker = all_actors[i+1] if i+1 < len(all_actors) else None
+
+ if controller and walker:
+ try:
+ controller.start()
+
+ # 设置目的地
+ target = self.world.get_random_location_from_navigation()
+ if target:
+ controller.go_to_location(target)
+
+ # 设置速度
+ walker_index = i // 2
+ if walker_index < len(self.walkers_list):
+ controller.set_max_speed(self.walkers_list[walker_index]["speed"])
+ except:
+ pass
+
+ def cleanup(self):
+ """清理交通"""
+ print("清理交通...")
+
+ # 销毁车辆
+ if self.vehicles_list:
+ self.client.apply_batch([carla.command.DestroyActor(x) for x in self.vehicles_list])
+
+ # 销毁行人
+ if self.all_id:
+ # 先停止控制器
+ all_actors = self.world.get_actors(self.all_id)
+ for i in range(0, len(self.all_id), 2):
+ if i < len(all_actors):
+ all_actors[i].stop()
+
+ # 销毁所有行人相关actor
+ self.client.apply_batch([carla.command.DestroyActor(x) for x in self.all_id])
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/trajectory_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/trajectory_manager.py
new file mode 100644
index 0000000000..a960e58821
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/trajectory_manager.py
@@ -0,0 +1,106 @@
+"""
+轨迹管理器 - 管理规划路线和轨迹点
+"""
+
+import carla
+import math
+
+class TrajectoryManager:
+ """轨迹管理器"""
+
+ def __init__(self, env):
+ self.env = env
+ self.route_points = []
+
+ def get_route_points(self):
+ """获取规划路线点"""
+ if not self.route_points:
+ self.route_points = self._extract_route_points()
+
+ return self.route_points
+
+ def _extract_route_points(self):
+ """从环境中提取规划路线的点"""
+ route_points = []
+ try:
+ # 获取规划轨迹
+ trajectory = self.env.trajectory(draw=False)
+
+ for waypoint, road_option in trajectory:
+ location = waypoint.transform.location
+ route_points.append((location.x, location.y, location.z))
+
+ print(f"📊 提取到 {len(route_points)} 个路径点")
+
+ return route_points
+
+ except Exception as e:
+ print(f"❌ 提取路线点失败: {e}")
+ return []
+
+ def calculate_route_length(self):
+ """计算规划路线总长度"""
+ if len(self.route_points) < 2:
+ return 0.0
+
+ total_length = 0.0
+ for i in range(len(self.route_points) - 1):
+ x1, y1, _ = self.route_points[i]
+ x2, y2, _ = self.route_points[i+1]
+ total_length += math.sqrt((x2-x1)**2 + (y2-y1)**2)
+
+ return total_length
+
+ def find_closest_point(self, x, y):
+ """找到距离给定位置最近的路径点"""
+ if not self.route_points:
+ return -1, float('inf')
+
+ min_distance = float('inf')
+ closest_index = -1
+
+ for i, point in enumerate(self.route_points):
+ px, py, _ = point
+ distance = math.sqrt((px - x)**2 + (py - y)**2)
+
+ if distance < min_distance:
+ min_distance = distance
+ closest_index = i
+
+ return closest_index, min_distance
+
+ def get_remaining_route(self, current_x, current_y):
+ """获取剩余路线"""
+ if not self.route_points:
+ return []
+
+ closest_idx, _ = self.find_closest_point(current_x, current_y)
+
+ if closest_idx >= 0:
+ return self.route_points[closest_idx:]
+ else:
+ return self.route_points
+
+ def reset(self):
+ """重置轨迹管理器"""
+ self.route_points = []
+ print("🔄 轨迹管理器已重置")
+
+ def get_route_info(self):
+ """获取路线信息"""
+ if not self.route_points:
+ return {
+ 'point_count': 0,
+ 'total_length': 0.0,
+ 'has_route': False
+ }
+
+ total_length = self.calculate_route_length()
+
+ return {
+ 'point_count': len(self.route_points),
+ 'total_length': total_length,
+ 'has_route': True,
+ 'start_point': self.route_points[0] if self.route_points else None,
+ 'end_point': self.route_points[-1] if self.route_points else None
+ }
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/vehicle_tracker.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/vehicle_tracker.py
new file mode 100644
index 0000000000..667413d7fd
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/src/vehicle_tracker.py
@@ -0,0 +1,504 @@
+"""
+车辆跟踪和视角控制模块 - 从后方跟随车辆(独立更新版本)
+"""
+
+import carla
+import math
+import time
+import numpy as np
+import threading
+import config as cfg
+
+class VehicleTracker:
+ """车辆跟踪器 - 负责视角控制和状态获取"""
+
+ def __init__(self, world):
+ self.world = world
+ self.spectator = world.get_spectator()
+
+ # 平滑跟随参数
+ self.smooth_factor = cfg.SMOOTH_FOLLOW_FACTOR
+ self.min_smooth_factor = cfg.MIN_SMOOTH_FACTOR
+ self.max_smooth_factor = cfg.MAX_SMOOTH_FACTOR
+ self.adaptive_smoothing = cfg.SMOOTH_FACTOR_ADAPTIVE
+ self.distance_threshold = cfg.DISTANCE_THRESHOLD
+
+ # 插值参数
+ self.use_interpolation = cfg.USE_SMOOTH_INTERPOLATION
+ self.interpolation_steps = cfg.INTERPOLATION_STEPS
+
+ # 状态追踪
+ self.last_camera_transform = None
+ self.last_vehicle_transform = None
+ self.last_update_time = time.time()
+ self.frame_count = 0
+
+ # 预测参数
+ self.prediction_enabled = True
+ self.velocity_history = []
+ self.max_velocity_history = 10
+
+ # 后方跟随参数
+ self.follow_distance = 8.0 # 跟随距离(米)
+ self.follow_height = 3.0 # 跟随高度(米)
+ self.follow_pitch = -20.0 # 俯视角(度)
+
+ # 车辆引用
+ self.current_vehicle = None
+
+ # 线程控制
+ self.view_update_enabled = cfg.VIEW_UPDATE_ENABLED
+ self.view_update_fps = cfg.VIEW_UPDATE_FPS
+ self.view_update_thread = None
+ self.is_running = False
+ self.last_frame_time = 0
+
+ print(f"📐 车辆跟踪器初始化完成,后方跟随模式")
+ print(f" 跟随距离: {self.follow_distance}m")
+ print(f" 跟随高度: {self.follow_height}m")
+ print(f" 俯视角: {self.follow_pitch}°")
+ print(f" 视角更新FPS: {self.view_update_fps}")
+
+ def start_view_update_thread(self, vehicle):
+ """启动独立视角更新线程"""
+ if not self.view_update_enabled:
+ return
+
+ self.current_vehicle = vehicle
+
+ if self.view_update_thread and self.is_running:
+ self.stop_view_update_thread()
+
+ self.is_running = True
+ self.view_update_thread = threading.Thread(
+ target=self._view_update_loop,
+ daemon=True
+ )
+ self.view_update_thread.start()
+ print(f"🔧 启动独立视角更新线程,频率: {self.view_update_fps}Hz")
+
+ def stop_view_update_thread(self):
+ """停止视角更新线程"""
+ if self.view_update_thread:
+ self.is_running = False
+ self.view_update_thread.join(timeout=1.0)
+ self.view_update_thread = None
+ print("🛑 停止视角更新线程")
+
+ def _view_update_loop(self):
+ """独立视角更新循环"""
+ target_interval = 1.0 / self.view_update_fps
+
+ while self.is_running and self.current_vehicle:
+ try:
+ start_time = time.time()
+
+ # 检查是否有足够的车辆信息
+ if not self.current_vehicle:
+ time.sleep(target_interval)
+ continue
+
+ # 更新视角
+ self._update_camera_view()
+
+ # 计算实际耗时并补偿
+ elapsed = time.time() - start_time
+ sleep_time = max(0, target_interval - elapsed)
+
+ if sleep_time > 0:
+ time.sleep(sleep_time)
+ else:
+ # 如果处理时间超过目标间隔,跳过等待
+ if cfg.DEBUG_MODE and self.frame_count % 100 == 0:
+ print(f"[视角] 帧率下降: {elapsed:.3f}s > {target_interval:.3f}s")
+
+ except Exception as e:
+ if cfg.DEBUG_MODE:
+ print(f"⚠️ 视角更新线程异常: {e}")
+ time.sleep(target_interval)
+
+ def _update_camera_view(self):
+ """更新相机视角(线程安全)"""
+ if not self.current_vehicle:
+ return
+
+ try:
+ current_time = time.time()
+ time_delta = current_time - self.last_update_time
+
+ # 限制最小时间间隔
+ if time_delta < 0.001:
+ return
+
+ self.last_update_time = current_time
+ self.frame_count += 1
+
+ # 获取车辆当前状态
+ vehicle_transform = self.current_vehicle.get_transform()
+ vehicle_location = vehicle_transform.location
+ vehicle_rotation = vehicle_transform.rotation
+
+ # 获取车辆速度用于预测
+ vehicle_velocity = self.current_vehicle.get_velocity()
+ speed = math.sqrt(vehicle_velocity.x**2 + vehicle_velocity.y**2 + vehicle_velocity.z**2)
+
+ # 更新速度历史
+ self.velocity_history.append(speed)
+ if len(self.velocity_history) > self.max_velocity_history:
+ self.velocity_history.pop(0)
+
+ # 计算车辆的后方偏移方向
+ yaw_rad = math.radians(vehicle_rotation.yaw)
+
+ # 计算目标相机位置(车辆后方)
+ offset_x = -self.follow_distance * math.cos(yaw_rad)
+ offset_y = -self.follow_distance * math.sin(yaw_rad)
+
+ target_location = carla.Location(
+ x=vehicle_location.x + offset_x,
+ y=vehicle_location.y + offset_y,
+ z=vehicle_location.z + self.follow_height
+ )
+
+ # 目标相机旋转(朝向车辆)
+ target_rotation = carla.Rotation(
+ pitch=self.follow_pitch,
+ yaw=vehicle_rotation.yaw,
+ roll=0.0
+ )
+
+ # 自适应平滑系数
+ effective_smooth_factor = self._calculate_adaptive_smooth_factor(
+ vehicle_location, target_location, speed, time_delta
+ )
+
+ # 预测目标位置(如果启用)
+ if self.prediction_enabled and len(self.velocity_history) > 1:
+ avg_speed = np.mean(self.velocity_history[-3:]) if len(self.velocity_history) >= 3 else speed
+ target_location = self._predict_follow_position(
+ target_location, vehicle_rotation, avg_speed, time_delta
+ )
+
+ # 计算平滑移动
+ if self.last_camera_transform:
+ if self.use_interpolation:
+ # 使用多步插值
+ smooth_transform = self._multi_step_interpolation(
+ self.last_camera_transform,
+ target_location,
+ target_rotation,
+ effective_smooth_factor
+ )
+ else:
+ # 使用单步平滑
+ smooth_loc = self._lerp_location(
+ self.last_camera_transform.location,
+ target_location,
+ effective_smooth_factor
+ )
+
+ smooth_rot = self._lerp_rotation(
+ self.last_camera_transform.rotation,
+ target_rotation,
+ effective_smooth_factor
+ )
+
+ smooth_transform = carla.Transform(smooth_loc, smooth_rot)
+ else:
+ # 第一次直接设置
+ smooth_transform = carla.Transform(target_location, target_rotation)
+
+ # 设置相机
+ self.spectator.set_transform(smooth_transform)
+ self.last_camera_transform = smooth_transform
+ self.last_vehicle_transform = vehicle_transform
+
+ # 调试信息
+ if cfg.DEBUG_MODE and self.frame_count % 200 == 0:
+ print(f"[视角] 帧: {self.frame_count}, "
+ f"平滑系数: {effective_smooth_factor:.3f}, "
+ f"速度: {speed:.2f}m/s")
+
+ except Exception as e:
+ if cfg.DEBUG_MODE:
+ print(f"⚠️ 视角更新失败: {e}")
+
+ def set_follow_view(self, vehicle):
+ """设置后方跟随视角"""
+ if vehicle is None:
+ return False
+
+ try:
+ transform = vehicle.get_transform()
+ location = transform.location
+ rotation = transform.rotation
+
+ # 计算车辆的后方偏移方向
+ yaw_rad = math.radians(rotation.yaw)
+
+ # 计算相机位置(车辆后方)
+ offset_x = -self.follow_distance * math.cos(yaw_rad)
+ offset_y = -self.follow_distance * math.sin(yaw_rad)
+
+ camera_location = carla.Location(
+ x=location.x + offset_x,
+ y=location.y + offset_y,
+ z=location.z + self.follow_height
+ )
+
+ # 相机朝向车辆
+ camera_rotation = carla.Rotation(
+ pitch=self.follow_pitch,
+ yaw=rotation.yaw,
+ roll=0.0
+ )
+
+ camera_transform = carla.Transform(camera_location, camera_rotation)
+ self.spectator.set_transform(camera_transform)
+ self.last_camera_transform = camera_transform
+ self.last_vehicle_transform = transform
+
+ # 设置当前车辆并启动更新线程
+ self.current_vehicle = vehicle
+ if cfg.VIEW_UPDATE_THREADED:
+ self.start_view_update_thread(vehicle)
+
+ print(f"📐 设置后方跟随视角")
+ return True
+
+ except Exception as e:
+ print(f"❌ 设置后方跟随视角失败: {e}")
+ return False
+
+ def _predict_follow_position(self, target_loc, vehicle_rot, speed, time_delta):
+ """预测后方跟随位置"""
+ if speed < 0.1: # 速度太慢不预测
+ return target_loc
+
+ # 预测未来0.3秒的位置
+ prediction_time = 0.3
+ angle_rad = math.radians(vehicle_rot.yaw)
+
+ # 车辆前进方向
+ vehicle_dx = speed * math.cos(angle_rad) * prediction_time
+ vehicle_dy = speed * math.sin(angle_rad) * prediction_time
+
+ # 相机跟随车辆移动
+ predicted_x = target_loc.x + vehicle_dx
+ predicted_y = target_loc.y + vehicle_dy
+
+ return carla.Location(
+ x=predicted_x,
+ y=predicted_y,
+ z=target_loc.z
+ )
+
+ def _calculate_adaptive_smooth_factor(self, vehicle_loc, target_loc, speed, time_delta):
+ """计算自适应平滑系数"""
+ base_factor = self.smooth_factor
+
+ if not self.adaptive_smoothing:
+ return base_factor
+
+ # 计算当前位置与目标位置的距离
+ if self.last_camera_transform:
+ current_loc = self.last_camera_transform.location
+ distance = math.sqrt(
+ (target_loc.x - current_loc.x)**2 +
+ (target_loc.y - current_loc.y)**2 +
+ (target_loc.z - current_loc.z)**2
+ )
+
+ # 根据距离调整平滑系数
+ if distance > self.distance_threshold:
+ # 距离较远,使用较大的平滑系数快速接近
+ factor = min(self.max_smooth_factor,
+ base_factor * (1.0 + distance / self.distance_threshold * 0.5))
+ else:
+ # 距离较近,使用较小的平滑系数保持平滑
+ factor = max(self.min_smooth_factor,
+ base_factor * (distance / self.distance_threshold))
+
+ # 根据速度调整
+ if speed > 5.0: # 高速时减小平滑系数,反应更快
+ factor = min(factor * 0.8, self.max_smooth_factor)
+
+ # 根据时间间隔调整
+ if time_delta > 0.05: # 帧间隔较大时增加平滑系数
+ factor = min(factor * 1.2, self.max_smooth_factor)
+
+ return max(self.min_smooth_factor, min(self.max_smooth_factor, factor))
+
+ return base_factor
+
+ def _multi_step_interpolation(self, current_transform, target_loc, target_rot, smooth_factor):
+ """多步插值,实现更平滑的移动"""
+ current_loc = current_transform.location
+ current_rot = current_transform.rotation
+
+ # 计算每一步的插值比例
+ step_factor = smooth_factor / self.interpolation_steps
+
+ intermediate_loc = current_loc
+ intermediate_rot = current_rot
+
+ for step in range(self.interpolation_steps):
+ # 逐步插值
+ intermediate_loc = self._lerp_location(
+ intermediate_loc, target_loc, step_factor
+ )
+
+ intermediate_rot = self._lerp_rotation(
+ intermediate_rot, target_rot, step_factor
+ )
+
+ return carla.Transform(intermediate_loc, intermediate_rot)
+
+ def _lerp_location(self, loc1, loc2, t):
+ """改进的线性插值位置(指数平滑)"""
+ # 使用指数平滑:exp(-t) 而不是线性
+ alpha = 1.0 - math.exp(-t * 10.0) # 调整系数控制平滑度
+
+ return carla.Location(
+ x=loc1.x + (loc2.x - loc1.x) * alpha,
+ y=loc1.y + (loc2.y - loc1.y) * alpha,
+ z=loc1.z + (loc2.z - loc1.z) * alpha
+ )
+
+ def _lerp_rotation(self, rot1, rot2, t):
+ """改进的线性插值旋转(处理角度环绕)"""
+ def lerp_angle(a1, a2, t):
+ # 使用球形线性插值(SLERP)的思路
+ diff = ((a2 - a1 + 180) % 360) - 180
+
+ # 使用更平滑的插值函数
+ smooth_t = math.sin(t * math.pi / 2) # 使用sin函数实现缓入效果
+
+ return a1 + diff * smooth_t
+
+ return carla.Rotation(
+ pitch=lerp_angle(rot1.pitch, rot2.pitch, t),
+ yaw=lerp_angle(rot1.yaw, rot2.yaw, t),
+ roll=lerp_angle(rot1.roll, rot2.roll, t)
+ )
+
+ def get_vehicle_state(self, vehicle):
+ """获取车辆状态信息"""
+ if vehicle is None:
+ return None
+
+ try:
+ transform = vehicle.get_transform()
+ velocity = vehicle.get_velocity()
+ control = vehicle.get_control()
+
+ # 计算速度
+ speed_3d = math.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2)
+ speed_2d = math.sqrt(velocity.x**2 + velocity.y**2)
+
+ # 计算加速度(简化版本)
+ acceleration = 0.0
+ if self.last_vehicle_transform:
+ time_diff = time.time() - self.last_update_time
+ if time_diff > 0:
+ last_speed = self._calculate_speed_from_transform(
+ self.last_vehicle_transform, transform, time_diff
+ )
+ acceleration = (speed_2d - last_speed) / time_diff
+
+ state = {
+ 'x': transform.location.x,
+ 'y': transform.location.y,
+ 'z': transform.location.z,
+ 'heading': transform.rotation.yaw,
+ 'pitch': transform.rotation.pitch,
+ 'roll': transform.rotation.roll,
+ 'speed_3d': speed_3d,
+ 'speed_2d': speed_2d,
+ 'acceleration': acceleration,
+ 'velocity_x': velocity.x,
+ 'velocity_y': velocity.y,
+ 'velocity_z': velocity.z,
+ 'throttle': control.throttle,
+ 'steer': control.steer,
+ 'brake': control.brake,
+ 'hand_brake': control.hand_brake,
+ 'reverse': control.reverse
+ }
+
+ return state
+
+ except Exception as e:
+ print(f"❌ 获取车辆状态失败: {e}")
+ return None
+
+ def _calculate_speed_from_transform(self, prev_transform, curr_transform, time_diff):
+ """从两个变换计算速度"""
+ prev_loc = prev_transform.location
+ curr_loc = curr_transform.location
+
+ distance = math.sqrt(
+ (curr_loc.x - prev_loc.x)**2 +
+ (curr_loc.y - prev_loc.y)**2
+ )
+
+ return distance / time_diff if time_diff > 0 else 0.0
+
+ def calculate_progress(self, x, y, route_points):
+ """计算行驶进度"""
+ if not route_points or len(route_points) < 2:
+ return "进度: N/A"
+
+ # 计算到起点和终点的距离
+ start_point = route_points[0]
+ end_point = route_points[-1]
+
+ dist_to_start = math.sqrt((x - start_point[0])**2 + (y - start_point[1])**2)
+ dist_to_end = math.sqrt((x - end_point[0])**2 + (y - end_point[1])**2)
+
+ # 计算总路线长度(估算)
+ total_distance = 0
+ for i in range(len(route_points) - 1):
+ x1, y1, _ = route_points[i]
+ x2, y2, _ = route_points[i+1]
+ total_distance += math.sqrt((x2-x1)**2 + (y2-y1)**2)
+
+ # 计算进度百分比
+ if total_distance > 0:
+ traveled = max(0, total_distance - dist_to_end)
+ progress = min(100, (traveled / total_distance) * 100)
+ else:
+ progress = 0
+
+ return f"进度: {progress:.1f}% | 距起点: {dist_to_start:.1f}m | 距终点: {dist_to_end:.1f}m"
+
+ def update_follow_params(self, distance=None, height=None, pitch=None):
+ """更新后方跟随参数"""
+ if distance is not None and distance > 0:
+ self.follow_distance = distance
+ if height is not None:
+ self.follow_height = height
+ if pitch is not None:
+ self.follow_pitch = pitch
+
+ print(f"🔄 后方跟随参数更新 - 距离: {self.follow_distance}m, 高度: {self.follow_height}m, 俯角: {self.follow_pitch}°")
+
+ def update_smooth_factor(self, factor):
+ """更新平滑系数"""
+ if 0 < factor <= 1:
+ self.smooth_factor = factor
+ print(f"🔄 平滑系数更新为: {factor}")
+ else:
+ print(f"❌ 无效的平滑系数: {factor},保持为: {self.smooth_factor}")
+
+ def cleanup(self):
+ """清理资源"""
+ self.stop_view_update_thread()
+ self.current_vehicle = None
+
+ def reset(self):
+ """重置跟踪器状态"""
+ self.velocity_history = []
+ self.frame_count = 0
+ self.last_update_time = time.time()
+ print("🔄 车辆跟踪器已重置")
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Reset.srv b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Reset.srv
new file mode 100644
index 0000000000..e96055c32a
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Reset.srv
@@ -0,0 +1,2 @@
+---
+std_msgs/Float32MultiArray state
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/StartEpisode.srv b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/StartEpisode.srv
new file mode 100644
index 0000000000..9710dbf6a8
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/StartEpisode.srv
@@ -0,0 +1,3 @@
+bool success
+string message
+---
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Stop.srv b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Stop.srv
new file mode 100644
index 0000000000..cf5bd38fdb
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/srv/Stop.srv
@@ -0,0 +1,3 @@
+---
+bool success
+string message
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/utils/install.sh b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/utils/install.sh
new file mode 100644
index 0000000000..2d71dd0569
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/utils/install.sh
@@ -0,0 +1,29 @@
+#!/bin/bash
+# CARLA自动驾驶ROS包安装脚本
+
+echo "安装CARLA自动驾驶ROS包..."
+
+# 检查ROS环境
+if [ -z "$ROS_DISTRO" ]; then
+ echo "错误: ROS环境未设置"
+ exit 1
+fi
+
+# 创建工作空间
+mkdir -p ~/carla_ros_ws/src
+cd ~/carla_ros_ws
+
+# 复制文件
+cp -r /path/to/your/carla_project/* src/
+
+# 安装依赖
+rosdep install --from-paths src --ignore-src -r -y
+
+# 编译
+catkin_make
+
+# 添加环境变量
+echo "source ~/carla_ros_ws/devel/setup.bash" >> ~/.bashrc
+
+echo "安装完成!"
+echo "请先启动CARLA服务器,然后运行: roslaunch carla_autonomous carla_autonomous.launch"
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/utils/run_carla.sh b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/utils/run_carla.sh
new file mode 100644
index 0000000000..fa48d6ce44
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/utils/run_carla.sh
@@ -0,0 +1,21 @@
+#!/bin/bash
+# 启动CARLA服务器和ROS节点
+
+# 启动CARLA服务器
+echo "启动CARLA服务器..."
+cd ~/CARLA_0.9.13
+./CarlaUE4.sh -quality-level=Low -fps=20 &
+
+# 等待CARLA启动
+sleep 10
+
+# 设置ROS环境
+source ~/carla_ros_ws/devel/setup.bash
+
+# 启动ROS节点
+echo "启动ROS节点..."
+roslaunch carla_autonomous carla_autonomous.launch
+
+# 清理
+echo "正在关闭..."
+pkill CarlaUE4
\ No newline at end of file
diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/utils/setup_env.sh b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/utils/setup_env.sh
new file mode 100644
index 0000000000..2560289fca
--- /dev/null
+++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/carla_ros_ws/src/carla_autonomous/utils/setup_env.sh
@@ -0,0 +1,13 @@
+#!/bin/bash
+# 设置环境变量
+
+export CARLA_ROOT=/path/to/carla
+export PYTHONPATH=$PYTHONPATH:$CARLA_ROOT/PythonAPI/carla/dist/carla-0.9.14-py3.7-linux-x86_64.egg
+export PYTHONPATH=$PYTHONPATH:$CARLA_ROOT/PythonAPI/carla
+export ROS_MASTER_URI=http://localhost:11311
+export ROS_IP=localhost
+
+# 添加工作空间
+source ~/carla_ros_ws/devel/setup.bash
+
+echo "环境变量设置完成"
\ No newline at end of file