From c5f376743c3f47098cf93ad26ca859792c9b5b13 Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Mon, 15 Sep 2025 08:20:59 +0800 Subject: [PATCH 01/16] =?UTF-8?q?=E4=BA=BA=E8=BD=A6=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=E7=B3=BB=E7=BB=9F=E6=A8=A1=E5=9D=97=E5=BC=80=E5=8F=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/manual_control_manayume/README.md | 1 + 1 file changed, 1 insertion(+) create mode 100644 src/manual_control_manayume/README.md diff --git a/src/manual_control_manayume/README.md b/src/manual_control_manayume/README.md new file mode 100644 index 0000000000..c97fde8982 --- /dev/null +++ b/src/manual_control_manayume/README.md @@ -0,0 +1 @@ +这是manayume的人车控制模块开发 \ No newline at end of file From 6210960a47bd125a6faede843f2cb5a45cd3879b Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Mon, 15 Sep 2025 11:04:54 +0800 Subject: [PATCH 02/16] =?UTF-8?q?=E6=99=BA=E8=83=BD=E9=A9=BE=E9=A9=B6?= =?UTF-8?q?=E7=B3=BB=E7=BB=9F=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/manual_ISS/README.md | 1 + 1 file changed, 1 insertion(+) create mode 100644 src/manual_ISS/README.md diff --git a/src/manual_ISS/README.md b/src/manual_ISS/README.md new file mode 100644 index 0000000000..17776d5443 --- /dev/null +++ b/src/manual_ISS/README.md @@ -0,0 +1 @@ +这是manayume的智能驾驶系统模块开发 \ No newline at end of file From 742f3739b679238f4b9d6550ecee01b8a5fe842e Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Mon, 15 Sep 2025 11:17:46 +0800 Subject: [PATCH 03/16] =?UTF-8?q?=E5=88=A0=E9=99=A4=E4=BA=86=E5=A4=9A?= =?UTF-8?q?=E4=BD=99=E7=9A=84=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/manual_control_manayume/README.md | 1 - 1 file changed, 1 deletion(-) delete mode 100644 src/manual_control_manayume/README.md diff --git a/src/manual_control_manayume/README.md b/src/manual_control_manayume/README.md deleted file mode 100644 index c97fde8982..0000000000 --- a/src/manual_control_manayume/README.md +++ /dev/null @@ -1 +0,0 @@ -这是manayume的人车控制模块开发 \ No newline at end of file From a3a45903ba06b40e876004eae8556084a374be8a Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Tue, 14 Oct 2025 11:01:51 +0800 Subject: [PATCH 04/16] =?UTF-8?q?=E6=9B=B4=E6=94=B9=E4=BA=86=E6=89=80?= =?UTF-8?q?=E9=80=89=E7=9A=84=E9=A1=B9=E7=9B=AE=EF=BC=8C=E5=88=A0=E9=99=A4?= =?UTF-8?q?=E4=BA=86manual=5FISS=E6=96=87=E4=BB=B6=E5=A4=B9=EF=BC=8C?= =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86Autonomous-Vehicle-Navigation-Using-?= =?UTF-8?q?Deep-Learning-master=E6=96=87=E4=BB=B6=E5=B9=B6=E8=BF=90?= =?UTF-8?q?=E8=A1=8C=E4=BA=86=E9=A1=B9=E7=9B=AE=E6=96=87=E4=BB=B6=EF=BC=8C?= =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E6=96=87=E4=BB=B6=E4=B8=BA=E8=BF=90=E8=A1=8C?= =?UTF-8?q?=E6=88=90=E5=8A=9F=E7=9A=84=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 17 + .../config.py | 327 +++++++++++++++ .../generate_traffic.py | 383 ++++++++++++++++++ .../pedestrians_1.py | 149 +++++++ .../pedestrians_2.py | 152 +++++++ .../requirements.txt | 100 +++++ 6 files changed, 1128 insertions(+) create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/README.md create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/config.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/generate_traffic.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_1.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_2.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/requirements.txt diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/README.md b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/README.md new file mode 100644 index 0000000000..f60838bd84 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/README.md @@ -0,0 +1,17 @@ +这是manayume的使用深度学习的自动驾驶汽车导航项目测试 +参考项目：https://github.com/varunpratap222/Autonomous-Vehicle-Navigation-Using-Deep-Learning.git +测试环境：使用ubuntu20.04版本，conda虚拟环境，python3.7版本，安装的软件包按照requirements.txt文件安装，使用Carla0.9.13版本进行模拟 + + +启动流程 +## Run +1. Run Carla Server using: `./CarlaUE4.sh` +2. Run `config.py` file to load Town02 +3. Generate traffic either using `generate_traffic.py` or spawn pedestrians at random location along Trajectory 1 and 2 using the `pedestrians_1.py` and `pedestrians_2.py` script. Change the number of vehicles and pedestrians to spawn using `generate_traffic.py` by passing the corresponding arguments. +4. Select an existing trajectory (Trajectory 1, Trajectory 2, Trajectory 3, Trajectory 4) or set custom trajectory using the format given in `test_everything.py` arguments. +5. To find the initial and final locations of the custom trajectory, make use of `get_location.py` file and navigate the map using W,A,S,D, E, and Q keys. +6. Enter locations or select existing trajectories and run the `test_everything.py` file. + + + + diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/config.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/config.py new file mode 100644 index 0000000000..5740c0f6fd --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/config.py @@ -0,0 +1,327 @@ +#!/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' + 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) diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/generate_traffic.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/generate_traffic.py new file mode 100644 index 0000000000..e3717baf40 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/generate_traffic.py @@ -0,0 +1,383 @@ +#!/usr/bin/env python + +# Copyright (c) 2021 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 . + +"""Example script to generate traffic in the simulation""" + +import glob +import os +import sys +import time + +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 + +import argparse +import logging +from numpy import random + +def get_actor_blueprints(world, filter, generation): + bps = world.get_blueprint_library().filter(filter) + + if generation.lower() == "all": + return bps + + # If the filter returns only one bp, we assume that this one needed + # and therefore, we ignore the generation + if len(bps) == 1: + return bps + + try: + int_generation = int(generation) + # Check if generation is in available generations + if int_generation in [1, 2]: + bps = [x for x in bps if int(x.get_attribute('generation')) == int_generation] + return bps + else: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + except: + print(" Warning! Actor Generation is not valid. No actor will be spawned.") + return [] + +def main(): + argparser = argparse.ArgumentParser( + description=__doc__) + argparser.add_argument( + '--host', + metavar='H', + default='127.0.0.1', + help='IP of the host server (default: 127.0.0.1)') + argparser.add_argument( + '-p', '--port', + metavar='P', + default=2000, + type=int, + help='TCP port to listen to (default: 2000)') + argparser.add_argument( + '-n', '--number-of-vehicles', + metavar='N', + default=35, + type=int, + help='Number of vehicles (default: 30)') + argparser.add_argument( + '-w', '--number-of-walkers', + metavar='W', + default=80, + type=int, + help='Number of walkers (default: 10)') + argparser.add_argument( + '--safe', + action='store_true', + help='Avoid spawning vehicles prone to accidents') + argparser.add_argument( + '--filterv', + metavar='PATTERN', + default='vehicle.*', + help='Filter vehicle model (default: "vehicle.*")') + argparser.add_argument( + '--generationv', + metavar='G', + default='1', + help='restrict to certain vehicle generation (values: "1","2","All" - default: "All")') + argparser.add_argument( + '--filterw', + metavar='PATTERN', + default='walker.pedestrian.*', + help='Filter pedestrian type (default: "walker.pedestrian.*")') + argparser.add_argument( + '--generationw', + metavar='G', + default='2', + help='restrict to certain pedestrian generation (values: "1","2","All" - default: "2")') + argparser.add_argument( + '--tm-port', + metavar='P', + default=8000, + type=int, + help='Port to communicate with TM (default: 8000)') + argparser.add_argument( + '--asynch', + action='store_true', + help='Activate asynchronous mode execution') + argparser.add_argument( + '--hybrid', + action='store_true', + help='Activate hybrid mode for Traffic Manager') + argparser.add_argument( + '-s', '--seed', + metavar='S', + type=int, + help='Set random device seed and deterministic mode for Traffic Manager') + argparser.add_argument( + '--seedw', + metavar='S', + default=0, + type=int, + help='Set the seed for pedestrians module') + argparser.add_argument( + '--car-lights-on', + action='store_true', + default=False, + help='Enable automatic car light management') + argparser.add_argument( + '--hero', + action='store_true', + default=False, + help='Set one of the vehicles as hero') + argparser.add_argument( + '--respawn', + action='store_true', + default=False, + help='Automatically respawn dormant vehicles (only in large maps)') + argparser.add_argument( + '--no-rendering', + action='store_true', + default=False, + help='Activate no rendering mode') + + args = argparser.parse_args() + + logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO) + + vehicles_list = [] + walkers_list = [] + all_id = [] + client = carla.Client(args.host, args.port) + client.set_timeout(10.0) + synchronous_master = False + random.seed(args.seed if args.seed is not None else int(time.time())) + + try: + world = client.get_world() + + traffic_manager = client.get_trafficmanager(args.tm_port) + traffic_manager.set_global_distance_to_leading_vehicle(2.5) + if args.respawn: + traffic_manager.set_respawn_dormant_vehicles(True) + if args.hybrid: + traffic_manager.set_hybrid_physics_mode(True) + traffic_manager.set_hybrid_physics_radius(70.0) + if args.seed is not None: + traffic_manager.set_random_device_seed(args.seed) + + settings = world.get_settings() + if not args.asynch: + traffic_manager.set_synchronous_mode(True) + if not settings.synchronous_mode: + synchronous_master = True + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + else: + synchronous_master = False + else: + print("You are currently in asynchronous mode. If this is a traffic simulation, \ + you could experience some issues. If it's not working correctly, switch to synchronous \ + mode by using traffic_manager.set_synchronous_mode(True)") + + if args.no_rendering: + settings.no_rendering_mode = True + world.apply_settings(settings) + + blueprints = get_actor_blueprints(world, args.filterv, args.generationv) + blueprintsWalkers = get_actor_blueprints(world, args.filterw, args.generationw) + + + + if args.safe: + 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 = world.get_map().get_spawn_points() + number_of_spawn_points = len(spawn_points) + + if args.number_of_vehicles < number_of_spawn_points: + random.shuffle(spawn_points) + elif args.number_of_vehicles > number_of_spawn_points: + msg = 'requested %d vehicles, but could only find %d spawn points' + logging.warning(msg, args.number_of_vehicles, number_of_spawn_points) + args.number_of_vehicles = number_of_spawn_points + + # @todo cannot import these directly. + SpawnActor = carla.command.SpawnActor + SetAutopilot = carla.command.SetAutopilot + FutureActor = carla.command.FutureActor + + # -------------- + # Spawn vehicles + # -------------- + batch = [] + hero = args.hero + for n, transform in enumerate(spawn_points): + if n >= args.number_of_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) + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id').recommended_values) + blueprint.set_attribute('driver_id', driver_id) + if hero: + blueprint.set_attribute('role_name', 'hero') + hero = False + else: + blueprint.set_attribute('role_name', 'autopilot') + + # spawn the cars and set their autopilot and light state all together + batch.append(SpawnActor(blueprint, transform) + .then(SetAutopilot(FutureActor, True, traffic_manager.get_port()))) + + for response in client.apply_batch_sync(batch, synchronous_master): + if response.error: + logging.error(response.error) + else: + vehicles_list.append(response.actor_id) + + # Set automatic vehicle lights update if specified + if args.car_lights_on: + all_vehicle_actors = world.get_actors(vehicles_list) + for actor in all_vehicle_actors: + traffic_manager.update_vehicle_lights(actor, True) + + # ------------- + # Spawn Walkers + # ------------- + # some settings + percentagePedestriansRunning = 0.0 # how many pedestrians will run + percentagePedestriansCrossing = 0.0 # how many pedestrians will walk through the road + if args.seedw: + world.set_pedestrians_seed(args.seedw) + random.seed(args.seedw) + # 1. take all the random locations to spawn + spawn_points = [] + for i in range(args.number_of_walkers): + spawn_point = carla.Transform() + loc = world.get_random_location_from_navigation() + if (loc != None): + spawn_point.location = loc + spawn_points.append(spawn_point) + # 2. we spawn the walker object + batch = [] + walker_speed = [] + for spawn_point in spawn_points: + walker_bp = random.choice(blueprintsWalkers) + # set as not invincible + if walker_bp.has_attribute('is_invincible'): + walker_bp.set_attribute('is_invincible', 'false') + # set the max speed + if walker_bp.has_attribute('speed'): + if (random.random() > percentagePedestriansRunning): + # walking + walker_speed.append(walker_bp.get_attribute('speed').recommended_values[1]) + else: + # running + walker_speed.append(walker_bp.get_attribute('speed').recommended_values[2]) + else: + print("Walker has no speed") + walker_speed.append(0.0) + batch.append(SpawnActor(walker_bp, spawn_point)) + results = client.apply_batch_sync(batch, True) + walker_speed2 = [] + for i in range(len(results)): + if results[i].error: + logging.error(results[i].error) + else: + walkers_list.append({"id": results[i].actor_id}) + walker_speed2.append(walker_speed[i]) + walker_speed = walker_speed2 + # 3. we spawn the walker controller + batch = [] + walker_controller_bp = world.get_blueprint_library().find('controller.ai.walker') + for i in range(len(walkers_list)): + batch.append(SpawnActor(walker_controller_bp, carla.Transform(), walkers_list[i]["id"])) + results = client.apply_batch_sync(batch, True) + for i in range(len(results)): + if results[i].error: + logging.error(results[i].error) + else: + walkers_list[i]["con"] = results[i].actor_id + # 4. we put together the walkers and controllers id to get the objects from their id + for i in range(len(walkers_list)): + all_id.append(walkers_list[i]["con"]) + all_id.append(walkers_list[i]["id"]) + all_actors = world.get_actors(all_id) + + # wait for a tick to ensure client receives the last transform of the walkers we have just created + if args.asynch or not synchronous_master: + world.wait_for_tick() + else: + world.tick() + + # 5. initialize each controller and set target to walk to (list is [controler, actor, controller, actor ...]) + # set how many pedestrians can cross the road + world.set_pedestrians_cross_factor(percentagePedestriansCrossing) + for i in range(0, len(all_id), 2): + # start walker + all_actors[i].start() + # set walk to random point + all_actors[i].go_to_location(world.get_random_location_from_navigation()) + # max speed + all_actors[i].set_max_speed(float(walker_speed[int(i/2)])) + + print('spawned %d vehicles and %d walkers, press Ctrl+C to exit.' % (len(vehicles_list), len(walkers_list))) + + # Example of how to use Traffic Manager parameters + traffic_manager.global_percentage_speed_difference(30.0) + + while True: + if not args.asynch and synchronous_master: + world.tick() + else: + world.wait_for_tick() + + finally: + + if not args.asynch and synchronous_master: + settings = world.get_settings() + settings.synchronous_mode = False + settings.no_rendering_mode = False + settings.fixed_delta_seconds = None + world.apply_settings(settings) + + print('\ndestroying %d vehicles' % len(vehicles_list)) + client.apply_batch([carla.command.DestroyActor(x) for x in vehicles_list]) + + # stop walker controllers (list is [controller, actor, controller, actor ...]) + for i in range(0, len(all_id), 2): + all_actors[i].stop() + + print('\ndestroying %d walkers' % len(walkers_list)) + client.apply_batch([carla.command.DestroyActor(x) for x in all_id]) + + time.sleep(0.5) + +if __name__ == '__main__': + + try: + main() + except KeyboardInterrupt: + pass + finally: + print('\ndone.') diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_1.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_1.py new file mode 100644 index 0000000000..ce441044a8 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_1.py @@ -0,0 +1,149 @@ +import carla +import random +import time +import sys + + + + +print('Inside pedestrians........') + +client = carla.Client('localhost', 2000) # connect to the CARLA server +client.set_timeout(10.0) # set a timeout for connecting to the server + +world = client.get_world() # get the current world + +spawn_point = carla.Transform(carla.Location(x=112.2033, y=310.975, z=5), carla.Rotation(yaw=180)) + + +''' +to spawn the locations for pedestrians to walk across the road +''' +pedestrians_crossing = [] + + +p1 = carla.Transform(carla.Location(x=100, y=310.975, z=5), carla.Rotation(yaw=180)) +p2 = carla.Transform(carla.Location(x=120, y=310.975, z=5), carla.Rotation(yaw=180)) +p3 = carla.Transform(carla.Location(x=142, y=310.975, z=5), carla.Rotation(yaw=180)) +#p4 = carla.Transform(carla.Location(x=162, y=310.975, z=5), carla.Rotation(yaw=180)) + +pedestrians_crossing.append(p1) +pedestrians_crossing.append(p2) +pedestrians_crossing.append(p3) +#pedestrians_crossing.append(p4) + + + +''' +locations for pedestrians to walk in the opposite direction of car on the sidewalk +''' +pedestrians_walking_oppo = [] + +p1 = carla.Transform(carla.Location(x=105, y=311.975, z=5), carla.Rotation(yaw=180)) +p2 = carla.Transform(carla.Location(x=125, y=311.975, z=5), carla.Rotation(yaw=180)) +p3 = carla.Transform(carla.Location(x=145, y=311.975, z=5), carla.Rotation(yaw=180)) +#p4 = carla.Transform(carla.Location(x=165, y=311.975, z=5), carla.Rotation(yaw=180)) + +pedestrians_walking_oppo.append(p1) +pedestrians_walking_oppo.append(p2) +pedestrians_walking_oppo.append(p3) +#pedestrians_walking_oppo.append(p4) + + + +''' +locations for pedestrains to walk in the direction of car on the sidewalk +''' +pedestrians_walking_away = [] + +p1 = carla.Transform(carla.Location(x=95, y=310, z=5), carla.Rotation(yaw=180)) +p2 = carla.Transform(carla.Location(x=115, y=310, z=5), carla.Rotation(yaw=180)) +p3 = carla.Transform(carla.Location(x=135, y=310, z=5), carla.Rotation(yaw=180)) +#p4 = carla.Transform(carla.Location(x=155, y=310, z=5), carla.Rotation(yaw=180)) + +pedestrians_walking_away.append(p1) +pedestrians_walking_away.append(p2) +pedestrians_walking_away.append(p3) +#pedestrians_walking_away.append(p4) + + + + +print("Have taken the spawn point....") + +#time.sleep(5) + + +for cross, opposite, away in zip(pedestrians_crossing, pedestrians_walking_oppo, pedestrians_walking_away): + + #spawn_point = random.choice(spawn_points) # select a random spawn point + + # select a random pedestrian blueprint + pedestrian_bp = random.choice(world.get_blueprint_library().filter("walker.pedestrian.*")) + + + # spawn the pedestrian at the selected spawn point + pedestrian_cross = world.try_spawn_actor(pedestrian_bp, cross) + pedestrian_opposite = world.try_spawn_actor(pedestrian_bp, opposite) + pedestrian_away = world.try_spawn_actor(pedestrian_bp, away) + + # actor_list.append(pedestrian_cross) + # actor_list.append(pedestrian_opposite) + # actor_list.append(pedestrians_walking_away) + + + ''' + TO make the pedestrians move in a specified direction depending on their type + ''' + # to cross + if pedestrian_cross is not None: + walker_control = carla.WalkerControl() + walker_control.speed = 0.5 + #walker_control.direction = carla.Vector3D(x=random.uniform(-1, 1), y=random.uniform(-1, 1), z=0) + walker_control.direction = carla.Vector3D(x=0, y=-1, z=0) + pedestrian_cross.apply_control(walker_control) + print("Spawned crossing pedestrian.....") + + + # in opposite direction + if pedestrian_opposite is not None: + walker_control = carla.WalkerControl() + walker_control.speed = 0.5 + #walker_control.direction = carla.Vector3D(x=random.uniform(-1, 1), y=random.uniform(-1, 1), z=0) + walker_control.direction = carla.Vector3D(x=-1, y=0, z=0) + pedestrian_opposite.apply_control(walker_control) + print("Spawned pedestrian moving in opposite direction......") + + + # in car's direction (away from the car) + if pedestrian_away is not None: + walker_control = carla.WalkerControl() + walker_control.speed = 0.5 + #walker_control.direction = carla.Vector3D(x=random.uniform(-1, 1), y=random.uniform(-1, 1), z=0) + walker_control.direction = carla.Vector3D(x=1, y=0, z=0) + pedestrian_away.apply_control(walker_control) + print("Spawned pedestrian moving away......") + + + + time.sleep(8) + + + +print("End of episode.....") +print("Destroying all the pedestrians.....") + +# time.sleep(5) + + +actor_list = world.get_actors() +# for actor in actor_list_new: +# if 'walker.pedestrian' in actor.type_id: +# actor.destroy() +# #print("PEDESTRIAN DESTROYED!!!!!") + + + + + + diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_2.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_2.py new file mode 100644 index 0000000000..ebc65380e5 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_2.py @@ -0,0 +1,152 @@ +import carla +import random +import time +import sys + + + + +print('Inside pedestrians........') + +client = carla.Client('localhost', 2000) # connect to the CARLA server +client.set_timeout(10.0) # set a timeout for connecting to the server + +world = client.get_world() # get the current world + +spawn_point = carla.Transform(carla.Location(x=112.2033, y=310.975, z=5), carla.Rotation(yaw=180)) + + +''' +to spawn the locations for pedestrians to walk across the road +''' +pedestrians_crossing = [] + + +p1 = carla.Transform(carla.Location(x=-12.538, y=262, z=5), carla.Rotation(yaw=180)) +p2 = carla.Transform(carla.Location(x=-11.3415, y=268, z=5), carla.Rotation(yaw=180)) +p3 = carla.Transform(carla.Location(x=-11.34, y=265, z=5), carla.Rotation(yaw=180)) +#p4 = carla.Transform(carla.Location(x=162, y=310.975, z=5), carla.Rotation(yaw=180)) + +pedestrians_crossing.append(p1) +pedestrians_crossing.append(p2) +pedestrians_crossing.append(p3) +#pedestrians_crossing.append(p4) + + + +''' +locations for pedestrians to walk in the opposite direction of car on the sidewalk +''' +pedestrians_walking_oppo = [] + +p1 = carla.Transform(carla.Location(x=-12, y=250.975, z=5), carla.Rotation(yaw=180)) +p2 = carla.Transform(carla.Location(x=-11, y=252.975, z=5), carla.Rotation(yaw=180)) +p3 = carla.Transform(carla.Location(x=-11, y=253.975, z=5), carla.Rotation(yaw=180)) +#p4 = carla.Transform(carla.Location(x=165, y=311.975, z=5), carla.Rotation(yaw=180)) + +pedestrians_walking_oppo.append(p1) +pedestrians_walking_oppo.append(p2) +pedestrians_walking_oppo.append(p3) +#pedestrians_walking_oppo.append(p4) + + + +''' +locations for pedestrains to walk in the direction of car on the sidewalk +''' +pedestrians_walking_away = [] + +p1 = carla.Transform(carla.Location(x=-12, y=265, z=5), carla.Rotation(yaw=180)) +p2 = carla.Transform(carla.Location(x=-11, y=267, z=5), carla.Rotation(yaw=180)) +p3 = carla.Transform(carla.Location(x=-11, y=250, z=5), carla.Rotation(yaw=180)) +#p4 = carla.Transform(carla.Location(x=155, y=310, z=5), carla.Rotation(yaw=180)) + +pedestrians_walking_away.append(p1) +pedestrians_walking_away.append(p2) +pedestrians_walking_away.append(p3) +#pedestrians_walking_away.append(p4) + + + + +print("Have taken the spawn point....") + +#time.sleep(5) + + +for cross, opposite, away in zip(pedestrians_crossing, pedestrians_walking_oppo, pedestrians_walking_away): + + i = 1 + print('Epoch number 1: ', i) + #spawn_point = random.choice(spawn_points) # select a random spawn point + + # select a random pedestrian blueprint + pedestrian_bp = random.choice(world.get_blueprint_library().filter("walker.pedestrian.*")) + + + # spawn the pedestrian at the selected spawn point + pedestrian_cross = world.try_spawn_actor(pedestrian_bp, cross) + pedestrian_opposite = world.try_spawn_actor(pedestrian_bp, opposite) + pedestrian_away = world.try_spawn_actor(pedestrian_bp, away) + + # actor_list.append(pedestrian_cross) + # actor_list.append(pedestrian_opposite) + # actor_list.append(pedestrians_walking_away) + + + ''' + TO make the pedestrians move in a specified direction depending on their type + ''' + # to cross + if pedestrian_cross is not None: + walker_control = carla.WalkerControl() + walker_control.speed = 0.5 + #walker_control.direction = carla.Vector3D(x=random.uniform(-1, 1), y=random.uniform(-1, 1), z=0) + walker_control.direction = carla.Vector3D(x=1, y=0, z=0) + pedestrian_cross.apply_control(walker_control) + print("Spawned crossing pedestrian.....") + + + # in opposite direction + if pedestrian_opposite is not None: + walker_control = carla.WalkerControl() + walker_control.speed = 0.5 + #walker_control.direction = carla.Vector3D(x=random.uniform(-1, 1), y=random.uniform(-1, 1), z=0) + walker_control.direction = carla.Vector3D(x=0, y=1, z=0) + pedestrian_opposite.apply_control(walker_control) + print("Spawned pedestrian moving in opposite direction......") + + + # in car's direction (away from the car) + if pedestrian_away is not None: + walker_control = carla.WalkerControl() + walker_control.speed = 0.5 + #walker_control.direction = carla.Vector3D(x=random.uniform(-1, 1), y=random.uniform(-1, 1), z=0) + walker_control.direction = carla.Vector3D(x=0, y=-1, z=0) + pedestrian_away.apply_control(walker_control) + print("Spawned pedestrian moving away......") + + + + time.sleep(1) + i = i + 1 + + + +# print("End of episode.....") +# print("Destroying all the pedestrians.....") + +# time.sleep(5) + + +# actor_list = world.get_actors() +# for actor in actor_list_new: +# if 'walker.pedestrian' in actor.type_id: +# actor.destroy() +# #print("PEDESTRIAN DESTROYED!!!!!") + + + + + + diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/requirements.txt b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/requirements.txt new file mode 100644 index 0000000000..a41f3a4bbe --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/requirements.txt @@ -0,0 +1,100 @@ +absl-py==1.4.0 +addict==2.4.0 +astor==0.8.1 +attrs==22.2.0 +backcall==0.2.0 +carla==0.9.13 +certifi @ file:///croot/certifi_1671487769961/work/certifi +click==8.1.3 +ConfigArgParse==1.5.3 +cycler==0.11.0 +dash==2.8.1 +dash-core-components==2.0.0 +dash-html-components==2.0.0 +dash-table==5.0.0 +debugpy==1.6.6 +decorator==5.1.1 +entrypoints==0.4 +exceptiongroup==1.1.0 +fastjsonschema==2.16.2 +Flask==2.2.3 +fonttools==4.38.0 +future==0.18.3 +gast==0.2.2 +google-pasta==0.2.0 +graphviz==0.20.1 +grpcio==1.51.1 +h5py==2.10.0 +importlib-metadata==6.0.0 +importlib-resources==5.12.0 +iniconfig==2.0.0 +ipykernel==6.16.2 +ipython==7.34.0 +ipywidgets==8.0.4 +itsdangerous==2.1.2 +jedi==0.18.2 +Jinja2==3.1.2 +joblib==1.2.0 +jsonschema==4.17.3 +jupyter_client==7.4.9 +jupyter_core==4.12.0 +jupyterlab-widgets==3.0.5 +Keras==2.1.6 +Keras-Applications==1.0.8 +Keras-Preprocessing==1.1.2 +kiwisolver==1.4.4 +Markdown==3.4.1 +MarkupSafe==2.1.2 +matplotlib==3.5.3 +matplotlib-inline==0.1.6 +nbformat==5.5.0 +nest-asyncio==1.5.6 +networkx==2.6.3 +numpy==1.18.4 +open3d==0.16.0 +opencv-python==4.7.0.68 +opt-einsum==3.3.0 +packaging==23.0 +pandas==1.1.5 +parso==0.8.3 +pexpect==4.8.0 +pickleshare==0.7.5 +Pillow==9.4.0 +pkgutil_resolve_name==1.3.10 +plotly==5.13.0 +pluggy==1.0.0 +prompt-toolkit==3.0.36 +protobuf==3.20.3 +psutil==5.9.4 +ptyprocess==0.7.0 +pydot==1.4.2 +pygame==2.1.3 +Pygments==2.14.0 +pyparsing==3.0.9 +pyquaternion==0.9.9 +pyrsistent==0.19.3 +pytest==7.2.2 +pytest-faulthandler==2.0.1 +python-dateutil==2.8.2 +pytz==2022.7.1 +PyYAML==6.0 +pyzmq==25.0.0 +scikit-learn==1.0.2 +scipy==1.7.3 +six==1.16.0 +tenacity==8.2.1 +tensorboard==1.15.0 +tensorflow==1.15.0 +tensorflow-estimator==1.15.1 +termcolor==2.2.0 +threadpoolctl==3.1.0 +tomli==2.0.1 +tornado==6.2 +tqdm==4.64.1 +traitlets==5.9.0 +typing_extensions==4.5.0 +wcwidth==0.2.6 +Werkzeug==2.2.3 +widgetsnbextension==4.0.5 +wrapt==1.14.1 +zipp==3.14.0 From 6c145445e30ead851c1d546dc98549c0efef7cb4 Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Sat, 18 Oct 2025 12:37:16 +0800 Subject: [PATCH 05/16] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E4=BA=86=E8=BF=90?= =?UTF-8?q?=E8=A1=8C=E6=88=90=E5=8A=9F=E7=9A=84=E4=BB=A3=E7=A0=81=EF=BC=8C?= =?UTF-8?q?=E6=9B=B4=E6=94=B9=E4=BA=86test=5Feveryyhing.py=E4=B8=AD?= =?UTF-8?q?=E7=9A=84MODEL=5FPATH=E5=8F=98=E9=87=8F=E6=89=80=E6=8C=87?= =?UTF-8?q?=E5=90=91=E7=9A=84=E6=96=87=E4=BB=B6=EF=BC=8C=E5=8E=9F=E5=8F=98?= =?UTF-8?q?=E9=87=8F=E6=89=80=E6=8C=87=E5=90=91=E7=9A=84=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E5=9C=A8=E9=A1=B9=E7=9B=AE=E4=B8=AD=E6=9C=AA=E6=89=BE=E5=88=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ...ax__282.00avg__282.00min__1679121006.model | Bin 0 -> 19104 bytes ...ax_6030.00avg_6030.00min__1679109656.model | Bin 0 -> 24376 bytes .../test_everything.py | 128 ++++++++++++++++++ 3 files changed, 128 insertions(+) create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/models/Braking___282.00max__282.00avg__282.00min__1679121006.model create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/models/Braking___282.00max__282.00avg__282.00min__1679121006.model b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/models/Braking___282.00max__282.00avg__282.00min__1679121006.model new file mode 100644 index 0000000000000000000000000000000000000000..6345372b28af1650b3e2bc8dd58ef3a06f401d13 GIT binary patch literal 19104 zcmeHOU2GIp6u#RcRxDZ+LqWl1<4+&3-QBiysY+WYPz(wYV;V@ao$gNC3ESCbXBJvW zBM&}kVie;GFD4{tqHjEsXaEyUOdzq3Jo4a^iAfWE;0@2Y=bUb5cBfkkrL8-YvUl!1 z=bn4+H|LzW_uS$0f&RmrH|^M@@M>sK8&s3Nm7fQEU3rgMq%QPfzKL-88J9oQ(M?1( zD5N!WdlSo#)>A_>cl_wXLx&V_ihL=#+q?jgGv6NG)C2+phx$(7L9twQ zAv`j9d(;}kA*<>2CHf`H!&~v!Sq6L@g}@c$*Y`IP{Vo-%nqLpRLX12l_X>+zLBQ|V zQJ(k}wj;qIYub*PNL%(~(Zx2Zm;5Ouj7u!VuZPiz;kwAO|Q>eT>VoJ<%A2B51l0LsXGQF)D#n4Yvc0r%U98kZK7Y!AH>|j^R`CNgpxX z%s3fL+8H+{aj4UFF_+sDO%B0OS$A5u#b6SK+n&Vk&ZJRrka5(=IVLoSiXI(ifMD4p z`J$ao+b}P!?+~>5buQPoAI;dOsCNt`x&_0sjgcH`;ueai$Z}>=v@I8POG1Am<6378 z7hX=#5X)J%X=uQTj+q{@3}oYx;7w-3E7XF{mXX6(C(!(l^JAl68Ft1znE@7i%r_$k zT2c2zYaE8g$BToG8g|+^t9i4Q;})!uqUZyfj^+y!D8@0(EO6t4nn9pZxhl$>G7EXf ztAg&zGbxy3#T@8MGa(Bgu@xyADa)NsSERB`uws}2JSCTP6-~6pi&_Cx_F z_`5JOG&D3b1j2NnOPAcCVuvB*%oknofC{XD2ucNkGfKxa^wS{!)$0+UODTRYVR$D8 zh6}OMifVx?d!4;6Yv^aWfFXxgw6ArazPdWP+PhL+ok{bpcqbxfx<)zy4Dr?!bL>rY zrV<^=giw>33dA^EiDas)V{g2@9jI9|V@zuz+L}4(SUGLu36R|dEyjd1Rxr?{v`=fp z$mJb0_}B#b;)Dstn9*Jgev&B`jLdWlp>>Rj$(+e6$28k98xYEnH6_T>_KE=WbxnYl zhOi6~2oVSo2oVSo2oVSo2oVSoSOW;0{NQ+h1a|k5e@k&bMDhT`zWj*f6;cUOex8r( z7u(msq*g(NQr`8^^TD(SA1<;@5L8dvOrIJ>hL$c1_}l z#D}I`Bud6faeXjK?V{K0(_X%7oY$Q7=}TPy|6j>V`*A467d+Q#R&>HuN=$Ew_HYdE zRUOZ&jt88Ol{|Gha+TvtQ2ux}N)j%yplatj@z;q_o-=m~iyA?|ABV)+rlMW|C=FUV zcNp-JAG#*~!R#QMIi>ZTp0Wc?xEO^tw6{h-X%K)kki< zsc-ZNG~MCv3mhe|kA3Q;Z21XNiFYOLBA_jfE$<0=UYBDu+V$6{tQKFU-XOj(z7P(z z_Un%?Q3l{H+mGNO4l8x>ShPXBQdBJYm)1G>^#fg#Y_}rMZ^ug(Tw{#>_2R9z#Q_k! zhj%Z|S8LRF-P<>2>aO;D^Uv2k(LcVpdF1DPH(EZ~((}vPgv2YmkM!L6(Y*QZ+kfr< z_NzNRKb^XM^VeP9-z=WL-Ir}E_C2`zckg$<)%A@&Q#|nLpDhP!4Zm={Ap#)+Ap#)+ zAp+|Wf%5$&?W}_KvCH32&hmazR$_!!`F?V0RbedU$(r={&-x=R_(CmB-y&80{#k$L z#8aeipMv)FmA|vR&*vX4ydkKzkDcQUNaayh70Ph%CxAfEKK6?591TCILv_TSMK#%X zKI}eL@-D-^bGt@0S2a(z0|uA)!s&Y@CF7)!^We+TmuM`{v_j<^$;e8V{6Qvhw_#I_XUheJ^Y2r+#`1uo^L^#;0BjA6(h?U>>#cNqtOI#m#M9qWjcdCa3U+$x_ zn*9zQB)-8%)F*{BiyRey$Mtg#vT$5#*U#DY=mEd`-n@64%p_?b*dNWD zHgE2`ciy}AzI*R`@4lG_iwbWUJu-D9gRi6{CXq>zZsF%=dKt{524KLK(Hj*lx6$(8 zFmj_}k{FCr)A|(Jz9xZGsOf{tm(H8VAWCQ-O8QuL1JF))ef(xfXrO4Gu^0@^J@j8KlH_Nuh&WTpuD3U22L z<|<_4YelEvGS`Vtm#xNu>L;KMuqWYv(pM6?gL{U#37 z?MRMG+rg>`@HJX$hv>sn?_u!;_ss5jdjwH`OFBQWxBk6Wq%e}=dK&qXT1 zugkC#qpN}Tm&}w=KTJjzu4e4QdK5bxLbZsa3Bz28I!Z>RN=164(-;F~FrUC9eh&D= z_xSoH{2j9n{#i6ZYPQ=y2^9Tze!=axNs{0|g)Y#e6rT{BnXXTtjY z&I)_Y-S{b{TYf2iAh=#@u?sGjq&aP#M!N{dW$jFjmV_cyRjn#@wvmDRmos?#DN1b0>@ ze%w_nI9-^qLa@6;Q~@l?c-V*#Hb;4l$6+-)kX#bKrKncPb11ik*o+m7%KN}bw^Oh= zgmOFP#O?H8Mq-i1!z|QEv{swT?X;D9 zfE&bTMUAr>H{%jTE2753v=OTrGZ%%KJ4I)WOU{DyD3!!1R(kA+Um^(^f6{DNjHD-< zd%Za1u=Z1mDaqi=NW1L;w~Fj* zqZF^{w8un>=Mev)gn0m_<;irSsMiKo*g>IP9{i0~LGtb(0y~Z?bJFKXS8jH0R<0p8 zCsUlI!Sjchce;5J~7f!dNHn51d)(@59UK~?eFxCY`Hh-)COfw%_Z8i;G4Uo^06 z@$H2ww7ZApiGeN*ArGL;k3jxiGncRnbVU@dV*5pme(yApcfI(0Fj;fcgp5Km70OOw zox?al{-=2|A#9-ZB=5>ZK2M=^@nUFdK$&+1|5wv|JZJ^{yBI1ivLx{)q1%29Q%Hl* zPZ@<9G^8H*YNGi~p!3;AoM9s+R)pZ};CauOCu zV2{;sG}iM>_9!O%XPUTH(R_BOel9Eyoz;*QcG8BTo&S_vM+jk@>0|L?h-g3=hrrqz zHF5=zglXV>qDYRuU-shH8y`H#|n@cGXejHmtr=OR!NJ~K$7I8R4G zoW{YMfe4w5PP= z^KN*E5yJiy_B)*!Ju3k8y=_|0bVCiBG_J{a{NZujjt3WT?a$xko3XQrt8;7F9h)cn zmY({PZ{>z9+>3=DvhQV1=O#MZ*|#^;>W|z*x3sfm>QA_L*6;I{4X<~yU1NS= zU)ubXmouo@tR;uIA3HL=m3#JZk7RtCU$JK+n|-dG+t+xAdt|`_+{YOs*ze0XbIYs$ z%H2Btv$ocejhy$LJT7N$2fJqKV(!4^4)2#M>$#>c?)9EZOyyqx_aofzMm^1X&ZxYf zeywL8`)Cr^@p&3|YQ@9ccdvNa)!*{mQ|q2(7oSMsA1xZgRcMO2wGB6N!zbRu?b}SO zVACBUzwGh*@*iK~_PGxq#Y5St=`Tf-lun+$iIL6J6@(^jc?^$jPHp>TfOH_r}~yo8soFi z+U|ca>5$Jc<-L}--hb4eysgPw@~>ImaUUFN-Tlw}Hc!hM@6M(g-+Xhu@6~+=+NK?A zU=Ji*;VVo(>6^D(@PAy^;Ir>&Z*iR~^*6Td^#1RrXMDETzHYmGOcVPeztlIU#^B%n z-xBX*t$SNXP28N{b?mLSyd}+Ealh98T9?i@vte&@e(F^Jy}!45%g;REU-{v4zW=1W z(X!{-t^TZuHm~LI2;ZtBN7}yM@lF0yx4hsLJC=H1nDU0{)+u{UhwS&5W^Yd^NSK{c zz;2mZu%O{8)4QkMH0`%CtoYN_>}wM&?4p(o_Kq1Zv-6*CV!!gAojdos_WYE?$Jvpu z^Q=m@ifgHQk~^Kcmt+4tnQPhA#Wq!Tu`Qy>G-=C=Mz!Mw(~a)61z+r&W^8Gm$K6@? zoT>0{$9bRUO4G8_TTK(be28Co|Ehvi^H$@Cn_n`$@yv{Zk(Oh|gaZcSS1;>Lt5$TG z9&J9z&p0~WsQu=e!XMtvG+lReQ^700=_+u4u4mK#@CAQ-(&b#zx3}@k-;eOQpA{H4 zocjyE!hXQma%PRG{qY-k$M_7>6T_Z0@`-;m`4=uP=-9*zB`8Df8PGo4+D_cIU4s2)kjdYmiW` z_t5x4!T{v2z(3%9G9?^)yeT3bF^w{YLgP)94By*8r2JJw4(Yiumz2@+S7mbu9q6mK z`Hr_gur#2IFB;08pV|*_+uQl85^A@Qzf#&U@DVTN8c^o14(7@6N(lVCC|ijc+HmOn zB45A4yaHwH^GPS2Pp*q5B3bIfO&^*x1c&| z3Fn5&_tTP@-KuEky!h1;*dLFkJwb{2%&@u9SHJixrtZUCOa-iGNHo&7Qs`4I>G(H1 z%B)Ao2&l}+sI17S?8vB`$f((oQMr*(hRCQnkx_bGo-TsOdaRD~-)E2@2KJGF_9ys0 z1yvIak0AWLb)@s}AJQa(?vpwKg~t1x1TnyVBkeOfjZ`WcccaDUqw^^J0Fy7yPTy!i z8K24Oieb{I{QwurkOzLp1HlEBus$Zf$}oOvx6rQAj$!?vDIy&KT@i(=ZoNPTE)jC& z`vy(?B_dcKixIldAkzcnv6Sy!DdE`n9f>y(QRM>{IuBL#sYW8rFG_!lHor73B5Wu1 zDfjWt28srh^9#iBZHjme>;LY!evtx1vfreIq~SDbm&AUDMf_f(b1|WZ{YTjy1m1m- z{KX$192x-oEu)5cl(OBtx_9|k&g&3Ke$_J_tfzIwJ=3L8G{2;0I*3EXOM9mKrLm#wBO)2(Nnn15GWPXQAzcq?WFmyF zht|pPeGP=_gNz`o7eNn@msY~D>&Hs<(5D^R%Xco9_sES)I^!}uUE7{;fE!7x5D42JPJVK9sj2ZLdJA{Y$g qW58e-pZx{H_`pvFlT$tzFR&~8%s{yM&fc;Ig}wEiJ`96GAO8m_?|ni5 literal 0 HcmV?d00001 diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py new file mode 100644 index 0000000000..19730fcd66 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py @@ -0,0 +1,128 @@ +import os +import random +from collections import deque +import numpy as np +import cv2 +import time +import tensorflow as tf +import keras.backend.tensorflow_backend as backend +from keras.models import load_model +from car_env import CarEnv, MEMORY_FRACTION +import carla +from carla import Transform +from carla import Location +from carla import Rotation +from agents.navigation.global_route_planner import GlobalRoutePlanner + + +#Trajectory 1 +town2 = {1: [80, 306.6, 5, 0], 2:[135.25,206]} + +#Trajectory 2 +town2 = {1: [-7.498, 284.716, 5, 90], 2:[81.98,241.954]} + +#Trajectory 3 +#town2 = {1: [-7.498, 165.809, 5, 90], 2:[81.98,241.954]} + +#Trajectory 4 +#town2 = {1: [106.411, 191.63, 5, 0], 2:[170.551,240.054]} + +# custom trajectory +# town2 = {1: [\initial_destination], 2:[\final_destination]} + + +# to load the pretrained models for braking and driving +MODEL_PATH = "models/Braking___282.00max__282.00avg__282.00min__1679121006.model" + +MODEL_PATH2 = "models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model" + + + +if __name__ == '__main__': + + FPS = 60 + + # Memory fraction + gpu_options = tf.GPUOptions(per_process_gpu_memory_fraction=MEMORY_FRACTION) + backend.set_session(tf.Session(config=tf.ConfigProto(gpu_options=gpu_options))) + + # Load the model + model = load_model(MODEL_PATH) + model2 = load_model(MODEL_PATH2) + + # Create environment + env = CarEnv(town2[1], town2[2]) + + # For agent speed measurements - keeps last 60 frametimes + fps_counter = deque(maxlen=60) + + # Initialize predictions - first prediction takes longer as of initialization that has to be done + # It's better to do a first prediction then before we start iterating over episode steps + model.predict(np.array([[0,0]])) + model2.predict(np.array([[0,0]])) + + + # Loop over episodes + for i in range(2): + + print('Restarting episode') + + # Reset environment and get initial state + current_state = env.reset() + env.collision_hist = [] + env.trajectory() + done = False + + # Loop over steps + while True: + + # For FPS counter + step_start = time.time() + + # Show current frame + #cv2.imshow(f'Agent - preview', current_state[0]) + #cv2.waitKey(1) + + # Traffic Lights + if env.vehicle.is_at_traffic_light(): + if env.vehicle.get_traffic_light().get_state() == carla.TrafficLightState.Red: + print("Red") + action = 0 + time.sleep(1/FPS) + else: + print("Green") + qs = model.predict(np.array(current_state[:2]).reshape(-1, *np.array(current_state[:2]).shape))[0] + action = np.argmax(qs) + if action == 1: + qs2 = model2.predict(np.array(current_state[2:]).reshape(-1, *np.array(current_state[2:]).shape))[0] + action = np.argmax(qs2) + 1 + + else: + # Predict an action based on current observation space + # Get action from Q table + qs = model.predict(np.array(current_state[:2]).reshape(-1, *np.array(current_state[:2]).shape))[0] + action = np.argmax(qs) + if action == 1: + qs2 = model2.predict(np.array(current_state[2:]).reshape(-1, *np.array(current_state[2:]).shape))[0] + action = np.argmax(qs2) + 1 + + + # Step environment (additional flag informs environment to not break an episode by time limit) + new_state, reward, done, _ = env.step(action, current_state) + + # Set current step for next loop iteration + current_state = new_state + + # If done - agent crashed, break an episode + if done: + break + + # Measure step time, append to a deque, then print mean FPS for last 60 frames, q values and taken action + frame_time = time.time() - step_start + fps_counter.append(frame_time) + print(f'Agent: {len(fps_counter)/sum(fps_counter):>4.1f} FPS | Action: [{qs[0]:>5.2f}, {qs[1]:>5.2f}] {action}') + + + # Destroy an actor at end of episode + for actor in env.actor_list: + actor.destroy() From 7d49a562de6d704067c6f64f3f336435a37ebfbc Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Wed, 22 Oct 2025 14:33:44 +0800 Subject: [PATCH 06/16] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E4=BA=86=E8=BF=90?= =?UTF-8?q?=E8=A1=8C=E9=80=9A=E8=BF=87=E7=9A=84=E4=BB=A3=E7=A0=81=EF=BC=8C?= =?UTF-8?q?=E5=8F=8A=E6=89=80=E9=9C=80=E7=9A=84=E5=BA=93=EF=BC=8C=E5=B0=86?= =?UTF-8?q?tensorflow=E7=89=88=E6=9C=AC=E5=8D=87=E7=BA=A7=E4=B8=BA2.5.0?= =?UTF-8?q?=E7=89=88=E6=9C=AC=E4=BB=A5=E9=80=82=E5=BA=94python3.7=E7=8E=AF?= =?UTF-8?q?=E5=A2=83=EF=BC=8C=E4=BC=98=E5=8C=96=E4=BA=86test=5Feverything?= =?UTF-8?q?=E7=9A=84=E4=BB=A3=E7=A0=81=E7=BB=93=E6=9E=84=E5=92=8C=E6=89=80?= =?UTF-8?q?=E8=B0=83=E7=94=A8=E7=9A=84=E5=BA=93=EF=BC=8C=E4=BB=A5=E9=80=82?= =?UTF-8?q?=E5=BA=94=E6=96=B0=E7=9A=84tensorflow=E7=89=88=E6=9C=AC?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../agents/__init__.py | 0 .../__pycache__/__init__.cpython-37.pyc | Bin 0 -> 177 bytes .../agents/navigation/__init__.py | 0 .../__pycache__/__init__.cpython-37.pyc | Bin 0 -> 188 bytes .../__pycache__/basic_agent.cpython-37.pyc | Bin 0 -> 11078 bytes .../__pycache__/controller.cpython-37.pyc | Bin 0 -> 8627 bytes .../global_route_planner.cpython-37.pyc | Bin 0 -> 11662 bytes .../__pycache__/local_planner.cpython-37.pyc | Bin 0 -> 10125 bytes .../agents/navigation/basic_agent.py | 355 +++++++++++ .../agents/navigation/behavior_agent.py | 319 +++++++++ .../agents/navigation/behavior_types.py | 37 ++ .../agents/navigation/controller.py | 258 ++++++++ .../agents/navigation/global_route_planner.py | 392 ++++++++++++ .../agents/navigation/local_planner.py | 337 ++++++++++ .../agents/tools/__init__.py | 0 .../tools/__pycache__/__init__.cpython-37.pyc | Bin 0 -> 183 bytes .../tools/__pycache__/misc.cpython-37.pyc | Bin 0 -> 5535 bytes .../agents/tools/misc.py | 171 +++++ .../car_env.py | 603 ++++++++++++++++++ .../requirements.txt | 13 +- .../test_everything.py | 266 ++++++-- 21 files changed, 2676 insertions(+), 75 deletions(-) create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__init__.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__pycache__/__init__.cpython-37.pyc create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__init__.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/__init__.cpython-37.pyc create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/basic_agent.cpython-37.pyc create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/controller.cpython-37.pyc create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/global_route_planner.cpython-37.pyc create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/local_planner.cpython-37.pyc create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/basic_agent.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/behavior_agent.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/behavior_types.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/controller.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/global_route_planner.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/local_planner.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__init__.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__pycache__/__init__.cpython-37.pyc create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__pycache__/misc.cpython-37.pyc create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/misc.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/car_env.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__init__.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__pycache__/__init__.cpython-37.pyc b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__pycache__/__init__.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0ceced42b3f087895b315de419a0ecf7a00c1944 GIT binary patch literal 177 zcmZ?b<>g`kf}kS43=sVoM8E(ekl_Ht#VkM~g&~+hlhJP_LlH4_zo`FXmb#hH2Ox-O}y1-d?|iA8xJ uUT$J>NotXPVtQ&`NwI!>d}dx|NqoFsLFFwDo80`A(wtN~ke#1_m;nIuZ!UiT literal 0 HcmV?d00001 diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__init__.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/__init__.cpython-37.pyc b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/__init__.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..8381a155c4f3bb029f3c4e371813c5ad17231433 GIT binary patch literal 188 zcmZ?b<>g`kf}kS43=sVoM8E(ekl_Ht#VkM~g&~+hlhJP_LlH4_zo`FXmb#hH2Ox-O}y1-d?|iA8xJ yUT$J>NotXPVtQ&`NwIz&T(N$9d}dx|NqoFsLFFwDo80`A(wtN~koBK|m;nGXa5Cut literal 0 HcmV?d00001 diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/basic_agent.cpython-37.pyc b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/basic_agent.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9eb3886895ecb4120cbb699ad23f49b8cac4ab61 GIT binary patch literal 11078 zcmcIq%WoxDTCb}6u6up`Y`foG>CU5X(y@okgqbwWOedXm2sBQUm_(|XP%7V3z7N;E zb^BD=Zd+vusiWET&_t|2NOpt}`~wKFV8H^R5o}OawLp~@+A zVy<1MPJMMA-}%n>KK0hrl&RtOAOH55`$xAl?Vsr(`;}3-jVtV<5SpzCT^PP@>$sPE zV^p$BI>E|*c~r40T(9`mQO&M#z3SIT4ZFehns1Jpc5~FSTcas^YSgydy7sOn>Z0+f zCK}%Kqmn%%j1RSrx%aQ;T7M8)qd;u=o;4Uvd~f89qtJ54mfJ(kiu!J3$-o@}CWEc9 zuv{yWZgWO4-5IQ@9s6QAxVh~2|xa%d)(d%>tyZ}55dOc4%K1PoQ z!8n=e2BQha%sX`nsq9xlUpHea(1q1z>4473eHltlSc-7brYsD7&16;Ts) z+^gaxVTvZ+)I>{60a6!jF^zjeToA})!`sLhBQ;)<|Pn-#B$=fqXi=ESmiLA;3CyqEzCtt94KZaC<^NzCHD zrE8jL;dcc4dZrq?+k>7PdDwYuXV~?|-ZFP)mQ>tF%;9yxkH?uz0)eUQXE3yW? zaezKn7zGn6#1oi^@Bt(BU4Xr9Pl9PSw#HrB5b*T2JMi6gFh5vFam+5`qxg!spU_k2 zjJCokWw0AOF2>yPeBZ(y{kwg4+=Cba--#>Tkr#O~v<71^G5FTC#9(8?lO$C_4z@`) zAb4mWTEYtl(h~~VuNJWXT~Z13qdQ%9aE1ScBGzNg(PC|=9q5O+I|k~;L5b^SL2oMp zFsI68jmvsWkcKFsG({O@lgpN=s{Nn<0v?-d*sSoU)&_tf+o5SUigjCO21=4 zsqR1__8FU0H(EObu+k{l_D&lwm_$$;-RLIRh?&=wyE$)#!H5;(3D(?=JVLJG1$Jwp(Rno_YV4YPW;2hOD5?XBg=TUF#h+{PDsDf!x6%g-u0-C@4Oi~C z(s!K?2cs>pP9RtA??(M#{O0}lR=V_Ng#|QRNeuw@a0ROxbRC8-Pj;~t&H!@lIF~SX zcoT)zsOl~JRZCU<*k7rx*LCyjuF{OYH~&^{pq8_7y9SMkAKmFcQ25a|NT9Iz;eZJSY`!%;Qby zt=OWSwxTUL4o}r_sam3KS^h|QXm=E{ots6pp+I1&1{(uUcNa^}S1lt$e!uu}v+Z z+Z`H!xRlJw0XFndCl48$Y3z;jb=+aAt-KH=f~mGn>B?N0weUbJkCja%uu_cM)(MALuT8`-h~1kPPSY7xybOBIfn0n zQ(;%OJ!LPI5oL=ccE|2$0P_jsm%2E_j>p#PcDsROcR#fgia7pY1e0_s>#=}X*Vlu< z2l2qS7*kp>wBvwn79#`81J;KStb2X9i-FA1%?O&zD~De>=Fr46VNP0}1@t`}Kw_xgc=dnm05lmXmUSj~|)TF0`K z_mL{uj?S!rn}tD6V3*92rQ!CO`t{IC?Yyyfv4HDS4Sx}sUcuyFgXtV0aNJT13aA!^ z_34(YU6N4x)o|qw?vL;g8{s06cBl`HVQC*B(0*y$2BdsYIn?lM?8A*6Ru5{iaj5T; zgGhBq_dcB2gL+(!D~HB@mB(nrRXCKT#|9+7jLoXJS0F?=8?pD3?p#-T9FDk%KjG&? z{Es5qbuZfSpjurhS64=)Rmqq1#?14yUF3_Xpnz1wI(!m@C}URKDwdEQ>0v@HX5nKk8t37@?y9+;y@-uaf|z@y>>X5y|XQipS|rHVBx9 z$JTXr50>wx)P!2t7>wby=POJ#GSf5P!lFhdnmlW~r-^pzkZX708#a-yaL3MrEpN+P z?U*dH$+V)LlW|7UKx8@&$)+du3{N==E>2pRd=i!AnH-dNZK7+}b9gn$H0kkzFm-Cm zI<$%uO4_`De1-B`G-?XW$K@{BlilR;$=mLFCv^=^ir5qU%=|mTaN!&Z%`B6%Wte)| zfZK&@?|eb?5Uz!8$6{6TAX!5$qQW8w!^`od{+NkJBX74eNuz74J`o_@5xjGyy9k@@97kqNPLHLCc zbBv=9jbyZRSVLey#B=aZl*{05MZpNv>!xWiD{FL2ft<#z1^&88X)urS9Kfx+1-XO# z@X{f$w9dSL$4C}Aj(7?wW`@ix%I|{uowDMgGL0aA6%cN7f(y9mr+BG+t$I~A@b}by z7kD#Q-L%)a>L&Ll)=6zcJsW%{)r`zUT#CzwwD0=fKjjO#AEKai+Xu8|UY z&(^ou+E3RhN=C9anO6BYWejK)iY<^c$kls}Q(@yt?4B`%QcPKcd;n1VH6ru3sUXuS ze;GxmqAWcHGV)ibc!P@XQE@ddu2GGee}*e0|3a%#aI?2qki3G*$gMpEWBD#nXBbRm zfmD8I--wL^m`oT)>(DEZBlrLyxG>4VAvZRs8Jb<~3tB1yqO8d3_`hGpG z$u&{o@oLZP7R|VW69_E`Nen&A5rVP-5q+dsp6h=yhpp1B5Av zZzd!kMsJLCg-z&-U-&h6A(8jE#;TV3#tL;U_Z@S-#wm>P};sOx5fxXpgcNgkEt!( z?CHL{=Sqw~6l9^VB*` za^#KYijCDvc^xR8V*&rpoxX>|fy|#LeWpSQ%D=+F>h|FQQJSh?KbF(rC@Bjt$LZdt{58z< zSGYo&TQl~i3sOPdj)gtP@#BPP8uz_&{cW$4#TKc)C5%IM+*7Vg4WsxLhgRWbr9#Z72Z>LNu`zK8lr4rl}RHw7Gval})J zw_gB$=l&P`rt$u73-}eijT2eG-1;=po`MHv#09V=F`_hSqhme1j3@oTIMfbsegZuN zA0)gESQ#1-`cB@ba#)RO!}_qXS(0}-H(nWYc6?abfW1Ns^PmZhM%uD% z>u+nH9PC%3c3gR+bD%+Qt9Vm=p2+*Txtr6FUb8%xhx1gam zkJ|i#wnX&>)EB6}Z^ovmY-;igj8Me!Ump0;85)T(TSUg+pwpn9@u)1(5;^h;vigks z1OH={>ZrMYQ1rEaQ$?xpd?1Ms)B&HSNj`;eA)rF>zNIuv<}Rg*=yZEq&~euFEOb)d z*qYMf_vkzihL>Mb0?364MhI1WZ3XK?I_f!!bI2Og5An__1RNm}(&NV^loH^iV)*n3 z2b6q5c$A`|-f6Oi)MutQt?OCN22iBaVH|LPcZvcaCzNvMI4K$koZR3n(eu4+WYwR6 zCE-Ki9}r7((JC28ffmj$kFe_)cUPaXbgOHU6zo?vip~y6n2)iTppJGwY}A($A-# z4yw=^l)|VXp#e>G&^Xjm{bGs|bi?n&72HdFFTX)vT$Obdw;r~JQ^WQ?k{_f?%Gz)` zni6LdnSNqC{DQRF6XTPglE=ogw`lfZJ#KOD`Qd`7J~sBJ zpnuNDFXE};nJ0Sq1ouTi7B@@bzoTxVZjzt4S$d+&8yI0}c$QA8ALB!YbJ~78ZlhQ0 z!ROqs&Hbou#_eK1yl?SprsHP1mYKMjlDImYeq?NvKGYt3-qZHy=y`vhD9rQB?Jt0i7eMvs~>C8bMYKv zhT10hME)b$+2|@k_Rl~33ERXU6Yu2v)f`@9x?bBfum=Xl_>$MvRJ_dVD#`N%lqJtX*$7$QTb`~gZ|XW`PbZ1Fd@a;3YgU;Q_Jh4_y3r2H7V5kBj0 z;U+(zx}G$UsK8n7?!@~F`VtTuCeE9LYkBlRo;{@N$HRYL-dgf3mx1!{GV@aEt z7z2DbMa}p~9it$R-t|J;#5ZF|9Fz0cxuSfEcWBz*MqxL%CP1l&*gfjoEqrix^weZR z1sfa4oY_<4a^${=U8BdHNo3Da<+xX4n*>vowCDLf<*N(5+i8|J-E4a%eLfoB%<{mP zqCUo0JlUBPdSbq*6r4_(wL&y33zODWqIP{fX%SAD+mzH*H)q$bNFse=OnwR1Yl#7m z=Ut`|wV$Z3+3ZF-+4{PjC988vH`8~-pmw&%wBre3kZ)q-|HKvk7{zht30Z^EoL}ja5)* zfj%|B>89s6_*BAC-$A0@a*+3T{q#-EaYWE{9Qiyb!Konm3f1T!Q7%)lLd7jADDTQy zJV|aJXDv8EASnWu6y{2DOPRa{l3yj*H7ZOLq!Bn*Q?Hhd_N(<)y=KG?=^1UnU#|IvN9-p_`Xq+?y_gXzB}>!-Q}K#uQMXKs|4{18?$h^kbPs_l^r;$ zPeWdOx>cG3R*Xu*j&Cm2CnK|I;|RyO;qqt@c2zcx^#?h@N{LjJX=9{MTYXZ$rS{GnvdLX||i~3xg1J=q6z)v^11v>29}#UBdPW462M{duBWt+ta^2 z$tE~lwn$uBafL&ba6xc@Yq`LI3pWnjd_|%}NT_Ed9>4GJ$9SfbZd;_PIP&NJ`}sY7 z-#2-Be!ec?d7d?U)lUn;UnrA5G7?wthC3j(&=zdbmIk6HwIz|#@<49O{HwH8{;jkt z_$q^{r?oXvcvi4gyYiY~SMEsd8gf-zLrz1kj$F;IBUeXm&X!*gT62dl=r8u%Q1=3R zG;nk)@FUapL;cp)HJ)L?VBoM&j{<$@uui}{ePBioGY7iq+xj5zyKXeHT^|M18SU5f zQ!HL+iHT}Eua2B;q-tF!GQy$b*cR30kAlP%yx|u>9HA{@%aSc(i|6c$U42b#E4F6W zUK6m{x;=+)#ctU1_*U&EmcNiJzwGo}Yv5qLSJUaS+{0hgbv(1GbnG?i(MG);EIM)n zUk^HZv>!}wnGf;sS(|N@PXq(<)`MVQA2@qXaTL0#@1^V0J1p>YYv4M5q=&AZc3>j* z{We}af7^WfQU3gS^X;Ggl%CT~OH1nn`Vwzk#T$MHB(B7QA;c=E7*|21SOk@0p)VgR zCj!!_L%MPz^r>EpYLxGYvHS+LtVeT{kCl!TOT46Ei#Srr>|!Fm9XdlCY8bJR6$MQ9 zoTwMrYxSaXV`wtd%eT0po4grV#T*U?2XsP%zyf0o2d3{k47sjr>A0p~dI*zsLnAxu z4c&Gvx)_rk(A}VJlduIjf~Fbo)!Nq3hhpRDt{m@~-C4Nh}ldfu-BJdAQuJ89;XgA;KLgLXh@X^q7 zumlqV+d(sosBe$F9ny6@^wb?c7^Htp`{o)@SnJ`TRS~ zF?V^VOgln7jn}^2nonw;dE3}w=B|?_X{;zshLPj2L^HC}Nww&Hzy@|wG1BiGDb{T0 zL`^%)56uwTmO6v)lZ#-Kn&fJt0R^6S$I6mrYMbi!$!2CT3!1gGL z7=^1Zry6y2+uU>MJ_CRCB@*}5YmPHqz3G_Dr<{jFaM-%pCDpO+7me3T7co4*=^C!@ zMuu^LROFW4n*y8~G`6Dkx#P3kbN z;UO?q4{{lSiAZwIgJIzMQPIVWP}zkP`&p9|SCq^WY4uFqrh}v{plO}ZWl$&?*XkUQ z6a$yK<&$pYs~|hZv4Z1UfsG-|5Ua?WO%B1FXvBQd>tSU2mZLxH`eZwyS~zMTP4=#y zUGNKkx~c8|?Iyj4mX}2zzcPYE{{JYSkzoXQJhsXg5sD zav;w9h}sL*h>?vPYoKbz9c?Wab1vj5d61U#36RI|h7W@Xa}9`|hQFq$K@69aCKtuJ zgc7NVl2ky?5X}6fEQ!X#Lth#1RlFgcCKzAniya{r-w+8_6YM#B{3_0%3%fw~!+{OI zGL8;Wa;6g9Zb|7CEn0j4W84=tbZ%^g2&uL4j*|;n>oS5iFo}U?Az!(4cxk5MIiur8 zyj^w)3(Yr3x^sGiOZhreb3%KwN_+e4d+u%AXS=0vNnk3?Q6WN#7A6Rq+c!POFp|1q zr2Y%i4a0a9+?&-@4Z{vB!(b0lN8&QJN@R`5_(ph&Ga`8;BoSc)L{k-65bFW!Ac!vA3$QIG{@D|q}XEGQ^G){_u%iL1v+!Xnl#G)y* z$UKU7HnRkLjJF>8(chnY>&;DzPLLk@7D7SC!Tkpzmf#z}M*x+`p1`9ZA47@A+mv1> z&!NISN5#CqY!je?&h`^Znl)3sAIeuK4Fk}=aTmy*S%4eHwT-*r_M+t$K!OGorCul~jG)OI0$qOk|75wM0UfG5>PbB^LMaa@G~>jjJ< zwx*YBOH#)X=}Ykrff$KB5Wzn`mSgz@B_e8$6)=u`B5@u1*JAgxFv3jc1%}Qoe%Yv} zw)y=S^$aI)TMqc4-|*JQu)Tb7&d#X20bCLc2>2tE0X%@BF3?J`Y~DUM;JFFS1+8dx zxO6o&ap@}cuj)TIYN?FGDZOKL2;?0l6^fUb5p>efwR4yaea4MK3#}SsspdrCdf~dGnQHUV=ec_JC zLFqQ7ZRsMnm9~6QSfP}BKj36U02J*gzbqW7zGf>&3SKo*j}ewYxB=z(0;(V(2hRTJT?5}*trQBe8Z^ya6&&T&g_elxwH@0cOb7lJlqrNP!ZB1{~1gyXP8 z>C(%9Wkb)=++BHP;yO0;ESudng^MtGzM`2dH_bxXW-=U9Fw!YW{po-3ocg-9BBo%~K93MIZDusQ8O5||lC6W)7g6PtaeHmqJgUAygxe6g>W6yvj zD%UWia~u%YnPxU(k=OC2u24-A8&XrePh0}iDbnF*-b0g=c-$gpqldnhSkdP`&LRQ7oymx)kFvjjWn1UP0Yt%|RY>u$mUJ-i_q4NX?0 z`eOZ@cAnz{3q{342YLnXhmM%Cd(ivvI%h%J`Lwe?fi}|0T>oVurV`L7$1i81jVdOi zhrTji&IfdO86ObN5IN3(Z_CjJ#WfX>^=+$xej&cdk|dCb|kFCI%L0!KiixmIS-nILFNrf}QbDK6Ne zY+IsW-()t08Z3v=4>Nh-U^Xzy9r7Y~hex2&$N*`D!O9AWH^6LoD9aVje`e24CAGwN= z<-P(eRqZ3_*H>2%ec-cbk<_0%Zp7M&n5C2x5$LKG*D$xlehqw8$Mwt~H!;#VybCe# z)rl0>?$2-))>=pTBGjpNA|1`eb9VJ2FdTw`|A_0h`h;*qYh60NC$4k6w@f+Uz3Q&| z6OlbeIa}k1Z^^D@i0?cQUn9CVZ3%iFYnlh*6OZt7(a}7u1*I3_I^X?D+{h5$#dv-K z@zu@hgk+QvU%`jqJ4ETH@YM%Id^yYCMFzM{&oSJX3+Q EFDZuv4*&oF literal 0 HcmV?d00001 diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/global_route_planner.cpython-37.pyc b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/global_route_planner.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6165532e8a2ad3b4395946f5f957c8d9f6f952d7 GIT binary patch literal 11662 zcmbVS%X1vZeV+HuKCu8nkdPF4OfwFbCP9nwQ)Xz&rYOl_K`M$8?I|g%;m!cq#bRgK zGXMe9tjj4nWtXdzQf@iq0956WxRPtGN#&3~Ag5$1hg9VxA5yvGqD%69-Lta`5Vn=< z*7S7ubpQJO{a*dOX0xW@_dv9^{_!nM`wu3jKMRpNc#?Mzgqp8~dT0!FUzcygH|5*% zZTWV52XAv&*>QbWXIX1l-KqICU3)_d?a=wD7CKS=ndvt|<6W&&IsA6*;Z{HKcH(e% z7dAyrZ>u4cdIo3Dy@IrX;r@G$`SHlG@{$!X{fgCv>FLg4L1;=tSLZ*FuQ;f5W_`i$k3FbB}1$k@}0CYaPh8Y+EMLT(^2-QUc4K{yUBN` zVF2+n{>md z*H75iLih1*e@HS#Q;i}+HQ4TM^g(GD$4Nw*Nxs;{#6{iSRxsL(y6F0L$IPuH8gBTO z?Dn}chV<8(IFEjO_TGWGHnEA58>6!DjZL}RF)di!E*BU3eQkV@yIo97Kkare za#a~0ORp4pkZfi|%XtD%XSbx>_&^+6Lf>w}gI z7gn_oj7ekCELv=2P1#B-YlYS-dM1|8uQq7OH2SuXE~57`rACk~g!TpP8NR683^XV9 z$VG~++S!S|Jh_#z_s*)ei7_FDR^%IHnS-&Z_G)Zv|8G!wA*}ofMtfKL@GmCqtbLlI zn#-(mQ48H|9X3|_(*$l>yeh3INH_29f>I=8tp{sRvdaFN_WL5 z@rL~*_2LchN-q$@VC_+GFphCe!08 zl^%?vycz9dpFb@~=GHh!w?4J`y@kUf4$4pKpAr;0D|w^KGTj7+tG6X8EZW$N=4jZ0 zr#v@@(I{_qIfN29M3u#DVo#z#rgs0e_1EvcegFR3k3^FnonSl$W{5fyl>)Hx+7zPt zVP26~g2;J=Grb{jCP{H(fh+ncUQN-9WFrvMDE*A*s!N-xT7naJC{T_G?l<(&A-b5+XRT<^0wkT#aQF}FBg@uD1%n(lFK zj9$TWGp~-K{ZxT*b2gAL+RxqTqJW3Z77?SwM+uB7SRvWE?5D!94lb5AwPTz`VAi)LdIk&wjzExE1<#xqz(8YQ4y?>%h#YHTr}mMPSwu51 zM{7{U`KsV?@l-b<}voiFp^1Y z|6$f*-%pjhOj@hj5pYo=YU^aJC;$0K8@*7LIYCpF*9NSMR&?#j@*^M>Q6d=P1eS+b ziY*(vBgwf56`~J8CBd0jJlT^#kZwg@5(!8%UXXaf6iP}e+@>h7N%0QfAp!+`kq~UA zyU&Xs4+6_s8JCNv))K-IM5Z8uB`?&h=}fC?mm;U7jDzX-fMChu)QR7m_S!T-m>NaP6?e zI;xr9N&+DH+xo&HpNGz`?aAjMK~Z*SDOd#Csg2Porp53>3>l1iFSb z9|s~xqtJVNAlG9iN|sY6c*F&zA3!#l0gw!pb5|xXj=aJ&A`;xzI}5^P4e|tE@`UNe z5Z9Q0os^8xSmNm@%=P_TKgjjN+~P(NYb--x&MktX_%fm(DKWZI>`~u*I(~)0&DpyLW~^zhOdsCY^mE|GB&G+THiGX5zhGu4jtW2q9P<}<#n;%x&hhrks_ zBIQSCA|=#2AdUs}gbakxmKbVJ93mv-!2%=ku9-wviI4lYiGv#kq@kO@O3HNBc12{V zj6KtVlMUu&bs(QMFd*?k<_n#V4Iobj`GnsnM-ugbuf)8JGLj7{kW4v>0VGG%h8n}z zs)jG0&AyrZVkDRqz2I3jx|Vwkf@4y8|92an|07nO1>4ye8Bn4 zNf(cnvW3ApJj-~_Z<@!t%xfdBjl6b|w<2j@kW?=`GqOs0aj+V?AJ;Li%RtmB5VaQk z_pe7(so@YZM>s&S*xvV~as#A=K9t}DNRfgNGTr3Yyq<`V3rI@<#UdDbO0Ap$+^gl; zx?YgGG9|xuxBmv!HJsvU*69{Wnb3*u3_}kZ-8BiPGolnA*P*`=rpJ*;`%!Z8BMYEs zX|z8B!Ev1Qsr`8n7)#1!KUw2-h%dj9q{M@+(*IxecA+_;6&eG)U>LL06bMHtkbWL- zPqe{yq-^y0sFYaw91%?3nLXh`VeWccu?Q7!l)a{m%yyrnGp`uG>OGF*VH9wVfGBE> z`-&iCJAf?5N#+YK>7mq2sV)g$_mmp$U74{(EH;*L%!QTgddiqUhk7__%vCRB;i>Ze zJRL0Hm2gbfp55U0X?k%Ya3W(yn3@De1TMYgHfimQ9he+V>RC5P%{ARkP%KZ+@ zy?`KZ%rAOgpIf`UHd}+dF+Ue#6*bp8FMtW``;}tDOPxTxPQtW?Nx@(8k-Xkdx`RT( z5x>W8R~S&h6_*g?ZV(m*3-XuT6|p*%{t^`u2>(w55f-~a6_%3t9Tv6&A%X+(J;Vh8 zTHIw|G9aIl+KKpG#5#+T1&KEh_p55uXqm{SkvxeSSa+&M11? z?T6wUEbvX{EGe3jnHQb)7bWretkbNkjjDTi5{^JysF76}Z7?lYGOrb*Wz- zLZk-^YlDeFAut^X`VBl5a`?3M!!P`L#wNE;$<@fwO0M>I;A)``eg%nj0)2S@FA-BV zia&>R4?CaaS!SWzTLS|!y@|(?JW9&(kju$W95IA^P7VQWU#29VVTyJQ<~&&@_xq-X z_EepU_(kdzJP0!UT>XmptJxP)z*mi$1o~4A?Mz>mc9E=BaHXOZ$;qrEm$raO9k!Fk z{zv48+f``px3jtmDf?q2j@0szE7N9HIgz$9w@5dRs;Z2X?Pcj{3s3$XJhma0p$njA zgW8}@4qV2YbMe-ODY@_hIAbkqh86N?jA>!8NR1|4${OI57C2=Cyi(GDRTY2C_PKO< zaK7Mv)F&Kib@)XI2g($ztPZr@B`0KQ3j2WWtza)Ayx~N~FqHKuJVLUPia|*UCsOJL zJH#L2jy(S(@WQj$XIV{&ngk#PN)$Q;%@D9N3!a-!RlLhQiB=^F2?}rGJq9$rs#8ah z>8waeQpuyIjv*}zb9i%)Ny?ftA@wvWP`wpCgAW)Gd&G|z7z`vxv?NIQ?OwbK1HsJn z>9@Ax@tmDj!3ZT%Ld74k{;BTGfT1F711QEp12}GWi8i!}&QyOvA@gtXBtJkvmEXZ> zre$&k=e`9jFreg5Pd`zD`4;kd>f5q}G7~VRaKXj<@Jnam!700Li3t?@OHBAVFab#L z6YNc;`KJ?T%di_l-$#C^^nWSozAxojNRS|L3upIEmi@o>&&UjMIWxuOccgs#q=r*&iVKjkoXLy`9m zBN2PO81_XP<&&wfFcoP}OAS-mSE}uML4O!L?hpGZ)cXyORyl}Q6!jA2BcV*!G49=) zi5MMo9X};}P`SL zf!Idp{JKo3YPl`>s9(E}65p2{^z8?%`gUTcC zFMR*a*Vf;7owmydZ@uy6L%3aNWE}P}pQWm+1RIGM6m0y(VYG3oo7R0~`Azns{OGr4 z#P7d;?;)m$DzE~Mnk=>fkO^U9l;f5O@hXbsO$8W*G1qU)q!b&KCDL!moI;mAjU;qZ z{3{+MQ8+*mV~NHpT@!b7_{((dN7~5v zp)#%;hktzX0F*W}#puYR<_$-R*9G2Ye-BoZsp+^R0hcKZCp5=jE9~<;3nU^H0cbum zU7)PA-l<5Vi108jCE%U}{oMV-=-_oBV$reGYD*cEiz>dx;1uY8ggg@mC?V)-HbprD zc3m#R;Y(*ud$Bu9j}&<;r(8}qrGMHcIKhz4Al*=+!Ni+$xsc?FTq}hw0Ny8H;zg7K z${2?~DhGSFu(5kv(XK$e-}7)Xdb`7b3L%7YOdyO{%71{#D=R`#p+|5{xh>0V#r_>} z2b~ljG9Ww7Z7N6!v=ewYhMO=M!PMWe)#Q~dRJ_UZZ!tJIgZVj_hLkU$TlTZf8Ff%t zvsb_-55IiYOg>+&oKse<;E{96mce^qsiFNCZ%CA|%9s-?)v=8=%rjj0nZAIm3mN#{F6NgJ{8>^Cgt@cIh+q zUb|W>iZaqp2Rd8F8LAib2=E@Pj#ywirtTMhmcrOerhqBNVVRm3$_D2;@tf;|1Vp9h z2`JZ?Y+n+P1BUr}Y8frG=ft~$Oo*V;)?2E#C41^vxs$}Z0zM)KBL$B8 z)p9%K&eMnptrTfTe1r<(hY0){{Lz7sel*1-Hu&kw2s)11!8e%kRRp;$r!Y4$4dNB% zp4`ad3&>mL#%&;?RaXq^bJX?D*627;4=lcnL9*T-#mIg zaop6Mi8+yIy%d|t`v@B3?}>+iuCos>PB6`!Jcc3M-E`&uneoH|5a@C<$vx09a%1hQ z;`dPv*|V*&f{f2CwB<}JW{b1&K{vXZwP*8I++SG?;dhw8 zGjLQ*YxKotOUFhCF9SI!Zd(9biu{(!pPWk@M@{6l@GQ{NaBQTtLHlTt{@x-6-|e#Y zBDBA-ehfdu1Re*B<6qcHh%|Wfa%Re#BEP~sE={bFj}nG(vU4ds6wh>gLki!P@$I(! zb_w6k%WquUOZc{eZ^*@}vh>%A(ruZ;GV4VdSIrtqmoeZau9sy5hQ?_z_tYH{Lfr{K z@~%9X-QFw2vc4eCrXZV?;2GfV12}MT^xc!K`_1xF9C0ddOmBur)KYgk7!}`Qo8?!|EKy8J!;!ojBVrb*rckCO zs^vy+5BBjrza~c{ueA8?h?2zPSon2_$S}PXMH3X0DxEBgYm&IBC;d{Vr3B&=bR#X{ zOlS@*J}w?!1mSfLT8f~_;a(f9Krcg$pe@J+C@w+QQ*VBub(z;kTwE1#WIb1YXBpa- z@EA4I1)ak$o&}^QE(4vAKZu; zHmD_N;8szoGl*}agk-%^@m8+uQ-ax56!|fyoO-o}$>C(GX|ga)hgZ&;AZcKm-2;+M zuXmPsA#oJ!MBQ#)>vq+hUBnyR?vveMSbV8;yJ6hxcEtsD?mG)!>ysAMlE@@G1yyJ=hCDi6%i!&D6I^7uEVO|`LWY?IwXPL^Iu zN|lr;zf4uRDpWm9xEBOL2dp3f9V7Ym{$=08`h1))Dy>WhbMbGz+@W-kzfu4^^|KXN=jG##@W zG@P#4?>fHkN;3+~zAKx7?BT)wTG!okyXL-g&<{L6GMhmcwVvNHcbw1_X5gC-@89Op zBp$gkTsL3gZI!wj>v#M?57(k_pAXzEl$M{kZLiUF(d8EAPkPj~xF)2tZ)cN)@l4B& z?6B{;qCsP29~Fg7{K9J>u41X+sU}pbAT&!C1@KP??}}Dglq^G(tr=lh6)}Uiuf&x{ zfg?WZM_%9`4k{)-qY6{Hed&g-ABASrcFleehTaaC6b1dD8?-#)4;a?;S_A3O5TUu> zc75=2k66)^K@WTk-Bu4{m_gG7FR8Z~U^uy6;rU+u0-sm(4zDyK1tKIOuZl!UAY1j> z*syKi>AAKYS8O}kER?IZ{e0kb(1^@9OO%Zskx z97KU1^nyXSmTutMmb2%zm=$Y33c-W5+pgPRd*C|Kr@NjLMy|Z>w1}tI{k-vYmNh$- zwe|jiEMwM1{K5qgL)X>uFF&0S0b<%FHQxgn7XeRcLb*f%0`| z>H|`lhl$2G|BOB59UiYWyjgxQ!@~2lO;o z21uk6nNVEt(uMS~(yc+R)|%aeRJ67sv?FEh1tg+02nq9EqH;7sBk1)9G{DY*IiOuL zqy1o7f3NXk0?03V3`oae3wWe1CYOmOkm{O_FSwqCeAFuNAu|Kv`D0D5HsrtyU6XaX zp04^StI(hudEk`UccM0|k#tRG&+)nr>EBhD9GDbvg>ST{om>EpHFX+Uf>7shu<14(hwob;JSo1P`&WhS&65vCgmGAUUHCT}eG} z;TQe^HhQQI6Zn`c zx&?7Atvippb6mG5Q08{mPG!(_RG5?-HtTR- zQ7}D3(2s24HKGlZC|Jk9bPFBW8K|-?9pMc^T67pW zeq%H%8G(^C!D87%Q7xI_iNR70w+S3T!&5fwIo$ztEVU<@6_|AUlf#UefUh2Ftl^X$ zni#O>ys)FT45Fy(ZkV_OBYUt0Neyo?RF^h+%;LQ^!j6Qk7<D zIwJ(lX6Uj&ow7GFfK49Tq0Fo{AX~s=Ioo|Vf1@pL;R-{n5_r*d{Z|6!>2B*U{+vEP zyV1fOKHu3)3(98e+x+>(jkL`_-l*$V;Y0f&k^4k$$C`-J_FvwR1XI$x|GAO&{@1Mc zt84~iQ@(|UR&`u>t@$aLw3bhkD{Hkow)+!e5?9AJJf5LD-mT<8#`(CE@YPx*ENb^W z9|D^&I6jkj*91h`U9aawVf{S7Rq75Ch`ArguCPirU*j_AU(e~sI@2>*+iYz>xAA=X zrkO&;OKF7!ytH9Dpxlz2M!Yb>vw&{yz>P2P60@F3rH$QdMeX>U9Rh6TUF-&Y6OCmR zY!StC_Bewv^c08VB^%Zwv@<@F;lhwf-8h%|K?Sdu>Ju>Lh`C9bxf_@5gtr*^b>=SH zr*Idm@_a8EV^+No>!I6i#^sFZMrK<_U{hWsW&!kaXZu%h7d`_~Y9+O*R<)9fuT~{m z;Ig`eZ&G54T0^Z&{3^JA!?$pFb{wUxXY)2l;ORqGg6Hh;DYRoZp12(C@a!Xm2@zWv zw-v0V?}gAt{t}wvvVqI=%gs@25oOZkqeVzFDB9tLiM32L%NImVXYk_%{RD-G&=B$nsb(S=Dh-vld?yGWiY`DNQ2ZB=R=MR((lQ zsKacMze=TFBl12Ggw2Zl4v}9cQYUg1B(9P|X8MxckrQVfD^sg;M1GkF$&IA2FfPI2 zyS}Jvych98V<5>(=i^N6j{F|=#4lV1Q6O)l0Hj)2QWw<2%M-gaE#(c~OX`~bc%#na z25ePiEmDR060Bw5sHF_v%}x2Ow&S4a+B(;fTgnj%&lxu$&m7=kGmLIA>Kn{50p_O(9tJGjjlqN%mqa zY-Tr`2A5-+v%6uogMDnUe=r8K&^?WZ^EqDIC24?ZjD?u7TlIOiG4e91;{sfFTmb5b ziwHhjZmf5KK4RT>Uqtd-)G@bF@uCf_lpb=-t&7RPP?7_j0gp#yG6`C?BD z6aFby7MdW6v7}bCnp#sA;FxtdX+t#%8oOZD*>@+z^bNx`$RE&$cuZxsn};Y23zA$C z2iU3t*@DbvGJ6ivI3$M$*SD+7Cy-eL-CsZ^U!12}NN)`iRXi>ZOXSd=fACa+v>{~2 zbCT@^NFR+|B7zlI z%;+`EZ8>nSC27WeBv`JQUSvA7YFtwV3A&V~9R&|O)#+&SN@5x5Vd9Y-Wd3?^xD5oN z2_U8SN^>o_D!yg{GoMRL+sN5*_08<4=Pb{>-Kl7J+Nvfo{}|h*8%+B`SY6WHt$H~D z3q|THslbVau35s|76YZ#YRLV!z&nuz`hw+D^2sc4apbFL0;1-IVT4(-59n#Ey z4fQU57fS!U^H6!-fwCgI3&sDp7Z;)hxD`@qQh3a9yhypB=l}3j$>u;y%7xXUrQ3)hX`;Ra{^c5xZ6cdQ2uI5Af>?8t=>a286wH7#en`h%u|fO7 z!Nq-aiOc-}dFf^p7ujybwI@j?J&ldxViGAESsE%XCOL`<5WPtA6^j5JIJU7{urhYo z_L|XHxI%$WY+$<)`{MwLkeYlCBy;~bH^atQRYaL~wlr%ljX4vvY%Sx6ZDduXixVkV z#74Gd@^@&C>O^X!oM!i?Y>sr%^k47`zYn4q2)_#}YOPRVKd-~eAnevkpcPO9wY9Nm-4@ci2+{_Ozmp^fa3|o zeGEFj;>~mcpK&37tJ2}(#8%K#M3LLz3&nYLZ5zeAxFa<~pfZn(Tp&Wnt8$6RZxZ={ z2xmR+P>Ev9#AH(1jje$6sT2_D6S*)+C}Srtfl%%@q)dlV(Y5L=LpSD)YGtPKR^?*l z{mO!&bE{nCNd|KCLBVee2;#;1kU;x;Q0&4nLe3Ll2@W}-{v0+Ife8?9GFYz=Sf>n? z^WWeLbM#hnrpdY$9rWR7o8*i9RJF6CmvKpB$2Q8i=jWI>oJo!%Gg|V8vuF*#rgmHBvy7CibN0+MX7 zAER~QB&dO)fL=CVb&`BeLW*?w47O$+H*#&8;sL*1F0onsGQIZE|0FVeatg=!&k(fw zsVN8^X}2zLa<5id_fw-mhlyJM2hLjC})-ysdGqM%d&y>%EzwWdjW^6DLs{G^zqyMP}X zl^qm@DkWPw_myd#0M?t{qpQ;rQ)lgPvXo;cNW3>!Cpeg$>c3I+ zrshgSE{WhK2Ew7~q_|HCCy7=jc5isW2}PU|;!$3dT<%yzzMlPWhNt6)Qw`{kGjDvz z|DxfiiO7DomxA2Snwziw7UGarKiQ<<^J7#1`UMf=&A4#j>?i)6VmwKiXn7W-USjcO zsf}j#C-k$S?x)W}4$;66v^?~&kgQoD-d5yXiGvNg-VYrt%p zq!w@%5nUG9J0KE;fs*7H. + +""" +This module implements an agent that roams around a track following random +waypoints and avoiding other vehicles. The agent also responds to traffic lights. +It can also make use of the global route planner to follow a specifed route +""" + +import carla +from enum import Enum +from shapely.geometry import Polygon + +from agents.navigation.local_planner import LocalPlanner +from agents.navigation.global_route_planner import GlobalRoutePlanner +from agents.tools.misc import get_speed, is_within_distance, get_trafficlight_trigger_location, compute_distance + + +class BasicAgent(object): + """ + BasicAgent implements an agent that navigates the scene. + This agent respects traffic lights and other vehicles, but ignores stop signs. + It has several functions available to specify the route that the agent must follow, + as well as to change its parameters in case a different driving mode is desired. + """ + + def __init__(self, vehicle, target_speed=20, opt_dict={}): + """ + Initialization the agent paramters, the local and the global planner. + + :param vehicle: actor to apply to agent logic onto + :param target_speed: speed (in Km/h) at which the vehicle will move + :param opt_dict: dictionary in case some of its parameters want to be changed. + This also applies to parameters related to the LocalPlanner. + """ + self._vehicle = vehicle + self._world = self._vehicle.get_world() + self._map = self._world.get_map() + self._last_traffic_light = None + + # Base parameters + self._ignore_traffic_lights = False + self._ignore_stop_signs = False + self._ignore_vehicles = False + self._target_speed = target_speed + self._sampling_resolution = 2.0 + self._base_tlight_threshold = 5.0 # meters + self._base_vehicle_threshold = 5.0 # meters + self._max_brake = 0.5 + + # Change parameters according to the dictionary + opt_dict['target_speed'] = target_speed + if 'ignore_traffic_lights' in opt_dict: + self._ignore_traffic_lights = opt_dict['ignore_traffic_lights'] + if 'ignore_stop_signs' in opt_dict: + self._ignore_stop_signs = opt_dict['ignore_stop_signs'] + if 'ignore_vehicles' in opt_dict: + self._ignore_vehicles = opt_dict['ignore_vehicles'] + if 'sampling_resolution' in opt_dict: + self._sampling_resolution = opt_dict['sampling_resolution'] + if 'base_tlight_threshold' in opt_dict: + self._base_tlight_threshold = opt_dict['base_tlight_threshold'] + if 'base_vehicle_threshold' in opt_dict: + self._base_vehicle_threshold = opt_dict['base_vehicle_threshold'] + if 'max_brake' in opt_dict: + self._max_steering = opt_dict['max_brake'] + + # Initialize the planners + self._local_planner = LocalPlanner(self._vehicle, opt_dict=opt_dict) + self._global_planner = GlobalRoutePlanner(self._map, self._sampling_resolution) + + def add_emergency_stop(self, control): + """ + Overwrites the throttle a brake values of a control to perform an emergency stop. + The steering is kept the same to avoid going out of the lane when stopping during turns + + :param speed (carl.VehicleControl): control to be modified + """ + control.throttle = 0.0 + control.brake = self._max_brake + control.hand_brake = False + return control + + def set_target_speed(self, speed): + """ + Changes the target speed of the agent + :param speed (float): target speed in Km/h + """ + self._local_planner.set_speed(speed) + + def follow_speed_limits(self, value=True): + """ + If active, the agent will dynamically change the target speed according to the speed limits + + :param value (bool): whether or not to activate this behavior + """ + self._local_planner.follow_speed_limits(value) + + def get_local_planner(self): + """Get method for protected member local planner""" + return self._local_planner + + def get_global_planner(self): + """Get method for protected member local planner""" + return self._global_planner + + def set_destination(self, end_location, start_location=None): + """ + This method creates a list of waypoints between a starting and ending location, + based on the route returned by the global router, and adds it to the local planner. + If no starting location is passed, the vehicle local planner's target location is chosen, + which corresponds (by default), to a location about 5 meters in front of the vehicle. + + :param end_location (carla.Location): final location of the route + :param start_location (carla.Location): starting location of the route + """ + if not start_location: + start_location = self._local_planner.target_waypoint.transform.location + clean_queue = True + else: + start_location = self._vehicle.get_location() + clean_queue = False + + start_waypoint = self._map.get_waypoint(start_location) + end_waypoint = self._map.get_waypoint(end_location) + + route_trace = self.trace_route(start_waypoint, end_waypoint) + self._local_planner.set_global_plan(route_trace, clean_queue=clean_queue) + + def set_global_plan(self, plan, stop_waypoint_creation=True, clean_queue=True): + """ + Adds a specific plan to the agent. + + :param plan: list of [carla.Waypoint, RoadOption] representing the route to be followed + :param stop_waypoint_creation: stops the automatic random creation of waypoints + :param clean_queue: resets the current agent's plan + """ + self._local_planner.set_global_plan( + plan, + stop_waypoint_creation=stop_waypoint_creation, + clean_queue=clean_queue + ) + + def trace_route(self, start_waypoint, end_waypoint): + """ + Calculates the shortest route between a starting and ending waypoint. + + :param start_waypoint (carla.Waypoint): initial waypoint + :param end_waypoint (carla.Waypoint): final waypoint + """ + start_location = start_waypoint.transform.location + end_location = end_waypoint.transform.location + return self._global_planner.trace_route(start_location, end_location) + + def run_step(self): + """Execute one step of navigation.""" + hazard_detected = False + + # Retrieve all relevant actors + actor_list = self._world.get_actors() + vehicle_list = actor_list.filter("*vehicle*") + lights_list = actor_list.filter("*traffic_light*") + + vehicle_speed = get_speed(self._vehicle) / 3.6 + + # Check for possible vehicle obstacles + max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed + affected_by_vehicle, _, _ = self._vehicle_obstacle_detected(vehicle_list, max_vehicle_distance) + if affected_by_vehicle: + hazard_detected = True + + # Check if the vehicle is affected by a red traffic light + max_tlight_distance = self._base_tlight_threshold + vehicle_speed + affected_by_tlight, _ = self._affected_by_traffic_light(lights_list, max_tlight_distance) + if affected_by_tlight: + hazard_detected = True + + control = self._local_planner.run_step() + if hazard_detected: + control = self.add_emergency_stop(control) + + return control + + def done(self): + """Check whether the agent has reached its destination.""" + return self._local_planner.done() + + def ignore_traffic_lights(self, active=True): + """(De)activates the checks for traffic lights""" + self._ignore_traffic_lights = active + + def ignore_stop_signs(self, active=True): + """(De)activates the checks for stop signs""" + self._ignore_stop_signs = active + + def ignore_vehicles(self, active=True): + """(De)activates the checks for stop signs""" + self._ignore_vehicles = active + + def _affected_by_traffic_light(self, lights_list=None, max_distance=None): + """ + Method to check if there is a red light affecting the vehicle. + + :param lights_list (list of carla.TrafficLight): list containing TrafficLight objects. + If None, all traffic lights in the scene are used + :param max_distance (float): max distance for traffic lights to be considered relevant. + If None, the base threshold value is used + """ + if self._ignore_traffic_lights: + return (False, None) + + if not lights_list: + lights_list = self._world.get_actors().filter("*traffic_light*") + + if not max_distance: + max_distance = self._base_tlight_threshold + + if self._last_traffic_light: + if self._last_traffic_light.state != carla.TrafficLightState.Red: + self._last_traffic_light = None + else: + return (True, self._last_traffic_light) + + ego_vehicle_location = self._vehicle.get_location() + ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) + + for traffic_light in lights_list: + object_location = get_trafficlight_trigger_location(traffic_light) + object_waypoint = self._map.get_waypoint(object_location) + + if object_waypoint.road_id != ego_vehicle_waypoint.road_id: + continue + + ve_dir = ego_vehicle_waypoint.transform.get_forward_vector() + wp_dir = object_waypoint.transform.get_forward_vector() + dot_ve_wp = ve_dir.x * wp_dir.x + ve_dir.y * wp_dir.y + ve_dir.z * wp_dir.z + + if dot_ve_wp < 0: + continue + + if traffic_light.state != carla.TrafficLightState.Red: + continue + + if is_within_distance(object_waypoint.transform, self._vehicle.get_transform(), max_distance, [0, 90]): + self._last_traffic_light = traffic_light + return (True, traffic_light) + + return (False, None) + + def _vehicle_obstacle_detected(self, vehicle_list=None, max_distance=None, up_angle_th=90, low_angle_th=0, lane_offset=0): + """ + Method to check if there is a vehicle in front of the agent blocking its path. + + :param vehicle_list (list of carla.Vehicle): list contatining vehicle objects. + If None, all vehicle in the scene are used + :param max_distance: max freespace to check for obstacles. + If None, the base threshold value is used + """ + if self._ignore_vehicles: + return (False, None, -1) + + if not vehicle_list: + vehicle_list = self._world.get_actors().filter("*vehicle*") + + if not max_distance: + max_distance = self._base_vehicle_threshold + + ego_transform = self._vehicle.get_transform() + ego_wpt = self._map.get_waypoint(self._vehicle.get_location()) + + # Get the right offset + if ego_wpt.lane_id < 0 and lane_offset != 0: + lane_offset *= -1 + + # Get the transform of the front of the ego + ego_forward_vector = ego_transform.get_forward_vector() + ego_extent = self._vehicle.bounding_box.extent.x + ego_front_transform = ego_transform + ego_front_transform.location += carla.Location( + x=ego_extent * ego_forward_vector.x, + y=ego_extent * ego_forward_vector.y, + ) + + for target_vehicle in vehicle_list: + target_transform = target_vehicle.get_transform() + target_wpt = self._map.get_waypoint(target_transform.location, lane_type=carla.LaneType.Any) + + # Simplified version for outside junctions + if not ego_wpt.is_junction or not target_wpt.is_junction: + + if target_wpt.road_id != ego_wpt.road_id or target_wpt.lane_id != ego_wpt.lane_id + lane_offset: + next_wpt = self._local_planner.get_incoming_waypoint_and_direction(steps=3)[0] + if not next_wpt: + continue + if target_wpt.road_id != next_wpt.road_id or target_wpt.lane_id != next_wpt.lane_id + lane_offset: + continue + + target_forward_vector = target_transform.get_forward_vector() + target_extent = target_vehicle.bounding_box.extent.x + target_rear_transform = target_transform + target_rear_transform.location -= carla.Location( + x=target_extent * target_forward_vector.x, + y=target_extent * target_forward_vector.y, + ) + + if is_within_distance(target_rear_transform, ego_front_transform, max_distance, [low_angle_th, up_angle_th]): + return (True, target_vehicle, compute_distance(target_transform.location, ego_transform.location)) + + # Waypoints aren't reliable, check the proximity of the vehicle to the route + else: + route_bb = [] + ego_location = ego_transform.location + extent_y = self._vehicle.bounding_box.extent.y + r_vec = ego_transform.get_right_vector() + p1 = ego_location + carla.Location(extent_y * r_vec.x, extent_y * r_vec.y) + p2 = ego_location + carla.Location(-extent_y * r_vec.x, -extent_y * r_vec.y) + route_bb.append([p1.x, p1.y, p1.z]) + route_bb.append([p2.x, p2.y, p2.z]) + + for wp, _ in self._local_planner.get_plan(): + if ego_location.distance(wp.transform.location) > max_distance: + break + + r_vec = wp.transform.get_right_vector() + p1 = wp.transform.location + carla.Location(extent_y * r_vec.x, extent_y * r_vec.y) + p2 = wp.transform.location + carla.Location(-extent_y * r_vec.x, -extent_y * r_vec.y) + route_bb.append([p1.x, p1.y, p1.z]) + route_bb.append([p2.x, p2.y, p2.z]) + + if len(route_bb) < 3: + # 2 points don't create a polygon, nothing to check + return (False, None, -1) + ego_polygon = Polygon(route_bb) + + # Compare the two polygons + for target_vehicle in vehicle_list: + target_extent = target_vehicle.bounding_box.extent.x + if target_vehicle.id == self._vehicle.id: + continue + if ego_location.distance(target_vehicle.get_location()) > max_distance: + continue + + target_bb = target_vehicle.bounding_box + target_vertices = target_bb.get_world_vertices(target_vehicle.get_transform()) + target_list = [[v.x, v.y, v.z] for v in target_vertices] + target_polygon = Polygon(target_list) + + if ego_polygon.intersects(target_polygon): + return (True, target_vehicle, compute_distance(target_vehicle.get_location(), ego_location)) + + return (False, None, -1) + + return (False, None, -1) 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/agents/navigation/behavior_agent.py new file mode 100644 index 0000000000..2a50718245 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/behavior_agent.py @@ -0,0 +1,319 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + + +""" This module implements an agent that roams around a track following random +waypoints and avoiding other vehicles. The agent also responds to traffic lights, +traffic signs, and has different possible configurations. """ + +import random +import numpy as np +import carla +from agents.navigation.basic_agent import BasicAgent +from agents.navigation.local_planner import RoadOption +from agents.navigation.behavior_types import Cautious, Aggressive, Normal + +from agents.tools.misc import get_speed, positive, is_within_distance, compute_distance + +class BehaviorAgent(BasicAgent): + """ + BehaviorAgent implements an agent that navigates scenes to reach a given + target destination, by computing the shortest possible path to it. + This agent can correctly follow traffic signs, speed limitations, + traffic lights, while also taking into account nearby vehicles. Lane changing + decisions can be taken by analyzing the surrounding environment such as tailgating avoidance. + Adding to these are possible behaviors, the agent can also keep safety distance + from a car in front of it by tracking the instantaneous time to collision + and keeping it in a certain range. Finally, different sets of behaviors + are encoded in the agent, from cautious to a more aggressive ones. + """ + + def __init__(self, vehicle, behavior='normal'): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param ignore_traffic_light: boolean to ignore any traffic light + :param behavior: type of agent to apply + """ + + super(BehaviorAgent, self).__init__(vehicle) + self._look_ahead_steps = 0 + + # Vehicle information + self._speed = 0 + self._speed_limit = 0 + self._direction = None + self._incoming_direction = None + self._incoming_waypoint = None + self._min_speed = 5 + self._behavior = None + self._sampling_resolution = 4.5 + + # Parameters for agent behavior + if behavior == 'cautious': + self._behavior = Cautious() + + elif behavior == 'normal': + self._behavior = Normal() + + elif behavior == 'aggressive': + self._behavior = Aggressive() + + def _update_information(self): + """ + This method updates the information regarding the ego + vehicle based on the surrounding world. + """ + self._speed = get_speed(self._vehicle) + self._speed_limit = self._vehicle.get_speed_limit() + self._local_planner.set_speed(self._speed_limit) + self._direction = self._local_planner.target_road_option + if self._direction is None: + self._direction = RoadOption.LANEFOLLOW + + self._look_ahead_steps = int((self._speed_limit) / 10) + + self._incoming_waypoint, self._incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( + steps=self._look_ahead_steps) + if self._incoming_direction is None: + self._incoming_direction = RoadOption.LANEFOLLOW + + def traffic_light_manager(self): + """ + This method is in charge of behaviors for red lights. + """ + actor_list = self._world.get_actors() + lights_list = actor_list.filter("*traffic_light*") + affected, _ = self._affected_by_traffic_light(lights_list) + + return affected + + def _tailgating(self, waypoint, vehicle_list): + """ + This method is in charge of tailgating behaviors. + + :param location: current location of the agent + :param waypoint: current waypoint of the agent + :param vehicle_list: list of all the nearby vehicles + """ + + left_turn = waypoint.left_lane_marking.lane_change + right_turn = waypoint.right_lane_marking.lane_change + + left_wpt = waypoint.get_left_lane() + right_wpt = waypoint.get_right_lane() + + behind_vehicle_state, behind_vehicle, _ = self._vehicle_obstacle_detected(vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, low_angle_th=160) + if behind_vehicle_state and self._speed < get_speed(behind_vehicle): + if (right_turn == carla.LaneChange.Right or right_turn == + carla.LaneChange.Both) and waypoint.lane_id * right_wpt.lane_id > 0 and right_wpt.lane_type == carla.LaneType.Driving: + new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=1) + if not new_vehicle_state: + print("Tailgating, moving to the right!") + end_waypoint = self._local_planner.target_waypoint + self._behavior.tailgate_counter = 200 + self.set_destination(end_waypoint.transform.location, + right_wpt.transform.location) + elif left_turn == carla.LaneChange.Left and waypoint.lane_id * left_wpt.lane_id > 0 and left_wpt.lane_type == carla.LaneType.Driving: + new_vehicle_state, _, _ = self._vehicle_obstacle_detected(vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=-1) + if not new_vehicle_state: + print("Tailgating, moving to the left!") + end_waypoint = self._local_planner.target_waypoint + self._behavior.tailgate_counter = 200 + self.set_destination(end_waypoint.transform.location, + left_wpt.transform.location) + + def collision_and_car_avoid_manager(self, waypoint): + """ + This module is in charge of warning in case of a collision + and managing possible tailgating chances. + + :param location: current location of the agent + :param waypoint: current waypoint of the agent + :return vehicle_state: True if there is a vehicle nearby, False if not + :return vehicle: nearby vehicle + :return distance: distance to nearby vehicle + """ + + vehicle_list = self._world.get_actors().filter("*vehicle*") + def dist(v): return v.get_location().distance(waypoint.transform.location) + vehicle_list = [v for v in vehicle_list if dist(v) < 45 and v.id != self._vehicle.id] + + if self._direction == RoadOption.CHANGELANELEFT: + vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( + vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=-1) + elif self._direction == RoadOption.CHANGELANERIGHT: + vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( + vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=180, lane_offset=1) + else: + vehicle_state, vehicle, distance = self._vehicle_obstacle_detected( + vehicle_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 3), up_angle_th=30) + + # Check for tailgating + if not vehicle_state and self._direction == RoadOption.LANEFOLLOW \ + and not waypoint.is_junction and self._speed > 10 \ + and self._behavior.tailgate_counter == 0: + self._tailgating(waypoint, vehicle_list) + + return vehicle_state, vehicle, distance + + def pedestrian_avoid_manager(self, waypoint): + """ + This module is in charge of warning in case of a collision + with any pedestrian. + + :param location: current location of the agent + :param waypoint: current waypoint of the agent + :return vehicle_state: True if there is a walker nearby, False if not + :return vehicle: nearby walker + :return distance: distance to nearby walker + """ + + walker_list = self._world.get_actors().filter("*walker.pedestrian*") + def dist(w): return w.get_location().distance(waypoint.transform.location) + walker_list = [w for w in walker_list if dist(w) < 10] + + if self._direction == RoadOption.CHANGELANELEFT: + walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=90, lane_offset=-1) + elif self._direction == RoadOption.CHANGELANERIGHT: + walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 2), up_angle_th=90, lane_offset=1) + else: + walker_state, walker, distance = self._vehicle_obstacle_detected(walker_list, max( + self._behavior.min_proximity_threshold, self._speed_limit / 3), up_angle_th=60) + + return walker_state, walker, distance + + def car_following_manager(self, vehicle, distance, debug=False): + """ + Module in charge of car-following behaviors when there's + someone in front of us. + + :param vehicle: car to follow + :param distance: distance from vehicle + :param debug: boolean for debugging + :return control: carla.VehicleControl + """ + + vehicle_speed = get_speed(vehicle) + delta_v = max(1, (self._speed - vehicle_speed) / 3.6) + ttc = distance / delta_v if delta_v != 0 else distance / np.nextafter(0., 1.) + + # Under safety time distance, slow down. + if self._behavior.safety_time > ttc > 0.0: + target_speed = min([ + positive(vehicle_speed - self._behavior.speed_decrease), + self._behavior.max_speed, + self._speed_limit - self._behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) + + # Actual safety distance area, try to follow the speed of the vehicle in front. + elif 2 * self._behavior.safety_time > ttc >= self._behavior.safety_time: + target_speed = min([ + max(self._min_speed, vehicle_speed), + self._behavior.max_speed, + self._speed_limit - self._behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) + + # Normal behavior. + else: + target_speed = min([ + self._behavior.max_speed, + self._speed_limit - self._behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) + + return control + + def run_step(self, debug=False): + """ + Execute one step of navigation. + + :param debug: boolean for debugging + :return control: carla.VehicleControl + """ + self._update_information() + + control = None + if self._behavior.tailgate_counter > 0: + self._behavior.tailgate_counter -= 1 + + ego_vehicle_loc = self._vehicle.get_location() + ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc) + + # 1: Red lights and stops behavior + if self.traffic_light_manager(): + return self.emergency_stop() + + # 2.1: Pedestrian avoidance behaviors + walker_state, walker, w_distance = self.pedestrian_avoid_manager(ego_vehicle_wp) + + if walker_state: + # Distance is computed from the center of the two cars, + # we use bounding boxes to calculate the actual distance + distance = w_distance - max( + walker.bounding_box.extent.y, walker.bounding_box.extent.x) - max( + self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) + + # Emergency brake if the car is very close. + if distance < self._behavior.braking_distance: + return self.emergency_stop() + + # 2.2: Car following behaviors + vehicle_state, vehicle, distance = self.collision_and_car_avoid_manager(ego_vehicle_wp) + + if vehicle_state: + # Distance is computed from the center of the two cars, + # we use bounding boxes to calculate the actual distance + distance = distance - max( + vehicle.bounding_box.extent.y, vehicle.bounding_box.extent.x) - max( + self._vehicle.bounding_box.extent.y, self._vehicle.bounding_box.extent.x) + + # Emergency brake if the car is very close. + if distance < self._behavior.braking_distance: + return self.emergency_stop() + else: + control = self.car_following_manager(vehicle, distance) + + # 3: Intersection behavior + elif self._incoming_waypoint.is_junction and (self._incoming_direction in [RoadOption.LEFT, RoadOption.RIGHT]): + target_speed = min([ + self._behavior.max_speed, + self._speed_limit - 5]) + self._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) + + # 4: Normal behavior + else: + target_speed = min([ + self._behavior.max_speed, + self._speed_limit - self._behavior.speed_lim_dist]) + self._local_planner.set_speed(target_speed) + control = self._local_planner.run_step(debug=debug) + + return control + + def emergency_stop(self): + """ + Overwrites the throttle a brake values of a control to perform an emergency stop. + The steering is kept the same to avoid going out of the lane when stopping during turns + + :param speed (carl.VehicleControl): control to be modified + """ + control = carla.VehicleControl() + control.throttle = 0.0 + control.brake = self._max_brake + control.hand_brake = False + return control \ No newline at end of file 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/agents/navigation/behavior_types.py new file mode 100644 index 0000000000..3008f9f73b --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/behavior_types.py @@ -0,0 +1,37 @@ +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" This module contains the different parameters sets for each behavior. """ + + +class Cautious(object): + """Class for Cautious agent.""" + max_speed = 40 + speed_lim_dist = 6 + speed_decrease = 12 + safety_time = 3 + min_proximity_threshold = 12 + braking_distance = 6 + tailgate_counter = 0 + + +class Normal(object): + """Class for Normal agent.""" + max_speed = 50 + speed_lim_dist = 3 + speed_decrease = 10 + safety_time = 3 + min_proximity_threshold = 10 + braking_distance = 5 + tailgate_counter = 0 + + +class Aggressive(object): + """Class for Aggressive agent.""" + max_speed = 70 + speed_lim_dist = 1 + speed_decrease = 8 + safety_time = 3 + min_proximity_threshold = 8 + braking_distance = 4 + tailgate_counter = -1 diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/controller.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/controller.py new file mode 100644 index 0000000000..a61803a9f3 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/controller.py @@ -0,0 +1,258 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" This module contains PID controllers to perform lateral and longitudinal control. """ + +from collections import deque +import math +import numpy as np +import carla +from agents.tools.misc import get_speed + + +class VehiclePIDController(): + """ + VehiclePIDController is the combination of two PID controllers + (lateral and longitudinal) to perform the + low level control a vehicle from client side + """ + + + def __init__(self, vehicle, args_lateral, args_longitudinal, offset=0, max_throttle=0.75, max_brake=0.3, + max_steering=0.8): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param args_lateral: dictionary of arguments to set the lateral PID controller + using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + :param args_longitudinal: dictionary of arguments to set the longitudinal + PID controller using the following semantics: + K_P -- Proportional term + K_D -- Differential term + K_I -- Integral term + :param offset: If different than zero, the vehicle will drive displaced from the center line. + Positive values imply a right offset while negative ones mean a left one. Numbers high enough + to cause the vehicle to drive through other lanes might break the controller. + """ + + self.max_brake = max_brake + self.max_throt = max_throttle + self.max_steer = max_steering + + self._vehicle = vehicle + self._world = self._vehicle.get_world() + self.past_steering = self._vehicle.get_control().steer + self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal) + self._lat_controller = PIDLateralController(self._vehicle, offset, **args_lateral) + + def run_step(self, target_speed, waypoint): + """ + Execute one step of control invoking both lateral and longitudinal + PID controllers to reach a target waypoint + at a given target_speed. + + :param target_speed: desired vehicle speed + :param waypoint: target location encoded as a waypoint + :return: distance (in meters) to the waypoint + """ + + acceleration = self._lon_controller.run_step(target_speed) + current_steering = self._lat_controller.run_step(waypoint) + control = carla.VehicleControl() + if acceleration >= 0.0: + control.throttle = min(acceleration, self.max_throt) + control.brake = 0.0 + else: + control.throttle = 0.0 + control.brake = min(abs(acceleration), self.max_brake) + + # Steering regulation: changes cannot happen abruptly, can't steer too much. + + if current_steering > self.past_steering + 0.1: + current_steering = self.past_steering + 0.1 + elif current_steering < self.past_steering - 0.1: + current_steering = self.past_steering - 0.1 + + if current_steering >= 0: + steering = min(self.max_steer, current_steering) + else: + steering = max(-self.max_steer, current_steering) + + control.steer = steering + control.hand_brake = False + control.manual_gear_shift = False + self.past_steering = steering + + return control + + + def change_longitudinal_PID(self, args_longitudinal): + """Changes the parameters of the PIDLongitudinalController""" + self._lon_controller.change_parameters(**args_longitudinal) + + def change_lateral_PID(self, args_lateral): + """Changes the parameters of the PIDLongitudinalController""" + self._lon_controller.change_parameters(**args_lateral) + + +class PIDLongitudinalController(): + """ + PIDLongitudinalController implements longitudinal control using a PID. + """ + + def __init__(self, vehicle, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param K_P: Proportional term + :param K_D: Differential term + :param K_I: Integral term + :param dt: time differential in seconds + """ + self._vehicle = vehicle + self._k_p = K_P + self._k_i = K_I + self._k_d = K_D + self._dt = dt + self._error_buffer = deque(maxlen=10) + + def run_step(self, target_speed, debug=False): + """ + Execute one step of longitudinal control to reach a given target speed. + + :param target_speed: target speed in Km/h + :param debug: boolean for debugging + :return: throttle control + """ + current_speed = get_speed(self._vehicle) + + if debug: + print('Current speed = {}'.format(current_speed)) + + return self._pid_control(target_speed, current_speed) + + def _pid_control(self, target_speed, current_speed): + """ + Estimate the throttle/brake of the vehicle based on the PID equations + + :param target_speed: target speed in Km/h + :param current_speed: current speed of the vehicle in Km/h + :return: throttle/brake control + """ + + error = target_speed - current_speed + self._error_buffer.append(error) + + if len(self._error_buffer) >= 2: + _de = (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt + _ie = sum(self._error_buffer) * self._dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + + def change_parameters(self, K_P, K_I, K_D, dt): + """Changes the PID parameters""" + self._k_p = K_P + self._k_i = K_I + self._k_d = K_D + self._dt = dt + + +class PIDLateralController(): + """ + PIDLateralController implements lateral control using a PID. + """ + + def __init__(self, vehicle, offset=0, K_P=1.0, K_I=0.0, K_D=0.0, dt=0.03): + """ + Constructor method. + + :param vehicle: actor to apply to local planner logic onto + :param offset: distance to the center line. If might cause issues if the value + is large enough to make the vehicle invade other lanes. + :param K_P: Proportional term + :param K_D: Differential term + :param K_I: Integral term + :param dt: time differential in seconds + """ + self._vehicle = vehicle + self._k_p = K_P + self._k_i = K_I + self._k_d = K_D + self._dt = dt + self._offset = offset + self._e_buffer = deque(maxlen=10) + + def run_step(self, waypoint): + """ + Execute one step of lateral control to steer + the vehicle towards a certain waypoin. + + :param waypoint: target waypoint + :return: steering control in the range [-1, 1] where: + -1 maximum steering to left + +1 maximum steering to right + """ + return self._pid_control(waypoint, self._vehicle.get_transform()) + + def _pid_control(self, waypoint, vehicle_transform): + """ + Estimate the steering angle of the vehicle based on the PID equations + + :param waypoint: target waypoint + :param vehicle_transform: current transform of the vehicle + :return: steering control in the range [-1, 1] + """ + # Get the ego's location and forward vector + ego_loc = vehicle_transform.location + v_vec = vehicle_transform.get_forward_vector() + v_vec = np.array([v_vec.x, v_vec.y, 0.0]) + + # Get the vector vehicle-target_wp + if self._offset != 0: + # Displace the wp to the side + w_tran = waypoint.transform + r_vec = w_tran.get_right_vector() + w_loc = w_tran.location + carla.Location(x=self._offset*r_vec.x, + y=self._offset*r_vec.y) + else: + w_loc = waypoint.transform.location + + w_vec = np.array([w_loc.x - ego_loc.x, + w_loc.y - ego_loc.y, + 0.0]) + + wv_linalg = np.linalg.norm(w_vec) * np.linalg.norm(v_vec) + if wv_linalg == 0: + _dot = 1 + else: + _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / (wv_linalg), -1.0, 1.0)) + _cross = np.cross(v_vec, w_vec) + if _cross[2] < 0: + _dot *= -1.0 + + self._e_buffer.append(_dot) + if len(self._e_buffer) >= 2: + _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt + _ie = sum(self._e_buffer) * self._dt + else: + _de = 0.0 + _ie = 0.0 + + return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + + def change_parameters(self, K_P, K_I, K_D, dt): + """Changes the PID parameters""" + self._k_p = K_P + self._k_i = K_I + self._k_d = K_D + self._dt = dt \ No newline at end of file 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/agents/navigation/global_route_planner.py new file mode 100644 index 0000000000..56c3b6de25 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/global_route_planner.py @@ -0,0 +1,392 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + + +""" +This module provides GlobalRoutePlanner implementation. +""" + +import math +import numpy as np +import networkx as nx + +import carla +from agents.navigation.local_planner import RoadOption +from agents.tools.misc import vector + +class GlobalRoutePlanner(object): + """ + This class provides a very high level route plan. + """ + + def __init__(self, wmap, sampling_resolution): + self._sampling_resolution = sampling_resolution + self._wmap = wmap + self._topology = None + self._graph = None + self._id_map = None + self._road_id_to_edge = None + + self._intersection_end_node = -1 + self._previous_decision = RoadOption.VOID + + # Build the graph + self._build_topology() + self._build_graph() + self._find_loose_ends() + self._lane_change_link() + + def trace_route(self, origin, destination): + """ + This method returns list of (carla.Waypoint, RoadOption) + from origin to destination + """ + route_trace = [] + route = self._path_search(origin, destination) + current_waypoint = self._wmap.get_waypoint(origin) + destination_waypoint = self._wmap.get_waypoint(destination) + + for i in range(len(route) - 1): + road_option = self._turn_decision(i, route) + edge = self._graph.edges[route[i], route[i+1]] + path = [] + + if edge['type'] != RoadOption.LANEFOLLOW and edge['type'] != RoadOption.VOID: + route_trace.append((current_waypoint, road_option)) + exit_wp = edge['exit_waypoint'] + n1, n2 = self._road_id_to_edge[exit_wp.road_id][exit_wp.section_id][exit_wp.lane_id] + next_edge = self._graph.edges[n1, n2] + if next_edge['path']: + closest_index = self._find_closest_in_list(current_waypoint, next_edge['path']) + closest_index = min(len(next_edge['path'])-1, closest_index+5) + current_waypoint = next_edge['path'][closest_index] + else: + current_waypoint = next_edge['exit_waypoint'] + route_trace.append((current_waypoint, road_option)) + + else: + path = path + [edge['entry_waypoint']] + edge['path'] + [edge['exit_waypoint']] + closest_index = self._find_closest_in_list(current_waypoint, path) + for waypoint in path[closest_index:]: + current_waypoint = waypoint + route_trace.append((current_waypoint, road_option)) + if len(route)-i <= 2 and waypoint.transform.location.distance(destination) < 2*self._sampling_resolution: + break + elif len(route)-i <= 2 and current_waypoint.road_id == destination_waypoint.road_id and current_waypoint.section_id == destination_waypoint.section_id and current_waypoint.lane_id == destination_waypoint.lane_id: + destination_index = self._find_closest_in_list(destination_waypoint, path) + if closest_index > destination_index: + break + + return route_trace + + def _build_topology(self): + """ + This function retrieves topology from the server as a list of + road segments as pairs of waypoint objects, and processes the + topology into a list of dictionary objects with the following attributes + + - entry (carla.Waypoint): waypoint of entry point of road segment + - entryxyz (tuple): (x,y,z) of entry point of road segment + - exit (carla.Waypoint): waypoint of exit point of road segment + - exitxyz (tuple): (x,y,z) of exit point of road segment + - path (list of carla.Waypoint): list of waypoints between entry to exit, separated by the resolution + """ + self._topology = [] + # Retrieving waypoints to construct a detailed topology + for segment in self._wmap.get_topology(): + wp1, wp2 = segment[0], segment[1] + l1, l2 = wp1.transform.location, wp2.transform.location + # Rounding off to avoid floating point imprecision + x1, y1, z1, x2, y2, z2 = np.round([l1.x, l1.y, l1.z, l2.x, l2.y, l2.z], 0) + wp1.transform.location, wp2.transform.location = l1, l2 + seg_dict = dict() + seg_dict['entry'], seg_dict['exit'] = wp1, wp2 + seg_dict['entryxyz'], seg_dict['exitxyz'] = (x1, y1, z1), (x2, y2, z2) + seg_dict['path'] = [] + endloc = wp2.transform.location + if wp1.transform.location.distance(endloc) > self._sampling_resolution: + w = wp1.next(self._sampling_resolution)[0] + while w.transform.location.distance(endloc) > self._sampling_resolution: + seg_dict['path'].append(w) + w = w.next(self._sampling_resolution)[0] + else: + seg_dict['path'].append(wp1.next(self._sampling_resolution)[0]) + self._topology.append(seg_dict) + + def _build_graph(self): + """ + This function builds a networkx graph representation of topology, creating several class attributes: + - graph (networkx.DiGraph): networkx graph representing the world map, with: + Node properties: + vertex: (x,y,z) position in world map + Edge properties: + entry_vector: unit vector along tangent at entry point + exit_vector: unit vector along tangent at exit point + net_vector: unit vector of the chord from entry to exit + intersection: boolean indicating if the edge belongs to an intersection + - id_map (dictionary): mapping from (x,y,z) to node id + - road_id_to_edge (dictionary): map from road id to edge in the graph + """ + + self._graph = nx.DiGraph() + self._id_map = dict() # Map with structure {(x,y,z): id, ... } + self._road_id_to_edge = dict() # Map with structure {road_id: {lane_id: edge, ... }, ... } + + for segment in self._topology: + entry_xyz, exit_xyz = segment['entryxyz'], segment['exitxyz'] + path = segment['path'] + entry_wp, exit_wp = segment['entry'], segment['exit'] + intersection = entry_wp.is_junction + road_id, section_id, lane_id = entry_wp.road_id, entry_wp.section_id, entry_wp.lane_id + + for vertex in entry_xyz, exit_xyz: + # Adding unique nodes and populating id_map + if vertex not in self._id_map: + new_id = len(self._id_map) + self._id_map[vertex] = new_id + self._graph.add_node(new_id, vertex=vertex) + n1 = self._id_map[entry_xyz] + n2 = self._id_map[exit_xyz] + if road_id not in self._road_id_to_edge: + self._road_id_to_edge[road_id] = dict() + if section_id not in self._road_id_to_edge[road_id]: + self._road_id_to_edge[road_id][section_id] = dict() + self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2) + + entry_carla_vector = entry_wp.transform.rotation.get_forward_vector() + exit_carla_vector = exit_wp.transform.rotation.get_forward_vector() + + # Adding edge with attributes + self._graph.add_edge( + n1, n2, + length=len(path) + 1, path=path, + entry_waypoint=entry_wp, exit_waypoint=exit_wp, + entry_vector=np.array( + [entry_carla_vector.x, entry_carla_vector.y, entry_carla_vector.z]), + exit_vector=np.array( + [exit_carla_vector.x, exit_carla_vector.y, exit_carla_vector.z]), + net_vector=vector(entry_wp.transform.location, exit_wp.transform.location), + intersection=intersection, type=RoadOption.LANEFOLLOW) + + def _find_loose_ends(self): + """ + This method finds road segments that have an unconnected end, and + adds them to the internal graph representation + """ + count_loose_ends = 0 + hop_resolution = self._sampling_resolution + for segment in self._topology: + end_wp = segment['exit'] + exit_xyz = segment['exitxyz'] + road_id, section_id, lane_id = end_wp.road_id, end_wp.section_id, end_wp.lane_id + if road_id in self._road_id_to_edge \ + and section_id in self._road_id_to_edge[road_id] \ + and lane_id in self._road_id_to_edge[road_id][section_id]: + pass + else: + count_loose_ends += 1 + if road_id not in self._road_id_to_edge: + self._road_id_to_edge[road_id] = dict() + if section_id not in self._road_id_to_edge[road_id]: + self._road_id_to_edge[road_id][section_id] = dict() + n1 = self._id_map[exit_xyz] + n2 = -1*count_loose_ends + self._road_id_to_edge[road_id][section_id][lane_id] = (n1, n2) + next_wp = end_wp.next(hop_resolution) + path = [] + while next_wp is not None and next_wp \ + and next_wp[0].road_id == road_id \ + and next_wp[0].section_id == section_id \ + and next_wp[0].lane_id == lane_id: + path.append(next_wp[0]) + next_wp = next_wp[0].next(hop_resolution) + if path: + n2_xyz = (path[-1].transform.location.x, + path[-1].transform.location.y, + path[-1].transform.location.z) + self._graph.add_node(n2, vertex=n2_xyz) + self._graph.add_edge( + n1, n2, + length=len(path) + 1, path=path, + entry_waypoint=end_wp, exit_waypoint=path[-1], + entry_vector=None, exit_vector=None, net_vector=None, + intersection=end_wp.is_junction, type=RoadOption.LANEFOLLOW) + + def _lane_change_link(self): + """ + This method places zero cost links in the topology graph + representing availability of lane changes. + """ + + for segment in self._topology: + left_found, right_found = False, False + + for waypoint in segment['path']: + if not segment['entry'].is_junction: + next_waypoint, next_road_option, next_segment = None, None, None + + if waypoint.right_lane_marking and waypoint.right_lane_marking.lane_change & carla.LaneChange.Right and not right_found: + next_waypoint = waypoint.get_right_lane() + if next_waypoint is not None \ + and next_waypoint.lane_type == carla.LaneType.Driving \ + and waypoint.road_id == next_waypoint.road_id: + next_road_option = RoadOption.CHANGELANERIGHT + next_segment = self._localize(next_waypoint.transform.location) + if next_segment is not None: + self._graph.add_edge( + self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint, + exit_waypoint=next_waypoint, intersection=False, exit_vector=None, + path=[], length=0, type=next_road_option, change_waypoint=next_waypoint) + right_found = True + if waypoint.left_lane_marking and waypoint.left_lane_marking.lane_change & carla.LaneChange.Left and not left_found: + next_waypoint = waypoint.get_left_lane() + if next_waypoint is not None \ + and next_waypoint.lane_type == carla.LaneType.Driving \ + and waypoint.road_id == next_waypoint.road_id: + next_road_option = RoadOption.CHANGELANELEFT + next_segment = self._localize(next_waypoint.transform.location) + if next_segment is not None: + self._graph.add_edge( + self._id_map[segment['entryxyz']], next_segment[0], entry_waypoint=waypoint, + exit_waypoint=next_waypoint, intersection=False, exit_vector=None, + path=[], length=0, type=next_road_option, change_waypoint=next_waypoint) + left_found = True + if left_found and right_found: + break + + def _localize(self, location): + """ + This function finds the road segment that a given location + is part of, returning the edge it belongs to + """ + waypoint = self._wmap.get_waypoint(location) + edge = None + try: + edge = self._road_id_to_edge[waypoint.road_id][waypoint.section_id][waypoint.lane_id] + except KeyError: + pass + return edge + + def _distance_heuristic(self, n1, n2): + """ + Distance heuristic calculator for path searching + in self._graph + """ + l1 = np.array(self._graph.nodes[n1]['vertex']) + l2 = np.array(self._graph.nodes[n2]['vertex']) + return np.linalg.norm(l1-l2) + + def _path_search(self, origin, destination): + """ + This function finds the shortest path connecting origin and destination + using A* search with distance heuristic. + origin : carla.Location object of start position + destination : carla.Location object of of end position + return : path as list of node ids (as int) of the graph self._graph + connecting origin and destination + """ + start, end = self._localize(origin), self._localize(destination) + + route = nx.astar_path( + self._graph, source=start[0], target=end[0], + heuristic=self._distance_heuristic, weight='length') + route.append(end[1]) + return route + + def _successive_last_intersection_edge(self, index, route): + """ + This method returns the last successive intersection edge + from a starting index on the route. + This helps moving past tiny intersection edges to calculate + proper turn decisions. + """ + + last_intersection_edge = None + last_node = None + for node1, node2 in [(route[i], route[i+1]) for i in range(index, len(route)-1)]: + candidate_edge = self._graph.edges[node1, node2] + if node1 == route[index]: + last_intersection_edge = candidate_edge + if candidate_edge['type'] == RoadOption.LANEFOLLOW and candidate_edge['intersection']: + last_intersection_edge = candidate_edge + last_node = node2 + else: + break + + return last_node, last_intersection_edge + + def _turn_decision(self, index, route, threshold=math.radians(35)): + """ + This method returns the turn decision (RoadOption) for pair of edges + around current index of route list + """ + + decision = None + previous_node = route[index-1] + current_node = route[index] + next_node = route[index+1] + next_edge = self._graph.edges[current_node, next_node] + if index > 0: + if self._previous_decision != RoadOption.VOID \ + and self._intersection_end_node > 0 \ + and self._intersection_end_node != previous_node \ + and next_edge['type'] == RoadOption.LANEFOLLOW \ + and next_edge['intersection']: + decision = self._previous_decision + else: + self._intersection_end_node = -1 + current_edge = self._graph.edges[previous_node, current_node] + calculate_turn = current_edge['type'] == RoadOption.LANEFOLLOW and not current_edge[ + 'intersection'] and next_edge['type'] == RoadOption.LANEFOLLOW and next_edge['intersection'] + if calculate_turn: + last_node, tail_edge = self._successive_last_intersection_edge(index, route) + self._intersection_end_node = last_node + if tail_edge is not None: + next_edge = tail_edge + cv, nv = current_edge['exit_vector'], next_edge['exit_vector'] + if cv is None or nv is None: + return next_edge['type'] + cross_list = [] + for neighbor in self._graph.successors(current_node): + select_edge = self._graph.edges[current_node, neighbor] + if select_edge['type'] == RoadOption.LANEFOLLOW: + if neighbor != route[index+1]: + sv = select_edge['net_vector'] + cross_list.append(np.cross(cv, sv)[2]) + next_cross = np.cross(cv, nv)[2] + deviation = math.acos(np.clip( + np.dot(cv, nv)/(np.linalg.norm(cv)*np.linalg.norm(nv)), -1.0, 1.0)) + if not cross_list: + cross_list.append(0) + if deviation < threshold: + decision = RoadOption.STRAIGHT + elif cross_list and next_cross < min(cross_list): + decision = RoadOption.LEFT + elif cross_list and next_cross > max(cross_list): + decision = RoadOption.RIGHT + elif next_cross < 0: + decision = RoadOption.LEFT + elif next_cross > 0: + decision = RoadOption.RIGHT + else: + decision = next_edge['type'] + + else: + decision = next_edge['type'] + + self._previous_decision = decision + return decision + + def _find_closest_in_list(self, current_waypoint, waypoint_list): + min_distance = float('inf') + closest_index = -1 + for i, waypoint in enumerate(waypoint_list): + distance = waypoint.transform.location.distance( + current_waypoint.transform.location) + if distance < min_distance: + min_distance = distance + closest_index = i + + return closest_index 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/agents/navigation/local_planner.py new file mode 100644 index 0000000000..08141cec55 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/local_planner.py @@ -0,0 +1,337 @@ +# Copyright (c) # Copyright (c) 2018-2020 CVC. +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +""" This module contains a local planner to perform low-level waypoint following based on PID controllers. """ + +from enum import Enum +from collections import deque +import random + +import carla +from agents.navigation.controller import VehiclePIDController +from agents.tools.misc import draw_waypoints, get_speed + + +class RoadOption(Enum): + """ + RoadOption represents the possible topological configurations when moving from a segment of lane to other. + + """ + VOID = -1 + LEFT = 1 + RIGHT = 2 + STRAIGHT = 3 + LANEFOLLOW = 4 + CHANGELANELEFT = 5 + CHANGELANERIGHT = 6 + + +class LocalPlanner(object): + """ + LocalPlanner implements the basic behavior of following a + trajectory of waypoints that is generated on-the-fly. + + The low-level motion of the vehicle is computed by using two PID controllers, + one is used for the lateral control and the other for the longitudinal control (cruise speed). + + When multiple paths are available (intersections) this local planner makes a random choice, + unless a given global plan has already been specified. + """ + + def __init__(self, vehicle, opt_dict={}): + """ + :param vehicle: actor to apply to local planner logic onto + :param opt_dict: dictionary of arguments with different parameters: + dt: time between simulation steps + target_speed: desired cruise speed in Km/h + sampling_radius: distance between the waypoints part of the plan + lateral_control_dict: values of the lateral PID controller + longitudinal_control_dict: values of the longitudinal PID controller + max_throttle: maximum throttle applied to the vehicle + max_brake: maximum brake applied to the vehicle + max_steering: maximum steering applied to the vehicle + offset: distance between the route waypoints and the center of the lane + """ + self._vehicle = vehicle + self._world = self._vehicle.get_world() + self._map = self._world.get_map() + + self._vehicle_controller = None + self.target_waypoint = None + self.target_road_option = None + + self._waypoints_queue = deque(maxlen=10000) + self._min_waypoint_queue_length = 100 + self._stop_waypoint_creation = False + + # Base parameters + self._dt = 1.0 / 20.0 + self._target_speed = 20.0 # Km/h + self._sampling_radius = 2.0 + self._args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': self._dt} + self._args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': self._dt} + self._max_throt = 0.75 + self._max_brake = 0.3 + self._max_steer = 0.8 + self._offset = 0 + self._base_min_distance = 3.0 + self._follow_speed_limits = False + + # Overload parameters + if opt_dict: + if 'dt' in opt_dict: + self._dt = opt_dict['dt'] + if 'target_speed' in opt_dict: + self._target_speed = opt_dict['target_speed'] + if 'sampling_radius' in opt_dict: + self._sampling_radius = opt_dict['sampling_radius'] + if 'lateral_control_dict' in opt_dict: + self._args_lateral_dict = opt_dict['lateral_control_dict'] + if 'longitudinal_control_dict' in opt_dict: + self._args_longitudinal_dict = opt_dict['longitudinal_control_dict'] + if 'max_throttle' in opt_dict: + self._max_throt = opt_dict['max_throttle'] + if 'max_brake' in opt_dict: + self._max_brake = opt_dict['max_brake'] + if 'max_steering' in opt_dict: + self._max_steer = opt_dict['max_steering'] + if 'offset' in opt_dict: + self._offset = opt_dict['offset'] + if 'base_min_distance' in opt_dict: + self._base_min_distance = opt_dict['base_min_distance'] + if 'follow_speed_limits' in opt_dict: + self._follow_speed_limits = opt_dict['follow_speed_limits'] + + # initializing controller + self._init_controller() + + def reset_vehicle(self): + """Reset the ego-vehicle""" + self._vehicle = None + + def _init_controller(self): + """Controller initialization""" + self._vehicle_controller = VehiclePIDController(self._vehicle, + args_lateral=self._args_lateral_dict, + args_longitudinal=self._args_longitudinal_dict, + offset=self._offset, + max_throttle=self._max_throt, + max_brake=self._max_brake, + max_steering=self._max_steer) + + # Compute the current vehicle waypoint + current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) + self.target_waypoint, self.target_road_option = (current_waypoint, RoadOption.LANEFOLLOW) + self._waypoints_queue.append((self.target_waypoint, self.target_road_option)) + + def set_speed(self, speed): + """ + Changes the target speed + + :param speed: new target speed in Km/h + :return: + """ + if self._follow_speed_limits: + print("WARNING: The max speed is currently set to follow the speed limits. " + "Use 'follow_speed_limits' to deactivate this") + self._target_speed = speed + + def follow_speed_limits(self, value=True): + """ + Activates a flag that makes the max speed dynamically vary according to the spped limits + + :param value: bool + :return: + """ + self._follow_speed_limits = value + + def _compute_next_waypoints(self, k=1): + """ + Add new waypoints to the trajectory queue. + + :param k: how many waypoints to compute + :return: + """ + # check we do not overflow the queue + available_entries = self._waypoints_queue.maxlen - len(self._waypoints_queue) + k = min(available_entries, k) + + for _ in range(k): + last_waypoint = self._waypoints_queue[-1][0] + next_waypoints = list(last_waypoint.next(self._sampling_radius)) + + if len(next_waypoints) == 0: + break + elif len(next_waypoints) == 1: + # only one option available ==> lanefollowing + next_waypoint = next_waypoints[0] + road_option = RoadOption.LANEFOLLOW + else: + # random choice between the possible options + road_options_list = _retrieve_options( + next_waypoints, last_waypoint) + road_option = random.choice(road_options_list) + next_waypoint = next_waypoints[road_options_list.index( + road_option)] + + self._waypoints_queue.append((next_waypoint, road_option)) + + def set_global_plan(self, current_plan, stop_waypoint_creation=True, clean_queue=True): + """ + Adds a new plan to the local planner. A plan must be a list of [carla.Waypoint, RoadOption] pairs + The 'clean_queue` parameter erases the previous plan if True, otherwise, it adds it to the old one + The 'stop_waypoint_creation' flag stops the automatic creation of random waypoints + + :param current_plan: list of (carla.Waypoint, RoadOption) + :param stop_waypoint_creation: bool + :param clean_queue: bool + :return: + """ + if clean_queue: + self._waypoints_queue.clear() + + # Remake the waypoints queue if the new plan has a higher length than the queue + new_plan_length = len(current_plan) + len(self._waypoints_queue) + if new_plan_length > self._waypoints_queue.maxlen: + new_waypoint_queue = deque(maxlen=new_plan_length) + for wp in self._waypoints_queue: + new_waypoint_queue.append(wp) + self._waypoints_queue = new_waypoint_queue + + for elem in current_plan: + self._waypoints_queue.append(elem) + + self._stop_waypoint_creation = stop_waypoint_creation + + def run_step(self, debug=False): + """ + Execute one step of local planning which involves running the longitudinal and lateral PID controllers to + follow the waypoints trajectory. + + :param debug: boolean flag to activate waypoints debugging + :return: control to be applied + """ + if self._follow_speed_limits: + self._target_speed = self._vehicle.get_speed_limit() + + # Add more waypoints too few in the horizon + if not self._stop_waypoint_creation and len(self._waypoints_queue) < self._min_waypoint_queue_length: + self._compute_next_waypoints(k=self._min_waypoint_queue_length) + + # Purge the queue of obsolete waypoints + veh_location = self._vehicle.get_location() + vehicle_speed = get_speed(self._vehicle) / 3.6 + self._min_distance = self._base_min_distance + 0.5 *vehicle_speed + + num_waypoint_removed = 0 + for waypoint, _ in self._waypoints_queue: + + if len(self._waypoints_queue) - num_waypoint_removed == 1: + min_distance = 1 # Don't remove the last waypoint until very close by + else: + min_distance = self._min_distance + + if veh_location.distance(waypoint.transform.location) < min_distance: + num_waypoint_removed += 1 + else: + break + + if num_waypoint_removed > 0: + for _ in range(num_waypoint_removed): + self._waypoints_queue.popleft() + + # Get the target waypoint and move using the PID controllers. Stop if no target waypoint + if len(self._waypoints_queue) == 0: + control = carla.VehicleControl() + control.steer = 0.0 + control.throttle = 0.0 + control.brake = 1.0 + control.hand_brake = False + control.manual_gear_shift = False + else: + self.target_waypoint, self.target_road_option = self._waypoints_queue[0] + control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) + + if debug: + draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) + + return control + + def get_incoming_waypoint_and_direction(self, steps=3): + """ + Returns direction and waypoint at a distance ahead defined by the user. + + :param steps: number of steps to get the incoming waypoint. + """ + if len(self._waypoints_queue) > steps: + return self._waypoints_queue[steps] + + else: + try: + wpt, direction = self._waypoints_queue[-1] + return wpt, direction + except IndexError as i: + return None, RoadOption.VOID + + def get_plan(self): + """Returns the current plan of the local planner""" + return self._waypoints_queue + + def done(self): + """ + Returns whether or not the planner has finished + + :return: boolean + """ + return len(self._waypoints_queue) == 0 + + +def _retrieve_options(list_waypoints, current_waypoint): + """ + Compute the type of connection between the current active waypoint and the multiple waypoints present in + list_waypoints. The result is encoded as a list of RoadOption enums. + + :param list_waypoints: list with the possible target waypoints in case of multiple options + :param current_waypoint: current active waypoint + :return: list of RoadOption enums representing the type of connection from the active waypoint to each + candidate in list_waypoints + """ + options = [] + for next_waypoint in list_waypoints: + # this is needed because something we are linking to + # the beggining of an intersection, therefore the + # variation in angle is small + next_next_waypoint = next_waypoint.next(3.0)[0] + link = _compute_connection(current_waypoint, next_next_waypoint) + options.append(link) + + return options + + +def _compute_connection(current_waypoint, next_waypoint, threshold=35): + """ + Compute the type of topological connection between an active waypoint (current_waypoint) and a target waypoint + (next_waypoint). + + :param current_waypoint: active waypoint + :param next_waypoint: target waypoint + :return: the type of topological connection encoded as a RoadOption enum: + RoadOption.STRAIGHT + RoadOption.LEFT + RoadOption.RIGHT + """ + n = next_waypoint.transform.rotation.yaw + n = n % 360.0 + + c = current_waypoint.transform.rotation.yaw + c = c % 360.0 + + diff_angle = (n - c) % 180.0 + if diff_angle < threshold or diff_angle > (180 - threshold): + return RoadOption.STRAIGHT + elif diff_angle > 90.0: + return RoadOption.LEFT + else: + return RoadOption.RIGHT diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__init__.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__pycache__/__init__.cpython-37.pyc b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__pycache__/__init__.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..280ae737210454c2718edd89d1ecc1929cfd1ae3 GIT binary patch literal 183 zcmZ?b<>g`kf}kS43=sVoM8E(ekl_Ht#VkM~g&~+hlhJP_LlH4_zo`FXmb#hH2Ox-O}y1-d?|iA8xJ zUT$J>NotXPVtQ&`NwI!Oetu4|etdjpUS>&ryk0@&Ee@O9{FKt1R6CH(pMjVG04B*W AN9iEVpiSKjy#%C zdRmsHn#}=xqAGWallH(NoH=pf9|#w?%>}ABP`SaC;=uRoc}UhKl^l?&d%Ao2`Fni7 zujjSx?WTd>1KB?QRgdt#Ow(@T~L8yveul#^o)(jg|&)^K*DM`3`>u&nS8xy{YDjn0+D z3*MVCpN7Jl1?t%Ir;md$@a5baOryRE;wahmKEg|ZoVj7#_eqhN zqtBMPZmci?%sCQ68tzBKP-K>f_{WC4f?of5Z|^uBi@o_2*)ULhuTE7Q#p8II zTzgj>2mMf7yYD{=hI%{KevC~FuN?|8x%P(eWkj{HpFmQ3{!l>Kdn%5@WN#cK{oTnt zYwL|x8#{*;lNK_=ZL%6W&)V!cCclb$pHVA*CQ=PaTk>@jQe$bH8Ygf-#g=BuMiw_O z7!|uI77{mCB)pXC{pFg2r>y=OGpq*OQ z8Vc2WBw!kWn#(OTW_I#{RMJLKI+T$%VSV*tKV1vmwes@pSsK%zY4l0IiE2WFjk;?} zs#UTjl&EB@P$4jT0@)TaHdAxNMkX-BTw3sXd*lEcY+b6II4M*0r3(bGQfJgSwN!Jt zm0G9lS0?boR`pRUt&O&k+T6}@1N~cR9c}ed8wq2eA7_yZ?OrLvL`xfnPD)yLR8$~7 zDJD`RWL)~ClEH8Yw|*3du-S45ylgNC`oIC8il3lX?aBq|E-qE~j&>mRz z?zmdzk{lz~_zsn{+8Zck?z_OfSl&9!t-#J4J)NfJlc<=G5IN!94R^F+n$L(TR zLMC9?9`nq_Z8Doec3O0(Xz}(+tlGUpr7*d>yFQO3ckP%fBki2awu`xPw3FNDA={Ku zV9MI%#JS!i*8R49jBGs@37t?Zf8>TveZkavkBAOf2Y zZTUkEKT56Sfo?-Ye%62oTHL|2p0Z^fcnsRE57rxFsHYzKnsXrf`~7}Ajnq{%)0)?xI!Hu2jX+v3 z@s00Zzv{)2Ku4U47ZIZ%D$18n**s`S5zs4G(mmGcOEI((!pd&Xj;=a=#sp*kaj%r@ zzK2>co4Hgs46d7gBj1-8=J)yB8b&h=3Qg zH9f*Xy@{j|1tYeBQ5I0e2C9Hv6sQ6$OKStV=2-qOp>k~tB|(z{ObYjb+}^a?mG)a% zrSBEOt~`vq6Cg8{z!Fs<`I*ba#;~(amUme9u(hkm=JaQWS7!4$GsH5${xo}r%dILU zLVW)C(tU9B=|taX#*;Y79gP66k`eHX>N%Qf@ljPCI+K5%iJ<6^l>(smFHl*I&*%V~9wI&OBgT~&Z>^sPC zh0gtDGb~^3=ajxT_J>iRrd;IxMi`WkksFQYSj&8ooZx$?RTPl;6s3De$sJ@cc@RB> zUOY!cd*KS8y?GtuYJTMHTA>lma3OnGsS}auh0pXl#=q4##GpH(N)jTsparVIIyNwt;=tq zt>a{FxrgkTGmB&v9ByqK-5J+#YNs?pAg{hGrg)zaw=?QBw!?Nz`F)ftMG*C@6!BA3 zND&ab=G`b0Cudp}Kqr(*Ew-{WCK2Pdhd?G}Eo%y2Ek-VV#{d%tgTXt>ZOVsq%V3KS z&nRgMqrt6?LPSnRN-*i&ASG3`n`g9Cy}EIAty}qx;*Bcm>(3O$M}x+i*eo11_pZvF zGG`D(gBTCGB;rm!NpKx(u2xzr-`t!^`{RZ#YWc~2jiqvDsI?gVpED)d{b%do_5Y6! z94+x*Xo>$|ib%^5@K+vDa*JDso+jSOmlp791i3=z3uG$qlAI4Hdj%P6|5Yke==?Gg z+$r&;wCdkr;M1~X@(^v+jhfgCNO2A<1c)i39vFHn1!Lv}7Utq-Yj&uCI=>l4)A13m zu^`bv^?>5vRZoP8@UEW`4!Z7p;KEaI*f{4~dd>pFaX2yse5JC`a}zY(a{Aw)PePkD zYLe96VUlVccE8ik>b)M1`@LRHogB. + +""" Module with auxiliary functions. """ + +import math +import numpy as np +import carla + +def draw_waypoints(world, waypoints, z=0.5): + """ + Draw a list of waypoints at a certain height given in z. + + :param world: carla.world object + :param waypoints: list or iterable container with the waypoints to draw + :param z: height in meters + """ + for wpt in waypoints: + wpt_t = wpt.transform + begin = wpt_t.location + carla.Location(z=z) + angle = math.radians(wpt_t.rotation.yaw) + end = begin + carla.Location(x=math.cos(angle), y=math.sin(angle)) + world.debug.draw_arrow(begin, end, arrow_size=0.3, life_time=1.0) + + +def get_speed(vehicle): + """ + Compute speed of a vehicle in Km/h. + + :param vehicle: the vehicle for which speed is calculated + :return: speed as a float in Km/h + """ + vel = vehicle.get_velocity() + + return 3.6 * math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2) + +def get_trafficlight_trigger_location(traffic_light): + """ + Calculates the yaw of the waypoint that represents the trigger volume of the traffic light + """ + def rotate_point(point, radians): + """ + rotate a given point by a given angle + """ + rotated_x = math.cos(radians) * point.x - math.sin(radians) * point.y + rotated_y = math.sin(radians) * point.x - math.cos(radians) * point.y + + return carla.Vector3D(rotated_x, rotated_y, point.z) + + base_transform = traffic_light.get_transform() + base_rot = base_transform.rotation.yaw + area_loc = base_transform.transform(traffic_light.trigger_volume.location) + area_ext = traffic_light.trigger_volume.extent + + point = rotate_point(carla.Vector3D(0, 0, area_ext.z), math.radians(base_rot)) + point_location = area_loc + carla.Location(x=point.x, y=point.y) + + return carla.Location(point_location.x, point_location.y, point_location.z) + + +def is_within_distance(target_transform, reference_transform, max_distance, angle_interval=None): + """ + Check if a location is both within a certain distance from a reference object. + By using 'angle_interval', the angle between the location and reference transform + will also be tkaen into account, being 0 a location in front and 180, one behind. + + :param target_transform: location of the target object + :param reference_transform: location of the reference object + :param max_distance: maximum allowed distance + :param angle_interval: only locations between [min, max] angles will be considered. This isn't checked by default. + :return: boolean + """ + target_vector = np.array([ + target_transform.location.x - reference_transform.location.x, + target_transform.location.y - reference_transform.location.y + ]) + norm_target = np.linalg.norm(target_vector) + + # If the vector is too short, we can simply stop here + if norm_target < 0.001: + return True + + # Further than the max distance + if norm_target > max_distance: + return False + + # We don't care about the angle, nothing else to check + if not angle_interval: + return True + + min_angle = angle_interval[0] + max_angle = angle_interval[1] + + fwd = reference_transform.get_forward_vector() + forward_vector = np.array([fwd.x, fwd.y]) + angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) + + return min_angle < angle < max_angle + + +def compute_magnitude_angle(target_location, current_location, orientation): + """ + Compute relative angle and distance between a target_location and a current_location + + :param target_location: location of the target object + :param current_location: location of the reference object + :param orientation: orientation of the reference object + :return: a tuple composed by the distance to the object and the angle between both objects + """ + target_vector = np.array([target_location.x - current_location.x, target_location.y - current_location.y]) + norm_target = np.linalg.norm(target_vector) + + forward_vector = np.array([math.cos(math.radians(orientation)), math.sin(math.radians(orientation))]) + d_angle = math.degrees(math.acos(np.clip(np.dot(forward_vector, target_vector) / norm_target, -1., 1.))) + + return (norm_target, d_angle) + + +def distance_vehicle(waypoint, vehicle_transform): + """ + Returns the 2D distance from a waypoint to a vehicle + + :param waypoint: actual waypoint + :param vehicle_transform: transform of the target vehicle + """ + loc = vehicle_transform.location + x = waypoint.transform.location.x - loc.x + y = waypoint.transform.location.y - loc.y + + return math.sqrt(x * x + y * y) + + +def vector(location_1, location_2): + """ + Returns the unit vector from location_1 to location_2 + + :param location_1, location_2: carla.Location objects + """ + x = location_2.x - location_1.x + y = location_2.y - location_1.y + z = location_2.z - location_1.z + norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps + + return [x / norm, y / norm, z / norm] + + +def compute_distance(location_1, location_2): + """ + Euclidean distance between 3D points + + :param location_1, location_2: 3D points + """ + x = location_2.x - location_1.x + y = location_2.y - location_1.y + z = location_2.z - location_1.z + norm = np.linalg.norm([x, y, z]) + np.finfo(float).eps + return norm + + +def positive(num): + """ + Return the given number if positive, else 0 + + :param num: value to check + """ + return num if num > 0.0 else 0.0 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 new file mode 100644 index 0000000000..78eae4c4d1 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/car_env.py @@ -0,0 +1,603 @@ +from __future__ import print_function + +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 + +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 + + +#from tensorboard import * + +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 + + +#MODEL_PATH = 'models/Xception__-518.00max_-766.40avg_-1097.00min__1677834457.model' + + + +''' +Defining the Carla Environment Class. +''' +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 + + + def reset(self): + + # 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 = [] + + self.transform = Transform(Location(x=self.initial_pos[0], y=self.initial_pos[1], z=self.initial_pos[2]), Rotation(yaw=self.initial_pos[3])) + # to spawn the actor; the veichle + self.vehicle = self.world.spawn_actor(self.model_3, self.transform) + self.actor_list.append(self.vehicle) + + print("Spawning my agent.....") + + # to use the RGB camera + self.depth_camera = self.blueprint_library.find("sensor.camera.depth") + #self.depth_camera.set_attribute('image_type', '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) + #print("Segmentation camera image sent for processing....") + self.actor_list.append(self.seg_camera_sensor) + + self.seg_camera_sensor.listen(lambda data: self.image_seg(data)) + + # to initialize the car quickly and get it going + self.vehicle.apply_control(carla.VehicleControl(throttle = 0.0, brake = 0.0)) + time.sleep(4) + + + # 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)) + + + traj = self.trajectory() + self.path = [] + for el in traj: + self.path.append(el[0]) + + while self.cam is None or self.seg is None: + time.sleep(0.01) + + self.process_images() + + # going to keep an episode length of 10 seconds otherwise the car learns to go around a circle and keeps doing the same thing + self.episode_start = time.time() + + self.vehicle.apply_control(carla.VehicleControl(throttle = 1.0, brake = 0.0)) + + return [(self.distance-300)/300, -1, 0, 0] + + # to record the collision data + 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)) + + if action == 1: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=0*self.STEER_AMT)) + + if action == 2: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=-0.6*self.STEER_AMT)) + + if action == 3: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=0.6*self.STEER_AMT)) + + if action == 4: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=-0.1*self.STEER_AMT)) + + if action == 5: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=0.1*self.STEER_AMT)) + + + if action != 0: + action = 1 + + 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 + + # to get the closest waypoint to the car + waypoint = self.client.get_world().get_map().get_waypoint(pos, project_to_road=True) + #path = self.trajectory() + #waypoint = path[0][0] + waypoint_ind = self.get_closest_waypoint(self.path, waypoint) + print(waypoint_ind) + waypoint = self.path[waypoint_ind] + + + if len(self.path) - waypoint_ind != 1: + next_waypoint = self.path[waypoint_ind+1] + else: + next_waypoint = waypoint + waypoint_loc = waypoint.transform.location + waypoint_rot = waypoint.transform.rotation + next_waypoint_loc = next_waypoint.transform.location + next_waypoint_rot = next_waypoint.transform.rotation + + dist_from_goal = np.sqrt((pos.x - self.final_destination[0])**2 + (pos.y-self.final_destination[1])**2) + + done = False + + + ''' + TO DEFINE THE REWARDS + ''' + + # to get the orientation difference between the car and the road "phi" + orientation_diff = waypoint_rot.yaw - rot.yaw + phi = orientation_diff%360 -360*(orientation_diff%360>180) + + u = [waypoint_loc.x-next_waypoint_loc.x, waypoint_loc.y-next_waypoint_loc.y] + v = [pos.x-next_waypoint_loc.x, pos.y-next_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 + + current_state[3] = current_state[3]/15 + + print(signed_dis) + print((current_state[1]+current_state[0])*30+30) + print(current_state[0]*300+300) + print(current_state[2]) + + + ''' + To define the rewards based on the combination of braking_dqn and driving_dqn models + ''' + # optimal policy for braking + if (current_state[0]*300+300)< (((current_state[1]+current_state[0])*30+30)*10 + 10): + if action == 0: + reward += 3 + else: + reward -= 1 + else: + if action == 1: + reward += 2 + else: + reward -= 2 + + if current_state[0]*300+300 > 100 and (current_state[1]+current_state[0])*30+30 < 1: + if action == 0: + reward -= 10 + + # Defining the Reward function by comparing the action taken to a suboptimal policy for driving + if abs(current_state[2])<5: + if action == 0: + reward += 2 + else: + reward -= 1 + elif abs(current_state[2])<10: + if current_state[2]<0: + if action == 3: + reward += 2 + elif action == 1: + reward += 1 + else: + reward -= 1 + else: + if action == 4: + reward += 2 + elif action == 2: + reward += 1 + else: + reward -= 1 + else: + if current_state[2]<0: + if action == 1: + reward += 2 + elif action == 3: + reward += 1 + else: + reward -= 1 + else: + if action == 2: + reward += 2 + elif action == 4: + reward += 1 + else: + reward -= 1 + + if abs(current_state[3])<0.1: + if action == 0: + reward += 4 + else: + reward -= 2 + elif abs(current_state[3])<0.5: + if current_state[3]<0: + if action == 3: + reward += 2 + elif action == 1: + reward += 1 + else: + reward -= 1 + else: + if action == 4: + reward += 2 + elif action == 2: + reward += 1 + else: + reward -= 1 + else: + if current_state[3]<0: + if action == 1: + reward += 2 + elif action == 3: + reward += 1 + else: + reward -= 1 + else: + if action == 2: + reward += 2 + elif action == 4: + reward += 1 + else: + reward -= 1 + + # for collision + if len(self.collision_history) != 0: + done = True + reward = - 200 + + if abs(phi)>100: + done = True + reward = -200 + + if abs(signed_dis)>3: + #done = True + reward = -200 + + # to end the episode + if dist_from_goal < 10: + self.reached = 1 + done = True + + # to run each episode for just 200 seconds + if self.episode_start + 200 < time.time(): + done = False + + print(reward) + + return [(self.distance-300)/300, (kmh-30)/30-(self.distance-300)/300, phi, signed_dis*15], reward, done, 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 get_closest_waypoint(self, waypoint_list, target_waypoint): + + closest_waypoint = None + closest_distance = float('inf') + for i, waypoint in enumerate(waypoint_list): + distance = math.sqrt((waypoint.transform.location.x - target_waypoint.transform.location.x)**2 + + (waypoint.transform.location.y - target_waypoint.transform.location.y)**2) + if distance < closest_distance: + closest_waypoint = i + closest_distance = distance + return closest_waypoint + diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/requirements.txt b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/requirements.txt index a41f3a4bbe..0016a89d5e 100644 --- a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/requirements.txt +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/requirements.txt @@ -39,9 +39,6 @@ jsonschema==4.17.3 jupyter_client==7.4.9 jupyter_core==4.12.0 jupyterlab-widgets==3.0.5 -Keras==2.1.6 -Keras-Applications==1.0.8 -Keras-Preprocessing==1.1.2 kiwisolver==1.4.4 Markdown==3.4.1 MarkupSafe==2.1.2 @@ -50,7 +47,7 @@ matplotlib-inline==0.1.6 nbformat==5.5.0 nest-asyncio==1.5.6 networkx==2.6.3 -numpy==1.18.4 +numpy==1.19.5 open3d==0.16.0 opencv-python==4.7.0.68 opt-einsum==3.3.0 @@ -81,11 +78,11 @@ PyYAML==6.0 pyzmq==25.0.0 scikit-learn==1.0.2 scipy==1.7.3 -six==1.16.0 +six==1.17.0 tenacity==8.2.1 tensorboard==1.15.0 -tensorflow==1.15.0 -tensorflow-estimator==1.15.1 +tensorflow==2.5.0 +tensorflow-estimator==2.5.0 termcolor==2.2.0 threadpoolctl==3.1.0 tomli==2.0.1 @@ -96,5 +93,5 @@ typing_extensions==4.5.0 wcwidth==0.2.6 Werkzeug==2.2.3 widgetsnbextension==4.0.5 -wrapt==1.14.1 +wrapt==1.12.1 zipp==3.14.0 diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py index 19730fcd66..f277169a86 100644 --- a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py @@ -1,12 +1,12 @@ import os +os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' # 只显示错误信息 import random from collections import deque import numpy as np import cv2 import time import tensorflow as tf -import keras.backend.tensorflow_backend as backend -from keras.models import load_model +from tensorflow.keras.models import load_model from car_env import CarEnv, MEMORY_FRACTION import carla from carla import Transform @@ -14,12 +14,11 @@ from carla import Rotation from agents.navigation.global_route_planner import GlobalRoutePlanner - #Trajectory 1 town2 = {1: [80, 306.6, 5, 0], 2:[135.25,206]} #Trajectory 2 -town2 = {1: [-7.498, 284.716, 5, 90], 2:[81.98,241.954]} +#town2 = {1: [-7.498, 284.716, 5, 90], 2:[81.98,241.954]} #Trajectory 3 #town2 = {1: [-7.498, 165.809, 5, 90], 2:[81.98,241.954]} @@ -27,102 +26,235 @@ #Trajectory 4 #town2 = {1: [106.411, 191.63, 5, 0], 2:[170.551,240.054]} -# custom trajectory -# town2 = {1: [\initial_destination], 2:[\final_destination]} - +#custom trajectory +#town2 = {1: [\initial_destination], 2:[\final_destination]} -# to load the pretrained models for braking and driving +# 模型路径 MODEL_PATH = "models/Braking___282.00max__282.00avg__282.00min__1679121006.model" - MODEL_PATH2 = "models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model" +def safe_load_model(model_path): + """安全加载模型，处理可能的兼容性问题""" + try: + print(f"尝试加载模型: {model_path}") + + # 检查文件是否存在 + if not os.path.exists(model_path): + print(f"❌ 模型文件不存在: {model_path}") + return None + + try: + model = load_model(model_path) + print(f"✅ 成功加载模型: {model_path}") + return model + except Exception as e: + print(f"标准加载失败: {e}") + + except Exception as e: + print(f"❌ 加载模型时发生错误 {model_path}: {e}") + return None +def setup_tensorflow(): + """设置 TensorFlow 2.x 配置""" + # 设置日志级别减少输出 + os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' + + print(f"TensorFlow 版本: {tf.__version__}") + + # GPU 配置 + gpus = tf.config.list_physical_devices('GPU') + if gpus: + try: + # 设置GPU内存按需增长 + 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}") + # 如果GPU设置失败，回退到CPU + os.environ['CUDA_VISIBLE_DEVICES'] = '-1' + print("使用CPU运行") + else: + print("ℹ️ 未找到GPU，使用CPU运行") + +def preprocess_state_for_prediction(state_data, model_type="braking"): + """预处理状态数据用于模型预测""" + try: + if model_type == "braking": + # 对于刹车模型，使用前两个状态 + if isinstance(state_data, list): + state_array = np.array(state_data[:2]) + else: + state_array = state_data[:2] + else: + # 对于驾驶模型，使用后两个状态 + if isinstance(state_data, list): + state_array = np.array(state_data[2:]) + else: + state_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]]) if __name__ == '__main__': FPS = 60 + EPISODES = 2 - # Memory fraction - gpu_options = tf.GPUOptions(per_process_gpu_memory_fraction=MEMORY_FRACTION) - backend.set_session(tf.Session(config=tf.ConfigProto(gpu_options=gpu_options))) + # 设置 TensorFlow + setup_tensorflow() - # Load the model - model = load_model(MODEL_PATH) - model2 = load_model(MODEL_PATH2) + # 加载模型 + print("\n" + "="*50) + print("加载自动驾驶模型") + print("="*50) + + model = safe_load_model(MODEL_PATH) + model2 = safe_load_model(MODEL_PATH2) + + # 如果模型加载失败，创建新的模型 + print(f"模型加载完成:") + print(f"{model.input_shape} -> {model.output_shape}") + print(f"{model2.input_shape} -> {model2.output_shape}") - # Create environment + # 创建环境 env = CarEnv(town2[1], town2[2]) - # For agent speed measurements - keeps last 60 frametimes + # 用于FPS计算 - 保持最近60帧的时间 fps_counter = deque(maxlen=60) - # Initialize predictions - first prediction takes longer as of initialization that has to be done - # It's better to do a first prediction then before we start iterating over episode steps - model.predict(np.array([[0,0]])) - model2.predict(np.array([[0,0]])) + # 初始化预测 - 第一次预测需要初始化时间 + print("预热模型...") + try: + # 使用正确的预处理 + dummy_state = preprocess_state_for_prediction([0, 0, 0, 0], "braking") + model.predict(dummy_state, verbose=0) + model2.predict(dummy_state, verbose=0) + print("✅ 模型预热完成") + except Exception as e: + print(f"⚠️ 模型预热警告: {e}") + # 循环 episodes + for episode in range(EPISODES): + print(f'\n{"="*50}') + print(f'开始 Episode {episode + 1}/{EPISODES}') + print(f'{"="*50}') - # Loop over episodes - for i in range(2): - - print('Restarting episode') - - # Reset environment and get initial state + # 重置环境并获取初始状态 current_state = env.reset() - env.collision_hist = [] - env.trajectory() + if hasattr(env, 'collision_hist'): + env.collision_hist = [] + + # 生成轨迹 + if hasattr(env, 'trajectory'): + env.trajectory() + done = False + step_count = 0 - # Loop over steps - while True: - - # For FPS counter + # 循环步骤 + while not done: + step_count += 1 + + # FPS 计数器 step_start = time.time() - # Show current frame - #cv2.imshow(f'Agent - preview', current_state[0]) - #cv2.waitKey(1) + # 显示当前帧（可选） + # if len(current_state) > 0 and isinstance(current_state[0], np.ndarray): + # cv2.imshow(f'Agent - preview', current_state[0]) + # cv2.waitKey(1) - # Traffic Lights - if env.vehicle.is_at_traffic_light(): - if env.vehicle.get_traffic_light().get_state() == carla.TrafficLightState.Red: - print("Red") - action = 0 - time.sleep(1/FPS) + # 交通灯处理 + action = None + try: + if hasattr(env, 'vehicle') and env.vehicle and env.vehicle.is_at_traffic_light(): + traffic_light_state = env.vehicle.get_traffic_light().get_state() + if traffic_light_state == carla.TrafficLightState.Red: + print("红灯 - 停车") + action = 0 + time.sleep(1/FPS) + else: + print("绿灯 - 使用刹车模型预测") + # 预处理状态数据 + state_for_model = preprocess_state_for_prediction(current_state, "braking") + qs = model.predict(state_for_model, verbose=0)[0] + action = np.argmax(qs) + + if action == 1: # 如果需要进一步决策 + state_for_model2 = preprocess_state_for_prediction(current_state, "driving") + qs2 = model2.predict(state_for_model2, verbose=0)[0] + action = np.argmax(qs2) + 1 else: - print("Green") - qs = model.predict(np.array(current_state[:2]).reshape(-1, *np.array(current_state[:2]).shape))[0] + # 基于当前观察空间预测动作 + state_for_model = preprocess_state_for_prediction(current_state, "braking") + qs = model.predict(state_for_model, verbose=0)[0] action = np.argmax(qs) - if action == 1: - qs2 = model2.predict(np.array(current_state[2:]).reshape(-1, *np.array(current_state[2:]).shape))[0] + + if action == 1: # 如果需要进一步决策 + state_for_model2 = preprocess_state_for_prediction(current_state, "driving") + qs2 = model2.predict(state_for_model2, verbose=0)[0] action = np.argmax(qs2) + 1 - - else: - # Predict an action based on current observation space - # Get action from Q table - qs = model.predict(np.array(current_state[:2]).reshape(-1, *np.array(current_state[:2]).shape))[0] - action = np.argmax(qs) - if action == 1: - qs2 = model2.predict(np.array(current_state[2:]).reshape(-1, *np.array(current_state[2:]).shape))[0] - action = np.argmax(qs2) + 1 - - # Step environment (additional flag informs environment to not break an episode by time limit) - new_state, reward, done, _ = env.step(action, current_state) + except Exception as e: + print(f"❌ 预测错误: {e}") + action = 0 # 默认安全动作 - # Set current step for next loop iteration - current_state = new_state + # 环境步骤（额外的标志通知环境不要因时间限制而中断episode） + try: + new_state, reward, done, _ = env.step(action, current_state) + current_state = new_state + except Exception as e: + print(f"❌ 环境步骤错误: {e}") + done = True - # If done - agent crashed, break an episode + # 如果完成 - 代理崩溃，中断episode if done: + print(f"Episode {episode + 1} 完成，步数: {step_count}") break - # Measure step time, append to a deque, then print mean FPS for last 60 frames, q values and taken action + # 测量步骤时间，添加到deque，然后打印最近60帧的平均FPS、q值和采取的动作 frame_time = time.time() - step_start fps_counter.append(frame_time) - print(f'Agent: {len(fps_counter)/sum(fps_counter):>4.1f} FPS | Action: [{qs[0]:>5.2f}, {qs[1]:>5.2f}] {action}') - - # Destroy an actor at end of episode - for actor in env.actor_list: - actor.destroy() + if len(fps_counter) > 0: + current_fps = len(fps_counter) / sum(fps_counter) + else: + current_fps = 0 + + # 安全地打印Q值 + try: + qs_display = f"[{qs[0]:>5.2f}, {qs[1]:>5.2f}]" if 'qs' in locals() else "[N/A, N/A]" + print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Q-values: {qs_display} | Action: {action}') + except: + print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Action: {action}') + + # 在episode结束时销毁actor + print(f"清理 Episode {episode + 1} 的actor...") + try: + if hasattr(env, 'actor_list'): + for actor in env.actor_list: + try: + actor.destroy() + except Exception as e: + print(f"销毁actor错误: {e}") + except Exception as e: + print(f"清理错误: {e}") + + print("\n" + "="*50) + print("所有episodes完成!") + print("="*50) + + # 清理资源 + try: + cv2.destroyAllWindows() + except: + pass + + print("程序正常退出") \ No newline at end of file From b6c60f2f732010730bf4af6b80595e9478cd2e13 Mon Sep 17 00:00:00 2001 From: manayume <158073701+manayume@users.noreply.github.com> Date: Thu, 23 Oct 2025 15:01:43 +0800 Subject: [PATCH 07/16] Delete src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__pycache__ directory --- .../agents/__pycache__/__init__.cpython-37.pyc | Bin 177 -> 0 bytes 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__pycache__/__init__.cpython-37.pyc diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__pycache__/__init__.cpython-37.pyc b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__pycache__/__init__.cpython-37.pyc deleted file mode 100644 index 0ceced42b3f087895b315de419a0ecf7a00c1944..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 177 zcmZ?b<>g`kf}kS43=sVoM8E(ekl_Ht#VkM~g&~+hlhJP_LlH4_zo`FXmb#hH2Ox-O}y1-d?|iA8xJ uUT$J>NotXPVtQ&`NwI!>d}dx|NqoFsLFFwDo80`A(wtN~ke#1_m;nIuZ!UiT From 6d01d0aa6d303570c55bf9c09c7daaa38587b8d6 Mon Sep 17 00:00:00 2001 From: manayume <158073701+manayume@users.noreply.github.com> Date: Thu, 23 Oct 2025 15:02:13 +0800 Subject: [PATCH 08/16] Delete src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__ directory --- .../__pycache__/__init__.cpython-37.pyc | Bin 188 -> 0 bytes .../__pycache__/basic_agent.cpython-37.pyc | Bin 11078 -> 0 bytes .../__pycache__/controller.cpython-37.pyc | Bin 8627 -> 0 bytes .../global_route_planner.cpython-37.pyc | Bin 11662 -> 0 bytes .../__pycache__/local_planner.cpython-37.pyc | Bin 10125 -> 0 bytes 5 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/__init__.cpython-37.pyc delete mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/basic_agent.cpython-37.pyc delete mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/controller.cpython-37.pyc delete mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/global_route_planner.cpython-37.pyc delete mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/local_planner.cpython-37.pyc diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/__init__.cpython-37.pyc b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/__init__.cpython-37.pyc deleted file mode 100644 index 8381a155c4f3bb029f3c4e371813c5ad17231433..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 188 zcmZ?b<>g`kf}kS43=sVoM8E(ekl_Ht#VkM~g&~+hlhJP_LlH4_zo`FXmb#hH2Ox-O}y1-d?|iA8xJ yUT$J>NotXPVtQ&`NwIz&T(N$9d}dx|NqoFsLFFwDo80`A(wtN~koBK|m;nGXa5Cut diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/basic_agent.cpython-37.pyc b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/basic_agent.cpython-37.pyc deleted file mode 100644 index 9eb3886895ecb4120cbb699ad23f49b8cac4ab61..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 11078 zcmcIq%WoxDTCb}6u6up`Y`foG>CU5X(y@okgqbwWOedXm2sBQUm_(|XP%7V3z7N;E zb^BD=Zd+vusiWET&_t|2NOpt}`~wKFV8H^R5o}OawLp~@+A zVy<1MPJMMA-}%n>KK0hrl&RtOAOH55`$xAl?Vsr(`;}3-jVtV<5SpzCT^PP@>$sPE zV^p$BI>E|*c~r40T(9`mQO&M#z3SIT4ZFehns1Jpc5~FSTcas^YSgydy7sOn>Z0+f zCK}%Kqmn%%j1RSrx%aQ;T7M8)qd;u=o;4Uvd~f89qtJ54mfJ(kiu!J3$-o@}CWEc9 zuv{yWZgWO4-5IQ@9s6QAxVh~2|xa%d)(d%>tyZ}55dOc4%K1PoQ z!8n=e2BQha%sX`nsq9xlUpHea(1q1z>4473eHltlSc-7brYsD7&16;Ts) z+^gaxVTvZ+)I>{60a6!jF^zjeToA})!`sLhBQ;)<|Pn-#B$=fqXi=ESmiLA;3CyqEzCtt94KZaC<^NzCHD zrE8jL;dcc4dZrq?+k>7PdDwYuXV~?|-ZFP)mQ>tF%;9yxkH?uz0)eUQXE3yW? zaezKn7zGn6#1oi^@Bt(BU4Xr9Pl9PSw#HrB5b*T2JMi6gFh5vFam+5`qxg!spU_k2 zjJCokWw0AOF2>yPeBZ(y{kwg4+=Cba--#>Tkr#O~v<71^G5FTC#9(8?lO$C_4z@`) zAb4mWTEYtl(h~~VuNJWXT~Z13qdQ%9aE1ScBGzNg(PC|=9q5O+I|k~;L5b^SL2oMp zFsI68jmvsWkcKFsG({O@lgpN=s{Nn<0v?-d*sSoU)&_tf+o5SUigjCO21=4 zsqR1__8FU0H(EObu+k{l_D&lwm_$$;-RLIRh?&=wyE$)#!H5;(3D(?=JVLJG1$Jwp(Rno_YV4YPW;2hOD5?XBg=TUF#h+{PDsDf!x6%g-u0-C@4Oi~C z(s!K?2cs>pP9RtA??(M#{O0}lR=V_Ng#|QRNeuw@a0ROxbRC8-Pj;~t&H!@lIF~SX zcoT)zsOl~JRZCU<*k7rx*LCyjuF{OYH~&^{pq8_7y9SMkAKmFcQ25a|NT9Iz;eZJSY`!%;Qby zt=OWSwxTUL4o}r_sam3KS^h|QXm=E{ots6pp+I1&1{(uUcNa^}S1lt$e!uu}v+Z z+Z`H!xRlJw0XFndCl48$Y3z;jb=+aAt-KH=f~mGn>B?N0weUbJkCja%uu_cM)(MALuT8`-h~1kPPSY7xybOBIfn0n zQ(;%OJ!LPI5oL=ccE|2$0P_jsm%2E_j>p#PcDsROcR#fgia7pY1e0_s>#=}X*Vlu< z2l2qS7*kp>wBvwn79#`81J;KStb2X9i-FA1%?O&zD~De>=Fr46VNP0}1@t`}Kw_xgc=dnm05lmXmUSj~|)TF0`K z_mL{uj?S!rn}tD6V3*92rQ!CO`t{IC?Yyyfv4HDS4Sx}sUcuyFgXtV0aNJT13aA!^ z_34(YU6N4x)o|qw?vL;g8{s06cBl`HVQC*B(0*y$2BdsYIn?lM?8A*6Ru5{iaj5T; zgGhBq_dcB2gL+(!D~HB@mB(nrRXCKT#|9+7jLoXJS0F?=8?pD3?p#-T9FDk%KjG&? z{Es5qbuZfSpjurhS64=)Rmqq1#?14yUF3_Xpnz1wI(!m@C}URKDwdEQ>0v@HX5nKk8t37@?y9+;y@-uaf|z@y>>X5y|XQipS|rHVBx9 z$JTXr50>wx)P!2t7>wby=POJ#GSf5P!lFhdnmlW~r-^pzkZX708#a-yaL3MrEpN+P z?U*dH$+V)LlW|7UKx8@&$)+du3{N==E>2pRd=i!AnH-dNZK7+}b9gn$H0kkzFm-Cm zI<$%uO4_`De1-B`G-?XW$K@{BlilR;$=mLFCv^=^ir5qU%=|mTaN!&Z%`B6%Wte)| zfZK&@?|eb?5Uz!8$6{6TAX!5$qQW8w!^`od{+NkJBX74eNuz74J`o_@5xjGyy9k@@97kqNPLHLCc zbBv=9jbyZRSVLey#B=aZl*{05MZpNv>!xWiD{FL2ft<#z1^&88X)urS9Kfx+1-XO# z@X{f$w9dSL$4C}Aj(7?wW`@ix%I|{uowDMgGL0aA6%cN7f(y9mr+BG+t$I~A@b}by z7kD#Q-L%)a>L&Ll)=6zcJsW%{)r`zUT#CzwwD0=fKjjO#AEKai+Xu8|UY z&(^ou+E3RhN=C9anO6BYWejK)iY<^c$kls}Q(@yt?4B`%QcPKcd;n1VH6ru3sUXuS ze;GxmqAWcHGV)ibc!P@XQE@ddu2GGee}*e0|3a%#aI?2qki3G*$gMpEWBD#nXBbRm zfmD8I--wL^m`oT)>(DEZBlrLyxG>4VAvZRs8Jb<~3tB1yqO8d3_`hGpG z$u&{o@oLZP7R|VW69_E`Nen&A5rVP-5q+dsp6h=yhpp1B5Av zZzd!kMsJLCg-z&-U-&h6A(8jE#;TV3#tL;U_Z@S-#wm>P};sOx5fxXpgcNgkEt!( z?CHL{=Sqw~6l9^VB*` za^#KYijCDvc^xR8V*&rpoxX>|fy|#LeWpSQ%D=+F>h|FQQJSh?KbF(rC@Bjt$LZdt{58z< zSGYo&TQl~i3sOPdj)gtP@#BPP8uz_&{cW$4#TKc)C5%IM+*7Vg4WsxLhgRWbr9#Z72Z>LNu`zK8lr4rl}RHw7Gval})J zw_gB$=l&P`rt$u73-}eijT2eG-1;=po`MHv#09V=F`_hSqhme1j3@oTIMfbsegZuN zA0)gESQ#1-`cB@ba#)RO!}_qXS(0}-H(nWYc6?abfW1Ns^PmZhM%uD% z>u+nH9PC%3c3gR+bD%+Qt9Vm=p2+*Txtr6FUb8%xhx1gam zkJ|i#wnX&>)EB6}Z^ovmY-;igj8Me!Ump0;85)T(TSUg+pwpn9@u)1(5;^h;vigks z1OH={>ZrMYQ1rEaQ$?xpd?1Ms)B&HSNj`;eA)rF>zNIuv<}Rg*=yZEq&~euFEOb)d z*qYMf_vkzihL>Mb0?364MhI1WZ3XK?I_f!!bI2Og5An__1RNm}(&NV^loH^iV)*n3 z2b6q5c$A`|-f6Oi)MutQt?OCN22iBaVH|LPcZvcaCzNvMI4K$koZR3n(eu4+WYwR6 zCE-Ki9}r7((JC28ffmj$kFe_)cUPaXbgOHU6zo?vip~y6n2)iTppJGwY}A($A-# z4yw=^l)|VXp#e>G&^Xjm{bGs|bi?n&72HdFFTX)vT$Obdw;r~JQ^WQ?k{_f?%Gz)` zni6LdnSNqC{DQRF6XTPglE=ogw`lfZJ#KOD`Qd`7J~sBJ zpnuNDFXE};nJ0Sq1ouTi7B@@bzoTxVZjzt4S$d+&8yI0}c$QA8ALB!YbJ~78ZlhQ0 z!ROqs&Hbou#_eK1yl?SprsHP1mYKMjlDImYeq?NvKGYt3-qZHy=y`vhD9rQB?Jt0i7eMvs~>C8bMYKv zhT10hME)b$+2|@k_Rl~33ERXU6Yu2v)f`@9x?bBfum=Xl_>$MvRJ_dVD#`N%lqJtX*$7$QTb`~gZ|XW`PbZ1Fd@a;3YgU;Q_Jh4_y3r2H7V5kBj0 z;U+(zx}G$UsK8n7?!@~F`VtTuCeE9LYkBlRo;{@N$HRYL-dgf3mx1!{GV@aEt z7z2DbMa}p~9it$R-t|J;#5ZF|9Fz0cxuSfEcWBz*MqxL%CP1l&*gfjoEqrix^weZR z1sfa4oY_<4a^${=U8BdHNo3Da<+xX4n*>vowCDLf<*N(5+i8|J-E4a%eLfoB%<{mP zqCUo0JlUBPdSbq*6r4_(wL&y33zODWqIP{fX%SAD+mzH*H)q$bNFse=OnwR1Yl#7m z=Ut`|wV$Z3+3ZF-+4{PjC988vH`8~-pmw&%wBre3kZ)q-|HKvk7{zht30Z^EoL}ja5)* zfj%|B>89s6_*BAC-$A0@a*+3T{q#-EaYWE{9Qiyb!Konm3f1T!Q7%)lLd7jADDTQy zJV|aJXDv8EASnWu6y{2DOPRa{l3yj*H7ZOLq!Bn*Q?Hhd_N(<)y=KG?=^1UnU#|IvN9-p_`Xq+?y_gXzB}>!-Q}K#uQMXKs|4{18?$h^kbPs_l^r;$ zPeWdOx>cG3R*Xu*j&Cm2CnK|I;|RyO;qqt@c2zcx^#?h@N{LjJX=9{MTYXZ$rS{GnvdLX||i~3xg1J=q6z)v^11v>29}#UBdPW462M{duBWt+ta^2 z$tE~lwn$uBafL&ba6xc@Yq`LI3pWnjd_|%}NT_Ed9>4GJ$9SfbZd;_PIP&NJ`}sY7 z-#2-Be!ec?d7d?U)lUn;UnrA5G7?wthC3j(&=zdbmIk6HwIz|#@<49O{HwH8{;jkt z_$q^{r?oXvcvi4gyYiY~SMEsd8gf-zLrz1kj$F;IBUeXm&X!*gT62dl=r8u%Q1=3R zG;nk)@FUapL;cp)HJ)L?VBoM&j{<$@uui}{ePBioGY7iq+xj5zyKXeHT^|M18SU5f zQ!HL+iHT}Eua2B;q-tF!GQy$b*cR30kAlP%yx|u>9HA{@%aSc(i|6c$U42b#E4F6W zUK6m{x;=+)#ctU1_*U&EmcNiJzwGo}Yv5qLSJUaS+{0hgbv(1GbnG?i(MG);EIM)n zUk^HZv>!}wnGf;sS(|N@PXq(<)`MVQA2@qXaTL0#@1^V0J1p>YYv4M5q=&AZc3>j* z{We}af7^WfQU3gS^X;Ggl%CT~OH1nn`Vwzk#T$MHB(B7QA;c=E7*|21SOk@0p)VgR zCj!!_L%MPz^r>EpYLxGYvHS+LtVeT{kCl!TOT46Ei#Srr>|!Fm9XdlCY8bJR6$MQ9 zoTwMrYxSaXV`wtd%eT0po4grV#T*U?2XsP%zyf0o2d3{k47sjr>A0p~dI*zsLnAxu z4c&Gvx)_rk(A}VJlduIjf~Fbo)!Nq3hhpRDt{m@~-C4Nh}ldfu-BJdAQuJ89;XgA;KLgLXh@X^q7 zumlqV+d(sosBe$F9ny6@^wb?c7^Htp`{o)@SnJ`TRS~ zF?V^VOgln7jn}^2nonw;dE3}w=B|?_X{;zshLPj2L^HC}Nww&Hzy@|wG1BiGDb{T0 zL`^%)56uwTmO6v)lZ#-Kn&fJt0R^6S$I6mrYMbi!$!2CT3!1gGL z7=^1Zry6y2+uU>MJ_CRCB@*}5YmPHqz3G_Dr<{jFaM-%pCDpO+7me3T7co4*=^C!@ zMuu^LROFW4n*y8~G`6Dkx#P3kbN z;UO?q4{{lSiAZwIgJIzMQPIVWP}zkP`&p9|SCq^WY4uFqrh}v{plO}ZWl$&?*XkUQ z6a$yK<&$pYs~|hZv4Z1UfsG-|5Ua?WO%B1FXvBQd>tSU2mZLxH`eZwyS~zMTP4=#y zUGNKkx~c8|?Iyj4mX}2zzcPYE{{JYSkzoXQJhsXg5sD zav;w9h}sL*h>?vPYoKbz9c?Wab1vj5d61U#36RI|h7W@Xa}9`|hQFq$K@69aCKtuJ zgc7NVl2ky?5X}6fEQ!X#Lth#1RlFgcCKzAniya{r-w+8_6YM#B{3_0%3%fw~!+{OI zGL8;Wa;6g9Zb|7CEn0j4W84=tbZ%^g2&uL4j*|;n>oS5iFo}U?Az!(4cxk5MIiur8 zyj^w)3(Yr3x^sGiOZhreb3%KwN_+e4d+u%AXS=0vNnk3?Q6WN#7A6Rq+c!POFp|1q zr2Y%i4a0a9+?&-@4Z{vB!(b0lN8&QJN@R`5_(ph&Ga`8;BoSc)L{k-65bFW!Ac!vA3$QIG{@D|q}XEGQ^G){_u%iL1v+!Xnl#G)y* z$UKU7HnRkLjJF>8(chnY>&;DzPLLk@7D7SC!Tkpzmf#z}M*x+`p1`9ZA47@A+mv1> z&!NISN5#CqY!je?&h`^Znl)3sAIeuK4Fk}=aTmy*S%4eHwT-*r_M+t$K!OGorCul~jG)OI0$qOk|75wM0UfG5>PbB^LMaa@G~>jjJ< zwx*YBOH#)X=}Ykrff$KB5Wzn`mSgz@B_e8$6)=u`B5@u1*JAgxFv3jc1%}Qoe%Yv} zw)y=S^$aI)TMqc4-|*JQu)Tb7&d#X20bCLc2>2tE0X%@BF3?J`Y~DUM;JFFS1+8dx zxO6o&ap@}cuj)TIYN?FGDZOKL2;?0l6^fUb5p>efwR4yaea4MK3#}SsspdrCdf~dGnQHUV=ec_JC zLFqQ7ZRsMnm9~6QSfP}BKj36U02J*gzbqW7zGf>&3SKo*j}ewYxB=z(0;(V(2hRTJT?5}*trQBe8Z^ya6&&T&g_elxwH@0cOb7lJlqrNP!ZB1{~1gyXP8 z>C(%9Wkb)=++BHP;yO0;ESudng^MtGzM`2dH_bxXW-=U9Fw!YW{po-3ocg-9BBo%~K93MIZDusQ8O5||lC6W)7g6PtaeHmqJgUAygxe6g>W6yvj zD%UWia~u%YnPxU(k=OC2u24-A8&XrePh0}iDbnF*-b0g=c-$gpqldnhSkdP`&LRQ7oymx)kFvjjWn1UP0Yt%|RY>u$mUJ-i_q4NX?0 z`eOZ@cAnz{3q{342YLnXhmM%Cd(ivvI%h%J`Lwe?fi}|0T>oVurV`L7$1i81jVdOi zhrTji&IfdO86ObN5IN3(Z_CjJ#WfX>^=+$xej&cdk|dCb|kFCI%L0!KiixmIS-nILFNrf}QbDK6Ne zY+IsW-()t08Z3v=4>Nh-U^Xzy9r7Y~hex2&$N*`D!O9AWH^6LoD9aVje`e24CAGwN= z<-P(eRqZ3_*H>2%ec-cbk<_0%Zp7M&n5C2x5$LKG*D$xlehqw8$Mwt~H!;#VybCe# z)rl0>?$2-))>=pTBGjpNA|1`eb9VJ2FdTw`|A_0h`h;*qYh60NC$4k6w@f+Uz3Q&| z6OlbeIa}k1Z^^D@i0?cQUn9CVZ3%iFYnlh*6OZt7(a}7u1*I3_I^X?D+{h5$#dv-K z@zu@hgk+QvU%`jqJ4ETH@YM%Id^yYCMFzM{&oSJX3+Q EFDZuv4*&oF diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/global_route_planner.cpython-37.pyc b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/navigation/__pycache__/global_route_planner.cpython-37.pyc deleted file mode 100644 index 6165532e8a2ad3b4395946f5f957c8d9f6f952d7..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 11662 zcmbVS%X1vZeV+HuKCu8nkdPF4OfwFbCP9nwQ)Xz&rYOl_K`M$8?I|g%;m!cq#bRgK zGXMe9tjj4nWtXdzQf@iq0956WxRPtGN#&3~Ag5$1hg9VxA5yvGqD%69-Lta`5Vn=< z*7S7ubpQJO{a*dOX0xW@_dv9^{_!nM`wu3jKMRpNc#?Mzgqp8~dT0!FUzcygH|5*% zZTWV52XAv&*>QbWXIX1l-KqICU3)_d?a=wD7CKS=ndvt|<6W&&IsA6*;Z{HKcH(e% z7dAyrZ>u4cdIo3Dy@IrX;r@G$`SHlG@{$!X{fgCv>FLg4L1;=tSLZ*FuQ;f5W_`i$k3FbB}1$k@}0CYaPh8Y+EMLT(^2-QUc4K{yUBN` zVF2+n{>md z*H75iLih1*e@HS#Q;i}+HQ4TM^g(GD$4Nw*Nxs;{#6{iSRxsL(y6F0L$IPuH8gBTO z?Dn}chV<8(IFEjO_TGWGHnEA58>6!DjZL}RF)di!E*BU3eQkV@yIo97Kkare za#a~0ORp4pkZfi|%XtD%XSbx>_&^+6Lf>w}gI z7gn_oj7ekCELv=2P1#B-YlYS-dM1|8uQq7OH2SuXE~57`rACk~g!TpP8NR683^XV9 z$VG~++S!S|Jh_#z_s*)ei7_FDR^%IHnS-&Z_G)Zv|8G!wA*}ofMtfKL@GmCqtbLlI zn#-(mQ48H|9X3|_(*$l>yeh3INH_29f>I=8tp{sRvdaFN_WL5 z@rL~*_2LchN-q$@VC_+GFphCe!08 zl^%?vycz9dpFb@~=GHh!w?4J`y@kUf4$4pKpAr;0D|w^KGTj7+tG6X8EZW$N=4jZ0 zr#v@@(I{_qIfN29M3u#DVo#z#rgs0e_1EvcegFR3k3^FnonSl$W{5fyl>)Hx+7zPt zVP26~g2;J=Grb{jCP{H(fh+ncUQN-9WFrvMDE*A*s!N-xT7naJC{T_G?l<(&A-b5+XRT<^0wkT#aQF}FBg@uD1%n(lFK zj9$TWGp~-K{ZxT*b2gAL+RxqTqJW3Z77?SwM+uB7SRvWE?5D!94lb5AwPTz`VAi)LdIk&wjzExE1<#xqz(8YQ4y?>%h#YHTr}mMPSwu51 zM{7{U`KsV?@l-b<}voiFp^1Y z|6$f*-%pjhOj@hj5pYo=YU^aJC;$0K8@*7LIYCpF*9NSMR&?#j@*^M>Q6d=P1eS+b ziY*(vBgwf56`~J8CBd0jJlT^#kZwg@5(!8%UXXaf6iP}e+@>h7N%0QfAp!+`kq~UA zyU&Xs4+6_s8JCNv))K-IM5Z8uB`?&h=}fC?mm;U7jDzX-fMChu)QR7m_S!T-m>NaP6?e zI;xr9N&+DH+xo&HpNGz`?aAjMK~Z*SDOd#Csg2Porp53>3>l1iFSb z9|s~xqtJVNAlG9iN|sY6c*F&zA3!#l0gw!pb5|xXj=aJ&A`;xzI}5^P4e|tE@`UNe z5Z9Q0os^8xSmNm@%=P_TKgjjN+~P(NYb--x&MktX_%fm(DKWZI>`~u*I(~)0&DpyLW~^zhOdsCY^mE|GB&G+THiGX5zhGu4jtW2q9P<}<#n;%x&hhrks_ zBIQSCA|=#2AdUs}gbakxmKbVJ93mv-!2%=ku9-wviI4lYiGv#kq@kO@O3HNBc12{V zj6KtVlMUu&bs(QMFd*?k<_n#V4Iobj`GnsnM-ugbuf)8JGLj7{kW4v>0VGG%h8n}z zs)jG0&AyrZVkDRqz2I3jx|Vwkf@4y8|92an|07nO1>4ye8Bn4 zNf(cnvW3ApJj-~_Z<@!t%xfdBjl6b|w<2j@kW?=`GqOs0aj+V?AJ;Li%RtmB5VaQk z_pe7(so@YZM>s&S*xvV~as#A=K9t}DNRfgNGTr3Yyq<`V3rI@<#UdDbO0Ap$+^gl; zx?YgGG9|xuxBmv!HJsvU*69{Wnb3*u3_}kZ-8BiPGolnA*P*`=rpJ*;`%!Z8BMYEs zX|z8B!Ev1Qsr`8n7)#1!KUw2-h%dj9q{M@+(*IxecA+_;6&eG)U>LL06bMHtkbWL- zPqe{yq-^y0sFYaw91%?3nLXh`VeWccu?Q7!l)a{m%yyrnGp`uG>OGF*VH9wVfGBE> z`-&iCJAf?5N#+YK>7mq2sV)g$_mmp$U74{(EH;*L%!QTgddiqUhk7__%vCRB;i>Ze zJRL0Hm2gbfp55U0X?k%Ya3W(yn3@De1TMYgHfimQ9he+V>RC5P%{ARkP%KZ+@ zy?`KZ%rAOgpIf`UHd}+dF+Ue#6*bp8FMtW``;}tDOPxTxPQtW?Nx@(8k-Xkdx`RT( z5x>W8R~S&h6_*g?ZV(m*3-XuT6|p*%{t^`u2>(w55f-~a6_%3t9Tv6&A%X+(J;Vh8 zTHIw|G9aIl+KKpG#5#+T1&KEh_p55uXqm{SkvxeSSa+&M11? z?T6wUEbvX{EGe3jnHQb)7bWretkbNkjjDTi5{^JysF76}Z7?lYGOrb*Wz- zLZk-^YlDeFAut^X`VBl5a`?3M!!P`L#wNE;$<@fwO0M>I;A)``eg%nj0)2S@FA-BV zia&>R4?CaaS!SWzTLS|!y@|(?JW9&(kju$W95IA^P7VQWU#29VVTyJQ<~&&@_xq-X z_EepU_(kdzJP0!UT>XmptJxP)z*mi$1o~4A?Mz>mc9E=BaHXOZ$;qrEm$raO9k!Fk z{zv48+f``px3jtmDf?q2j@0szE7N9HIgz$9w@5dRs;Z2X?Pcj{3s3$XJhma0p$njA zgW8}@4qV2YbMe-ODY@_hIAbkqh86N?jA>!8NR1|4${OI57C2=Cyi(GDRTY2C_PKO< zaK7Mv)F&Kib@)XI2g($ztPZr@B`0KQ3j2WWtza)Ayx~N~FqHKuJVLUPia|*UCsOJL zJH#L2jy(S(@WQj$XIV{&ngk#PN)$Q;%@D9N3!a-!RlLhQiB=^F2?}rGJq9$rs#8ah z>8waeQpuyIjv*}zb9i%)Ny?ftA@wvWP`wpCgAW)Gd&G|z7z`vxv?NIQ?OwbK1HsJn z>9@Ax@tmDj!3ZT%Ld74k{;BTGfT1F711QEp12}GWi8i!}&QyOvA@gtXBtJkvmEXZ> zre$&k=e`9jFreg5Pd`zD`4;kd>f5q}G7~VRaKXj<@Jnam!700Li3t?@OHBAVFab#L z6YNc;`KJ?T%di_l-$#C^^nWSozAxojNRS|L3upIEmi@o>&&UjMIWxuOccgs#q=r*&iVKjkoXLy`9m zBN2PO81_XP<&&wfFcoP}OAS-mSE}uML4O!L?hpGZ)cXyORyl}Q6!jA2BcV*!G49=) zi5MMo9X};}P`SL zf!Idp{JKo3YPl`>s9(E}65p2{^z8?%`gUTcC zFMR*a*Vf;7owmydZ@uy6L%3aNWE}P}pQWm+1RIGM6m0y(VYG3oo7R0~`Azns{OGr4 z#P7d;?;)m$DzE~Mnk=>fkO^U9l;f5O@hXbsO$8W*G1qU)q!b&KCDL!moI;mAjU;qZ z{3{+MQ8+*mV~NHpT@!b7_{((dN7~5v zp)#%;hktzX0F*W}#puYR<_$-R*9G2Ye-BoZsp+^R0hcKZCp5=jE9~<;3nU^H0cbum zU7)PA-l<5Vi108jCE%U}{oMV-=-_oBV$reGYD*cEiz>dx;1uY8ggg@mC?V)-HbprD zc3m#R;Y(*ud$Bu9j}&<;r(8}qrGMHcIKhz4Al*=+!Ni+$xsc?FTq}hw0Ny8H;zg7K z${2?~DhGSFu(5kv(XK$e-}7)Xdb`7b3L%7YOdyO{%71{#D=R`#p+|5{xh>0V#r_>} z2b~ljG9Ww7Z7N6!v=ewYhMO=M!PMWe)#Q~dRJ_UZZ!tJIgZVj_hLkU$TlTZf8Ff%t zvsb_-55IiYOg>+&oKse<;E{96mce^qsiFNCZ%CA|%9s-?)v=8=%rjj0nZAIm3mN#{F6NgJ{8>^Cgt@cIh+q zUb|W>iZaqp2Rd8F8LAib2=E@Pj#ywirtTMhmcrOerhqBNVVRm3$_D2;@tf;|1Vp9h z2`JZ?Y+n+P1BUr}Y8frG=ft~$Oo*V;)?2E#C41^vxs$}Z0zM)KBL$B8 z)p9%K&eMnptrTfTe1r<(hY0){{Lz7sel*1-Hu&kw2s)11!8e%kRRp;$r!Y4$4dNB% zp4`ad3&>mL#%&;?RaXq^bJX?D*627;4=lcnL9*T-#mIg zaop6Mi8+yIy%d|t`v@B3?}>+iuCos>PB6`!Jcc3M-E`&uneoH|5a@C<$vx09a%1hQ z;`dPv*|V*&f{f2CwB<}JW{b1&K{vXZwP*8I++SG?;dhw8 zGjLQ*YxKotOUFhCF9SI!Zd(9biu{(!pPWk@M@{6l@GQ{NaBQTtLHlTt{@x-6-|e#Y zBDBA-ehfdu1Re*B<6qcHh%|Wfa%Re#BEP~sE={bFj}nG(vU4ds6wh>gLki!P@$I(! zb_w6k%WquUOZc{eZ^*@}vh>%A(ruZ;GV4VdSIrtqmoeZau9sy5hQ?_z_tYH{Lfr{K z@~%9X-QFw2vc4eCrXZV?;2GfV12}MT^xc!K`_1xF9C0ddOmBur)KYgk7!}`Qo8?!|EKy8J!;!ojBVrb*rckCO zs^vy+5BBjrza~c{ueA8?h?2zPSon2_$S}PXMH3X0DxEBgYm&IBC;d{Vr3B&=bR#X{ zOlS@*J}w?!1mSfLT8f~_;a(f9Krcg$pe@J+C@w+QQ*VBub(z;kTwE1#WIb1YXBpa- z@EA4I1)ak$o&}^QE(4vAKZu; zHmD_N;8szoGl*}agk-%^@m8+uQ-ax56!|fyoO-o}$>C(GX|ga)hgZ&;AZcKm-2;+M zuXmPsA#oJ!MBQ#)>vq+hUBnyR?vveMSbV8;yJ6hxcEtsD?mG)!>ysAMlE@@G1yyJ=hCDi6%i!&D6I^7uEVO|`LWY?IwXPL^Iu zN|lr;zf4uRDpWm9xEBOL2dp3f9V7Ym{$=08`h1))Dy>WhbMbGz+@W-kzfu4^^|KXN=jG##@W zG@P#4?>fHkN;3+~zAKx7?BT)wTG!okyXL-g&<{L6GMhmcwVvNHcbw1_X5gC-@89Op zBp$gkTsL3gZI!wj>v#M?57(k_pAXzEl$M{kZLiUF(d8EAPkPj~xF)2tZ)cN)@l4B& z?6B{;qCsP29~Fg7{K9J>u41X+sU}pbAT&!C1@KP??}}Dglq^G(tr=lh6)}Uiuf&x{ zfg?WZM_%9`4k{)-qY6{Hed&g-ABASrcFleehTaaC6b1dD8?-#)4;a?;S_A3O5TUu> zc75=2k66)^K@WTk-Bu4{m_gG7FR8Z~U^uy6;rU+u0-sm(4zDyK1tKIOuZl!UAY1j> z*syKi>AAKYS8O}kER?IZ{e0kb(1^@9OO%Zskx z97KU1^nyXSmTutMmb2%zm=$Y33c-W5+pgPRd*C|Kr@NjLMy|Z>w1}tI{k-vYmNh$- zwe|jiEMwM1{K5qgL)X>uFF&0S0b<%FHQxgn7XeRcLb*f%0`| z>H|`lhl$2G|BOB59UiYWyjgxQ!@~2lO;o z21uk6nNVEt(uMS~(yc+R)|%aeRJ67sv?FEh1tg+02nq9EqH;7sBk1)9G{DY*IiOuL zqy1o7f3NXk0?03V3`oae3wWe1CYOmOkm{O_FSwqCeAFuNAu|Kv`D0D5HsrtyU6XaX zp04^StI(hudEk`UccM0|k#tRG&+)nr>EBhD9GDbvg>ST{om>EpHFX+Uf>7shu<14(hwob;JSo1P`&WhS&65vCgmGAUUHCT}eG} z;TQe^HhQQI6Zn`c zx&?7Atvippb6mG5Q08{mPG!(_RG5?-HtTR- zQ7}D3(2s24HKGlZC|Jk9bPFBW8K|-?9pMc^T67pW zeq%H%8G(^C!D87%Q7xI_iNR70w+S3T!&5fwIo$ztEVU<@6_|AUlf#UefUh2Ftl^X$ zni#O>ys)FT45Fy(ZkV_OBYUt0Neyo?RF^h+%;LQ^!j6Qk7<D zIwJ(lX6Uj&ow7GFfK49Tq0Fo{AX~s=Ioo|Vf1@pL;R-{n5_r*d{Z|6!>2B*U{+vEP zyV1fOKHu3)3(98e+x+>(jkL`_-l*$V;Y0f&k^4k$$C`-J_FvwR1XI$x|GAO&{@1Mc zt84~iQ@(|UR&`u>t@$aLw3bhkD{Hkow)+!e5?9AJJf5LD-mT<8#`(CE@YPx*ENb^W z9|D^&I6jkj*91h`U9aawVf{S7Rq75Ch`ArguCPirU*j_AU(e~sI@2>*+iYz>xAA=X zrkO&;OKF7!ytH9Dpxlz2M!Yb>vw&{yz>P2P60@F3rH$QdMeX>U9Rh6TUF-&Y6OCmR zY!StC_Bewv^c08VB^%Zwv@<@F;lhwf-8h%|K?Sdu>Ju>Lh`C9bxf_@5gtr*^b>=SH zr*Idm@_a8EV^+No>!I6i#^sFZMrK<_U{hWsW&!kaXZu%h7d`_~Y9+O*R<)9fuT~{m z;Ig`eZ&G54T0^Z&{3^JA!?$pFb{wUxXY)2l;ORqGg6Hh;DYRoZp12(C@a!Xm2@zWv zw-v0V?}gAt{t}wvvVqI=%gs@25oOZkqeVzFDB9tLiM32L%NImVXYk_%{RD-G&=B$nsb(S=Dh-vld?yGWiY`DNQ2ZB=R=MR((lQ zsKacMze=TFBl12Ggw2Zl4v}9cQYUg1B(9P|X8MxckrQVfD^sg;M1GkF$&IA2FfPI2 zyS}Jvych98V<5>(=i^N6j{F|=#4lV1Q6O)l0Hj)2QWw<2%M-gaE#(c~OX`~bc%#na z25ePiEmDR060Bw5sHF_v%}x2Ow&S4a+B(;fTgnj%&lxu$&m7=kGmLIA>Kn{50p_O(9tJGjjlqN%mqa zY-Tr`2A5-+v%6uogMDnUe=r8K&^?WZ^EqDIC24?ZjD?u7TlIOiG4e91;{sfFTmb5b ziwHhjZmf5KK4RT>Uqtd-)G@bF@uCf_lpb=-t&7RPP?7_j0gp#yG6`C?BD z6aFby7MdW6v7}bCnp#sA;FxtdX+t#%8oOZD*>@+z^bNx`$RE&$cuZxsn};Y23zA$C z2iU3t*@DbvGJ6ivI3$M$*SD+7Cy-eL-CsZ^U!12}NN)`iRXi>ZOXSd=fACa+v>{~2 zbCT@^NFR+|B7zlI z%;+`EZ8>nSC27WeBv`JQUSvA7YFtwV3A&V~9R&|O)#+&SN@5x5Vd9Y-Wd3?^xD5oN z2_U8SN^>o_D!yg{GoMRL+sN5*_08<4=Pb{>-Kl7J+Nvfo{}|h*8%+B`SY6WHt$H~D z3q|THslbVau35s|76YZ#YRLV!z&nuz`hw+D^2sc4apbFL0;1-IVT4(-59n#Ey z4fQU57fS!U^H6!-fwCgI3&sDp7Z;)hxD`@qQh3a9yhypB=l}3j$>u;y%7xXUrQ3)hX`;Ra{^c5xZ6cdQ2uI5Af>?8t=>a286wH7#en`h%u|fO7 z!Nq-aiOc-}dFf^p7ujybwI@j?J&ldxViGAESsE%XCOL`<5WPtA6^j5JIJU7{urhYo z_L|XHxI%$WY+$<)`{MwLkeYlCBy;~bH^atQRYaL~wlr%ljX4vvY%Sx6ZDduXixVkV z#74Gd@^@&C>O^X!oM!i?Y>sr%^k47`zYn4q2)_#}YOPRVKd-~eAnevkpcPO9wY9Nm-4@ci2+{_Ozmp^fa3|o zeGEFj;>~mcpK&37tJ2}(#8%K#M3LLz3&nYLZ5zeAxFa<~pfZn(Tp&Wnt8$6RZxZ={ z2xmR+P>Ev9#AH(1jje$6sT2_D6S*)+C}Srtfl%%@q)dlV(Y5L=LpSD)YGtPKR^?*l z{mO!&bE{nCNd|KCLBVee2;#;1kU;x;Q0&4nLe3Ll2@W}-{v0+Ife8?9GFYz=Sf>n? z^WWeLbM#hnrpdY$9rWR7o8*i9RJF6CmvKpB$2Q8i=jWI>oJo!%Gg|V8vuF*#rgmHBvy7CibN0+MX7 zAER~QB&dO)fL=CVb&`BeLW*?w47O$+H*#&8;sL*1F0onsGQIZE|0FVeatg=!&k(fw zsVN8^X}2zLa<5id_fw-mhlyJM2hLjC})-ysdGqM%d&y>%EzwWdjW^6DLs{G^zqyMP}X zl^qm@DkWPw_myd#0M?t{qpQ;rQ)lgPvXo;cNW3>!Cpeg$>c3I+ zrshgSE{WhK2Ew7~q_|HCCy7=jc5isW2}PU|;!$3dT<%yzzMlPWhNt6)Qw`{kGjDvz z|DxfiiO7DomxA2Snwziw7UGarKiQ<<^J7#1`UMf=&A4#j>?i)6VmwKiXn7W-USjcO zsf}j#C-k$S?x)W}4$;66v^?~&kgQoD-d5yXiGvNg-VYrt%p zq!w@%5nUG9J0KE;fs*7H Date: Thu, 23 Oct 2025 15:02:32 +0800 Subject: [PATCH 09/16] Delete src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__pycache__ directory --- .../tools/__pycache__/__init__.cpython-37.pyc | Bin 183 -> 0 bytes .../agents/tools/__pycache__/misc.cpython-37.pyc | Bin 5535 -> 0 bytes 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__pycache__/__init__.cpython-37.pyc delete mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__pycache__/misc.cpython-37.pyc diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__pycache__/__init__.cpython-37.pyc b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/tools/__pycache__/__init__.cpython-37.pyc deleted file mode 100644 index 280ae737210454c2718edd89d1ecc1929cfd1ae3..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 183 zcmZ?b<>g`kf}kS43=sVoM8E(ekl_Ht#VkM~g&~+hlhJP_LlH4_zo`FXmb#hH2Ox-O}y1-d?|iA8xJ zUT$J>NotXPVtQ&`NwI!Oetu4|etdjpUS>&ryk0@&Ee@O9{FKt1R6CH(pMjVG04B*W AN9iEVpiSKjy#%C zdRmsHn#}=xqAGWallH(NoH=pf9|#w?%>}ABP`SaC;=uRoc}UhKl^l?&d%Ao2`Fni7 zujjSx?WTd>1KB?QRgdt#Ow(@T~L8yveul#^o)(jg|&)^K*DM`3`>u&nS8xy{YDjn0+D z3*MVCpN7Jl1?t%Ir;md$@a5baOryRE;wahmKEg|ZoVj7#_eqhN zqtBMPZmci?%sCQ68tzBKP-K>f_{WC4f?of5Z|^uBi@o_2*)ULhuTE7Q#p8II zTzgj>2mMf7yYD{=hI%{KevC~FuN?|8x%P(eWkj{HpFmQ3{!l>Kdn%5@WN#cK{oTnt zYwL|x8#{*;lNK_=ZL%6W&)V!cCclb$pHVA*CQ=PaTk>@jQe$bH8Ygf-#g=BuMiw_O z7!|uI77{mCB)pXC{pFg2r>y=OGpq*OQ z8Vc2WBw!kWn#(OTW_I#{RMJLKI+T$%VSV*tKV1vmwes@pSsK%zY4l0IiE2WFjk;?} zs#UTjl&EB@P$4jT0@)TaHdAxNMkX-BTw3sXd*lEcY+b6II4M*0r3(bGQfJgSwN!Jt zm0G9lS0?boR`pRUt&O&k+T6}@1N~cR9c}ed8wq2eA7_yZ?OrLvL`xfnPD)yLR8$~7 zDJD`RWL)~ClEH8Yw|*3du-S45ylgNC`oIC8il3lX?aBq|E-qE~j&>mRz z?zmdzk{lz~_zsn{+8Zck?z_OfSl&9!t-#J4J)NfJlc<=G5IN!94R^F+n$L(TR zLMC9?9`nq_Z8Doec3O0(Xz}(+tlGUpr7*d>yFQO3ckP%fBki2awu`xPw3FNDA={Ku zV9MI%#JS!i*8R49jBGs@37t?Zf8>TveZkavkBAOf2Y zZTUkEKT56Sfo?-Ye%62oTHL|2p0Z^fcnsRE57rxFsHYzKnsXrf`~7}Ajnq{%)0)?xI!Hu2jX+v3 z@s00Zzv{)2Ku4U47ZIZ%D$18n**s`S5zs4G(mmGcOEI((!pd&Xj;=a=#sp*kaj%r@ zzK2>co4Hgs46d7gBj1-8=J)yB8b&h=3Qg zH9f*Xy@{j|1tYeBQ5I0e2C9Hv6sQ6$OKStV=2-qOp>k~tB|(z{ObYjb+}^a?mG)a% zrSBEOt~`vq6Cg8{z!Fs<`I*ba#;~(amUme9u(hkm=JaQWS7!4$GsH5${xo}r%dILU zLVW)C(tU9B=|taX#*;Y79gP66k`eHX>N%Qf@ljPCI+K5%iJ<6^l>(smFHl*I&*%V~9wI&OBgT~&Z>^sPC zh0gtDGb~^3=ajxT_J>iRrd;IxMi`WkksFQYSj&8ooZx$?RTPl;6s3De$sJ@cc@RB> zUOY!cd*KS8y?GtuYJTMHTA>lma3OnGsS}auh0pXl#=q4##GpH(N)jTsparVIIyNwt;=tq zt>a{FxrgkTGmB&v9ByqK-5J+#YNs?pAg{hGrg)zaw=?QBw!?Nz`F)ftMG*C@6!BA3 zND&ab=G`b0Cudp}Kqr(*Ew-{WCK2Pdhd?G}Eo%y2Ek-VV#{d%tgTXt>ZOVsq%V3KS z&nRgMqrt6?LPSnRN-*i&ASG3`n`g9Cy}EIAty}qx;*Bcm>(3O$M}x+i*eo11_pZvF zGG`D(gBTCGB;rm!NpKx(u2xzr-`t!^`{RZ#YWc~2jiqvDsI?gVpED)d{b%do_5Y6! z94+x*Xo>$|ib%^5@K+vDa*JDso+jSOmlp791i3=z3uG$qlAI4Hdj%P6|5Yke==?Gg z+$r&;wCdkr;M1~X@(^v+jhfgCNO2A<1c)i39vFHn1!Lv}7Utq-Yj&uCI=>l4)A13m zu^`bv^?>5vRZoP8@UEW`4!Z7p;KEaI*f{4~dd>pFaX2yse5JC`a}zY(a{Aw)PePkD zYLe96VUlVccE8ik>b)M1`@LRHogB Date: Sat, 25 Oct 2025 17:48:58 +0800 Subject: [PATCH 10/16] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E4=BA=86=E8=B7=91?= =?UTF-8?q?=E9=80=9A=E7=9A=84=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../braking_dqn.py | 668 ++++++++++++++++++ .../driving_dqn.py | 655 +++++++++++++++++ .../get_location.py | 45 ++ .../test_braking.py | 265 +++++++ .../test_driving.py | 294 ++++++++ 5 files changed, 1927 insertions(+) create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/braking_dqn.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/driving_dqn.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/get_location.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_braking.py create mode 100644 src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_driving.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/braking_dqn.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/braking_dqn.py new file mode 100644 index 0000000000..2e1714e262 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/braking_dqn.py @@ -0,0 +1,668 @@ +from __future__ import print_function + +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 + +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 tensorflow as tf +from tensorflow import keras + +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 tensorflow.keras.applications.xception import Xception +from tensorflow.keras.optimizers import Adam +from tensorflow.keras.models import Model +from tensorflow.keras.callbacks import TensorBoard +from tensorflow.keras.models import Sequential, Model, load_model +from tensorflow.keras.layers import AveragePooling2D, Conv2D, Activation, Flatten, GlobalAveragePooling2D, Dense, Concatenate, Input + +from threading import Thread +from tensorflow.keras import regularizers + +from tqdm import tqdm + +# 设置 TensorFlow 日志级别 +os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' + +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 = 40 +DISCOUNT = 0.99 +epsilon = 0.5 +EPSILON_DECAY = 0.95 #0.95 ## 0.9975 99975 +MIN_EPSILON = 0.01 + +AGGREGATE_STATS_EVERY = 1 + +# to set the intial and final locations +town2 = {1: [80, 306.6, 5, 0], 2:[194.01885986328125,262.87078857421875]} +curves = [0, town2] + +''' +Custom Tensorboard class. Updates logs after every episode +''' +class ModifiedTensorBoard(TensorBoard): + # Overriding init to set initial step and writer (we want one log file for all .fit() calls) + def __init__(self, model, log_dir): + super().__init__(log_dir) + self.log_dir = log_dir + self.model = model + self.step = 1 + print("SELF: LOG DIR: ", self.log_dir) + # TensorFlow 2.x 使用新的 FileWriter API + self.writer = tf.summary.create_file_writer(self.log_dir) + + # Overriding this method to stop creating default log writer + def set_model(self, model): + self.model = model + + # Overrided, saves logs with our step number + # (otherwise every .fit() will start writing from 0th step) + def on_epoch_end(self, epoch, logs=None): + self.update_stats(**logs) + + # Overrided + # We train for one batch only, no need to save anything at epoch end + def on_batch_end(self, batch, logs=None): + pass + + # Overrided, so won't close writer + def on_train_end(self, _): + pass + + # Custom method for saving own metrics + # Creates writer, writes custom metrics and closes writer + def update_stats(self, **stats): + self._write_logs(stats, self.step) + + def _write_logs(self, logs, index): + with self.writer.as_default(): + for name, value in logs.items(): + tf.summary.scalar(name, value, step=index) + self.writer.flush() + +''' +Defining the Carla Environment Class. +''' +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): + # 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.curves = 1 + self.reached = 0 + self.waypoint = self.client.get_world().get_map().get_waypoint(Location(x=curves[self.curves][1][0], y=curves[self.curves][1][1], z=curves[self.curves][1][2]), project_to_road=True) + self.final_destination = [180, 306.6] + + self.distance = None + self.cam = None + self.seg = None + + def reset(self): + # 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 = [] + + ''' + To spawn the Vehicle (agent) + ''' + initial_pos = curves[self.curves][1] + self.transform = Transform(Location(x=initial_pos[0], y=initial_pos[1], z=initial_pos[2]), Rotation(yaw=initial_pos[3])) + # to spawn the actor; the veichle + self.vehicle = self.world.spawn_actor(self.model_3, self.transform) + self.actor_list.append(self.vehicle) + + print("Spawning my agent.....") + + # 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)) + + # to initialize the car quickly and get it going + self.vehicle.apply_control(carla.VehicleControl(throttle = 0.0, brake = 0.0)) + time.sleep(4) + + ''' + To spawn the collision sensor + ''' + 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 spawn the lane crossing sensor + ''' + 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() + + self.episode_start = time.time() + + self.vehicle.apply_control(carla.VehicleControl(throttle = 1.0, brake = 0.0)) + + return [(self.distance-300)/300, -1] + + # to record the collision data + 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 + + # Calculate distance from camera to each point in world coordinates + distances = depth_map + + 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 + } + + for key in colors: + if key == 10: + self.vehicle_indices = np.where((self.seg_array == [0, 0, key]).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 + self.distance = dis + return dis + + def step(self, action, current_state): + ''' + To take 2 actions; braking or throttle + ''' + if action == 0: + self.vehicle.apply_control(carla.VehicleControl(throttle=0, brake = 1.0)) + else: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=0*self.STEER_AMT)) + + # 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 + + # to get the closest waypoint to the car + waypoint = self.trajectory()[0][0] + waypoint_loc = waypoint.transform.location + waypoint_rot = waypoint.transform.rotation + + dist_from_goal = np.sqrt((pos.x - self.final_destination[0])**2 + (pos.y-self.final_destination[1])**2) + + self.process_images() + + done = False + + ''' + TO DEFINE THE REWARDS + ''' + print(current_state[1]*30+30) + print(current_state[0]*300+300) + + if (current_state[0]*300+300)< (((current_state[1]+current_state[0])*30+30)*10 + 10): + if action == 0: + reward += 3 + else: + reward -= 3 + else: + if action == 1: + reward += 2 + else: + reward -= 2 + + if current_state[0]*300+300 > 100 and (current_state[1]+current_state[0])*30+30 < 1: + if action == 0: + reward -= 10 + + if self.distance<150 and kmh == 0: + done = True + reward = 200 + + # to avoid collisions + if len(self.collision_history) != 0: + done = True + reward = - 200 + + # to end the episode if the car reaches the final destination + if dist_from_goal < 1: + self.reached = 1 + done = True + + # to run each episode for just 30 secodns + if self.episode_start + 100 < time.time(): + done = True + + print(reward) + + return [(self.distance-300)/300, (kmh-30)/30-(self.distance-300)/300], reward, done, waypoint + + def trajectory(self, draw = False): + ''' + To get the trajectory + ''' + amap = self.world.get_map() + sampling_resolution = 2 + grp = GlobalRoutePlanner(amap, sampling_resolution) + + start_location = self.vehicle.get_transform().location + end_location = carla.Location(x=town2[2][0], y=town2[2][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() + a = a.transform.location + b = b.transform.location + w1 = grp.trace_route(a, b) + 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 + +''' +To define the Deep Q Network Agent +''' +class DQNAgent: + def __init__(self): + self.model = self.create_model() + self.target_model = self.create_model() + self.target_model.set_weights(self.model.get_weights()) + + self.replay_memory = deque(maxlen=REPLAY_MEMORY_SIZE) + + # TensorFlow 2.x 不再需要显式获取计算图 + # self.graph = tf.compat.v1.get_default_graph() + + self.tensorboard = ModifiedTensorBoard(self.model, log_dir=f"logs/{MODEL_NAME}-{int(time.time())}") + self.target_update_counter = 0 + + self.terminate = False + self.last_logged_episode = 0 + self.training_initialized = False + + def create_model(self): + # define the model + model3 = Sequential() + model3.add(Dense(2, input_shape=(2,), activation='linear', name='dense1')) + combined_model = Model(inputs=model3.input, outputs=model3.output) + + # compile the model - 使用新的学习率参数名 + combined_model.compile(loss='mse', optimizer=Adam(learning_rate=0.0001), metrics=['accuracy']) + + return combined_model + + def update_replay_memory(self, transition): + # transition = (current_state, action, reward, new_state, done) + self.replay_memory.append(transition) + + def train(self): + if len(self.replay_memory) < MIN_REPLAY_MEMORY_SIZE: + return + + # to sample a minibatch + minibatch = random.sample(self.replay_memory, MINIBATCH_SIZE) + + # to normalize the image + current_data = np.array([[transition[0][i] for i in range(2)] for transition in minibatch]) + # predicting all the datapoints present in the mini-batch + # TensorFlow 2.x 不再需要显式使用计算图 + current_qs_list = self.model.predict(current_data, PREDICTION_BATCH_SIZE, verbose=0) + + new_current_data = np.array([[transition[3][i] for i in range(2)] for transition in minibatch]) + future_qs_list = self.target_model.predict(new_current_data, PREDICTION_BATCH_SIZE, verbose=0) + + X_data = [] + y = [] + + for index, (current_state, action, reward, new_state, done) in enumerate(minibatch): + if not done: + max_future_q = np.max(future_qs_list[index]) + new_q = reward + DISCOUNT * max_future_q + else: + new_q = reward + + current_qs = current_qs_list[index] + current_qs[action] = new_q + + X_data.append([current_state[i] for i in range(2)]) + y.append(current_qs) + + log_this_step = False + if self.tensorboard.step > self.last_logged_episode: + log_this_step = True + self.last_logged_episode = self.tensorboard.step # 修复变量名错误 + + # to continuously train the base model + # TensorFlow 2.x 不再需要显式使用计算图 + self.model.fit(np.array(X_data), np.array(y), batch_size=TRAINING_BATCH_SIZE, + verbose=0, shuffle=False, + callbacks=[self.tensorboard] if log_this_step else None) + + if log_this_step: + self.target_update_counter += 1 + + # to assign the weights of the base model to the target model + if self.target_update_counter > UPDATE_TARGET_EVERY: + self.target_model.set_weights(self.model.get_weights()) + self.target_update_counter = 0 + + def get_qs(self, state): + return self.model.predict(np.array(state).reshape(-1, *np.array(state).shape), verbose=0)[0] + + def train_in_loop(self): + X2 = np.random.uniform(size=(1, 2)).astype(np.float32) + y = np.random.uniform(size=(1, 2)).astype(np.float32) + self.model.fit(X2, y, verbose=0, batch_size=1) + + self.training_initialized = True + + while True: + if self.terminate: + return + self.train() + time.sleep(0.01) + +if __name__ == '__main__': + FPS = 60 + + # For stats + ep_rewards = [-200] + + # For more repetitive results + random.seed(1) + np.random.seed(1) + tf.random.set_seed(1) # TensorFlow 2.x 设置随机种子 + + # TensorFlow 2.x 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}") + + fps_counter = deque(maxlen=60) + + # Create models folder + model_dir = "models" + if not os.path.isdir(model_dir): + os.makedirs(model_dir) + + # Create agent and environment + agent = DQNAgent() + env = CarEnv() + + # Start training thread and wait for training to be initialized + trainer_thread = Thread(target=agent.train_in_loop, daemon=True) + trainer_thread.start() + while not agent.training_initialized: + time.sleep(0.01) + + # Initialize predictions - first prediction takes longer as of initialization that has to be done + agent.get_qs([-1, -1]) + + # Connect to the Carla simulator + client = carla.Client('localhost', 2000) + client.set_timeout(20.0) + world = client.get_world() + + # Spawn a vehicle in the world + blueprint_library = world.get_blueprint_library() + vehicle_bp = blueprint_library.find('vehicle.tesla.model3') + + # to wait for the agent to spawn and start moving + time.sleep(5) + + ''' + Iterate over the episodes + ''' + for episode in tqdm(range(1, EPISODES + 1), ascii=True, unit='episodes'): + actor_list = [] + front_car_pos = np.random.randint(100,140) + spawn_point = carla.Transform(Location(x=front_car_pos, y=306.886, z=5), Rotation(yaw=0)) + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + actor_list.append(vehicle) + + env.waypoint = env.client.get_world().get_map().get_waypoint(Location(x=curves[env.curves][1][0], y=curves[env.curves][1][1], z=curves[env.curves][1][2]), project_to_road=True) + + env.reached = 0 + env.collision_hist = [] + env.via = 2 + # Update tensorboard step every episode + agent.tensorboard.step = episode + + # Restarting episode - reset episode reward and step number + episode_reward = 0 + step = 1 + + # Reset environment and get initial state + current_state = env.reset() + + # Reset flag and start iterating until episode ends + done = False + episode_start = time.time() + up_memory = [] + + # Play for given number of seconds only + while True: + # This part stays mostly the same, the change is to query a model for Q values + if np.random.random() > epsilon: + # Get action from Q table + qs = agent.get_qs(current_state) + print(qs, np.argmax(qs)) + action = np.argmax(qs) + else: + # Get random action + action = np.random.randint(0, 2) + if (current_state[0]*300+300)< (((current_state[1]+current_state[0])*30+30)*10 + 10): + action = 0 + else: + action = 1 + + # This takes no time, so we add a delay matching 60 FPS (prediction above takes longer) + time.sleep(1/FPS) + + new_state, reward, done, waypoint = env.step(action, current_state) + + # Transform new continous state to new discrete state and count reward + episode_reward += reward + + # Every step we update replay memory + agent.update_replay_memory((current_state, action, reward, new_state, done)) + + current_state = new_state + step += 1 + env.crossing=0 + + if done: + break + + print("EPISODE {} REWARD IS: {}".format(episode, episode_reward)) + + # End of episode - destroy agents + for actor in env.actor_list: + actor.destroy() + + for actor in actor_list: + actor.destroy() + + # Append episode reward to a list and log stats (every given number of episodes) + ep_rewards.append(episode_reward) + if not episode % AGGREGATE_STATS_EVERY or episode == 1: + average_reward = sum(ep_rewards[-AGGREGATE_STATS_EVERY:])/len(ep_rewards[-AGGREGATE_STATS_EVERY:]) + min_reward = min(ep_rewards[-AGGREGATE_STATS_EVERY:]) + max_reward = max(ep_rewards[-AGGREGATE_STATS_EVERY:]) + agent.tensorboard.update_stats(reward_avg=average_reward, reward_min=min_reward, reward_max=max_reward, epsilon=epsilon) + + # Save model, but only when min reward is greater or equal a set value + if min_reward >= MIN_REWARD: + agent.model.save(f'models/{MODEL_NAME}__{max_reward:_>7.2f}max_{average_reward:_>7.2f}avg_{min_reward:_>7.2f}min__{int(time.time())}.model') + + # Decay epsilon + if epsilon > MIN_EPSILON: + epsilon *= EPSILON_DECAY + epsilon = max(MIN_EPSILON, epsilon) + + # Set termination flag for training thread and wait for it to finish + agent.terminate = True + trainer_thread.join() + + # 保存最终模型 + if len(ep_rewards) > 1: + average_reward = sum(ep_rewards[-AGGREGATE_STATS_EVERY:])/len(ep_rewards[-AGGREGATE_STATS_EVERY:]) + min_reward = min(ep_rewards[-AGGREGATE_STATS_EVERY:]) + max_reward = max(ep_rewards[-AGGREGATE_STATS_EVERY:]) + agent.model.save(f'models/{MODEL_NAME}__{max_reward:_>7.2f}max_{average_reward:_>7.2f}avg_{min_reward:_>7.2f}min__{int(time.time())}.model') \ No newline at end of file diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/driving_dqn.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/driving_dqn.py new file mode 100644 index 0000000000..7cdff72c7a --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/driving_dqn.py @@ -0,0 +1,655 @@ +from __future__ import print_function + +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 + +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 tensorflow as tf +from tensorflow import keras + +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 tensorflow.keras.applications.xception import Xception +from tensorflow.keras.optimizers import Adam +from tensorflow.keras.models import Model +from tensorflow.keras.callbacks import TensorBoard +from tensorflow.keras.models import Sequential, Model, load_model +from tensorflow.keras.layers import AveragePooling2D, Conv2D, Activation, Flatten, GlobalAveragePooling2D, Dense, Concatenate, Input + +from threading import Thread +from tensorflow.keras import regularizers + +from tqdm import tqdm + +# 设置 TensorFlow 日志级别 +os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' + +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 = "Driving" + +MEMORY_FRACTION = 0.8 +MIN_REWARD = 0 + +EPISODES = 40 +DISCOUNT = 0.99 +epsilon = 0.5 +EPSILON_DECAY = 0.95 +MIN_EPSILON = 0.01 + +AGGREGATE_STATS_EVERY = 1 + +# path for training +town2 = {1: [193.75,269.2610168457031, 5, 270], 2:[135.25,206]} #left turn + +curves = [0, town2] + +''' +Custom Tensorboard class. Updates logs after every episode +''' +class ModifiedTensorBoard(TensorBoard): + # Overriding init to set initial step and writer (we want one log file for all .fit() calls) + def __init__(self, model, log_dir): + super().__init__(log_dir) + self.log_dir = log_dir + self.model = model + self.step = 1 + print("SELF: LOG DIR: ", self.log_dir) + # TensorFlow 2.x 使用 tf.summary.create_file_writer + self.writer = tf.summary.create_file_writer(self.log_dir) + + # Overriding this method to stop creating default log writer + def set_model(self, model): + self.model = model + # 在 TensorFlow 2.x 中不需要额外的设置 + + # Overrided, saves logs with our step number + def on_epoch_end(self, epoch, logs=None): + self.update_stats(**logs) + + # Overrided + # We train for one batch only, no need to save anything at epoch end + def on_batch_end(self, batch, logs=None): + pass + + # Overrided, so won't close writer + def on_train_end(self, _): + pass + + # Custom method for saving own metrics + # Creates writer, writes custom metrics and closes writer + def update_stats(self, **stats): + self._write_logs(stats, self.step) + + def _write_logs(self, logs, index): + with self.writer.as_default(): + for name, value in logs.items(): + tf.summary.scalar(name, value, step=index) + self.writer.flush() + +''' +Defining the Carla Environment Class. +''' +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): + # 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.via = 2 + self.crossing = 0 + self.curves = 1 + self.reached = 0 + self.start = town2[1] + self.phi = [] + self.dc = [] + self.vel = [] + self.time = [] + + def reset(self): + # 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 = [] + + ''' + To spawn the Vehicle (agent) + ''' + initial_pos = self.start + self.transform = Transform(Location(x=initial_pos[0], y=initial_pos[1], z=initial_pos[2]), Rotation(yaw=initial_pos[3])) + # to spawn the actor; the veichle + self.vehicle = self.world.spawn_actor(self.model_3, self.transform) + self.actor_list.append(self.vehicle) + + # 相机传感器设置（如果需要） + self.camera_spawn_point = carla.Transform(carla.Location(x=2, y=0, z=1.4)) + + # to initialize the car quickly and get it going + self.vehicle.apply_control(carla.VehicleControl(throttle = 0.0, brake = 0.0)) + time.sleep(4) + + ''' + To spawn the collision sensor + ''' + # 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)) + + traj = self.trajectory() + self.path = [] + for el in traj: + self.path.append(el[0]) + + # going to keep an episode length of 10 seconds otherwise the car learns to go around a circle and keeps doing the same thing + self.episode_start = time.time() + + self.vehicle.apply_control(carla.VehicleControl(throttle = 1.0, brake = 0.0)) + + return [0,0] #return [self.front_camera, 0,0, initial_pos[0], initial_pos[1]] + + def collision_data(self, event): + self.collision_history.append(event) + + def lanecrossing_data(self, event): + self.lanecrossing_history.append(event) + print("Lane crossing history: ", event) + + def step(self, action, current_state): + ''' + Take 5 actions; go straight, turn left, turn right, turn slightly left, turn slightly right + ''' + if action == 0: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=0*self.STEER_AMT)) + if action == 1: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=-0.6*self.STEER_AMT)) + if action == 2: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=0.6*self.STEER_AMT)) + if action == 3: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=-0.1*self.STEER_AMT)) + if action == 4: + self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=0.1*self.STEER_AMT)) + + # 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 + + # to get the closest waypoint to the car + waypoint = self.client.get_world().get_map().get_waypoint(pos, project_to_road=True) + waypoint_ind = self.get_closest_waypoint(self.path, waypoint) + 1 + print(waypoint_ind) + waypoint = self.path[waypoint_ind] + if len(self.path) != 1: + next_waypoint = self.path[waypoint_ind+1] + else: + next_waypoint = waypoint + waypoint_loc = waypoint.transform.location + waypoint_rot = waypoint.transform.rotation + next_waypoint_loc = next_waypoint.transform.location + next_waypoint_rot = next_waypoint.transform.rotation + + final = [curves[self.curves][2][0], curves[self.curves][2][1]] + final_destination = [curves[self.curves][self.via][0], curves[self.curves][self.via][1]] + dist_from_goal = np.sqrt((pos.x - final_destination[0])**2 + (pos.y-final_destination[1])**2) + + done = False + + ''' + TO DEFINE THE REWARDS + ''' + # to get the orientation difference between the car and the road "phi" + orientation_diff = waypoint_rot.yaw - rot.yaw + phi = orientation_diff%360 -360*(orientation_diff%360>180) + + current_state[1] = current_state[1]/15 + + u = [waypoint_loc.x-next_waypoint_loc.x, waypoint_loc.y-next_waypoint_loc.y] + v = [pos.x-next_waypoint_loc.x, pos.y-next_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 + + print(current_state[0]) + print(current_state[1]) + + # Defining the Reward function by comparing the action taken to a suboptimal policy + if abs(current_state[0])<5: + if action == 0: + reward += 2 + else: + reward -= 1 + elif abs(current_state[0])<10: + if current_state[0]<0: + if action == 3: + reward += 2 + elif action == 1: + reward += 1 + else: + reward -= 1 + else: + if action == 4: + reward += 2 + elif action == 2: + reward += 1 + else: + reward -= 1 + else: + if current_state[0]<0: + if action == 1: + reward += 2 + elif action == 3: + reward += 1 + else: + reward -= 1 + else: + if action == 2: + reward += 2 + elif action == 4: + reward += 1 + else: + reward -= 1 + + if abs(current_state[1])<0.1: + if action == 0: + reward += 4 + else: + reward -= 2 + elif abs(current_state[1])<0.5: + if current_state[1]<0: + if action == 3: + reward += 2 + elif action == 1: + reward += 1 + else: + reward -= 1 + else: + if action == 4: + reward += 2 + elif action == 2: + reward += 1 + else: + reward -= 1 + else: + if current_state[1]<0: + if action == 1: + reward += 2 + elif action == 3: + reward += 1 + else: + reward -= 1 + else: + if action == 2: + reward += 2 + elif action == 4: + reward += 1 + else: + reward -= 1 + + + if abs(signed_dis)>2: + reward -= 10 + + # to avoid collisions + if len(self.collision_history) != 0: + done = True + reward = - 200 + + # to end the episode if phi value goes high + if abs(phi)>100: + done = True + reward = -200 + + # Ending the episode if the distance to the centerline of the road is greater than 3 + if abs(signed_dis)>3: + done = True + reward = -200 + + # to end the episode if the car reaches close to the final destination + if dist_from_goal < 5: + self.reached = 1 + done = True + + # to run each episode for just 30 secodns + if self.episode_start + 200 < time.time(): + done = True + + print(reward) + + self.phi.append(phi) + self.dc.append(signed_dis) + self.vel.append(kmh) + self.time.append(time.time()) + + return [phi, signed_dis*15], reward, done, waypoint + + def trajectory(self, draw = False): + amap = self.world.get_map() + sampling_resolution = 0.5 + grp = GlobalRoutePlanner(amap, sampling_resolution) + + start_location = carla.Location(x=self.start[0], y=self.start[1], z=0) + end_location = carla.Location(x=town2[2][0], y=town2[2][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() + a = a.transform.location + b = b.transform.location + w1 = grp.trace_route(a, b) + 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 get_closest_waypoint(self, waypoint_list, target_waypoint): + closest_waypoint = None + closest_distance = float('inf') + for i, waypoint in enumerate(waypoint_list): + distance = math.sqrt((waypoint.transform.location.x - target_waypoint.transform.location.x)**2 + + (waypoint.transform.location.y - target_waypoint.transform.location.y)**2) + if distance < closest_distance: + closest_waypoint = i + closest_distance = distance + return closest_waypoint + +''' +TO define the Deep Q Network agent class +''' +class DQNAgent: + def __init__(self): + self.model = self.create_model() + self.target_model = self.create_model() + self.target_model.set_weights(self.model.get_weights()) + + self.replay_memory = deque(maxlen=REPLAY_MEMORY_SIZE) + + # TensorFlow 2.x 不再需要显式获取计算图 + # self.graph = tf.compat.v1.get_default_graph() + + self.tensorboard = ModifiedTensorBoard(self.model, log_dir=f"logs/{MODEL_NAME}-{int(time.time())}") + self.target_update_counter = 0 + + self.terminate = False + self.last_logged_episode = 0 + self.training_initialized = False + + def create_model(self): + model3 = Sequential() + model3.add(Dense(8, input_shape=(2,), activation='relu', name='dense1')) + model3.add(Dense(5, activation='linear', name='output')) + combined_model = Model(inputs=model3.input, outputs=model3.output) + + # compile the model - 使用新的学习率参数名 + combined_model.compile(loss='mse', optimizer=Adam(learning_rate=0.0001), metrics=['accuracy']) + + return combined_model + + def update_replay_memory(self, transition): + # transition = (current_state, action, reward, new_state, done) + self.replay_memory.append(transition) + + def train(self): + if len(self.replay_memory) < MIN_REPLAY_MEMORY_SIZE: + return + + # to sample a minibatch + minibatch = random.sample(self.replay_memory, MINIBATCH_SIZE) + + # to normalize the image + current_data = np.array([[transition[0][i] for i in range(2)] for transition in minibatch]) + # predicting all the datapoints present in the mini-batch + # TensorFlow 2.x 不再需要显式使用计算图 + current_qs_list = self.model.predict(current_data, PREDICTION_BATCH_SIZE, verbose=0) + + new_current_data = np.array([[transition[3][i] for i in range(2)] for transition in minibatch]) + future_qs_list = self.target_model.predict(new_current_data, PREDICTION_BATCH_SIZE, verbose=0) + + X_data = [] + y = [] + + for index, (current_state, action, reward, new_state, done) in enumerate(minibatch): + if not done: + max_future_q = np.max(future_qs_list[index]) + new_q = reward + DISCOUNT * max_future_q + else: + new_q = reward + + current_qs = current_qs_list[index] + current_qs[action] = new_q + + X_data.append([current_state[i] for i in range(2)]) + y.append(current_qs) + + log_this_step = False + if self.tensorboard.step > self.last_logged_episode: + log_this_step = True + self.last_logged_episode = self.tensorboard.step # 修复变量名错误 + + # to continuously train the base model + # TensorFlow 2.x 不再需要显式使用计算图 + self.model.fit(np.array(X_data), np.array(y), batch_size=TRAINING_BATCH_SIZE, + verbose=0, shuffle=False, + callbacks=[self.tensorboard] if log_this_step else None) + + if log_this_step: + self.target_update_counter += 1 + + # to assign the weights of the base model to the target model + if self.target_update_counter > UPDATE_TARGET_EVERY: + self.target_model.set_weights(self.model.get_weights()) + self.target_update_counter = 0 + + def get_qs(self, state): + return self.model.predict(np.array(state).reshape(-1, *np.array(state).shape), verbose=0)[0] + + def train_in_loop(self): + X2 = np.random.uniform(size=(1, 2)).astype(np.float32) + y = np.random.uniform(size=(1, 5)).astype(np.float32) + self.model.fit(X2, y, verbose=0, batch_size=1) + + self.training_initialized = True + + while True: + if self.terminate: + return + self.train() + time.sleep(0.01) + +if __name__ == '__main__': + FPS = 400 + # For stats + ep_rewards = [-200] + + # For more repetitive results + random.seed(1) + np.random.seed(1) + tf.random.set_seed(1) # TensorFlow 2.x 设置随机种子 + + # TensorFlow 2.x 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}") + + # 创建模型目录 + model_dir = "models" + if not os.path.isdir(model_dir): + os.makedirs(model_dir) + + # Create agent and environment + agent = DQNAgent() + env = CarEnv() + + # Start training thread and wait for training to be initialized + trainer_thread = Thread(target=agent.train_in_loop, daemon=True) + trainer_thread.start() + while not agent.training_initialized: + time.sleep(0.01) + + # Initialize predictions - first prediction takes longer as of initialization that has to be done + agent.get_qs([0,0]) + + # Iterate over episodes + for episode in tqdm(range(1, EPISODES + 1), ascii=True, unit='episodes'): + if episode%2 == 0: + town2 = {1: [182.65191650390625,236.9, 5, 180], 2:[135.25,206]} + env.start = town2[1] + else: + town2 = {1: [193.75,269.2610168457031, 5, 270], 2:[135.25,206]} + env.start = town2[1] + env.reached = 0 + env.collision_hist = [] + # Update tensorboard step every episode + agent.tensorboard.step = episode + + # Restarting episode - reset episode reward and step number + episode_reward = 0 + step = 1 + + # Reset environment and get initial state + current_state = env.reset() + + # Reset flag and start iterating until episode ends + done = False + episode_start = time.time() + + # Play for given number of seconds only + while True: + # This part stays mostly the same, the change is to query a model for Q values + if np.random.random() > epsilon: + # Get action from Q table + qs = agent.get_qs(current_state) + print(qs) + action = np.argmax(qs) + else: + # Get random action + action = np.random.randint(0, 5) + time.sleep(1/FPS) + + new_state, reward, done, waypoint = env.step(action, current_state) + + # Transform new continous state to new discrete state and count reward + episode_reward += reward + + # Every step we update replay memory + agent.update_replay_memory((current_state, action, reward, new_state, done)) + + current_state = new_state + step += 1 + + if done: + break + + print("EPISODE {} REWARD IS: {}".format(episode, episode_reward)) + + # End of episode - destroy agents + for actor in env.actor_list: + actor.destroy() + + # Append episode reward to a list and log stats (every given number of episodes) + ep_rewards.append(episode_reward) + if not episode % AGGREGATE_STATS_EVERY or episode == 1: + average_reward = sum(ep_rewards[-AGGREGATE_STATS_EVERY:])/len(ep_rewards[-AGGREGATE_STATS_EVERY:]) + min_reward = min(ep_rewards[-AGGREGATE_STATS_EVERY:]) + max_reward = max(ep_rewards[-AGGREGATE_STATS_EVERY:]) + agent.tensorboard.update_stats(reward_avg=average_reward, reward_min=min_reward, reward_max=max_reward, epsilon=epsilon) + + # Save model, but only when min reward is greater or equal a set value + if min_reward >= MIN_REWARD: + agent.model.save(f'models/{MODEL_NAME}__{max_reward:_>7.2f}max_{average_reward:_>7.2f}avg_{min_reward:_>7.2f}min__{int(time.time())}.model') + + # Decay epsilon + if epsilon > MIN_EPSILON: + epsilon *= EPSILON_DECAY + epsilon = max(MIN_EPSILON, epsilon) + + # Set termination flag for training thread and wait for it to finish + agent.terminate = True + trainer_thread.join() + + # 保存最终模型 + if len(ep_rewards) > 1: + average_reward = sum(ep_rewards[-AGGREGATE_STATS_EVERY:])/len(ep_rewards[-AGGREGATE_STATS_EVERY:]) + min_reward = min(ep_rewards[-AGGREGATE_STATS_EVERY:]) + max_reward = max(ep_rewards[-AGGREGATE_STATS_EVERY:]) + agent.model.save(f'models/{MODEL_NAME}__{max_reward:_>7.2f}max_{average_reward:_>7.2f}avg_{min_reward:_>7.2f}min__{int(time.time())}.model') \ No newline at end of file diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/get_location.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/get_location.py new file mode 100644 index 0000000000..4faf20c628 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/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/test_braking.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_braking.py new file mode 100644 index 0000000000..9f3d99aa40 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_braking.py @@ -0,0 +1,265 @@ +import os +os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' # 只显示错误信息 +import random +from collections import deque +import numpy as np +import cv2 +import time +import tensorflow as tf +from tensorflow.keras.models import load_model +from braking_dqn import CarEnv, MEMORY_FRACTION +import carla +from carla import Transform +from carla import Location +from carla import Rotation + +town2 = {1: [80, 306.6, 5, 0], 2:[150,306.6]} +curves = [0, town2] + +epsilon = 0 +MODEL_PATH = "models/Braking___282.model" + +def setup_tensorflow(): + """设置 TensorFlow 2.x 配置""" + # 设置日志级别减少输出 + os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' + + print(f"TensorFlow 版本: {tf.__version__}") + + # GPU 配置 + gpus = tf.config.list_physical_devices('GPU') + if gpus: + try: + # 设置GPU内存按需增长 + 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}") + # 如果GPU设置失败，回退到CPU + os.environ['CUDA_VISIBLE_DEVICES'] = '-1' + print("使用CPU运行") + else: + print("ℹ️ 未找到GPU，使用CPU运行") + +def safe_load_model(model_path): + """安全加载模型，处理可能的兼容性问题""" + try: + print(f"尝试加载模型: {model_path}") + + # 检查文件是否存在 + if not os.path.exists(model_path): + print(f"❌ 模型文件不存在: {model_path}") + return None + + # 尝试不同的加载方式 + try: + # 方式1: 直接加载 + model = load_model(model_path) + print(f"✅ 成功加载模型: {model_path}") + return model + except Exception as e: + print(f"标准加载失败，尝试自定义对象加载: {e}") + try: + # 方式2: 使用 compile=False + model = load_model(model_path, compile=False) + # 重新编译模型 + model.compile(optimizer='adam', loss='mse') + print(f"✅ 使用 compile=False 成功加载模型: {model_path}") + return model + except Exception as e2: + print(f"❌ 所有加载方式都失败: {e2}") + return None + + except Exception as e: + print(f"❌ 加载模型时发生错误 {model_path}: {e}") + return None + +def preprocess_state_for_prediction(state_data): + """预处理状态数据用于模型预测""" + try: + if isinstance(state_data, list): + state_array = np.array(state_data) + else: + state_array = state_data + + # 确保正确的形状 + 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]]) + +if __name__ == '__main__': + + FPS = 60 + EPISODES = 1 + + # 设置 TensorFlow + setup_tensorflow() + + # 加载模型 + print("\n" + "="*50) + print("加载刹车模型") + print("="*50) + + model = safe_load_model(MODEL_PATH) + + # 如果模型加载失败，创建新的模型 + if model is None: + print("❌ 无法加载模型，程序退出") + exit(1) + + print(f"✅ 模型加载完成:") + print(f" - 输入形状: {model.input_shape}") + print(f" - 输出形状: {model.output_shape}") + + # 创建环境 + print("\n初始化CARLA环境...") + env = CarEnv() + + # 用于FPS计算 - 保持最近60帧的时间 + fps_counter = deque(maxlen=60) + + # 初始化预测 - 第一次预测需要初始化时间 + print("预热模型...") + try: + # 使用正确的预处理 + dummy_state = preprocess_state_for_prediction([0, 0]) + model.predict(dummy_state, verbose=0) + print("✅ 模型预热完成") + except Exception as e: + print(f"⚠️ 模型预热警告: {e}") + + # 循环 episodes + for episode in range(EPISODES): + print(f'\n{"="*50}') + print(f'开始 Episode {episode + 1}/{EPISODES}') + print(f'{"="*50}') + + # 设置路径点 + try: + env.waypoint = env.client.get_world().get_map().get_waypoint( + Location(x=curves[env.curves][1][0], y=curves[env.curves][1][1], z=curves[env.curves][1][2]), + project_to_road=True + ) + except Exception as e: + print(f"⚠️ 设置路径点错误: {e}") + + # 重置环境并获取初始状态 + current_state = env.reset() + if hasattr(env, 'collision_hist'): + env.collision_hist = [] + + # 生成轨迹 + if hasattr(env, 'trajectory'): + env.trajectory() + + done = False + step_count = 0 + + # 循环步骤 + while not done: + step_count += 1 + + # FPS 计数器 + step_start = time.time() + + # 显示当前帧（可选） + if len(current_state) > 0 and isinstance(current_state[0], np.ndarray): + print("当前帧") + cv2.imshow(f'Agent - preview', current_state[0]) + cv2.waitKey(1) + + # 预测基于当前观察空间的动作 + action = None + qs = None + + try: + if np.random.random() > epsilon: + # 从 Q 表获取动作 + state_for_prediction = preprocess_state_for_prediction(current_state) + qs = model.predict(state_for_prediction, verbose=0)[0] + action = np.argmax(qs) + else: + # 获取随机动作 + action = np.random.randint(0, 2) + # 这不需要时间，所以我们添加匹配 60 FPS 的延迟（上面的预测需要更长时间） + if len(fps_counter) > 0: + time.sleep(sum(fps_counter) / len(fps_counter)) + else: + time.sleep(1/FPS) + + except Exception as e: + print(f"❌ 预测错误: {e}") + action = 0 # 默认安全动作 + qs = np.zeros(2) # 默认 Q 值 + + + # 环境步骤（额外的标志通知环境不要因时间限制而中断episode） + try: + + new_state, reward, done, waypoint = env.step(action, current_state) + + # 设置下一步的当前状态 + current_state = new_state + + # 保存路径点 + if hasattr(env, 'waypoint'): + env.waypoint = waypoint + + except Exception as e: + print(f"❌ 环境步骤错误: {e}") + done = True + + # 如果完成 - 代理崩溃，中断episode + if done: + print(f"Episode {episode + 1} 完成，步数: {step_count}") + break + + # 测量步骤时间，添加到deque，然后打印最近60帧的平均FPS、q值和采取的动作 + frame_time = time.time() - step_start + fps_counter.append(frame_time) + + if len(fps_counter) > 0: + current_fps = len(fps_counter) / sum(fps_counter) + else: + current_fps = 0 + + # 安全地打印Q值 + try: + if qs is not None: + qs_display = f"[{qs[0]:>5.2f}, {qs[1]:>5.2f}]" + else: + qs_display = "[N/A, N/A]" + + print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Q-values: {qs_display} | Action: {action}') + except Exception as e: + print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Action: {action}') + + # 在episode结束时销毁actor + print(f"清理 Episode {episode + 1} 的actor...") + try: + if hasattr(env, 'actor_list'): + for actor in env.actor_list: + try: + actor.destroy() + except Exception as e: + print(f"销毁actor错误: {e}") + except Exception as e: + print(f"清理错误: {e}") + + print("\n" + "="*50) + print("所有episodes完成!") + print("="*50) + + # 清理资源 + try: + cv2.destroyAllWindows() + except: + pass + + print("程序正常退出") \ No newline at end of file diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_driving.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_driving.py new file mode 100644 index 0000000000..ab70f07cc1 --- /dev/null +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_driving.py @@ -0,0 +1,294 @@ +import os +os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' # 只显示错误信息 +import random +from collections import deque +import numpy as np +import cv2 +import time +import tensorflow as tf +from tensorflow.keras.models import load_model +from driving_dqn import CarEnv, MEMORY_FRACTION + +epsilon = 0.05 +MODEL_PATH = "models/Driving__6030.model" + +def setup_tensorflow(): + """设置 TensorFlow 2.x 配置""" + # 设置日志级别减少输出 + os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' + + print(f"TensorFlow 版本: {tf.__version__}") + + # GPU 配置 + gpus = tf.config.list_physical_devices('GPU') + if gpus: + try: + # 设置GPU内存按需增长 + 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}") + # 如果GPU设置失败，回退到CPU + os.environ['CUDA_VISIBLE_DEVICES'] = '-1' + print("使用CPU运行") + else: + print("ℹ️ 未找到GPU，使用CPU运行") + +def safe_load_model(model_path): + """安全加载模型，处理可能的兼容性问题""" + try: + print(f"尝试加载模型: {model_path}") + + # 检查文件是否存在 + if not os.path.exists(model_path): + print(f"❌ 模型文件不存在: {model_path}") + return None + + # 尝试不同的加载方式 + try: + # 方式1: 直接加载 + model = load_model(model_path) + print(f"✅ 成功加载模型: {model_path}") + return model + except Exception as e: + print(f"标准加载失败，尝试自定义对象加载: {e}") + try: + # 方式2: 使用 compile=False + model = load_model(model_path, compile=False) + # 重新编译模型 + model.compile(optimizer='adam', loss='mse') + print(f"✅ 使用 compile=False 成功加载模型: {model_path}") + return model + except Exception as e2: + print(f"❌ 所有加载方式都失败: {e2}") + return None + + except Exception as e: + print(f"❌ 加载模型时发生错误 {model_path}: {e}") + return None + +def create_compatible_model(input_shape=(4,), output_units=5): + """创建兼容的模型（如果加载失败时使用）""" + print("创建新的驾驶模型...") + + model = tf.keras.Sequential([ + tf.keras.layers.Dense(128, activation='relu', input_shape=input_shape), + tf.keras.layers.Dropout(0.3), + tf.keras.layers.Dense(64, activation='relu'), + tf.keras.layers.Dropout(0.2), + tf.keras.layers.Dense(32, activation='relu'), + tf.keras.layers.Dense(output_units, activation='linear') + ]) + + model.compile( + optimizer=tf.keras.optimizers.Adam(learning_rate=0.001), + loss='mse', + metrics=['mae'] + ) + + return model + +def preprocess_state_for_prediction(state_data): + """预处理状态数据用于模型预测""" + try: + if isinstance(state_data, list): + state_array = np.array(state_data) + else: + state_array = state_data + + # 确保正确的形状 + 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, 0, 0]]) + +if __name__ == '__main__': + + FPS = 60 + EPISODES = 10 + + # 设置 TensorFlow + setup_tensorflow() + + # 加载模型 + print("\n" + "="*50) + print("加载驾驶模型") + print("="*50) + + model = safe_load_model(MODEL_PATH) + + # 如果模型加载失败，创建新的模型 + if model is None: + print("创建新的驾驶模型...") + model = create_compatible_model(input_shape=(4,), output_units=5) + + print(f"✅ 模型加载完成:") + print(f" - 输入形状: {model.input_shape}") + print(f" - 输出形状: {model.output_shape}") + + # 创建环境 + print("\n初始化CARLA环境...") + env = CarEnv() + + # 用于FPS计算 - 保持最近60帧的时间 + fps_counter = deque(maxlen=60) + + # 初始化预测 - 第一次预测需要初始化时间 + print("预热模型...") + try: + # 使用正确的预处理 + dummy_state = preprocess_state_for_prediction([0, 0, 0, 0]) + model.predict(dummy_state, verbose=0) + print("✅ 模型预热完成") + except Exception as e: + print(f"⚠️ 模型预热警告: {e}") + + # 循环 episodes + for episode in range(EPISODES): + print(f'\n{"="*50}') + print(f'开始 Episode {episode + 1}/{EPISODES}') + print(f'{"="*50}') + + # 重置环境并获取初始状态 + current_state = env.reset() + + # 重置环境数据 + if hasattr(env, 'collision_hist'): + env.collision_hist = [] + + # 初始化数据记录列表 + env.phi = [] + env.dc = [] + env.vel = [] + env.time = [] + + # 生成轨迹 + if hasattr(env, 'trajectory'): + env.trajectory() + + done = False + step_count = 0 + + # 循环步骤 + while not done: + step_count += 1 + + # FPS 计数器 + step_start = time.time() + + # 显示当前帧（可选） + # if len(current_state) > 0 and isinstance(current_state[0], np.ndarray): + # cv2.imshow(f'Agent - preview', current_state[0]) + # cv2.waitKey(1) + + # 预测基于当前观察空间的动作 + action = None + qs = None + + try: + if np.random.random() > epsilon or step_count == 1: + # 从 Q 表获取动作 + state_for_prediction = preprocess_state_for_prediction(current_state) + qs = model.predict(state_for_prediction, verbose=0)[0] + action = np.argmax(qs) + else: + # 获取随机动作 + action = np.random.randint(0, 5) + # 这不需要时间，所以我们添加匹配 60 FPS 的延迟（上面的预测需要更长时间） + if len(fps_counter) > 0: + time.sleep(sum(fps_counter) / len(fps_counter)) + else: + time.sleep(1/FPS) + + except Exception as e: + print(f"❌ 预测错误: {e}") + action = 0 # 默认安全动作 + qs = np.zeros(5) # 默认 Q 值 + + # 环境步骤（额外的标志通知环境不要因时间限制而中断episode） + try: + new_state, reward, done, waypoint = env.step(action, current_state) + + # 设置下一步的当前状态 + current_state = new_state + + # 保存路径点 + if hasattr(env, 'waypoint'): + env.waypoint = waypoint + + except Exception as e: + print(f"❌ 环境步骤错误: {e}") + done = True + + # 如果完成 - 代理崩溃，中断episode + if done: + print(f"Episode {episode + 1} 完成，步数: {step_count}") + break + + # 测量步骤时间，添加到deque，然后打印最近60帧的平均FPS、q值和采取的动作 + frame_time = time.time() - step_start + fps_counter.append(frame_time) + + if len(fps_counter) > 0: + current_fps = len(fps_counter) / sum(fps_counter) + else: + current_fps = 0 + + # 安全地打印Q值 + try: + if qs is not None: + qs_display = f"[{qs[0]:>5.2f}, {qs[1]:>5.2f}, {qs[2]:>5.2f}, {qs[3]:>5.2f}, {qs[4]:>5.2f}]" + else: + qs_display = "[N/A, N/A, N/A, N/A, N/A]" + + print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Q-values: {qs_display} | Action: {action}') + except Exception as e: + print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Action: {action}') + + # 保存数据 + print(f"保存 Episode {episode + 1} 的数据...") + try: + os.makedirs(f"data/traj4/file{episode}", exist_ok=True) + + # 安全地保存数据，检查列表是否为空 + if hasattr(env, 'phi') and env.phi: + np.savetxt(f"data/traj4/file{episode}/phi.txt", env.phi) + if hasattr(env, 'dc') and env.dc: + np.savetxt(f"data/traj4/file{episode}/d.txt", env.dc) + if hasattr(env, 'vel') and env.vel: + np.savetxt(f"data/traj4/file{episode}/vel.txt", env.vel) + if hasattr(env, 'time') and env.time: + np.savetxt(f"data/traj4/file{episode}/time.txt", env.time) + + print(f"✅ Episode {episode + 1} 数据保存完成") + except Exception as e: + print(f"❌ 保存数据错误: {e}") + + # 在episode结束时销毁actor + print(f"清理 Episode {episode + 1} 的actor...") + try: + if hasattr(env, 'actor_list'): + for actor in env.actor_list: + try: + actor.destroy() + except Exception as e: + print(f"销毁actor错误: {e}") + except Exception as e: + print(f"清理错误: {e}") + + print("\n" + "="*50) + print("所有episodes完成!") + print("="*50) + + # 清理资源 + try: + cv2.destroyAllWindows() + except: + pass + + print("程序正常退出") \ No newline at end of file From d7a2025376263863d01577f64e7d94f904c709f6 Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Tue, 18 Nov 2025 10:04:22 +0800 Subject: [PATCH 11/16] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E4=BA=86=E6=A8=A1?= =?UTF-8?q?=E5=9E=8B=E6=96=87=E4=BB=B6=E8=AF=BB=E5=8F=96=EF=BC=8C=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E4=BA=86=E8=BD=A6=E8=BE=86=E8=A7=86=E8=A7=92=E8=B7=9F?= =?UTF-8?q?=E9=9A=8F=E5=8A=9F=E8=83=BD=E5=92=8C=E7=94=9F=E6=88=90=E8=BD=A6?= =?UTF-8?q?=E8=BE=86=E6=8C=89=E8=B7=AF=E7=BA=BF=E5=BD=A2=E5=BC=8F=E5=8A=9F?= =?UTF-8?q?=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../car_env.py | 389 +++++++++--------- .../test_braking.py | 2 +- .../test_driving.py | 2 +- .../test_everything.py | 332 ++++++++------- 4 files changed, 372 insertions(+), 353 deletions(-) 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 78eae4c4d1..4cc0d3a86e 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 @@ -3,6 +3,7 @@ import glob import os import sys +import random try: sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( @@ -14,7 +15,6 @@ from agents.navigation.global_route_planner import GlobalRoutePlanner - import carla from carla import ColorConverter as cc @@ -47,9 +47,6 @@ from keras.models import Sequential, Model, load_model from keras.layers import AveragePooling2D, Conv2D, Activation, Flatten, GlobalAveragePooling2D, Dense, Concatenate, Input - -#from tensorboard import * - import tensorflow as tf import keras.backend.tensorflow_backend as backend from threading import Thread @@ -58,7 +55,6 @@ from tqdm import tqdm - SHOW_PREVIEW = False IM_WIDTH = 640 IM_HEIGHT = 480 @@ -82,14 +78,6 @@ AGGREGATE_STATS_EVERY = 10 - -#MODEL_PATH = 'models/Xception__-518.00max_-766.40avg_-1097.00min__1677834457.model' - - - -''' -Defining the Carla Environment Class. -''' class CarEnv: SHOW_CAM = SHOW_PREVIEW @@ -98,9 +86,8 @@ class CarEnv: im_height = IM_HEIGHT front_camera = None - def __init__(self, start, end): - # to initialize + # to initialize self.client = carla.Client("localhost", 2000) self.client.set_timeout(20.0) self.world = self.client.get_world() @@ -115,10 +102,56 @@ def __init__(self, start, end): 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 @@ -126,16 +159,53 @@ def reset(self): # store the number of times the vehicles crosses the lane marking self.lanecrossing_history = [] - self.transform = Transform(Location(x=self.initial_pos[0], y=self.initial_pos[1], z=self.initial_pos[2]), Rotation(yaw=self.initial_pos[3])) - # to spawn the actor; the veichle - self.vehicle = self.world.spawn_actor(self.model_3, self.transform) + # 先计算轨迹，以便获取正确的朝向 + 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_type', '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") @@ -162,15 +232,13 @@ def reset(self): # to spawn the camera self.seg_camera_sensor = self.world.spawn_actor(self.seg_camera, self.seg_camera_spawn_point, attach_to = self.vehicle) - #print("Segmentation camera image sent for processing....") self.actor_list.append(self.seg_camera_sensor) self.seg_camera_sensor.listen(lambda data: self.image_seg(data)) - # to initialize the car quickly and get it going - self.vehicle.apply_control(carla.VehicleControl(throttle = 0.0, brake = 0.0)) - time.sleep(4) - + # 不要初始化车辆控制，保持静止 + 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") @@ -182,7 +250,6 @@ def reset(self): # 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") @@ -193,25 +260,18 @@ def reset(self): # to record the data from the lanecrossing_sensor self.lanecrossing_sensor.listen(lambda event: self.lanecrossing_data(event)) - - traj = self.trajectory() - self.path = [] - for el in traj: - self.path.append(el[0]) - + # 等待传感器数据 while self.cam is None or self.seg is None: time.sleep(0.01) self.process_images() - # going to keep an episode length of 10 seconds otherwise the car learns to go around a circle and keeps doing the same thing + # episode计时 self.episode_start = time.time() - self.vehicle.apply_control(carla.VehicleControl(throttle = 1.0, brake = 0.0)) - + # 返回初始状态，车辆保持静止 return [(self.distance-300)/300, -1, 0, 0] - - # to record the collision data + # 其余方法保持不变... def collision_data(self, event): self.collision_history.append(event) @@ -355,28 +415,27 @@ 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)) - - if action == 1: + 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)) - - if action == 2: + print("执行动作: 直行") + elif action == 2: self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=-0.6*self.STEER_AMT)) - - if action == 3: + print("执行动作: 左转") + elif action == 3: self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=0.6*self.STEER_AMT)) - - if action == 4: + print("执行动作: 右转") + elif action == 4: self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=-0.1*self.STEER_AMT)) - - if action == 5: + print("执行动作: 微左") + elif action == 5: self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=0.1*self.STEER_AMT)) + print("执行动作: 微右") - - if action != 0: - action = 1 - + # 处理图像 self.process_images() # initialize a reward for a single action @@ -390,172 +449,75 @@ def step(self, action, current_state): pos = self.vehicle.get_transform().location rot = self.vehicle.get_transform().rotation - # to get the closest waypoint to the car - waypoint = self.client.get_world().get_map().get_waypoint(pos, project_to_road=True) - #path = self.trajectory() - #waypoint = path[0][0] - waypoint_ind = self.get_closest_waypoint(self.path, waypoint) - print(waypoint_ind) - waypoint = self.path[waypoint_ind] - - - if len(self.path) - waypoint_ind != 1: - next_waypoint = self.path[waypoint_ind+1] - else: - next_waypoint = waypoint - waypoint_loc = waypoint.transform.location - waypoint_rot = waypoint.transform.rotation - next_waypoint_loc = next_waypoint.transform.location - next_waypoint_rot = next_waypoint.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 - - ''' - TO DEFINE THE REWARDS - ''' - - # to get the orientation difference between the car and the road "phi" + # 计算角度差 + waypoint_rot = current_waypoint.transform.rotation orientation_diff = waypoint_rot.yaw - rot.yaw - phi = orientation_diff%360 -360*(orientation_diff%360>180) + phi = orientation_diff % 360 - 360 * (orientation_diff % 360 > 180) - u = [waypoint_loc.x-next_waypoint_loc.x, waypoint_loc.y-next_waypoint_loc.y] - v = [pos.x-next_waypoint_loc.x, pos.y-next_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)))) + # 计算横向偏差 + 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(signed_dis) - print((current_state[1]+current_state[0])*30+30) - print(current_state[0]*300+300) - print(current_state[2]) - + 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") - ''' - To define the rewards based on the combination of braking_dqn and driving_dqn models - ''' - # optimal policy for braking - if (current_state[0]*300+300)< (((current_state[1]+current_state[0])*30+30)*10 + 10): - if action == 0: - reward += 3 - else: - reward -= 1 - else: - if action == 1: - reward += 2 - else: - reward -= 2 - - if current_state[0]*300+300 > 100 and (current_state[1]+current_state[0])*30+30 < 1: - if action == 0: - reward -= 10 - - # Defining the Reward function by comparing the action taken to a suboptimal policy for driving - if abs(current_state[2])<5: - if action == 0: - reward += 2 - else: - reward -= 1 - elif abs(current_state[2])<10: - if current_state[2]<0: - if action == 3: - reward += 2 - elif action == 1: - reward += 1 - else: - reward -= 1 - else: - if action == 4: - reward += 2 - elif action == 2: - reward += 1 - else: - reward -= 1 - else: - if current_state[2]<0: - if action == 1: - reward += 2 - elif action == 3: - reward += 1 - else: - reward -= 1 - else: - if action == 2: - reward += 2 - elif action == 4: - reward += 1 - else: - reward -= 1 - - if abs(current_state[3])<0.1: - if action == 0: - reward += 4 - else: - reward -= 2 - elif abs(current_state[3])<0.5: - if current_state[3]<0: - if action == 3: - reward += 2 - elif action == 1: - reward += 1 - else: - reward -= 1 - else: - if action == 4: - reward += 2 - elif action == 2: - reward += 1 - else: - reward -= 1 - else: - if current_state[3]<0: - if action == 1: - reward += 2 - elif action == 3: - reward += 1 - else: - reward -= 1 - else: - if action == 2: - reward += 2 - elif action == 4: - reward += 1 - else: - reward -= 1 + # [原有的奖励计算代码保持不变...] - # for collision + # 检查是否完成 if len(self.collision_history) != 0: done = True - reward = - 200 + reward = -200 + print("❌ 发生碰撞!") - if abs(phi)>100: + if abs(phi) > 100: done = True reward = -200 + print("❌ 方向偏差过大!") - if abs(signed_dis)>3: - #done = True - reward = -200 + if abs(signed_dis) > 3: + reward = -50 + print("⚠️ 横向偏差过大") - # to end the episode + # 到达终点 if dist_from_goal < 10: self.reached = 1 done = True + reward = 1000 + print("✅ 成功到达终点!") - # to run each episode for just 200 seconds + # 超时检查 if self.episode_start + 200 < time.time(): - done = False - - print(reward) - - return [(self.distance-300)/300, (kmh-30)/30-(self.distance-300)/300, phi, signed_dis*15], reward, done, waypoint + 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() @@ -587,17 +549,44 @@ def trajectory(self, draw = False): 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, target_waypoint): - closest_waypoint = None + 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 - target_waypoint.transform.location.x)**2 + - (waypoint.transform.location.y - target_waypoint.transform.location.y)**2) + 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 - return closest_waypoint - + + # 确保不会返回最后一个点（除非非常接近终点） + 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/test_braking.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_braking.py index 9f3d99aa40..8386fba9f1 100644 --- a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_braking.py +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_braking.py @@ -17,7 +17,7 @@ curves = [0, town2] epsilon = 0 -MODEL_PATH = "models/Braking___282.model" +MODEL_PATH = "models/Braking___282.00max__282.00avg__282.00min__1679121006.model" def setup_tensorflow(): """设置 TensorFlow 2.x 配置""" diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_driving.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_driving.py index ab70f07cc1..7ab6ab0ea7 100644 --- a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_driving.py +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_driving.py @@ -10,7 +10,7 @@ from driving_dqn import CarEnv, MEMORY_FRACTION epsilon = 0.05 -MODEL_PATH = "models/Driving__6030.model" +MODEL_PATH = "models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model" def setup_tensorflow(): """设置 TensorFlow 2.x 配置""" diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py index f277169a86..e200f87a1b 100644 --- a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py +++ b/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py @@ -1,5 +1,5 @@ import os -os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' # 只显示错误信息 +os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' import random from collections import deque import numpy as np @@ -9,252 +9,282 @@ from tensorflow.keras.models import load_model from car_env import CarEnv, MEMORY_FRACTION import carla -from carla import Transform -from carla import Location -from carla import Rotation -from agents.navigation.global_route_planner import GlobalRoutePlanner +from carla import Transform, Location, Rotation -#Trajectory 1 -town2 = {1: [80, 306.6, 5, 0], 2:[135.25,206]} +# 轨迹定义 +trajectories = { + "custom_trajectory": { + "start": [-8.77956485748291,140.2951202392578,2.0014660358428955, 0], + "end": [74.17852020263672,-56.52183151245117,0.18172569572925568], + "description": "自定义轨迹" + } +} -#Trajectory 2 -#town2 = {1: [-7.498, 284.716, 5, 90], 2:[81.98,241.954]} +SELECTED_TRAJECTORY = "custom_trajectory" -#Trajectory 3 -#town2 = {1: [-7.498, 165.809, 5, 90], 2:[81.98,241.954]} - -#Trajectory 4 -#town2 = {1: [106.411, 191.63, 5, 0], 2:[170.551,240.054]} - -#custom trajectory -#town2 = {1: [\initial_destination], 2:[\final_destination]} - -# 模型路径 -MODEL_PATH = "models/Braking___282.00max__282.00avg__282.00min__1679121006.model" -MODEL_PATH2 = "models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model" +def get_selected_trajectory(): + """获取选定的轨迹""" + if SELECTED_TRAJECTORY in trajectories: + trajectory = trajectories[SELECTED_TRAJECTORY] + print(f"✅ 使用轨迹: {SELECTED_TRAJECTORY}") + print(f" 描述: {trajectory['description']}") + print(f" 起点: {trajectory['start']}") + print(f" 终点: {trajectory['end']}") + return trajectory + else: + print(f"❌ 轨迹 '{SELECTED_TRAJECTORY}' 不存在") + return None def safe_load_model(model_path): - """安全加载模型，处理可能的兼容性问题""" + """安全加载模型""" try: - print(f"尝试加载模型: {model_path}") - - # 检查文件是否存在 if not os.path.exists(model_path): print(f"❌ 模型文件不存在: {model_path}") return None - try: - model = load_model(model_path) - print(f"✅ 成功加载模型: {model_path}") - return model - except Exception as e: - print(f"标准加载失败: {e}") - + model = load_model(model_path) + print(f"✅ 成功加载模型: {model_path}") + return model + except Exception as e: - print(f"❌ 加载模型时发生错误 {model_path}: {e}") + print(f"❌ 加载模型失败: {e}") return None def setup_tensorflow(): - """设置 TensorFlow 2.x 配置""" - # 设置日志级别减少输出 + """设置 TensorFlow 配置""" os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' - print(f"TensorFlow 版本: {tf.__version__}") # GPU 配置 gpus = tf.config.list_physical_devices('GPU') if gpus: try: - # 设置GPU内存按需增长 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}") - # 如果GPU设置失败，回退到CPU os.environ['CUDA_VISIBLE_DEVICES'] = '-1' print("使用CPU运行") else: print("ℹ️ 未找到GPU，使用CPU运行") +def set_spectator_to_vehicle(world, vehicle): + """设置观察者视角""" + try: + spectator = world.get_spectator() + transform = vehicle.get_transform() + + # 更安全的视角 + spectator.set_transform(Transform( + transform.location + Location(z=15, x=-15), + Rotation(pitch=-30) + )) + print("✅ 观察者视角已设置") + + except Exception as e: + print(f"⚠️ 设置视角时出错: {e}") + def preprocess_state_for_prediction(state_data, model_type="braking"): """预处理状态数据用于模型预测""" try: if model_type == "braking": - # 对于刹车模型，使用前两个状态 - if isinstance(state_data, list): - state_array = np.array(state_data[:2]) - else: - state_array = state_data[:2] + state_array = np.array(state_data[:2]) else: - # 对于驾驶模型，使用后两个状态 - if isinstance(state_data, list): - state_array = np.array(state_data[2:]) - else: - state_array = state_data[2:] + 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]]) -if __name__ == '__main__': +def debug_vehicle_state(vehicle): + """调试车辆状态""" + if vehicle is None: + print("❌ 车辆为 None") + return - FPS = 60 - EPISODES = 2 + try: + transform = vehicle.get_transform() + velocity = vehicle.get_velocity() + print(f"📍 车辆位置: ({transform.location.x:.2f}, {transform.location.y:.2f}, {transform.location.z:.2f})") + print(f"🧭 车辆朝向: {transform.rotation.yaw:.2f}°") + print(f"🚀 车辆速度: {np.sqrt(velocity.x**2 + velocity.y**2 + velocity.z**2):.2f} m/s") + except Exception as e: + print(f"❌ 获取车辆状态失败: {e}") +def main(): # 设置 TensorFlow setup_tensorflow() - + + # 获取选定的轨迹 + trajectory = get_selected_trajectory() + if trajectory is None: + print("❌ 无法获取轨迹，退出程序") + return + + start_location = trajectory["start"] + end_location = trajectory["end"] + # 加载模型 print("\n" + "="*50) print("加载自动驾驶模型") print("="*50) + MODEL_PATH = "models/Braking___282.model" + MODEL_PATH2 = "models/Driving__6030.model" + model = safe_load_model(MODEL_PATH) model2 = safe_load_model(MODEL_PATH2) - # 如果模型加载失败，创建新的模型 - print(f"模型加载完成:") - print(f"{model.input_shape} -> {model.output_shape}") - print(f"{model2.input_shape} -> {model2.output_shape}") - + if model is None or model2 is None: + print("❌ 模型加载失败，退出程序") + return + # 创建环境 - env = CarEnv(town2[1], town2[2]) - - # 用于FPS计算 - 保持最近60帧的时间 - fps_counter = deque(maxlen=60) - - # 初始化预测 - 第一次预测需要初始化时间 - print("预热模型...") + print("\n初始化CARLA环境...") try: - # 使用正确的预处理 - dummy_state = preprocess_state_for_prediction([0, 0, 0, 0], "braking") - model.predict(dummy_state, verbose=0) - model2.predict(dummy_state, verbose=0) - print("✅ 模型预热完成") + env = CarEnv(start_location, end_location) + world = env.client.get_world() + + # 设置仿真设置 + settings = world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = 0.05 + world.apply_settings(settings) + except Exception as e: - print(f"⚠️ 模型预热警告: {e}") - - # 循环 episodes + print(f"❌ 初始化环境失败: {e}") + return + + # 主循环 + fps_counter = deque(maxlen=60) + EPISODES = 2 + for episode in range(EPISODES): print(f'\n{"="*50}') print(f'开始 Episode {episode + 1}/{EPISODES}') print(f'{"="*50}') - - # 重置环境并获取初始状态 - current_state = env.reset() - if hasattr(env, 'collision_hist'): - env.collision_hist = [] - # 生成轨迹 - if hasattr(env, 'trajectory'): - env.trajectory() + # 重置环境 - 这会生成车辆 + try: + print("重置环境...") + current_state = env.reset() + print(f"初始状态: {current_state}") + except Exception as e: + print(f"❌ 环境重置失败: {e}") + continue + + # 从环境中获取车辆 + ego_vehicle = env.vehicle + + if ego_vehicle is None: + print("❌ 环境中没有车辆，跳过此episode") + continue + + # 调试车辆状态 + debug_vehicle_state(ego_vehicle) + + # 设置观察者视角 + set_spectator_to_vehicle(world, ego_vehicle) done = False step_count = 0 - - # 循环步骤 - while not done: + max_steps = 1000 + + while not done and step_count < max_steps: step_count += 1 - - # FPS 计数器 step_start = time.time() - - # 显示当前帧（可选） - # if len(current_state) > 0 and isinstance(current_state[0], np.ndarray): - # cv2.imshow(f'Agent - preview', current_state[0]) - # cv2.waitKey(1) - - # 交通灯处理 - action = None + + # 定期更新视角 + if step_count % 20 == 0: + set_spectator_to_vehicle(world, ego_vehicle) + + # 动作预测 + action = 0 try: + # 检查交通灯 if hasattr(env, 'vehicle') and env.vehicle and env.vehicle.is_at_traffic_light(): - traffic_light_state = env.vehicle.get_traffic_light().get_state() - if traffic_light_state == carla.TrafficLightState.Red: - print("红灯 - 停车") + traffic_light = env.vehicle.get_traffic_light() + if traffic_light and traffic_light.get_state() == carla.TrafficLightState.Red: + print("🚦 红灯 - 停车") action = 0 - time.sleep(1/FPS) else: - print("绿灯 - 使用刹车模型预测") - # 预处理状态数据 - state_for_model = preprocess_state_for_prediction(current_state, "braking") - qs = model.predict(state_for_model, verbose=0)[0] + # 使用模型预测 + state_array = preprocess_state_for_prediction(current_state, "braking") + qs = model.predict(state_array, verbose=0)[0] action = np.argmax(qs) - if action == 1: # 如果需要进一步决策 - state_for_model2 = preprocess_state_for_prediction(current_state, "driving") - qs2 = model2.predict(state_for_model2, verbose=0)[0] + if action == 1: # 安全时才使用驾驶模型 + state_array2 = preprocess_state_for_prediction(current_state, "driving") + qs2 = model2.predict(state_array2, verbose=0)[0] action = np.argmax(qs2) + 1 else: - # 基于当前观察空间预测动作 - state_for_model = preprocess_state_for_prediction(current_state, "braking") - qs = model.predict(state_for_model, verbose=0)[0] + # 正常情况下的决策 + state_array = preprocess_state_for_prediction(current_state, "braking") + qs = model.predict(state_array, verbose=0)[0] action = np.argmax(qs) - if action == 1: # 如果需要进一步决策 - state_for_model2 = preprocess_state_for_prediction(current_state, "driving") - qs2 = model2.predict(state_for_model2, verbose=0)[0] + if action == 1: + state_array2 = preprocess_state_for_prediction(current_state, "driving") + qs2 = model2.predict(state_array2, verbose=0)[0] action = np.argmax(qs2) + 1 - + except Exception as e: print(f"❌ 预测错误: {e}") - action = 0 # 默认安全动作 - - # 环境步骤（额外的标志通知环境不要因时间限制而中断episode） + action = 0 + + # 执行动作 try: - new_state, reward, done, _ = env.step(action, current_state) + new_state, reward, done, waypoint = env.step(action, current_state) current_state = new_state + + # 显示额外信息 + if step_count % 10 == 0: + print(f"步骤 {step_count}, 奖励: {reward}, 完成: {done}") + except Exception as e: print(f"❌ 环境步骤错误: {e}") done = True - - # 如果完成 - 代理崩溃，中断episode - if done: - print(f"Episode {episode + 1} 完成，步数: {step_count}") - break - - # 测量步骤时间，添加到deque，然后打印最近60帧的平均FPS、q值和采取的动作 + + # 计算FPS frame_time = time.time() - step_start fps_counter.append(frame_time) + current_fps = len(fps_counter) / sum(fps_counter) if fps_counter else 0 - if len(fps_counter) > 0: - current_fps = len(fps_counter) / sum(fps_counter) - else: - current_fps = 0 - - # 安全地打印Q值 - try: - qs_display = f"[{qs[0]:>5.2f}, {qs[1]:>5.2f}]" if 'qs' in locals() else "[N/A, N/A]" - print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Q-values: {qs_display} | Action: {action}') - except: - print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Action: {action}') - - # 在episode结束时销毁actor - print(f"清理 Episode {episode + 1} 的actor...") - try: - if hasattr(env, 'actor_list'): - for actor in env.actor_list: - try: - actor.destroy() - except Exception as e: - print(f"销毁actor错误: {e}") - except Exception as e: - print(f"清理错误: {e}") - + # 显示动作名称 + action_names = ["刹车", "直行", "左转", "右转", "微左", "微右"] + action_name = action_names[action] if action < len(action_names) else str(action) + + print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Action: {action_name}') + + if done: + print(f"Episode {episode + 1} 完成，步数: {step_count}") + break + + if step_count >= max_steps: + print(f"Episode {episode + 1} 达到最大步数限制") + + # 等待一段时间再开始下一个episode + print(f"等待下一个episode...") + time.sleep(2.0) + + # 最终清理 print("\n" + "="*50) print("所有episodes完成!") print("="*50) - # 清理资源 + print("清理资源...") try: + # 环境会在重置时自动清理车辆 cv2.destroyAllWindows() except: pass - - print("程序正常退出") \ No newline at end of file + + print("程序结束") + +if __name__ == '__main__': + main() \ No newline at end of file From 693ae8ead04f09c757aa56345dcb33590db2ad49 Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Mon, 24 Nov 2025 08:48:25 +0800 Subject: [PATCH 12/16] =?UTF-8?q?=E6=9B=B4=E6=94=B9=E4=BA=86=E6=96=87?= =?UTF-8?q?=E4=BB=B6=E5=A4=B9=E5=90=8D=E7=A7=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 0 .../agents/__init__.py | 0 .../agents/navigation/__init__.py | 0 .../agents/navigation/basic_agent.py | 0 .../agents/navigation/behavior_agent.py | 0 .../agents/navigation/behavior_types.py | 0 .../agents/navigation/controller.py | 0 .../agents/navigation/global_route_planner.py | 0 .../agents/navigation/local_planner.py | 0 .../agents/tools/__init__.py | 0 .../agents/tools/misc.py | 0 .../braking_dqn.py | 0 .../car_env.py | 0 .../config.py | 0 .../driving_dqn.py | 0 .../generate_traffic.py | 0 .../get_location.py | 0 ...82.00max__282.00avg__282.00min__1679121006.model | Bin ...30.00max_6030.00avg_6030.00min__1679109656.model | Bin .../pedestrians_1.py | 0 .../pedestrians_2.py | 0 .../requirements.txt | 0 .../test_braking.py | 0 .../test_driving.py | 0 .../test_everything.py | 0 25 files changed, 0 insertions(+), 0 deletions(-) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/README.md (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/agents/__init__.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/agents/navigation/__init__.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/agents/navigation/basic_agent.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/agents/navigation/behavior_agent.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/agents/navigation/behavior_types.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/agents/navigation/controller.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/agents/navigation/global_route_planner.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/agents/navigation/local_planner.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/agents/tools/__init__.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/agents/tools/misc.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/braking_dqn.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/car_env.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/config.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/driving_dqn.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/generate_traffic.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/get_location.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/models/Braking___282.00max__282.00avg__282.00min__1679121006.model (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/pedestrians_1.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/pedestrians_2.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/requirements.txt (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/test_braking.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/test_driving.py (100%) rename src/{Autonomous-Vehicle-Navigation-Using-Deep-Learning-master => Autonomous_vehicle_navigation_using_deep_learning_master}/test_everything.py (100%) diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/README.md b/src/Autonomous_vehicle_navigation_using_deep_learning_master/README.md similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/README.md rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/README.md diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/agents/__init__.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/agents/tools/misc.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/braking_dqn.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/braking_dqn.py similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/braking_dqn.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/braking_dqn.py 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 similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/car_env.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/car_env.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/config.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/config.py similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/config.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/config.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/driving_dqn.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/driving_dqn.py similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/driving_dqn.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/driving_dqn.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/generate_traffic.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/generate_traffic.py similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/generate_traffic.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/generate_traffic.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/get_location.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/get_location.py similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/get_location.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/get_location.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/models/Braking___282.00max__282.00avg__282.00min__1679121006.model b/src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Braking___282.00max__282.00avg__282.00min__1679121006.model similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/models/Braking___282.00max__282.00avg__282.00min__1679121006.model rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Braking___282.00max__282.00avg__282.00min__1679121006.model diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model b/src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_1.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/pedestrians_1.py similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_1.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/pedestrians_1.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_2.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/pedestrians_2.py similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/pedestrians_2.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/pedestrians_2.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/requirements.txt b/src/Autonomous_vehicle_navigation_using_deep_learning_master/requirements.txt similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/requirements.txt rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/requirements.txt diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_braking.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_braking.py similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_braking.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/test_braking.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_driving.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_driving.py similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_driving.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/test_driving.py diff --git a/src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_everything.py similarity index 100% rename from src/Autonomous-Vehicle-Navigation-Using-Deep-Learning-master/test_everything.py rename to src/Autonomous_vehicle_navigation_using_deep_learning_master/test_everything.py From ed8e09caf913d724d5c4257c612c183011d20a2a Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Mon, 24 Nov 2025 10:44:32 +0800 Subject: [PATCH 13/16] =?UTF-8?q?=E6=9B=B4=E6=94=B9=E4=BA=86=E8=A7=86?= =?UTF-8?q?=E8=A7=92=E8=B7=9F=E9=9A=8F=E8=AE=BE=E5=AE=9A=EF=BC=8C=E4=BD=BF?= =?UTF-8?q?=E8=BD=A6=E8=BE=86=E5=A7=8B=E7=BB=88=E5=9C=A8=E8=A7=86=E8=A7=92?= =?UTF-8?q?=E8=8C=83=E5=9B=B4=E5=86=85=EF=BC=8C=E4=B8=94=E5=A7=8B=E7=BB=88?= =?UTF-8?q?=E5=9C=A8=E8=BD=A6=E5=B0=BE=EF=BC=8C=E9=99=8D=E4=BD=8E=E4=BA=86?= =?UTF-8?q?=E8=A7=86=E8=A7=92=E9=AB=98=E5=BA=A6=EF=BC=8C=E9=81=BF=E5=85=8D?= =?UTF-8?q?=E8=A7=86=E7=BA=BF=E8=A2=AB=E9=98=BB=E6=8C=A1=EF=BC=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../car_env.py | 8 +++---- .../test_everything.py | 24 ++++++++++++++----- 2 files changed, 22 insertions(+), 10 deletions(-) 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 4cc0d3a86e..1d461cb8fc 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 @@ -423,16 +423,16 @@ def step(self, action, current_state): self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=0*self.STEER_AMT)) print("执行动作: 直行") elif action == 2: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=-0.6*self.STEER_AMT)) + 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.6*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.4, steer=-0.1*self.STEER_AMT)) + self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=-0.05*self.STEER_AMT)) print("执行动作: 微左") elif action == 5: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=0.1*self.STEER_AMT)) + self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=0.05*self.STEER_AMT)) print("执行动作: 微右") # 处理图像 diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_everything.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_everything.py index e200f87a1b..6518e316f5 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_everything.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_everything.py @@ -70,17 +70,29 @@ def setup_tensorflow(): print("ℹ️ 未找到GPU，使用CPU运行") def set_spectator_to_vehicle(world, vehicle): - """设置观察者视角""" + """设置观察者视角 - 车辆正后方跟随""" try: spectator = world.get_spectator() transform = vehicle.get_transform() - # 更安全的视角 + # 计算车辆后方的位置 + # 使用车辆的旋转来确定方向 + rotation = transform.rotation + yaw = np.radians(rotation.yaw) + + # 在车辆后方一定距离（例如8米），高度3米 + distance_behind = 8.0 + height = 3.0 + + # 计算后方位置（与车辆朝向相反的方向） + behind_x = transform.location.x - distance_behind * np.cos(yaw) + behind_y = transform.location.y - distance_behind * np.sin(yaw) + + # 设置观察者在车辆正后方，稍微高一点 spectator.set_transform(Transform( - transform.location + Location(z=15, x=-15), - Rotation(pitch=-30) + Location(x=behind_x, y=behind_y, z=transform.location.z + height), + Rotation(pitch=-15, yaw=rotation.yaw) # 与车辆相同的水平朝向 )) - print("✅ 观察者视角已设置") except Exception as e: print(f"⚠️ 设置视角时出错: {e}") @@ -200,7 +212,7 @@ def main(): step_start = time.time() # 定期更新视角 - if step_count % 20 == 0: + if step_count%5==0: set_spectator_to_vehicle(world, ego_vehicle) # 动作预测 From 0b23b8c8e293d6e2ea8af15085c9422c031614a3 Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Mon, 24 Nov 2025 11:19:38 +0800 Subject: [PATCH 14/16] =?UTF-8?q?=E5=88=A0=E9=99=A4=E4=BA=86=E5=BA=9F?= =?UTF-8?q?=E5=BC=83=E7=9A=84=E9=A1=B9=E7=9B=AE=EF=BC=8C=E9=98=B2=E6=AD=A2?= =?UTF-8?q?=E9=80=89=E9=A2=98=E6=B7=B7=E4=B9=B1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/manual_ISS/README.md | 1 - 1 file changed, 1 deletion(-) delete mode 100644 src/manual_ISS/README.md diff --git a/src/manual_ISS/README.md b/src/manual_ISS/README.md deleted file mode 100644 index 17776d5443..0000000000 --- a/src/manual_ISS/README.md +++ /dev/null @@ -1 +0,0 @@ -这是manayume的智能驾驶系统模块开发 \ No newline at end of file From 61c6dfbce3a643835ae9e701b49ad94b9a17cd12 Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Mon, 15 Dec 2025 10:04:57 +0800 Subject: [PATCH 15/16] =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../driving_dqn.py | 477 +++++++++--------- .../test_braking.py | 4 +- .../test_driving.py | 358 +++++++------ 3 files changed, 449 insertions(+), 390 deletions(-) diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/driving_dqn.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/driving_dqn.py index 7cdff72c7a..b9f79b123f 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/driving_dqn.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/driving_dqn.py @@ -75,45 +75,34 @@ AGGREGATE_STATS_EVERY = 1 -# path for training -town2 = {1: [193.75,269.2610168457031, 5, 270], 2:[135.25,206]} #left turn - -curves = [0, town2] +# 路径配置 - 现在由外部传入 +default_town2 = {1: [193.75,269.2610168457031, 5, 270], 2:[135.25,206]} #left turn +curves = [0, default_town2] ''' Custom Tensorboard class. Updates logs after every episode ''' class ModifiedTensorBoard(TensorBoard): - # Overriding init to set initial step and writer (we want one log file for all .fit() calls) def __init__(self, model, log_dir): super().__init__(log_dir) self.log_dir = log_dir self.model = model self.step = 1 print("SELF: LOG DIR: ", self.log_dir) - # TensorFlow 2.x 使用 tf.summary.create_file_writer self.writer = tf.summary.create_file_writer(self.log_dir) - # Overriding this method to stop creating default log writer def set_model(self, model): self.model = model - # 在 TensorFlow 2.x 中不需要额外的设置 - # Overrided, saves logs with our step number def on_epoch_end(self, epoch, logs=None): self.update_stats(**logs) - # Overrided - # We train for one batch only, no need to save anything at epoch end def on_batch_end(self, batch, logs=None): pass - # Overrided, so won't close writer def on_train_end(self, _): pass - # Custom method for saving own metrics - # Creates writer, writes custom metrics and closes writer def update_stats(self, **stats): self._write_logs(stats, self.step) @@ -128,13 +117,12 @@ def _write_logs(self, logs, index): ''' 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] + STEER_AMT = 1.0 im_width = IM_WIDTH im_height = IM_HEIGHT front_camera = None - def __init__(self): - # to initialize + def __init__(self, start_point=None, end_point=None): self.client = carla.Client("localhost", 2000) self.client.set_timeout(20.0) self.world = self.client.get_world() @@ -144,12 +132,28 @@ def __init__(self): self.crossing = 0 self.curves = 1 self.reached = 0 - self.start = town2[1] + + # 允许从外部传入起点终点 + self.custom_start = start_point + self.custom_end = end_point + + # 如果提供了自定义起点，使用它 + if start_point is not None: + self.start = start_point + else: + self.start = default_town2[1] + + # 存储自定义终点 + self.end_point = end_point if end_point is not None else default_town2[2] + self.phi = [] self.dc = [] self.vel = [] self.time = [] - + self.last_position = None + self.current_waypoint_index = 0 + self.path_progress = 0 # 路径进度跟踪 + def reset(self): # store any collision detected self.collision_history = [] @@ -158,78 +162,131 @@ def reset(self): # store the number of times the vehicles crosses the lane marking self.lanecrossing_history = [] + # 重置路径跟踪 + self.current_waypoint_index = 0 + self.path_progress = 0 + self.last_position = None + ''' To spawn the Vehicle (agent) ''' initial_pos = self.start + print(f"生成车辆在起点: ({initial_pos[0]:.2f}, {initial_pos[1]:.2f}), 航向: {initial_pos[3]}°") + self.transform = Transform(Location(x=initial_pos[0], y=initial_pos[1], z=initial_pos[2]), Rotation(yaw=initial_pos[3])) - # to spawn the actor; the veichle - self.vehicle = self.world.spawn_actor(self.model_3, self.transform) - self.actor_list.append(self.vehicle) - - # 相机传感器设置（如果需要） + + # 尝试生成车辆 + try: + self.vehicle = self.world.spawn_actor(self.model_3, self.transform) + self.actor_list.append(self.vehicle) + print("✓ 车辆生成成功") + except Exception as e: + print(f"❌ 车辆生成失败: {e}") + # 尝试使用默认位置 + default_transform = Transform(Location(x=0, y=0, z=5), Rotation(yaw=0)) + self.vehicle = self.world.spawn_actor(self.model_3, default_transform) + self.actor_list.append(self.vehicle) + + # 相机传感器设置 self.camera_spawn_point = carla.Transform(carla.Location(x=2, y=0, z=1.4)) # to initialize the car quickly and get it going self.vehicle.apply_control(carla.VehicleControl(throttle = 0.0, brake = 0.0)) - time.sleep(4) + time.sleep(1) # 减少等待时间 ''' To spawn the collision sensor ''' - # 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 + # to introduce the lanecrossing sensor 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)) + # 生成轨迹 traj = self.trajectory() self.path = [] for el in traj: self.path.append(el[0]) + + print(f"生成的路径点数量: {len(self.path)}") + if len(self.path) > 0: + print(f"第一个路径点方向: {self.path[0].transform.rotation.yaw:.1f}°") + print(f"最后一个路径点方向: {self.path[-1].transform.rotation.yaw:.1f}°") + + # 获取车辆初始方向 + vehicle_transform = self.vehicle.get_transform() + print(f"车辆初始方向: {vehicle_transform.rotation.yaw:.1f}°") + + # 检查车辆与第一个路径点的方向差 + if len(self.path) > 0: + first_wp_direction = self.path[0].transform.rotation.yaw + direction_diff = first_wp_direction - vehicle_transform.rotation.yaw + phi_initial = direction_diff % 360 - 360 * (direction_diff % 360 > 180) + print(f"初始方向差: {phi_initial:.1f}°") - # going to keep an episode length of 10 seconds otherwise the car learns to go around a circle and keeps doing the same thing self.episode_start = time.time() - - self.vehicle.apply_control(carla.VehicleControl(throttle = 1.0, brake = 0.0)) - return [0,0] #return [self.front_camera, 0,0, initial_pos[0], initial_pos[1]] + # 设置初始控制，让车辆开始移动 + self.vehicle.apply_control(carla.VehicleControl(throttle = 0.5, brake = 0.0, steer = 0.0)) + time.sleep(0.5) # 给车辆一点时间开始移动 + + # 获取初始状态 + pos = self.vehicle.get_transform().location + rot = self.vehicle.get_transform().rotation + + # 获取最近的路径点 + waypoint = self.client.get_world().get_map().get_waypoint(pos, project_to_road=True) + closest_index = self.get_closest_waypoint(self.path, waypoint) + self.current_waypoint_index = max(0, min(closest_index, len(self.path)-1)) + + if len(self.path) > self.current_waypoint_index: + wp = self.path[self.current_waypoint_index] + wp_rot = wp.transform.rotation + direction_diff = wp_rot.yaw - rot.yaw + phi = direction_diff % 360 - 360 * (direction_diff % 360 > 180) + else: + phi = 0 + + return [phi, 0] # 初始状态 def collision_data(self, event): self.collision_history.append(event) def lanecrossing_data(self, event): self.lanecrossing_history.append(event) - print("Lane crossing history: ", event) def step(self, action, current_state): ''' Take 5 actions; go straight, turn left, turn right, turn slightly left, turn slightly right ''' + # 增加油门值，让车辆更快移动 if action == 0: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=0*self.STEER_AMT)) + self.vehicle.apply_control(carla.VehicleControl(throttle=0.6, steer=0*self.STEER_AMT)) if action == 1: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=-0.6*self.STEER_AMT)) + self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=-0.6*self.STEER_AMT)) if action == 2: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.1, steer=0.6*self.STEER_AMT)) + self.vehicle.apply_control(carla.VehicleControl(throttle=0.3, steer=0.6*self.STEER_AMT)) if action == 3: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=-0.1*self.STEER_AMT)) + self.vehicle.apply_control(carla.VehicleControl(throttle=0.7, steer=-0.1*self.STEER_AMT)) if action == 4: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=0.1*self.STEER_AMT)) + self.vehicle.apply_control(carla.VehicleControl(throttle=0.7, steer=0.1*self.STEER_AMT)) + + # 检查移动距离 + current_pos = self.vehicle.get_transform().location + if self.last_position: + distance_moved = math.sqrt( + (current_pos.x - self.last_position.x)**2 + + (current_pos.y - self.last_position.y)**2 + ) + if distance_moved < 0.1 and hasattr(self, 'step_count') and self.step_count > 20: + print(f"⚠️ 警告: 车辆移动距离太小! ({distance_moved:.2f}米)") + self.last_position = current_pos # initialize a reward for a single action reward = 0 @@ -241,23 +298,60 @@ def step(self, action, current_state): pos = self.vehicle.get_transform().location rot = self.vehicle.get_transform().rotation + print(f"车辆位置: ({pos.x:.1f}, {pos.y:.1f}), 方向: {rot.yaw:.1f}°, 速度: {kmh} km/h") + # to get the closest waypoint to the car waypoint = self.client.get_world().get_map().get_waypoint(pos, project_to_road=True) - waypoint_ind = self.get_closest_waypoint(self.path, waypoint) + 1 - print(waypoint_ind) - waypoint = self.path[waypoint_ind] - if len(self.path) != 1: - next_waypoint = self.path[waypoint_ind+1] + + # 安全地获取路径点索引 + if not hasattr(self, 'path') or len(self.path) == 0: + self.trajectory() + + # 使用改进的最近路径点查找 + closest_index = self.get_closest_waypoint(self.path, waypoint) + + # 确保索引在有效范围内 + closest_index = min(closest_index, len(self.path) - 1) + closest_index = max(closest_index, 0) + + # 更新当前路径点索引（允许前进，限制回退） + if closest_index > self.current_waypoint_index: + self.current_waypoint_index = closest_index + elif closest_index < self.current_waypoint_index - 2: # 允许少量回退 + self.current_waypoint_index = max(0, closest_index) + + # 获取当前路径点 + if len(self.path) > self.current_waypoint_index: + current_waypoint = self.path[self.current_waypoint_index] else: - next_waypoint = waypoint - waypoint_loc = waypoint.transform.location - waypoint_rot = waypoint.transform.rotation + current_waypoint = self.path[-1] if len(self.path) > 0 else waypoint + + # 计算到当前路径点的距离 + wp_location = current_waypoint.transform.location + distance_to_wp = math.sqrt((pos.x - wp_location.x)**2 + (pos.y - wp_location.y)**2) + + print(f"当前路径点: {self.current_waypoint_index}/{len(self.path)-1}") + print(f"路径点位置: ({wp_location.x:.1f}, {wp_location.y:.1f})") + print(f"到路径点距离: {distance_to_wp:.2f}") + + # 如果接近当前路径点且不是最后一个，前进到下一个 + if distance_to_wp < 5.0 and self.current_waypoint_index < len(self.path) - 1: + next_index = self.current_waypoint_index + 1 + next_waypoint = self.path[next_index] + else: + next_index = self.current_waypoint_index + next_waypoint = current_waypoint + + waypoint = current_waypoint next_waypoint_loc = next_waypoint.transform.location next_waypoint_rot = next_waypoint.transform.rotation - final = [curves[self.curves][2][0], curves[self.curves][2][1]] - final_destination = [curves[self.curves][self.via][0], curves[self.curves][self.via][1]] - dist_from_goal = np.sqrt((pos.x - final_destination[0])**2 + (pos.y-final_destination[1])**2) + waypoint_loc = waypoint.transform.location + waypoint_rot = waypoint.transform.rotation + + # 计算到终点的距离 + final_destination = self.end_point + dist_from_goal = np.sqrt((pos.x - final_destination[0])**2 + (pos.y - final_destination[1])**2) done = False @@ -266,28 +360,35 @@ def step(self, action, current_state): ''' # to get the orientation difference between the car and the road "phi" orientation_diff = waypoint_rot.yaw - rot.yaw - phi = orientation_diff%360 -360*(orientation_diff%360>180) + phi = orientation_diff % 360 - 360 * (orientation_diff % 360 > 180) + + print(f"车辆方向: {rot.yaw:.1f}°, 路径点方向: {waypoint_rot.yaw:.1f}°") + print(f"方向差: {orientation_diff:.1f}°, phi: {phi:.1f}°") - current_state[1] = current_state[1]/15 + current_state[1] = current_state[1] / 15 + + # 计算横向偏移 + u = [waypoint_loc.x - next_waypoint_loc.x, waypoint_loc.y - next_waypoint_loc.y] + v = [pos.x - next_waypoint_loc.x, pos.y - next_waypoint_loc.y] - u = [waypoint_loc.x-next_waypoint_loc.x, waypoint_loc.y-next_waypoint_loc.y] - v = [pos.x-next_waypoint_loc.x, pos.y-next_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)))) + cross_product = u[0]*v[1] - u[1]*v[0] + dot_product = u[0]*v[0] + u[1]*v[1] + angle = np.arctan2(cross_product, dot_product) + signed_dis = np.linalg.norm(v) * np.sin(angle) else: signed_dis = 0 - print(current_state[0]) - print(current_state[1]) + print(f"当前状态: phi={current_state[0]:.1f}, d={current_state[1]:.1f}") # Defining the Reward function by comparing the action taken to a suboptimal policy - if abs(current_state[0])<5: + if abs(current_state[0]) < 5: if action == 0: reward += 2 else: reward -= 1 - elif abs(current_state[0])<10: - if current_state[0]<0: + elif abs(current_state[0]) < 10: + if current_state[0] < 0: if action == 3: reward += 2 elif action == 1: @@ -302,7 +403,7 @@ def step(self, action, current_state): else: reward -= 1 else: - if current_state[0]<0: + if current_state[0] < 0: if action == 1: reward += 2 elif action == 3: @@ -316,14 +417,14 @@ def step(self, action, current_state): reward += 1 else: reward -= 1 - - if abs(current_state[1])<0.1: + + if abs(current_state[1]) < 0.1: if action == 0: reward += 4 else: reward -= 2 - elif abs(current_state[1])<0.5: - if current_state[1]<0: + elif abs(current_state[1]) < 0.5: + if current_state[1] < 0: if action == 3: reward += 2 elif action == 1: @@ -338,7 +439,7 @@ def step(self, action, current_state): else: reward -= 1 else: - if current_state[1]<0: + if current_state[1] < 0: if action == 1: reward += 2 elif action == 3: @@ -352,36 +453,53 @@ def step(self, action, current_state): reward += 1 else: reward -= 1 - - - if abs(signed_dis)>2: + + # 添加速度奖励 + if kmh > 20: + reward += 1 + elif kmh < 5: + reward -= 1 + + if abs(signed_dis) > 2: reward -= 10 + # 检查距离路径点是否过远 + if distance_to_wp > 30: + print(f"⚠️ 警告: 车辆距离路径点过远 ({distance_to_wp:.1f} > 30)") + done = True + reward = -100 + # to avoid collisions if len(self.collision_history) != 0: done = True - reward = - 200 + reward = -200 + print("❌ 发生碰撞!") # to end the episode if phi value goes high - if abs(phi)>100: + if abs(phi) > 100: done = True reward = -200 + print("❌ 方向偏差过大!") # Ending the episode if the distance to the centerline of the road is greater than 3 - if abs(signed_dis)>3: + if abs(signed_dis) > 3: done = True reward = -200 + print("❌ 偏离道路中心线过远!") # to end the episode if the car reaches close to the final destination if dist_from_goal < 5: self.reached = 1 done = True + reward += 100 + print("✅ 成功到达目的地!") - # to run each episode for just 30 secodns + # to run each episode for just 30 seconds if self.episode_start + 200 < time.time(): done = True + print("⏰ 时间到!") - print(reward) + print(f"奖励: {reward}") self.phi.append(phi) self.dc.append(signed_dis) @@ -390,19 +508,43 @@ def step(self, action, current_state): return [phi, signed_dis*15], reward, done, waypoint - def trajectory(self, draw = False): + def trajectory(self, draw=False): amap = self.world.get_map() sampling_resolution = 0.5 grp = GlobalRoutePlanner(amap, sampling_resolution) + # 使用自定义终点或默认终点 + if hasattr(self, 'end_point') and self.end_point is not None: + end_x, end_y = self.end_point + else: + end_x, end_y = default_town2[2][0], default_town2[2][1] + start_location = carla.Location(x=self.start[0], y=self.start[1], z=0) - end_location = carla.Location(x=town2[2][0], y=town2[2][1], z=0) + end_location = carla.Location(x=end_x, y=end_y, z=0) + + print(f"生成轨迹: 从 ({self.start[0]:.1f}, {self.start[1]:.1f}) 到 ({end_x:.1f}, {end_y:.1f})") + 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() - a = a.transform.location - b = b.transform.location - w1 = grp.trace_route(a, b) + + if a is None: + print("❌ 无法找到起点对应的道路点!") + # 尝试使用车辆当前位置 + if hasattr(self, 'vehicle'): + vehicle_loc = self.vehicle.get_transform().location + a = amap.get_waypoint(vehicle_loc, project_to_road=True) + + if b is None: + print("❌ 无法找到终点对应的道路点!") + return [] + + a_loc = a.transform.location + b_loc = b.transform.location + + w1 = grp.trace_route(a_loc, b_loc) + + print(f"生成的路径段数: {len(w1)}") + i = 0 if draw: for w in w1: @@ -418,14 +560,30 @@ def trajectory(self, draw = False): return w1 def get_closest_waypoint(self, waypoint_list, target_waypoint): - closest_waypoint = None + if not waypoint_list: + return 0 + + closest_waypoint = self.current_waypoint_index closest_distance = float('inf') - for i, waypoint in enumerate(waypoint_list): - distance = math.sqrt((waypoint.transform.location.x - target_waypoint.transform.location.x)**2 + - (waypoint.transform.location.y - target_waypoint.transform.location.y)**2) + + # 车辆位置 + vehicle_location = target_waypoint.transform.location + + # 从当前索引开始搜索，但允许查看前面的几个点 + start_index = max(0, self.current_waypoint_index - 3) + + for i in range(start_index, len(waypoint_list)): + waypoint = waypoint_list[i] + waypoint_location = waypoint.transform.location + distance = math.sqrt( + (waypoint_location.x - vehicle_location.x)**2 + + (waypoint_location.y - vehicle_location.y)**2 + ) + if distance < closest_distance: closest_waypoint = i closest_distance = distance + return closest_waypoint ''' @@ -439,9 +597,6 @@ def __init__(self): self.replay_memory = deque(maxlen=REPLAY_MEMORY_SIZE) - # TensorFlow 2.x 不再需要显式获取计算图 - # self.graph = tf.compat.v1.get_default_graph() - self.tensorboard = ModifiedTensorBoard(self.model, log_dir=f"logs/{MODEL_NAME}-{int(time.time())}") self.target_update_counter = 0 @@ -455,26 +610,20 @@ def create_model(self): model3.add(Dense(5, activation='linear', name='output')) combined_model = Model(inputs=model3.input, outputs=model3.output) - # compile the model - 使用新的学习率参数名 combined_model.compile(loss='mse', optimizer=Adam(learning_rate=0.0001), metrics=['accuracy']) return combined_model def update_replay_memory(self, transition): - # transition = (current_state, action, reward, new_state, done) self.replay_memory.append(transition) def train(self): if len(self.replay_memory) < MIN_REPLAY_MEMORY_SIZE: return - # to sample a minibatch minibatch = random.sample(self.replay_memory, MINIBATCH_SIZE) - # to normalize the image current_data = np.array([[transition[0][i] for i in range(2)] for transition in minibatch]) - # predicting all the datapoints present in the mini-batch - # TensorFlow 2.x 不再需要显式使用计算图 current_qs_list = self.model.predict(current_data, PREDICTION_BATCH_SIZE, verbose=0) new_current_data = np.array([[transition[3][i] for i in range(2)] for transition in minibatch]) @@ -499,10 +648,8 @@ def train(self): log_this_step = False if self.tensorboard.step > self.last_logged_episode: log_this_step = True - self.last_logged_episode = self.tensorboard.step # 修复变量名错误 + self.last_logged_episode = self.tensorboard.step - # to continuously train the base model - # TensorFlow 2.x 不再需要显式使用计算图 self.model.fit(np.array(X_data), np.array(y), batch_size=TRAINING_BATCH_SIZE, verbose=0, shuffle=False, callbacks=[self.tensorboard] if log_this_step else None) @@ -510,7 +657,6 @@ def train(self): if log_this_step: self.target_update_counter += 1 - # to assign the weights of the base model to the target model if self.target_update_counter > UPDATE_TARGET_EVERY: self.target_model.set_weights(self.model.get_weights()) self.target_update_counter = 0 @@ -529,127 +675,4 @@ def train_in_loop(self): if self.terminate: return self.train() - time.sleep(0.01) - -if __name__ == '__main__': - FPS = 400 - # For stats - ep_rewards = [-200] - - # For more repetitive results - random.seed(1) - np.random.seed(1) - tf.random.set_seed(1) # TensorFlow 2.x 设置随机种子 - - # TensorFlow 2.x 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}") - - # 创建模型目录 - model_dir = "models" - if not os.path.isdir(model_dir): - os.makedirs(model_dir) - - # Create agent and environment - agent = DQNAgent() - env = CarEnv() - - # Start training thread and wait for training to be initialized - trainer_thread = Thread(target=agent.train_in_loop, daemon=True) - trainer_thread.start() - while not agent.training_initialized: - time.sleep(0.01) - - # Initialize predictions - first prediction takes longer as of initialization that has to be done - agent.get_qs([0,0]) - - # Iterate over episodes - for episode in tqdm(range(1, EPISODES + 1), ascii=True, unit='episodes'): - if episode%2 == 0: - town2 = {1: [182.65191650390625,236.9, 5, 180], 2:[135.25,206]} - env.start = town2[1] - else: - town2 = {1: [193.75,269.2610168457031, 5, 270], 2:[135.25,206]} - env.start = town2[1] - env.reached = 0 - env.collision_hist = [] - # Update tensorboard step every episode - agent.tensorboard.step = episode - - # Restarting episode - reset episode reward and step number - episode_reward = 0 - step = 1 - - # Reset environment and get initial state - current_state = env.reset() - - # Reset flag and start iterating until episode ends - done = False - episode_start = time.time() - - # Play for given number of seconds only - while True: - # This part stays mostly the same, the change is to query a model for Q values - if np.random.random() > epsilon: - # Get action from Q table - qs = agent.get_qs(current_state) - print(qs) - action = np.argmax(qs) - else: - # Get random action - action = np.random.randint(0, 5) - time.sleep(1/FPS) - - new_state, reward, done, waypoint = env.step(action, current_state) - - # Transform new continous state to new discrete state and count reward - episode_reward += reward - - # Every step we update replay memory - agent.update_replay_memory((current_state, action, reward, new_state, done)) - - current_state = new_state - step += 1 - - if done: - break - - print("EPISODE {} REWARD IS: {}".format(episode, episode_reward)) - - # End of episode - destroy agents - for actor in env.actor_list: - actor.destroy() - - # Append episode reward to a list and log stats (every given number of episodes) - ep_rewards.append(episode_reward) - if not episode % AGGREGATE_STATS_EVERY or episode == 1: - average_reward = sum(ep_rewards[-AGGREGATE_STATS_EVERY:])/len(ep_rewards[-AGGREGATE_STATS_EVERY:]) - min_reward = min(ep_rewards[-AGGREGATE_STATS_EVERY:]) - max_reward = max(ep_rewards[-AGGREGATE_STATS_EVERY:]) - agent.tensorboard.update_stats(reward_avg=average_reward, reward_min=min_reward, reward_max=max_reward, epsilon=epsilon) - - # Save model, but only when min reward is greater or equal a set value - if min_reward >= MIN_REWARD: - agent.model.save(f'models/{MODEL_NAME}__{max_reward:_>7.2f}max_{average_reward:_>7.2f}avg_{min_reward:_>7.2f}min__{int(time.time())}.model') - - # Decay epsilon - if epsilon > MIN_EPSILON: - epsilon *= EPSILON_DECAY - epsilon = max(MIN_EPSILON, epsilon) - - # Set termination flag for training thread and wait for it to finish - agent.terminate = True - trainer_thread.join() - - # 保存最终模型 - if len(ep_rewards) > 1: - average_reward = sum(ep_rewards[-AGGREGATE_STATS_EVERY:])/len(ep_rewards[-AGGREGATE_STATS_EVERY:]) - min_reward = min(ep_rewards[-AGGREGATE_STATS_EVERY:]) - max_reward = max(ep_rewards[-AGGREGATE_STATS_EVERY:]) - agent.model.save(f'models/{MODEL_NAME}__{max_reward:_>7.2f}max_{average_reward:_>7.2f}avg_{min_reward:_>7.2f}min__{int(time.time())}.model') \ No newline at end of file + time.sleep(0.01) \ No newline at end of file diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_braking.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_braking.py index 8386fba9f1..e89771a8bb 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_braking.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_braking.py @@ -13,11 +13,11 @@ from carla import Location from carla import Rotation -town2 = {1: [80, 306.6, 5, 0], 2:[150,306.6]} +town2 = {1: [53.12553405761719,137.06280517578125,1.3652913570404053, 0], 2:[105.81783294677734,97.80741882324219]} curves = [0, town2] epsilon = 0 -MODEL_PATH = "models/Braking___282.00max__282.00avg__282.00min__1679121006.model" +MODEL_PATH = "models/Braking___282.model" def setup_tensorflow(): """设置 TensorFlow 2.x 配置""" diff --git a/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_driving.py b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_driving.py index 7ab6ab0ea7..65a36ad214 100644 --- a/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_driving.py +++ b/src/Autonomous_vehicle_navigation_using_deep_learning_master/test_driving.py @@ -1,62 +1,76 @@ import os -os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' # 只显示错误信息 +os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' import random from collections import deque import numpy as np import cv2 import time +import math import tensorflow as tf from tensorflow.keras.models import load_model -from driving_dqn import CarEnv, MEMORY_FRACTION +from driving_dqn import CarEnv epsilon = 0.05 -MODEL_PATH = "models/Driving__6030.00max_6030.00avg_6030.00min__1679109656.model" +MODEL_PATH = "models/Driving__6030.model" + +# ==================== 路径配置 ==================== +# 测试场景配置 + +LEFT_TURN_SCENARIO = { + "name": "左转测试", + "start": [53.12553405761719,137.06280517578125,1.3652913570404053, 0], # [x, y, z, yaw] + "end": [105.81783294677734,97.80741882324219] +} + +STRAIGHT_SCENARIO = { + "name": "直行测试", + "start": [-47.66019058227539,137.0165252685547,0.8818629384040833,0], + "end": [60.39826965332031,137.57113647460938] +} + +CUSTOM_SCENARIO = { + "name": "自定义测试", + "start": [200.0, 250.0, 5, 225], + "end": [150.0, 200.0] +} + +# 选择要测试的场景 +SELECTED_SCENARIO = LEFT_TURN_SCENARIO def setup_tensorflow(): - """设置 TensorFlow 2.x 配置""" - # 设置日志级别减少输出 os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2' print(f"TensorFlow 版本: {tf.__version__}") - # GPU 配置 gpus = tf.config.list_physical_devices('GPU') if gpus: try: - # 设置GPU内存按需增长 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}") - # 如果GPU设置失败，回退到CPU os.environ['CUDA_VISIBLE_DEVICES'] = '-1' print("使用CPU运行") else: print("ℹ️ 未找到GPU，使用CPU运行") def safe_load_model(model_path): - """安全加载模型，处理可能的兼容性问题""" try: print(f"尝试加载模型: {model_path}") - # 检查文件是否存在 if not os.path.exists(model_path): print(f"❌ 模型文件不存在: {model_path}") return None - # 尝试不同的加载方式 try: - # 方式1: 直接加载 model = load_model(model_path) print(f"✅ 成功加载模型: {model_path}") return model except Exception as e: - print(f"标准加载失败，尝试自定义对象加载: {e}") + print(f"标准加载失败: {e}") try: - # 方式2: 使用 compile=False model = load_model(model_path, compile=False) - # 重新编译模型 model.compile(optimizer='adam', loss='mse') print(f"✅ 使用 compile=False 成功加载模型: {model_path}") return model @@ -65,24 +79,19 @@ def safe_load_model(model_path): return None except Exception as e: - print(f"❌ 加载模型时发生错误 {model_path}: {e}") + print(f"❌ 加载模型时发生错误: {e}") return None -def create_compatible_model(input_shape=(4,), output_units=5): - """创建兼容的模型（如果加载失败时使用）""" +def create_compatible_model(input_shape=(2,), output_units=5): print("创建新的驾驶模型...") model = tf.keras.Sequential([ - tf.keras.layers.Dense(128, activation='relu', input_shape=input_shape), - tf.keras.layers.Dropout(0.3), - tf.keras.layers.Dense(64, activation='relu'), - tf.keras.layers.Dropout(0.2), - tf.keras.layers.Dense(32, activation='relu'), + tf.keras.layers.Dense(8, activation='relu', input_shape=input_shape), tf.keras.layers.Dense(output_units, activation='linear') ]) model.compile( - optimizer=tf.keras.optimizers.Adam(learning_rate=0.001), + optimizer=tf.keras.optimizers.Adam(learning_rate=0.0001), loss='mse', metrics=['mae'] ) @@ -90,59 +99,84 @@ def create_compatible_model(input_shape=(4,), output_units=5): return model def preprocess_state_for_prediction(state_data): - """预处理状态数据用于模型预测""" try: if isinstance(state_data, list): state_array = np.array(state_data) else: state_array = state_data - # 确保正确的形状 if len(state_array.shape) == 1: state_array = state_array.reshape(1, -1) + if state_array.shape[1] != 2: + if state_array.shape[1] > 2: + state_array = state_array[:, :2] + elif state_array.shape[1] < 2: + padding = np.zeros((1, 2 - state_array.shape[1])) + state_array = np.concatenate([state_array, padding], axis=1) + return state_array except Exception as e: print(f"状态预处理错误: {e}") - # 返回默认状态 - return np.array([[0, 0, 0, 0]]) + return np.array([[0.0, 0.0]]) + +def print_scenario_info(scenario): + print("\n" + "="*60) + print(f"测试场景: {scenario['name']}") + print("="*60) + print(f"起点坐标: ({scenario['start'][0]:.2f}, {scenario['start'][1]:.2f})") + print(f"起点高度: {scenario['start'][2]:.1f}m") + print(f"初始航向: {scenario['start'][3]}°") + print(f"终点坐标: ({scenario['end'][0]:.2f}, {scenario['end'][1]:.2f})") + + start_x, start_y = scenario['start'][0], scenario['start'][1] + end_x, end_y = scenario['end'][0], scenario['end'][1] + distance = np.sqrt((end_x - start_x)**2 + (end_y - start_y)**2) + print(f"起点到终点距离: {distance:.2f} 单位") + print("="*60) if __name__ == '__main__': FPS = 60 - EPISODES = 10 - + EPISODES = 3 + + # 选择测试场景 + print("请选择测试场景:") + print("1. 左转测试 (默认)") + print("2. 直行测试") + choice = input("请输入选择 (1-2): ") or "1" + + if choice == "2": + SELECTED_SCENARIO = STRAIGHT_SCENARIO + # 设置 TensorFlow setup_tensorflow() # 加载模型 - print("\n" + "="*50) - print("加载驾驶模型") - print("="*50) - model = safe_load_model(MODEL_PATH) - # 如果模型加载失败，创建新的模型 if model is None: print("创建新的驾驶模型...") - model = create_compatible_model(input_shape=(4,), output_units=5) + model = create_compatible_model(input_shape=(2,), output_units=5) print(f"✅ 模型加载完成:") print(f" - 输入形状: {model.input_shape}") print(f" - 输出形状: {model.output_shape}") - # 创建环境 + # 创建环境并传入起点终点 print("\n初始化CARLA环境...") - env = CarEnv() - - # 用于FPS计算 - 保持最近60帧的时间 - fps_counter = deque(maxlen=60) + env = CarEnv( + start_point=SELECTED_SCENARIO["start"], + end_point=SELECTED_SCENARIO["end"] + ) + + # 打印场景信息 + print_scenario_info(SELECTED_SCENARIO) - # 初始化预测 - 第一次预测需要初始化时间 + # 预热模型 print("预热模型...") try: - # 使用正确的预处理 - dummy_state = preprocess_state_for_prediction([0, 0, 0, 0]) + dummy_state = preprocess_state_for_prediction([0.0, 0.0]) model.predict(dummy_state, verbose=0) print("✅ 模型预热完成") except Exception as e: @@ -152,140 +186,142 @@ def preprocess_state_for_prediction(state_data): for episode in range(EPISODES): print(f'\n{"="*50}') print(f'开始 Episode {episode + 1}/{EPISODES}') + print(f'测试场景: {SELECTED_SCENARIO["name"]}') print(f'{"="*50}') - # 重置环境并获取初始状态 - current_state = env.reset() - - # 重置环境数据 - if hasattr(env, 'collision_hist'): - env.collision_hist = [] - - # 初始化数据记录列表 - env.phi = [] - env.dc = [] - env.vel = [] - env.time = [] - - # 生成轨迹 - if hasattr(env, 'trajectory'): - env.trajectory() - - done = False - step_count = 0 - - # 循环步骤 - while not done: - step_count += 1 + try: + # 重置环境 + print("正在重置环境...") + current_state = env.reset() + print(f"初始状态: phi={current_state[0]:.1f}°, d={current_state[1]:.1f}") - # FPS 计数器 - step_start = time.time() - - # 显示当前帧（可选） - # if len(current_state) > 0 and isinstance(current_state[0], np.ndarray): - # cv2.imshow(f'Agent - preview', current_state[0]) - # cv2.waitKey(1) - - # 预测基于当前观察空间的动作 - action = None - qs = None + if len(current_state) != 2: + print(f"⚠️ 警告: 状态维度为 {len(current_state)}，期望 2") + current_state = [0.0, 0.0] - try: - if np.random.random() > epsilon or step_count == 1: - # 从 Q 表获取动作 - state_for_prediction = preprocess_state_for_prediction(current_state) - qs = model.predict(state_for_prediction, verbose=0)[0] - action = np.argmax(qs) - else: - # 获取随机动作 - action = np.random.randint(0, 5) - # 这不需要时间，所以我们添加匹配 60 FPS 的延迟（上面的预测需要更长时间） - if len(fps_counter) > 0: - time.sleep(sum(fps_counter) / len(fps_counter)) + done = False + step_count = 0 + total_reward = 0 + + # 循环步骤 + while not done: + step_count += 1 + env.step_count = step_count # 用于检查移动距离 + + # 显示进度 + if step_count % 20 == 0: + print(f"\nEpisode {episode+1}, Step {step_count}") + print(f"累计奖励: {total_reward:.1f}") + if hasattr(env, 'current_waypoint_index'): + print(f"当前路径点: {env.current_waypoint_index}/{len(env.path)-1}") + + # 预测动作 + action = None + qs = None + + try: + if np.random.random() > epsilon or step_count <= 1: + state_for_prediction = preprocess_state_for_prediction(current_state) + qs = model.predict(state_for_prediction, verbose=0)[0] + action = np.argmax(qs) + if step_count <= 5: + print(f"Step {step_count}: 预测动作 {action}") else: - time.sleep(1/FPS) + action = np.random.randint(0, 5) + if step_count <= 5: + print(f"Step {step_count}: 随机动作 {action}") - except Exception as e: - print(f"❌ 预测错误: {e}") - action = 0 # 默认安全动作 - qs = np.zeros(5) # 默认 Q 值 + except Exception as e: + print(f"❌ 预测错误: {e}") + action = 0 + qs = np.zeros(5) - # 环境步骤（额外的标志通知环境不要因时间限制而中断episode） - try: - new_state, reward, done, waypoint = env.step(action, current_state) + # 环境步骤 + try: + new_state, reward, done, waypoint = env.step(action, current_state) + total_reward += reward + + # 显示重要信息 + if step_count <= 10 or done or abs(reward) > 10: + print(f"Step {step_count}: 动作={action}, 奖励={reward:.1f}, " + f"phi={new_state[0]:.1f}°, d={new_state[1]:.1f}, 完成={done}") - # 设置下一步的当前状态 - current_state = new_state - - # 保存路径点 - if hasattr(env, 'waypoint'): - env.waypoint = waypoint + # 更新状态 + current_state = new_state - except Exception as e: - print(f"❌ 环境步骤错误: {e}") - done = True + except Exception as e: + print(f"❌ 环境步骤错误: {e}") + import traceback + traceback.print_exc() + done = True - # 如果完成 - 代理崩溃，中断episode - if done: - print(f"Episode {episode + 1} 完成，步数: {step_count}") - break + # 如果完成 + if done: + print(f"\nEpisode {episode + 1} 完成!") + print(f"总步数: {step_count}") + print(f"总奖励: {total_reward:.1f}") + if hasattr(env, 'reached') and env.reached == 1: + print("✅ 成功到达目的地!") + else: + print("❌ 未到达目的地") + break - # 测量步骤时间，添加到deque，然后打印最近60帧的平均FPS、q值和采取的动作 - frame_time = time.time() - step_start - fps_counter.append(frame_time) - - if len(fps_counter) > 0: - current_fps = len(fps_counter) / sum(fps_counter) - else: - current_fps = 0 - - # 安全地打印Q值 + except KeyboardInterrupt: + print("\n用户中断，跳过当前episode...") + except Exception as e: + print(f"❌ Episode {episode + 1} 发生错误: {e}") + import traceback + traceback.print_exc() + + finally: + # 保存数据 + print(f"保存 Episode {episode + 1} 的数据...") try: - if qs is not None: - qs_display = f"[{qs[0]:>5.2f}, {qs[1]:>5.2f}, {qs[2]:>5.2f}, {qs[3]:>5.2f}, {qs[4]:>5.2f}]" - else: - qs_display = "[N/A, N/A, N/A, N/A, N/A]" + os.makedirs(f"data/traj_test", exist_ok=True) + + scenario_name = SELECTED_SCENARIO["name"].replace(" ", "_") + + if hasattr(env, 'phi') and env.phi: + np.savetxt(f"data/traj_test/ep{episode+1}_{scenario_name}_phi.txt", env.phi) + if hasattr(env, 'dc') and env.dc: + np.savetxt(f"data/traj_test/ep{episode+1}_{scenario_name}_d.txt", env.dc) + if hasattr(env, 'vel') and env.vel: + np.savetxt(f"data/traj_test/ep{episode+1}_{scenario_name}_vel.txt", env.vel) + if hasattr(env, 'time') and env.time: + np.savetxt(f"data/traj_test/ep{episode+1}_{scenario_name}_time.txt", env.time) + + # 保存场景信息 + with open(f"data/traj_test/ep{episode+1}_{scenario_name}_info.txt", "w") as f: + f.write(f"场景名称: {SELECTED_SCENARIO['name']}\n") + f.write(f"起点: {SELECTED_SCENARIO['start']}\n") + f.write(f"终点: {SELECTED_SCENARIO['end']}\n") + f.write(f"总步数: {step_count}\n") + f.write(f"总奖励: {total_reward}\n") + f.write(f"是否到达: {getattr(env, 'reached', 0)}\n") - print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Q-values: {qs_display} | Action: {action}') + print(f"✅ Episode {episode + 1} 数据保存完成") except Exception as e: - print(f'Step: {step_count:>3d} | FPS: {current_fps:>4.1f} | Action: {action}') - - # 保存数据 - print(f"保存 Episode {episode + 1} 的数据...") - try: - os.makedirs(f"data/traj4/file{episode}", exist_ok=True) - - # 安全地保存数据，检查列表是否为空 - if hasattr(env, 'phi') and env.phi: - np.savetxt(f"data/traj4/file{episode}/phi.txt", env.phi) - if hasattr(env, 'dc') and env.dc: - np.savetxt(f"data/traj4/file{episode}/d.txt", env.dc) - if hasattr(env, 'vel') and env.vel: - np.savetxt(f"data/traj4/file{episode}/vel.txt", env.vel) - if hasattr(env, 'time') and env.time: - np.savetxt(f"data/traj4/file{episode}/time.txt", env.time) - - print(f"✅ Episode {episode + 1} 数据保存完成") - except Exception as e: - print(f"❌ 保存数据错误: {e}") + print(f"❌ 保存数据错误: {e}") - # 在episode结束时销毁actor - print(f"清理 Episode {episode + 1} 的actor...") - try: - if hasattr(env, 'actor_list'): - for actor in env.actor_list: - try: - actor.destroy() - except Exception as e: - print(f"销毁actor错误: {e}") - except Exception as e: - print(f"清理错误: {e}") + # 清理actor + print(f"清理 Episode {episode + 1} 的actor...") + try: + if hasattr(env, 'actor_list'): + for actor in env.actor_list: + try: + actor.destroy() + except Exception as e: + pass + env.actor_list = [] + except Exception as e: + print(f"清理错误: {e}") - print("\n" + "="*50) - print("所有episodes完成!") - print("="*50) + print("\n" + "="*60) + print("所有测试完成!") + print(f"测试了 {EPISODES} 个episodes") + print(f"测试场景: {SELECTED_SCENARIO['name']}") + print("="*60) - # 清理资源 try: cv2.destroyAllWindows() except: From 0f386b83d2efad01fc7744a9cac8ca7f865e2949 Mon Sep 17 00:00:00 2001 From: yume <3545945359@qq.com> Date: Tue, 16 Dec 2025 08:26:53 +0800 Subject: [PATCH 16/16] =?UTF-8?q?=E5=B0=86tset=5Feverything.py=E6=96=87?= =?UTF-8?q?=E4=BB=B6=E6=A8=A1=E5=9D=97=E5=8C=96=EF=BC=8C=E5=B0=86=E8=AE=BE?= =?UTF-8?q?=E7=BD=AE=E6=96=87=E4=BB=B6(config.py)=E5=92=8C=E4=BA=A4?= =?UTF-8?q?=E9=80=9A=E7=94=9F=E6=88=90(generate=5Ftraffic.py)=E6=96=87?= =?UTF-8?q?=E4=BB=B6=E7=BA=B3=E5=85=A5main.py=E7=AE=A1=E7=90=86=EF=BC=8C?= =?UTF-8?q?=E6=96=B0=E5=A2=9E=E4=BA=86=E6=98=BE=E7=A4=BA=E8=A7=84=E5=88=92?= =?UTF-8?q?=E8=B7=AF=E5=BE=84=E5=92=8C=E5=8E=86=E5=8F=B2=E8=B7=AF=E5=BE=84?= =?UTF-8?q?=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../car_env.py | 10 +- .../config.py | 447 +++++------------- .../config_manager.py | 374 +++++++++++++++ .../main.py | 280 +++++++++++ .../model_manager.py | 153 ++++++ .../route_visualizer.py | 301 ++++++++++++ .../traffic_manager.py | 361 ++++++++++++++ .../trajectory_manager.py | 107 +++++ .../vehicle_tracker.py | 401 ++++++++++++++++ 9 files changed, 2106 insertions(+), 328 deletions(-) create mode 100644 src/Autonomous_vehicle_navigation_using_deep_learning_master/config_manager.py create mode 100644 src/Autonomous_vehicle_navigation_using_deep_learning_master/main.py create mode 100644 src/Autonomous_vehicle_navigation_using_deep_learning_master/model_manager.py create mode 100644 src/Autonomous_vehicle_navigation_using_deep_learning_master/route_visualizer.py create mode 100644 src/Autonomous_vehicle_navigation_using_deep_learning_master/traffic_manager.py create mode 100644 src/Autonomous_vehicle_navigation_using_deep_learning_master/trajectory_manager.py create mode 100644 src/Autonomous_vehicle_navigation_using_deep_learning_master/vehicle_tracker.py 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("🔄 车辆跟踪器已重置")