diff --git a/src/car_navigation_system/main.py b/src/car_navigation_system/main.py index bce974b7f0..fb2e10b2fd 100644 --- a/src/car_navigation_system/main.py +++ b/src/car_navigation_system/main.py @@ -1,16 +1,15 @@ # -------------------------- -# 1. 初始化CARLA连接和环境 +# 简化修复版：确保车辆正确生成 # -------------------------- + import carla import time import numpy as np import cv2 import math from collections import deque -import torch -import torch.nn as nn -import torch.nn.functional as F import random + import os @@ -62,9 +61,15 @@ def forward(self, image, state): throttle_brake = torch.sigmoid(control[:, :2]) steer = torch.tanh(control[:, 2:]) - return torch.cat([throttle_brake, steer], dim=1) +class SimpleController: + """简单但可靠的控制逻辑""" + + def __init__(self, world, vehicle): + self.world = world + self.vehicle = vehicle + # 新增：障碍物检测器类 class ObstacleDetector: def __init__(self, world, vehicle, max_distance=50.0): @@ -350,9 +355,23 @@ class TraditionalController: def __init__(self, world, obstacle_detector=None): self.world = world + self.map = world.get_map() - self.waypoint_distance = 10.0 + self.target_speed = 30.0 # km/h + self.waypoint_distance = 5.0 self.last_waypoint = None + + + def get_control(self): + """基于路点的简单控制""" + # 获取车辆状态 + location = self.vehicle.get_location() + transform = self.vehicle.get_transform() + velocity = self.vehicle.get_velocity() + + # 计算速度 + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 # km/h + self.obstacle_detector = obstacle_detector self.emergency_brake_distance = 6.0 self.safe_following_distance = 10.0 @@ -420,21 +439,30 @@ def get_control(self, vehicle): if self.obstacle_detector: obstacle_info = self.obstacle_detector.get_obstacle_info() + # 获取路点 waypoint = self.map.get_waypoint(location, project_to_road=True) + + if not waypoint: + # 如果没有找到路点，返回保守控制 + return 0.3, 0.0, 0.0 + + # 获取下一个路点 next_waypoints = waypoint.next(self.waypoint_distance) if not next_waypoints: - # 如果没有找到路点，尝试获取当前路点 - next_waypoints = [waypoint] + # 如果没有下一个路点，使用当前路点 + target_waypoint = waypoint + else: + target_waypoint = next_waypoints[0] - target_waypoint = next_waypoints[0] self.last_waypoint = target_waypoint # 计算转向 vehicle_yaw = math.radians(transform.rotation.yaw) target_loc = target_waypoint.transform.location + # 计算相对位置 dx = target_loc.x - location.x dy = target_loc.y - location.y @@ -445,6 +473,33 @@ def get_control(self, vehicle): steer = 0.0 else: angle = math.atan2(local_y, local_x) + + steer = max(-0.5, min(0.5, angle / 1.0)) + + # 速度控制 + if speed < self.target_speed * 0.8: + throttle, brake = 0.6, 0.0 + elif speed > self.target_speed * 1.2: + throttle, brake = 0.0, 0.3 + else: + throttle, brake = 0.3, 0.0 + + return throttle, brake, steer + + +class SimpleDrivingSystem: + def __init__(self): + self.client = None + self.world = None + self.vehicle = None + self.camera = None + self.controller = None + self.camera_image = None + + def connect(self): + """连接到CARLA服务器""" + print("正在连接到CARLA服务器...") + steer = np.clip(angle / math.radians(45), -1.0, 1.0) # 速度控制（基于障碍物距离调整） @@ -620,38 +675,36 @@ def get_control(self, vehicle): print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆") -# 配置传感器（简化配置） -third_camera_bp = blueprint_library.find('sensor.camera.rgb') -third_camera_bp.set_attribute('image_size_x', '640') -third_camera_bp.set_attribute('image_size_y', '480') -third_camera_bp.set_attribute('fov', '110') -third_camera_transform = carla.Transform( - carla.Location(x=-5.0, y=0.0, z=3.0), - carla.Rotation(pitch=-15.0) -) -third_camera = world.spawn_actor(third_camera_bp, third_camera_transform, attach_to=vehicle) - -front_camera_bp = blueprint_library.find('sensor.camera.rgb') -front_camera_bp.set_attribute('image_size_x', '640') -front_camera_bp.set_attribute('image_size_y', '480') -front_camera_bp.set_attribute('fov', '90') -front_camera_transform = carla.Transform( - carla.Location(x=2.0, y=0.0, z=1.5), - carla.Rotation(pitch=0.0) -) -front_camera = world.spawn_actor(front_camera_bp, front_camera_transform, attach_to=vehicle) -# 传感器数据存储 -third_image = None -front_image = None + try: + # 尝试多种连接方式 + self.client = carla.Client('localhost', 2000) + self.client.set_timeout(10.0) + # 检查可用地图 + available_maps = self.client.get_available_maps() + print(f"可用地图: {available_maps}") -def third_camera_callback(image): - global third_image - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - third_image = array[:, :, :3] + # 加载地图 + self.world = self.client.load_world('Town01') + print("地图加载成功") + # 设置同步模式 + settings = self.world.get_settings() + settings.synchronous_mode = False # 先使用异步模式确保连接 + settings.fixed_delta_seconds = None + self.world.apply_settings(settings) + + print("连接成功！") + return True + + except Exception as e: + print(f"连接失败: {e}") + print("请确保:") + print("1. CARLA服务器正在运行") + print("2. 服务器端口为2000") + print("3. 地图Town01可用") + return False def front_camera_callback(image): global front_image @@ -660,42 +713,94 @@ def front_camera_callback(image): front_image = array[:, :, :3] -third_camera.listen(third_camera_callback) -front_camera.listen(front_camera_callback) + def spawn_vehicle(self): + """生成车辆 - 简化版本""" + print("正在生成车辆...") + + try: + # 获取蓝图库 + blueprint_library = self.world.get_blueprint_library() + + # 选择车辆蓝图 + vehicle_bp = blueprint_library.find('vehicle.tesla.model3') + if not vehicle_bp: + print("未找到特斯拉蓝图，尝试其他车辆...") + vehicle_bp = blueprint_library.filter('vehicle.*')[0] + + + vehicle_bp.set_attribute('color', '255,0,0') # 红色 + + # 获取出生点 + spawn_points = self.world.get_map().get_spawn_points() + print(f"找到 {len(spawn_points)} 个出生点") + + if not spawn_points: + print("没有可用的出生点！") + return False -time.sleep(2.0) + # 选择第一个出生点 + spawn_point = spawn_points[0] -# 新增：初始化障碍物检测器 -print("初始化障碍物检测器...") -obstacle_detector = ObstacleDetector(world, vehicle, max_distance=50.0) + # 尝试生成车辆 + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) -# 修复6: 初始化控制器（传入障碍物检测器） -nn_controller = ImprovedNeuralController(obstacle_detector) -traditional_controller = TraditionalController(world, obstacle_detector) + if not self.vehicle: + print("无法生成车辆，尝试清理现有车辆...") + # 清理现有车辆 + for actor in self.world.get_actors().filter('vehicle.*'): + actor.destroy() + time.sleep(0.5) -# 控制变量 -throttle = 0.3 # 更保守的初始油门 -steer = 0.0 -brake = 0.0 -NEURAL_NETWORK_MODE = False # 默认使用传统控制，更稳定 + # 再次尝试 + self.vehicle = self.world.try_spawn_actor(vehicle_bp, spawn_point) -# 转向历史，用于平滑 -steer_history = deque(maxlen=10) + if self.vehicle: + print(f"车辆生成成功！ID: {self.vehicle.id}") + print(f"位置: {spawn_point.location}") -# 障碍物信息历史 -obstacle_history = deque(maxlen=5) + # 禁用自动驾驶 + self.vehicle.set_autopilot(False) + + return True + else: + print("车辆生成失败") + return False print("初始化车辆状态...") vehicle.set_simulate_physics(True) -# 修复7: 更温和的启动控制 -print("应用启动控制...") -vehicle.apply_control(carla.VehicleControl( - throttle=0.5, # 降低初始油门 - steer=0.0, - brake=0.0, - hand_brake=False -)) + + except Exception as e: + print(f"生成车辆时出错: {e}") + return False + + + def setup_camera(self): + """设置相机""" + print("正在设置相机...") + + try: + blueprint_library = self.world.get_blueprint_library() + camera_bp = blueprint_library.find('sensor.camera.rgb') + + # 设置相机属性 + camera_bp.set_attribute('image_size_x', '640') + camera_bp.set_attribute('image_size_y', '480') + camera_bp.set_attribute('fov', '90') + + # 相机位置（车辆后方） + camera_transform = carla.Transform( + carla.Location(x=-8.0, z=6.0), # 在车辆后方上方 + carla.Rotation(pitch=-20.0) # 向下看 + ) + + # 生成相机 + self.camera = self.world.spawn_actor( + camera_bp, camera_transform, attach_to=self.vehicle + ) + + # 设置回调函数 + self.camera.listen(lambda image: self.camera_callback(image)) # 添加NPC车辆管理函数 @@ -740,10 +845,66 @@ def check_and_reset_stuck_npcs(): last_collision_time = 0 # 上次碰撞时间 last_npc_check_time = 0 # 上次检查NPC的时间 - # 主循环 - while True: - world.tick() - frame_count += 1 + + print("相机设置成功") + return True + + + except Exception as e: + print(f"设置相机时出错: {e}") + return False + + def camera_callback(self, image): + """相机数据回调""" + try: + # 转换图像数据 + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + self.camera_image = array[:, :, :3] # RGB通道 + except: + pass + + def setup_controller(self): + """设置控制器""" + self.controller = SimpleController(self.world, self.vehicle) + print("控制器设置完成") + + def run(self): + """主运行循环""" + print("\n" + "=" * 50) + print("简化自动驾驶系统") + print("=" * 50) + + # 连接服务器 + if not self.connect(): + return + + # 生成车辆 + if not self.spawn_vehicle(): + return + + # 设置相机 + if not self.setup_camera(): + # 即使相机失败也继续运行 + print("警告：相机设置失败，继续运行...") + + # 设置控制器 + self.setup_controller() + + # 等待一会儿让系统稳定 + print("系统初始化中...") + time.sleep(2.0) + + # 设置天气 + weather = carla.WeatherParameters( + cloudiness=30.0, + precipitation=0.0, + sun_altitude_angle=70.0 + ) + self.world.set_weather(weather) + + # 生成一些NPC车辆 + self.spawn_npc_vehicles(2) # 定期检查NPC车辆状态（每100帧检查一次） if frame_count - last_npc_check_time > 100: @@ -760,40 +921,154 @@ def check_and_reset_stuck_npcs(): obstacle_info = obstacle_detector.get_obstacle_info() obstacle_history.append(obstacle_info) - # 显示障碍物信息 - if obstacle_info['has_obstacle']: - print(f"障碍物检测: 距离={obstacle_info['distance']:.1f}m, " - f"角度={obstacle_info['relative_angle']:.1f}°, " - f"类型={obstacle_info['obstacle_type']}") - print(f"帧 {frame_count}: 速度={vehicle_speed * 3.6:.1f}km/h, " - f"模式={'神经网络' if NEURAL_NETWORK_MODE else '传统'}, " - f"障碍物={'有' if obstacle_info['has_obstacle'] else '无'}") + print("\n系统准备就绪！") + print("控制指令:") + print(" q - 退出程序") + print(" r - 重置车辆") + print(" s - 紧急停止") + print("\n开始自动驾驶...\n") + + frame_count = 0 + running = True + + try: + while running: + # 获取车辆状态 + velocity = self.vehicle.get_velocity() + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 + + # 获取控制指令 + throttle, brake, steer = self.controller.get_control() + + # 应用控制 + control = carla.VehicleControl( + throttle=float(throttle), + brake=float(brake), + steer=float(steer), + hand_brake=False, + reverse=False + ) + self.vehicle.apply_control(control) + + # 更新显示 + if self.camera_image is not None: + display_img = self.camera_image.copy() + + # 添加状态信息 + cv2.putText(display_img, f"Speed: {speed:.1f} km/h", + (20, 40), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 255), 2) + cv2.putText(display_img, f"Throttle: {throttle:.2f}", + (20, 80), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 255), 2) + cv2.putText(display_img, f"Steer: {steer:.2f}", + (20, 120), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 255), 2) + cv2.putText(display_img, f"Frame: {frame_count}", + (20, 160), cv2.FONT_HERSHEY_SIMPLEX, + 0.8, (255, 255, 255), 2) + + cv2.imshow('Autonomous Driving - Simple Version', display_img) + + # 处理按键 + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + print("正在退出...") + running = False + elif key == ord('r'): + self.reset_vehicle() + elif key == ord('s'): + # 紧急停止 + self.vehicle.apply_control(carla.VehicleControl( + throttle=0.0, brake=1.0, hand_brake=True + )) + print("紧急停止") + + frame_count += 1 + + # 每100帧显示一次状态 + if frame_count % 100 == 0: + print(f"运行中... 帧数: {frame_count}, 速度: {speed:.1f} km/h") + + time.sleep(0.05) + + except KeyboardInterrupt: + print("\n用户中断") + except Exception as e: + print(f"运行错误: {e}") + finally: + self.cleanup() + + def spawn_npc_vehicles(self, count=2): + """生成NPC车辆（简化）""" + print(f"正在生成 {count} 辆NPC车辆...") + + try: + blueprint_library = self.world.get_blueprint_library() + spawn_points = self.world.get_map().get_spawn_points() + + npc_vehicles = [] # 修复8: 改进的卡住检测 current_position = vehicle_location distance_moved = current_position.distance(last_position) - # 更精确的卡住检测 - is_moving = distance_moved > 0.2 or vehicle_speed > 1.0 - if not is_moving: - stuck_count += 1 - else: - stuck_count = 0 - success_count += 1 # 成功运行一帧 + + for i in range(min(count, len(spawn_points))): + # 跳过主车辆的出生点 + if i == 0: + continue + + + try: + # 随机选择车辆类型 + vehicle_bps = list(blueprint_library.filter('vehicle.*')) + if vehicle_bps: + vehicle_bp = random.choice(vehicle_bps) + + # 生成NPC + npc = self.world.try_spawn_actor(vehicle_bp, spawn_points[i]) + + if npc: + npc.set_autopilot(True) + npc_vehicles.append(npc) + print(f"生成NPC车辆 {len(npc_vehicles)}") + except: + pass + + print(f"成功生成 {len(npc_vehicles)} 辆NPC车辆") + + except Exception as e: + print(f"生成NPC车辆时出错: {e}") + + def reset_vehicle(self): + """重置车辆位置""" + print("重置车辆...") last_position = current_position - # 修复9: 更智能的卡住恢复 - if stuck_count > 15: # 1.5秒后认为卡住 - print("检测到车辆卡住，执行恢复程序...") - # 先完全停止 - vehicle.apply_control(carla.VehicleControl( - throttle=0.0, steer=0.0, brake=1.0, hand_brake=True - )) + spawn_points = self.world.get_map().get_spawn_points() + if spawn_points: + new_spawn_point = random.choice(spawn_points) + self.vehicle.set_transform(new_spawn_point) + print(f"车辆已重置到新位置: {new_spawn_point.location}") + + # 等待重置完成 time.sleep(0.5) + def cleanup(self): + """清理资源""" + print("\n正在清理资源...") + + if self.camera: + try: + self.camera.stop() + self.camera.destroy() + except: + pass + # 检查是否有前方障碍物 if obstacle_info['has_obstacle'] and obstacle_info['distance'] < 10: print("前方有障碍物，尝试倒车...") @@ -824,13 +1099,32 @@ def check_and_reset_stuck_npcs(): front_image, vehicle_speed, steer_history, obstacle_info ) - # 修复10: 更激进的控制平滑 - throttle = 0.3 * throttle + 0.7 * nn_throttle - brake = 0.3 * brake + 0.7 * nn_brake - steer = 0.2 * steer + 0.8 * nn_steer - # 记录转向历史 - steer_history.append(steer) + if self.vehicle: + try: + self.vehicle.destroy() + except: + pass + + # 等待销毁完成 + time.sleep(1.0) + + + cv2.destroyAllWindows() + print("清理完成") + + +def main(): + """主函数""" + print("自动驾驶系统 - 简化版本") + print("确保CARLA服务器正在运行...") + + system = SimpleDrivingSystem() + system.run() + + +if __name__ == "__main__": + main() else: # 传统控制 - 更稳定 @@ -937,4 +1231,5 @@ def check_and_reset_stuck_npcs(): settings.synchronous_mode = False world.apply_settings(settings) cv2.destroyAllWindows() - print("资源清理完成") \ No newline at end of file + print("资源清理完成") +