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