diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/car_env.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/car_env.py index 1d461cb8fc..0c5caf86d9 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/car_env.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/car_env.py @@ -420,19 +420,19 @@ def step(self, action, current_state): self.vehicle.apply_control(carla.VehicleControl(throttle=0, brake=1.0)) print("执行动作: 刹车") elif action == 1: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=0*self.STEER_AMT)) + self.vehicle.apply_control(carla.VehicleControl(throttle=0.5, 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)) + 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.05*self.STEER_AMT)) + 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.05*self.STEER_AMT)) + self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer= 0.1*self.STEER_AMT)) print("执行动作: 微右") # 处理图像 @@ -498,7 +498,7 @@ def step(self, action, current_state): reward = -200 print("❌ 方向偏差过大!") - if abs(signed_dis) > 3: + if abs(signed_dis) > 2: reward = -50 print("⚠️ 横向偏差过大") diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/config.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/config.py index 5740c0f6fd..195518bacc 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/config.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/config.py @@ -1,327 +1,128 @@ -#!/usr/bin/env python - -# Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma de -# Barcelona (UAB). -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - """ -Configure and inspect an instance of CARLA Simulator. - -For further details, visit -https://carla.readthedocs.io/en/latest/configuring_the_simulation/ +配置文件 - 存储所有配置参数 """ -import glob -import os -import sys - -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 argparse -import datetime -import re -import socket -import textwrap - - -def get_ip(host): - if host in ['localhost', '127.0.0.1']: - sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - try: - sock.connect(('10.255.255.255', 1)) - host = sock.getsockname()[0] - except RuntimeError: - pass - finally: - sock.close() - return host - - -def find_weather_presets(): - 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 list_options(client): - maps = [m.replace('/Game/Carla/Maps/', '') for m in client.get_available_maps()] - indent = 4 * ' ' - def wrap(text): - return '\n'.join(textwrap.wrap(text, initial_indent=indent, subsequent_indent=indent)) - print('weather presets:\n') - print(wrap(', '.join(x for _, x in find_weather_presets())) + '.\n') - print('available maps:\n') - print(wrap(', '.join(sorted(maps))) + '.\n') - - -def list_blueprints(world, bp_filter): - blueprint_library = world.get_blueprint_library() - blueprints = [bp.id for bp in blueprint_library.filter(bp_filter)] - print('available blueprints (filter %r):\n' % bp_filter) - for bp in sorted(blueprints): - print(' ' + bp) - print('') - - -def inspect(args, client): - address = '%s:%d' % (get_ip(args.host), args.port) - - world = client.get_world() - elapsed_time = world.get_snapshot().timestamp.elapsed_seconds - elapsed_time = datetime.timedelta(seconds=int(elapsed_time)) - - actors = world.get_actors() - s = world.get_settings() - - weather = 'Custom' - current_weather = world.get_weather() - for preset, name in find_weather_presets(): - if current_weather == preset: - weather = name - - if s.fixed_delta_seconds is None: - frame_rate = 'variable' +# ==================== 轨迹配置 ==================== +TRAJECTORIES = { + "custom_trajectory": { + "start": [-8.77956485748291, 140.2951202392578, 2.0014660358428955, 0], + "end": [74.17852020263672, -56.52183151245117, 0.18172569572925568], + "description": "自定义轨迹 - 城镇道路" + }, + "test_trajectory": { + "start": [0.0, 0.0, 2.0, 0], + "end": [100.0, 0.0, 2.0], + "description": "测试轨迹 - 直线道路" + } +} + +# 当前使用的轨迹 +CURRENT_TRAJECTORY = "custom_trajectory" + +def get_current_trajectory(): + """获取当前轨迹配置""" + if CURRENT_TRAJECTORY in TRAJECTORIES: + return TRAJECTORIES[CURRENT_TRAJECTORY] else: - frame_rate = '%.2f ms (%d FPS)' % ( - 1000.0 * s.fixed_delta_seconds, - 1.0 / s.fixed_delta_seconds) - - print('-' * 34) - print('address:% 26s' % address) - print('version:% 26s\n' % client.get_server_version()) - print('map: % 22s' % world.get_map().name) - print('weather: % 22s\n' % weather) - print('time: % 22s\n' % elapsed_time) - print('frame rate: % 22s' % frame_rate) - print('rendering: % 22s' % ('disabled' if s.no_rendering_mode else 'enabled')) - print('sync mode: % 22s\n' % ('disabled' if not s.synchronous_mode else 'enabled')) - print('actors: % 22d' % len(actors)) - print(' * spectator:% 20d' % len(actors.filter('spectator'))) - print(' * static: % 20d' % len(actors.filter('static.*'))) - print(' * traffic: % 20d' % len(actors.filter('traffic.*'))) - print(' * vehicles: % 20d' % len(actors.filter('vehicle.*'))) - print(' * walkers: % 20d' % len(actors.filter('walker.*'))) - print('-' * 34) - - -def main(): - argparser = argparse.ArgumentParser( - description=__doc__) - argparser.add_argument( - '--host', - metavar='H', - default='localhost', - help='IP of the host CARLA Simulator (default: localhost)') - argparser.add_argument( - '-p', '--port', - metavar='P', - default=2000, - type=int, - help='TCP port of CARLA Simulator (default: 2000)') - argparser.add_argument( - '-d', '--default', - action='store_true', - help='set default settings') - argparser.add_argument( - '-m', '--map', - help='load a new map, use --list to see available maps') - argparser.add_argument( - '-r', '--reload-map', - action='store_true', - help='reload current map') - argparser.add_argument( - '--delta-seconds', - metavar='S', - type=float, - help='set fixed delta seconds, zero for variable frame rate') - argparser.add_argument( - '--fps', - metavar='N', - type=float, - help='set fixed FPS, zero for variable FPS (similar to --delta-seconds)') - argparser.add_argument( - '--rendering', - action='store_true', - help='enable rendering') - argparser.add_argument( - '--no-rendering', - action='store_true', - help='disable rendering') - argparser.add_argument( - '--no-sync', - action='store_true', - help='disable synchronous mode') - argparser.add_argument( - '--weather', - help='set weather preset, use --list to see available presets') - argparser.add_argument( - '-i', '--inspect', - action='store_true', - help='inspect simulation') - argparser.add_argument( - '-l', '--list', - action='store_true', - help='list available options') - argparser.add_argument( - '-b', '--list-blueprints', - metavar='FILTER', - help='list available blueprints matching FILTER (use \'*\' to list them all)') - argparser.add_argument( - '-x', '--xodr-path', - metavar='XODR_FILE_PATH', - help='load a new map with a minimum physical road representation of the provided OpenDRIVE') - argparser.add_argument( - '--osm-path', - metavar='OSM_FILE_PATH', - help='load a new map with a minimum physical road representation of the provided OpenStreetMaps') - argparser.add_argument( - '--tile-stream-distance', - metavar='N', - type=float, - help='Set tile streaming distance (large maps only)') - argparser.add_argument( - '--actor-active-distance', - metavar='N', - type=float, - help='Set actor active distance (large maps only)') - if len(sys.argv) < 2: - argparser.print_help() - return - - args = argparser.parse_args() - - client = carla.Client(args.host, args.port, worker_threads=1) - client.set_timeout(10.0) - - if args.default: - args.rendering = True - args.delta_seconds = 0.0 - args.weather = 'Default' - args.no_sync = True - - if args.map is not None: - print('load map %r.' % args.map) - world = client.load_world(args.map) - elif args.reload_map: - print('reload map.') - world = client.reload_world() - elif args.xodr_path is not None: - if os.path.exists(args.xodr_path): - with open(args.xodr_path, encoding='utf-8') as od_file: - try: - data = od_file.read() - except OSError: - print('file could not be readed.') - sys.exit() - print('load opendrive map %r.' % os.path.basename(args.xodr_path)) - vertex_distance = 2.0 # in meters - max_road_length = 500.0 # in meters - wall_height = 1.0 # in meters - extra_width = 0.6 # in meters - world = client.generate_opendrive_world( - data, carla.OpendriveGenerationParameters( - vertex_distance=vertex_distance, - max_road_length=max_road_length, - wall_height=wall_height, - additional_width=extra_width, - smooth_junctions=True, - enable_mesh_visibility=True)) - else: - print('file not found.') - elif args.osm_path is not None: - if os.path.exists(args.osm_path): - with open(args.osm_path, encoding='utf-8') as od_file: - try: - data = od_file.read() - except OSError: - print('file could not be readed.') - sys.exit() - print('Converting OSM data to opendrive') - xodr_data = carla.Osm2Odr.convert(data) - print('load opendrive map.') - vertex_distance = 2.0 # in meters - max_road_length = 500.0 # in meters - wall_height = 0.0 # in meters - extra_width = 0.6 # in meters - world = client.generate_opendrive_world( - xodr_data, carla.OpendriveGenerationParameters( - vertex_distance=vertex_distance, - max_road_length=max_road_length, - wall_height=wall_height, - additional_width=extra_width, - smooth_junctions=True, - enable_mesh_visibility=True)) - else: - print('file not found.') - - else: - world = client.get_world() - - settings = world.get_settings() - - if args.no_rendering: - print('disable rendering.') - settings.no_rendering_mode = True - elif args.rendering: - print('enable rendering.') - settings.no_rendering_mode = False - - if args.no_sync: - print('disable synchronous mode.') - settings.synchronous_mode = False - - if args.delta_seconds is not None: - settings.fixed_delta_seconds = args.delta_seconds - elif args.fps is not None: - settings.fixed_delta_seconds = (1.0 / args.fps) if args.fps > 0.0 else 0.0 - - if args.delta_seconds is not None or args.fps is not None: - if settings.fixed_delta_seconds > 0.0: - print('set fixed frame rate %.2f milliseconds (%d FPS)' % ( - 1000.0 * settings.fixed_delta_seconds, - 1.0 / settings.fixed_delta_seconds)) - else: - print('set variable frame rate.') - settings.fixed_delta_seconds = None - - if args.tile_stream_distance is not None: - settings.tile_stream_distance = args.tile_stream_distance - if args.actor_active_distance is not None: - settings.actor_active_distance = args.actor_active_distance - - world.apply_settings(settings) - - if args.weather is not None: - if not hasattr(carla.WeatherParameters, args.weather): - print('ERROR: weather preset %r not found.' % args.weather) - else: - print('set weather preset %r.' % args.weather) - world.set_weather(getattr(carla.WeatherParameters, args.weather)) - - if args.inspect: - inspect(args, client) - if args.list: - list_options(client) - if args.list_blueprints: - list_blueprints(world, args.list_blueprints) - - -if __name__ == '__main__': - - try: - - main() - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') - except RuntimeError as e: - print(e) + print(f"❌ 轨迹 '{CURRENT_TRAJECTORY}' 不存在") + return None + +# ==================== 模型配置 ==================== +MODEL_PATHS = { + 'braking': "models/Braking___282.model", + 'driving': "models/Driving__6030.model" +} + +# ==================== 动作配置 ==================== +ACTION_NAMES = ["刹车", "直行", "左转", "右转", "微左", "微右"] + +# ==================== 训练配置 ==================== +TOTAL_EPISODES = 3 # 总共运行的episode数 +MAX_STEPS_PER_EPISODE = 2000 # 每个episode最大步数 +EPISODE_INTERVAL = 2.0 # episode之间的间隔秒数 + +# ==================== 交通配置 ==================== +ENABLE_TRAFFIC = True # 是否启用交通流（暂时关闭，避免干扰） +TRAFFIC_VEHICLES = 15 # 交通车辆数量 +TRAFFIC_WALKERS = 20 # 交通行人数量 +TRAFFIC_SAFE_MODE = True # 交通安全模式 +TRAFFIC_HYBRID_MODE = True # 混合物理模式 +TRAFFIC_SYNC_MODE = False # 交通同步模式 +TRAFFIC_RESPAWN = False # 是否重生休眠车辆 + +# ==================== 可视化配置 ==================== +ROUTE_COLOR = (255, 0, 0) # 路线颜色 (红色) +PATH_COLOR = (0, 100, 255) # 路径颜色 (蓝色) +VEHICLE_COLOR = (0, 255, 0) # 车辆颜色 (绿色) + +ROUTE_HEIGHT = 0.3 # 路线显示高度 +PATH_HEIGHT = 0.2 # 路径显示高度 +VEHICLE_HEIGHT = 0.25 # 车辆显示高度 + +# ==================== 视角配置 ==================== +TOP_DOWN_HEIGHT = 30.0 # 俯视视角高度（提高） +TOP_DOWN_PITCH = -85.0 # 俯视角 (几乎垂直向下，-90是完全垂直) +SMOOTH_FOLLOW_FACTOR = 0.001 # 视角平滑系数 (0-1，越小越平滑) +MIN_SMOOTH_FACTOR = 0 # 最小平滑系数 +MAX_SMOOTH_FACTOR = 0.3 # 最大平滑系数 +SMOOTH_FACTOR_ADAPTIVE = True # 是否自适应平滑系数 +DISTANCE_THRESHOLD = 5.0 # 距离阈值，超过此值加大平滑系数 + +# ==================== 性能配置 ==================== +DEBUG_MODE = True # 调试模式 +FPS_LIMIT = 0 # FPS限制 (0为无限制，提高性能) +UPDATE_RATE = 1.0 # 更新率 (1.0 = 每步更新) +USE_SMOOTH_INTERPOLATION = True # 使用平滑插值 +INTERPOLATION_STEPS = 5 # 插值步数 +MAX_FRAME_SKIP = 2 # 最大跳帧数 + +# ==================== CARLA配置 ==================== +CARLA_HOST = "localhost" +CARLA_PORT = 2000 +CARLA_TIMEOUT = 20.0 + +# ==================== 仿真配置 ==================== +FIXED_DELTA_SECONDS = 0.0166 # 固定时间步长 (~30 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) diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/config_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/config_manager.py new file mode 100644 index 0000000000..a32097ae8d --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/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 + } diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/main.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main.py new file mode 100644 index 0000000000..104a8c1166 --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/main.py @@ -0,0 +1,280 @@ +""" +主程序入口 - 协调所有模块 +""" + +import os +os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' +import time +import numpy as np +from collections import deque + +from car_env import CarEnv +from route_visualizer import RouteVisualizer +from vehicle_tracker import VehicleTracker +from model_manager import ModelManager +from trajectory_manager import TrajectoryManager +from traffic_manager import TrafficManager +from config_manager import ConfigManager +import config as cfg + +def setup_environment(): + """设置整体环境""" + print("=" * 60) + print("CARLA自动驾驶系统启动") + print("=" * 60) + + # 获取配置 + trajectory = cfg.get_current_trajectory() + if trajectory is None: + print("❌ 无法获取轨迹配置") + return None, None, None, None, None, None, None + + print(f"📌 使用轨迹: {trajectory['description']}") + + # 创建CARLA环境 + try: + env = CarEnv(trajectory['start'], trajectory['end']) + print("✅ CARLA环境创建成功") + except Exception as e: + print(f"❌ 创建CARLA环境失败: {e}") + return None, None, None, None, None, None, None + + # 创建配置管理器 + config_mgr = ConfigManager(client=env.client) + + # 应用默认设置 + config_mgr.apply_default_settings() + + # 设置仿真参数（提高性能） + settings = env.world.get_settings() + settings.fixed_delta_seconds = cfg.FIXED_DELTA_SECONDS + settings.synchronous_mode = cfg.SYNCHRONOUS_MODE + settings.no_rendering_mode = cfg.NO_RENDERING_MODE + env.world.apply_settings(settings) + + print(f"📊 设置时间步长: {cfg.FIXED_DELTA_SECONDS}s ({1/cfg.FIXED_DELTA_SECONDS:.1f} FPS)") + + # 检查模拟器状态 + config_mgr.inspect_simulation() + + # 创建交通管理器 + traffic_mgr = TrafficManager(client=env.client) + + # 生成交通流 + if cfg.ENABLE_TRAFFIC: + print("\n🚦 生成交通流...") + traffic_mgr.generate_traffic( + num_vehicles=cfg.TRAFFIC_VEHICLES, + num_walkers=cfg.TRAFFIC_WALKERS, + safe_mode=cfg.TRAFFIC_SAFE_MODE, + hybrid_mode=cfg.TRAFFIC_HYBRID_MODE, + sync_mode=cfg.TRAFFIC_SYNC_MODE, + respawn_vehicles=cfg.TRAFFIC_RESPAWN + ) + + # 创建路线可视化器 + visualizer = RouteVisualizer(env.world) + + # 创建车辆跟踪器（控制视角） + tracker = VehicleTracker(env.world) + + # 创建模型管理器 + model_mgr = ModelManager() + + # 创建轨迹管理器 + traj_mgr = TrajectoryManager(env) + + return env, config_mgr, traffic_mgr, visualizer, tracker, model_mgr, traj_mgr + +def run_episode(env, config_mgr, traffic_mgr, visualizer, tracker, model_mgr, traj_mgr, episode_num): + """运行单个episode""" + print(f"\n{'='*60}") + print(f"Episode {episode_num}") + print(f"{'='*60}") + + # 重置环境 + try: + current_state = env.reset() + print(f"✅ 环境重置成功") + except Exception as e: + print(f"❌ 环境重置失败: {e}") + return False + + # 获取车辆 + ego_vehicle = env.vehicle + if ego_vehicle is None: + print("❌ 未找到车辆") + return False + + # 重置跟踪器 + tracker.reset() + + # 设置初始视角（俯视） + tracker.set_top_down_view(ego_vehicle, height=cfg.TOP_DOWN_HEIGHT) + + # 获取并绘制规划路线 + route_points = traj_mgr.get_route_points() + visualizer.draw_planned_route(route_points) + + # 重置可视化器历史 + visualizer.reset_history() + + done = False + step_count = 0 + frame_skip_count = 0 + fps_counter = deque(maxlen=120) # 增加历史长度 + last_frame_time = time.time() + + while not done and step_count < cfg.MAX_STEPS_PER_EPISODE: + step_count += 1 + step_start = time.time() + current_time = time.time() + + # 计算帧间隔 + frame_interval = current_time - last_frame_time + last_frame_time = current_time + + # 自适应跳帧逻辑 + should_skip_frame = False + if cfg.MAX_FRAME_SKIP > 0 and frame_interval > cfg.FIXED_DELTA_SECONDS * 1.5: + frame_skip_count += 1 + if frame_skip_count <= cfg.MAX_FRAME_SKIP: + should_skip_frame = True + if cfg.DEBUG_MODE and step_count % 50 == 0: + print(f"[跳过] 帧间隔过大: {frame_interval:.3f}s，跳过更新") + else: + frame_skip_count = 0 + + # 更新交通管理器（如果是同步模式） + if traffic_mgr and cfg.TRAFFIC_SYNC_MODE and not should_skip_frame: + traffic_mgr.update() + + # 获取车辆状态 + vehicle_state = tracker.get_vehicle_state(ego_vehicle) + + # 更新视角（跳过某些帧时也更新，但减少频率） + if not should_skip_frame or step_count % 2 == 0: + tracker.smooth_follow_vehicle(ego_vehicle, height=cfg.TOP_DOWN_HEIGHT) + + # 更新车辆可视化（可以适当降低频率） + if vehicle_state and (step_count % 2 == 0 or not should_skip_frame): + visualizer.update_vehicle_display( + vehicle_state['x'], + vehicle_state['y'], + vehicle_state['heading'] + ) + + # 模型预测动作 + action = model_mgr.predict_action(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 + + # 显示进度 + if step_count % 100 == 0: + progress_info = tracker.calculate_progress( + vehicle_state['x'] if vehicle_state else 0, + vehicle_state['y'] if vehicle_state else 0, + route_points + ) + print(f"步骤 {step_count}, 奖励: {reward:.2f}, {progress_info}") + + # 计算FPS + frame_time = time.time() - step_start + fps_counter.append(frame_time) + + # 计算平滑FPS + if len(fps_counter) >= 30: + avg_frame_time = np.mean(list(fps_counter)[-30:]) + current_fps = 1.0 / avg_frame_time if avg_frame_time > 0 else 0 + else: + current_fps = len(fps_counter) / sum(fps_counter) if fps_counter else 0 + + # 显示调试信息 + if cfg.DEBUG_MODE and step_count % 100 == 0: + if vehicle_state: + print(f"[{step_count:4d}] FPS: {current_fps:5.1f} | " + f"动作: {cfg.ACTION_NAMES[action]} | " + f"位置: ({vehicle_state['x']:.1f}, {vehicle_state['y']:.1f}) | " + f"速度: {vehicle_state['speed_2d']:.1f}m/s") + + if done: + print(f"✅ Episode {episode_num} 完成，步数: {step_count}") + break + + # 限制帧率（如果需要） + if cfg.FPS_LIMIT > 0: + target_frame_time = 1.0 / cfg.FPS_LIMIT + actual_frame_time = time.time() - step_start + if actual_frame_time < target_frame_time: + time.sleep(target_frame_time - actual_frame_time) + + if step_count >= cfg.MAX_STEPS_PER_EPISODE: + print(f"⏰ Episode {episode_num} 达到最大步数限制") + + # 显示统计信息 + path_length = visualizer.calculate_path_length() + avg_fps = 1.0 / (sum(fps_counter) / len(fps_counter)) if fps_counter else 0 + print(f"📊 行驶距离: {path_length:.1f}m, 平均FPS: {avg_fps:.1f}") + + return True + +def main(): + """主函数""" + # 初始化各模块 + result = setup_environment() + if result[0] is None: + return + + env, config_mgr, traffic_mgr, visualizer, tracker, model_mgr, traj_mgr = result + + # 加载模型 + if not model_mgr.load_models(): + print("❌ 模型加载失败") + return + + print("\n🚗 开始自动驾驶...") + + # 运行多个episode + for episode in range(cfg.TOTAL_EPISODES): + success = run_episode( + env, config_mgr, traffic_mgr, visualizer, tracker, model_mgr, traj_mgr, episode + 1 + ) + + if not success: + print(f"❌ Episode {episode + 1} 运行失败") + + # 等待片刻再开始下一个episode + if episode < cfg.TOTAL_EPISODES - 1: + print(f"\n等待 {cfg.EPISODE_INTERVAL} 秒开始下一个episode...") + time.sleep(cfg.EPISODE_INTERVAL) + + # 清理 + print("\n" + "=" * 60) + print("所有episode完成！") + print("=" * 60) + + # 清理交通流 + if traffic_mgr: + traffic_mgr.cleanup() + + # 显示最终统计 + print("\n📈 最终统计:") + print(f"总episodes: {cfg.TOTAL_EPISODES}") + print(f"每episode最大步数: {cfg.MAX_STEPS_PER_EPISODE}") + print(f"交通模式: {'启用' if cfg.ENABLE_TRAFFIC else '禁用'}") + + if cfg.ENABLE_TRAFFIC and traffic_mgr: + traffic_info = traffic_mgr.get_traffic_info() + print(f"交通车辆数: {traffic_info['num_vehicles']}") + print(f"交通行人数: {traffic_info['num_walkers']}") + + print("程序结束") + +if __name__ == '__main__': + main() diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/model_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/model_manager.py new file mode 100644 index 0000000000..f40c6a731a --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/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 diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/route_visualizer.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/route_visualizer.py new file mode 100644 index 0000000000..add06ba397 --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/route_visualizer.py @@ -0,0 +1,301 @@ +""" +路线可视化模块 - 在CARLA世界中绘制路线、车辆和路径 +""" + +import carla +import math +import time + +class RouteVisualizer: + """路线可视化器""" + + def __init__(self, world): + self.world = world + self.vehicle_history = [] # 存储车辆历史位置 + + # 颜色定义 + self.route_color = carla.Color(255, 0, 0) # 红色 - 规划路线 + self.path_color = carla.Color(0, 100, 255) # 蓝色 - 历史路径 + self.vehicle_color = carla.Color(0, 255, 0) # 绿色 - 车辆 + + # 显示高度 + self.route_height = 0.3 # 路线显示高度 + self.path_height = 0.2 # 路径显示高度 + self.vehicle_height = 0.25 # 车辆显示高度 + + # 存储绘制对象 + self.route_lines = [] + self.start_marker = None + self.end_marker = None + + def draw_planned_route(self, route_points): + """绘制规划路线（常亮显示）""" + # 清除之前的路线 + 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=0.15, + 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())) + + # 保持最近500个历史点 + if len(self.vehicle_history) > 500: + self.vehicle_history = self.vehicle_history[-500:] + + # 绘制车辆当前位置和朝向 + self._draw_vehicle_current(x, y, heading) + + # 绘制历史路径 + 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=0.4, + color=self.vehicle_color, + life_time=0.5, # 稍微延长显示时间 + persistent_lines=False + ) + + # 车辆朝向箭头 + arrow_length = 2.5 + 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=0.2, + arrow_size=0.6, + color=self.vehicle_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.15, + 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 + + # 绘制最近100个点的路径 + start_idx = max(0, len(self.vehicle_history) - 100) + + 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) + + # 根据时间远近调整透明度 + time_diff = t2 - t1 + if time_diff > 0: + alpha = min(1.0, 1.0 / (1.0 + (len(self.vehicle_history) - i) * 0.1)) + else: + alpha = 0.5 + + color = carla.Color( + int(self.path_color.r * alpha), + int(self.path_color.g * alpha), + int(self.path_color.b * alpha) + ) + + self.world.debug.draw_line( + start, end, + thickness=0.1, + 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)}" + 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 + ) + + 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 diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/traffic_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/traffic_manager.py new file mode 100644 index 0000000000..583e4a9d2d --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/traffic_manager.py @@ -0,0 +1,361 @@ +""" +交通管理器 - 封装generate_traffic.py功能，生成和管理交通流 +""" + +import sys +import os +import glob +import time +import logging +from numpy import random + +# 添加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 +from carla import VehicleLightState as vls + +class TrafficManager: + """交通管理器 - 负责生成和控制交通流""" + + 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() + + self.tm_port = 8000 + self.traffic_manager = None + self.vehicles_list = [] + self.walkers_list = [] + self.all_actors = [] + self.all_id = [] + + self.is_synchronous = False + self.synchronous_master = False + + print("🚦 交通管理器初始化完成") + + def generate_traffic(self, + num_vehicles=20, + num_walkers=30, + safe_mode=True, + hybrid_mode=True, + sync_mode=False, + respawn_vehicles=False): + """ + 生成交通流 + + Args: + num_vehicles: 车辆数量 + num_walkers: 行人数量 + safe_mode: 安全模式（避免事故倾向车辆） + hybrid_mode: 混合物理模式 + sync_mode: 同步模式 + respawn_vehicles: 是否重生休眠车辆 + + Returns: + bool: 是否成功 + """ + print("\n" + "="*50) + print("生成交通流") + print("="*50) + print(f"车辆: {num_vehicles}辆") + print(f"行人: {num_walkers}个") + print(f"安全模式: {'开启' if safe_mode else '关闭'}") + print(f"混合模式: {'开启' if hybrid_mode else '关闭'}") + print(f"同步模式: {'开启' if sync_mode else '关闭'}") + + try: + # 获取交通管理器 + self.traffic_manager = self.client.get_trafficmanager(self.tm_port) + + # 配置交通管理器 + self.traffic_manager.set_global_distance_to_leading_vehicle(2.5) + + if respawn_vehicles: + self.traffic_manager.set_respawn_dormant_vehicles(True) + + if hybrid_mode: + self.traffic_manager.set_hybrid_physics_mode(True) + self.traffic_manager.set_hybrid_physics_radius(70.0) + + # 设置仿真模式 + self.is_synchronous = sync_mode + settings = self.world.get_settings() + + if sync_mode: + self.traffic_manager.set_synchronous_mode(True) + if not settings.synchronous_mode: + self.synchronous_master = True + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + else: + self.synchronous_master = False + + self.world.apply_settings(settings) + print("✅ 同步模式已启用") + + # 生成车辆 + self._spawn_vehicles(num_vehicles, safe_mode) + + # 生成行人 + self._spawn_walkers(num_walkers) + + # 配置交通管理器参数 + self.traffic_manager.global_percentage_speed_difference(30.0) + + print(f"\n✅ 交通流生成完成!") + print(f" 生成车辆: {len(self.vehicles_list)}辆") + print(f" 生成行人: {len(self.walkers_list)}个") + + return True + + except Exception as e: + print(f"❌ 生成交通流失败: {e}") + return False + + def _spawn_vehicles(self, num_vehicles, safe_mode): + """生成车辆""" + print("🚗 生成车辆...") + + # 获取车辆蓝图 + blueprints = self.world.get_blueprint_library().filter('vehicle.*') + + if safe_mode: + # 过滤掉不安全或特殊车辆 + blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4] + blueprints = [x for x in blueprints if not x.id.endswith('microlino')] + blueprints = [x for x in blueprints if not x.id.endswith('carlacola')] + blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')] + blueprints = [x for x in blueprints if not x.id.endswith('t2')] + blueprints = [x for x in blueprints if not x.id.endswith('sprinter')] + blueprints = [x for x in blueprints if not x.id.endswith('firetruck')] + blueprints = [x for x in blueprints if not x.id.endswith('ambulance')] + + blueprints = sorted(blueprints, key=lambda bp: bp.id) + + # 获取生成点 + spawn_points = self.world.get_map().get_spawn_points() + random.shuffle(spawn_points) + + if num_vehicles > len(spawn_points): + print(f"⚠️ 请求的车辆数({num_vehicles})超过生成点数({len(spawn_points)})") + num_vehicles = len(spawn_points) + + # 批量生成车辆 + batch = [] + for n, transform in enumerate(spawn_points): + if n >= num_vehicles: + break + + blueprint = random.choice(blueprints) + + # 设置随机颜色 + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color').recommended_values) + blueprint.set_attribute('color', color) + + # 设置驾驶员ID + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + + blueprint.set_attribute('role_name', 'autopilot') + + # 添加到批量命令 + batch.append(carla.command.SpawnActor(blueprint, transform) + .then(carla.command.SetAutopilot( + carla.command.FutureActor, + True, + self.traffic_manager.get_port()))) + + # 执行批量命令 + for response in self.client.apply_batch_sync(batch, self.synchronous_master): + if response.error: + logging.error(f"生成车辆失败: {response.error}") + else: + self.vehicles_list.append(response.actor_id) + + print(f"✅ 生成 {len(self.vehicles_list)} 辆车辆") + + def _spawn_walkers(self, num_walkers): + """生成行人""" + print("🚶 生成行人...") + + if num_walkers <= 0: + return + + # 获取行人蓝图 + walker_bps = self.world.get_blueprint_library().filter('walker.pedestrian.*') + + # 获取随机位置 + spawn_points = [] + for i in range(num_walkers): + spawn_point = carla.Transform() + loc = self.world.get_random_location_from_navigation() + if loc: + spawn_point.location = loc + spawn_points.append(spawn_point) + + # 生成行人 + batch = [] + walker_speeds = [] + for spawn_point in spawn_points: + walker_bp = random.choice(walker_bps) + + # 设置为非无敌 + if walker_bp.has_attribute('is_invincible'): + walker_bp.set_attribute('is_invincible', 'false') + + # 设置速度 + speed = 0.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, spawn_point)) + + # 执行批量命令 + results = self.client.apply_batch_sync(batch, True) + + for i in range(len(results)): + if results[i].error: + logging.error(f"生成行人失败: {results[i].error}") + else: + self.walkers_list.append({"id": results[i].actor_id}) + + # 生成行人控制器 + batch = [] + walker_controller_bp = self.world.get_blueprint_library().find('controller.ai.walker') + + for i in range(len(self.walkers_list)): + batch.append(carla.command.SpawnActor( + walker_controller_bp, + carla.Transform(), + self.walkers_list[i]["id"])) + + results = self.client.apply_batch_sync(batch, True) + + for i in range(len(results)): + if results[i].error: + logging.error(f"生成行人控制器失败: {results[i].error}") + else: + self.walkers_list[i]["con"] = results[i].actor_id + self.all_id.append(results[i].actor_id) + self.all_id.append(self.walkers_list[i]["id"]) + + # 获取所有行人actor + all_actors = self.world.get_actors(self.all_id) + + # 初始化控制器 + for i in range(0, len(self.all_id), 2): + # 启动控制器 + all_actors[i].start() + # 设置随机目标 + all_actors[i].go_to_location(self.world.get_random_location_from_navigation()) + # 设置最大速度 + all_actors[i].set_max_speed(float(walker_speeds[int(i/2)])) + + print(f"✅ 生成 {len(self.walkers_list)} 个行人") + + def update(self): + """更新交通管理器（用于同步模式）""" + if self.is_synchronous and self.synchronous_master: + self.world.tick() + elif self.is_synchronous: + self.world.wait_for_tick() + + def set_vehicle_lights(self, enabled=True): + """设置车辆灯光""" + if not self.vehicles_list: + return + + try: + all_vehicle_actors = self.world.get_actors(self.vehicles_list) + for actor in all_vehicle_actors: + self.traffic_manager.update_vehicle_lights(actor, enabled) + + print(f"✅ 车辆灯光 {'开启' if enabled else '关闭'}") + except Exception as e: + print(f"⚠️ 设置车辆灯光失败: {e}") + + def set_global_speed_limit(self, percentage=30.0): + """设置全局速度限制百分比""" + if self.traffic_manager: + self.traffic_manager.global_percentage_speed_difference(percentage) + print(f"✅ 设置全局速度限制: {percentage}%") + + def cleanup(self): + """清理所有生成的交通""" + print("\n🧹 清理交通流...") + + try: + # 停止同步模式 + if self.is_synchronous and self.synchronous_master: + settings = self.world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + self.world.apply_settings(settings) + + # 销毁车辆 + if self.vehicles_list: + print(f"销毁 {len(self.vehicles_list)} 辆车辆...") + self.client.apply_batch([ + carla.command.DestroyActor(x) for x in self.vehicles_list + ]) + + # 销毁行人 + if self.all_id: + print(f"销毁 {len(self.walkers_list)} 个行人...") + + # 先停止控制器 + all_actors = self.world.get_actors(self.all_id) + for i in range(0, len(self.all_id), 2): + all_actors[i].stop() + + # 销毁所有actor + self.client.apply_batch([ + carla.command.DestroyActor(x) for x in self.all_id + ]) + + # 清空列表 + self.vehicles_list = [] + self.walkers_list = [] + self.all_id = [] + + print("✅ 交通流清理完成") + + except Exception as e: + print(f"❌ 清理交通流失败: {e}") + + def get_traffic_info(self): + """获取交通信息""" + return { + 'num_vehicles': len(self.vehicles_list), + 'num_walkers': len(self.walkers_list), + 'is_synchronous': self.is_synchronous, + 'tm_port': self.tm_port + } + + def __del__(self): + """析构函数，确保清理""" + if self.vehicles_list or self.walkers_list: + self.cleanup() diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/trajectory_manager.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/trajectory_manager.py new file mode 100644 index 0000000000..84c53162c7 --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/trajectory_manager.py @@ -0,0 +1,107 @@ +""" +轨迹管理器 - 管理规划路线和轨迹点 +""" + +import carla +import math +import config as cfg + +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 + } diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/vehicle_tracker.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/vehicle_tracker.py new file mode 100644 index 0000000000..d76c20a489 --- /dev/null +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/vehicle_tracker.py @@ -0,0 +1,401 @@ +""" +车辆跟踪和视角控制模块 - 平滑跟随车辆，获取车辆状态 +""" + +import carla +import math +import time +import numpy as np +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.target_height = cfg.TOP_DOWN_HEIGHT + self.pitch_angle = cfg.TOP_DOWN_PITCH + + print(f"📐 车辆跟踪器初始化完成，平滑系数: {self.smooth_factor}") + + def set_top_down_view(self, vehicle, height=None): + """设置俯视视角""" + if vehicle is None: + return False + + try: + if height is not None: + self.target_height = height + + transform = vehicle.get_transform() + location = transform.location + + # 设置相机在车辆正上方 + camera_location = carla.Location( + x=location.x, + y=location.y, + z=location.z + self.target_height + ) + + # 设置俯视角度 + camera_rotation = carla.Rotation( + pitch=self.pitch_angle, + yaw=transform.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 + + print(f"📐 设置俯视视角，高度: {self.target_height}m") + return True + + except Exception as e: + print(f"❌ 设置俯视视角失败: {e}") + return False + + def smooth_follow_vehicle(self, vehicle, height=None): + """平滑跟随车辆（每帧调用）- 改进版本""" + if vehicle is None: + return False + + try: + current_time = time.time() + time_delta = current_time - self.last_update_time + + # 限制最小时间间隔，避免计算过于频繁 + if time_delta < 0.001: + return True + + self.last_update_time = current_time + self.frame_count += 1 + + # 获取车辆当前状态 + vehicle_transform = vehicle.get_transform() + vehicle_location = vehicle_transform.location + vehicle_rotation = vehicle_transform.rotation + + # 获取车辆速度用于预测 + vehicle_velocity = 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) + + if height is not None: + self.target_height = height + + # 计算目标相机位置（车辆正上方） + target_location = carla.Location( + x=vehicle_location.x, + y=vehicle_location.y, + z=vehicle_location.z + self.target_height + ) + + # 目标相机旋转（保持俯视，yaw跟随车辆） + target_rotation = carla.Rotation( + pitch=self.pitch_angle, + 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_target_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 + + # 每100帧输出一次调试信息 + if cfg.DEBUG_MODE and self.frame_count % 100 == 0: + print(f"[视角] 帧: {self.frame_count}, " + f"平滑系数: {effective_smooth_factor:.3f}, " + f"速度: {speed:.2f}m/s, " + f"时差: {time_delta:.3f}s") + + return True + + except Exception as e: + if cfg.DEBUG_MODE: + print(f"⚠️ 视角更新失败: {e}") + return False + + 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 _predict_target_position(self, target_loc, vehicle_rot, speed, time_delta): + """预测目标位置""" + if speed < 0.1: # 速度太慢不预测 + return target_loc + + # 预测未来0.2秒的位置 + prediction_time = 0.2 + angle_rad = math.radians(vehicle_rot.yaw) + + predicted_x = target_loc.x + speed * math.cos(angle_rad) * prediction_time + predicted_y = target_loc.y + speed * math.sin(angle_rad) * prediction_time + + return carla.Location( + x=predicted_x, + y=predicted_y, + z=target_loc.z + ) + + 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_smooth_factor(self, factor): + """更新平滑系数""" + if 0 < factor <= 1: + self.smooth_factor = factor + print(f"🔄 平滑系数更新为: {factor}") + else: + print(f"❌ 无效的平滑系数: {factor}，保持为: {self.smooth_factor}") + + def reset(self): + """重置跟踪器状态""" + self.velocity_history = [] + self.frame_count = 0 + self.last_update_time = time.time() + print("🔄 车辆跟踪器已重置")