From 3663ceb291be774aeb3d868df1e5167d907e20a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=9E=A0=E8=BD=B2?= <3242212048@qq.com> Date: Fri, 8 May 2026 00:58:16 +0800 Subject: [PATCH 01/10] =?UTF-8?q?=E6=98=BE=E7=A4=BA=E9=81=93=E8=B7=AF?= =?UTF-8?q?=E9=99=90=E9=80=9F=20+=20R=E9=94=AE=E9=87=8D=E8=A7=84=E5=88=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/automatic_control_revise/main.py | 929 +-------------------------- 1 file changed, 32 insertions(+), 897 deletions(-) diff --git a/src/automatic_control_revise/main.py b/src/automatic_control_revise/main.py index c6165fa78d..4bfe953c2b 100644 --- a/src/automatic_control_revise/main.py +++ b/src/automatic_control_revise/main.py @@ -10,18 +10,6 @@ from __future__ import print_function -import argpars#!/usr/bin/env python - -# Copyright (c) 2018 Intel Labs. -# authors: German Ros (german.ros@intel.com) -# -# This work is licensed under the terms of the MIT license. -# For a copy, see . - -"""Example of automatic vehicle control from client side.""" - -from __future__ import print_function - import argparse import collections import datetime @@ -215,6 +203,7 @@ class KeyboardControl(object): def __init__(self, world): self.world = world self.manual_mode = False + self.reroute_requested = False world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) def parse_events(self): @@ -226,6 +215,9 @@ def parse_events(self): self.manual_mode = not self.manual_mode mode_str = "Manual" if self.manual_mode else "Auto" self.world.hud.notification("Switched to {} mode".format(mode_str), seconds=2.0) + if event.key == pygame.K_r: # 新增 + self.reroute_requested = True + self.world.hud.notification("Reroute requested", seconds=1.5) if event.type == pygame.KEYUP: if self._is_quit_shortcut(event.key): return True @@ -300,6 +292,7 @@ def tick(self, world, clock): 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (transform.location.x, transform.location.y)), 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), 'Height: % 18.0f m' % transform.location.z, + 'Speed limit: % 10.0f km/h' % world.player.get_speed_limit(), ''] if isinstance(control, carla.VehicleControl): self._info_text += [ @@ -755,6 +748,7 @@ def game_loop(args): world.agent = agent spawn_points = world.map.get_spawn_points() random.shuffle(spawn_points) + world.spawn_points = spawn_points if spawn_points[0].location != agent.vehicle.get_location(): destination = spawn_points[0].location @@ -798,6 +792,17 @@ def game_loop(args): world.player.apply_control(control) else: + # 重规划请求处理 + if controller.reroute_requested: + controller.reroute_requested = False + if hasattr(world, 'spawn_points') and world.spawn_points: + agent.reroute(world.spawn_points) + world.hud.notification("Rerouting...", seconds=1.5) + else: + temp_spawn = world.map.get_spawn_points() + if temp_spawn: + agent.reroute(temp_spawn) + world.hud.notification("Rerouting...", seconds=1.5) agent.update_information() world.tick(clock) @@ -823,6 +828,7 @@ def game_loop(args): # 手动控制：读取键盘输入生成 control keys = pygame.key.get_pressed() control = carla.VehicleControl() + control.throttle = 1.0 if (keys[pygame.K_UP] or keys[pygame.K_w]) else 0.0 control.brake = 1.0 if (keys[pygame.K_DOWN] or keys[pygame.K_s]) else 0.0 # 转向：左负右正，范围 -1.0 到 1.0 @@ -837,894 +843,23 @@ def game_loop(args): control.manual_gear_shift = False else: control = agent.run_step() - # 限速功能（新增） - if args.max_speed > 0: - vel = world.player.get_velocity() - current_speed = 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) - if current_speed > args.max_speed: - control.throttle = 0.0 - overshoot = (current_speed - args.max_speed) / args.max_speed - control.brake = min(1.0, overshoot) - - world.player.apply_control(control) - - finally: - if world is not None: - world.destroy() - - pygame.quit() - - -# ============================================================================== -# -- main() -------------------------------------------------------------- -# ============================================================================== - - -def main(): - """Main method""" - - argparser = argparse.ArgumentParser( - description='CARLA Automatic Control Client') - argparser.add_argument( - '-v', '--verbose', - action='store_true', - dest='debug', - help='Print debug information') - 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( - '--res', - metavar='WIDTHxHEIGHT', - default='1280x720', - help='Window resolution (default: 1280x720)') - argparser.add_argument( - '--filter', - metavar='PATTERN', - default='vehicle.*', - help='Actor filter (default: "vehicle.*")') - argparser.add_argument( - '--gamma', - default=2.2, - type=float, - help='Gamma correction of the camera (default: 2.2)') - argparser.add_argument( - '-l', '--loop', - action='store_true', - dest='loop', - help='Sets a new random destination upon reaching the previous one (default: False)') - argparser.add_argument( - '-b', '--behavior', type=str, - choices=["cautious", "normal", "aggressive"], - help='Choose one of the possible agent behaviors (default: normal) ', - default='normal') - argparser.add_argument("-a", "--agent", type=str, - choices=["Behavior", "Roaming", "Basic"], - help="select which agent to run", - default="Behavior") - argparser.add_argument( - '-s', '--seed', - help='Set seed for repeating executions (default: None)', - default=None, - type=int) - - argparser.add_argument( - '--max_speed', type=float, default=100.0, - help='Maximum speed in km/h for the autonomous agent (default: 100.0)') - - args = argparser.parse_args() - - args.width, args.height = [int(x) for x in args.res.split('x')] - - log_level = logging.DEBUG if args.debug else logging.INFO - logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) - - logging.info('listening to server %s:%s', args.host, args.port) - - print(__doc__) - - try: - game_loop(args) - - except KeyboardInterrupt: - print('\nCancelled by user. Bye!') - - -if __name__ == '__main__': - main() -import collections -import datetime -import glob -import logging -import math -import os -import random -import re -import sys -import weakref - -try: - import pygame - from pygame.locals import KMOD_CTRL - from pygame.locals import K_ESCAPE - from pygame.locals import K_q -except ImportError: - raise RuntimeError('cannot import pygame, make sure pygame package is installed') - -try: - import numpy as np -except ImportError: - raise RuntimeError( - 'cannot import numpy, make sure numpy package is installed') - -# ============================================================================== -# -- Find CARLA module --------------------------------------------------------- -# ============================================================================== -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 - -# ============================================================================== -# -- Add PythonAPI for release mode -------------------------------------------- -# ============================================================================== -try: - sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla') -except IndexError: - pass - -import carla -from carla import ColorConverter as cc - -from agents.navigation.behavior_agent import BehaviorAgent # pylint: disable=import-error -from agents.navigation.roaming_agent import RoamingAgent # pylint: disable=import-error -from agents.navigation.basic_agent import BasicAgent # pylint: disable=import-error - - -# ============================================================================== -# -- Global functions ---------------------------------------------------------- -# ============================================================================== - - -def find_weather_presets(): - """Method to find weather presets""" - rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)') - def name(x): return ' '.join(m.group(0) for m in rgx.finditer(x)) - presets = [x for x in dir(carla.WeatherParameters) if re.match('[A-Z].+', x)] - return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] - - -def get_actor_display_name(actor, truncate=250): - """Method to get actor display name""" - name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) - return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name - - -# ============================================================================== -# -- World --------------------------------------------------------------- -# ============================================================================== - -class World(object): - """ Class representing the surrounding environment """ - - def __init__(self, carla_world, hud, args): - """Constructor method""" - self.world = carla_world - try: - self.map = self.world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - print(' The server could not send the OpenDRIVE (.xodr) file:') - print(' Make sure it exists, has the same name of your town, and is correct.') - sys.exit(1) - self.hud = hud - self.player = None - self.collision_sensor = None - self.lane_invasion_sensor = None - self.gnss_sensor = None - self.camera_manager = None - self._weather_presets = find_weather_presets() - self._weather_index = 0 - self._actor_filter = args.filter - self._gamma = args.gamma - self.restart(args) - self.world.on_tick(hud.on_world_tick) - self.recording_enabled = False - self.recording_start = 0 - - def restart(self, args): - """Restart the world""" - # Keep same camera config if the camera manager exists. - cam_index = self.camera_manager.index if self.camera_manager is not None else 0 - cam_pos_id = self.camera_manager.transform_index if self.camera_manager is not None else 0 - # Set the seed if requested by user - if args.seed is not None: - random.seed(args.seed) + # ✅ 第四次修改：速度闭环控制（P控制） + vel = world.player.get_velocity() + current_speed = 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) - # Get a random blueprint. - blueprint = random.choice(self.world.get_blueprint_library().filter(self._actor_filter)) - blueprint.set_attribute('role_name', 'hero') - if blueprint.has_attribute('color'): - color = random.choice(blueprint.get_attribute('color').recommended_values) - blueprint.set_attribute('color', color) - # Spawn the player. - print("Spawning the player") - if self.player is not None: - spawn_point = self.player.get_transform() - spawn_point.location.z += 2.0 - spawn_point.rotation.roll = 0.0 - spawn_point.rotation.pitch = 0.0 - self.destroy() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) + target_speed = args.max_speed + error = target_speed - current_speed - while self.player is None: - if not self.map.get_spawn_points(): - print('There are no spawn points available in your map/town.') - print('Please add some Vehicle Spawn Point to your UE4 scene.') - sys.exit(1) - spawn_points = self.map.get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - self.player = self.world.try_spawn_actor(blueprint, spawn_point) - # Set up the sensors. - self.collision_sensor = CollisionSensor(self.player, self.hud) - self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) - self.gnss_sensor = GnssSensor(self.player) - self.camera_manager = CameraManager(self.player, self.hud, self._gamma) - self.camera_manager.transform_index = cam_pos_id - self.camera_manager.set_sensor(cam_index, notify=False) - actor_type = get_actor_display_name(self.player) - self.hud.notification(actor_type) + Kp = 0.05 + base_throttle = 0.25 - def next_weather(self, reverse=False): - """Get next weather setting""" - self._weather_index += -1 if reverse else 1 - self._weather_index %= len(self._weather_presets) - preset = self._weather_presets[self._weather_index] - self.hud.notification('Weather: %s' % preset[1]) - self.player.get_world().set_weather(preset[0]) - - def tick(self, clock): - """Method for every tick""" - self.hud.tick(self, clock) - - def render(self, display): - """Render world""" - self.camera_manager.render(display) - self.hud.render(display) - - def destroy_sensors(self): - """Destroy sensors""" - self.camera_manager.sensor.destroy() - self.camera_manager.sensor = None - self.camera_manager.index = None - - def destroy(self): - """Destroys all actors""" - actors = [ - self.camera_manager.sensor, - self.collision_sensor.sensor, - self.lane_invasion_sensor.sensor, - self.gnss_sensor.sensor, - self.player] - for actor in actors: - if actor is not None: - actor.destroy() - - -# ============================================================================== -# -- KeyboardControl ----------------------------------------------------------- -# ============================================================================== - - -class KeyboardControl(object): - def __init__(self, world): - self.world = world - self.manual_mode = False - world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) - - def parse_events(self): - for event in pygame.event.get(): - if event.type == pygame.QUIT: - return True - if event.type == pygame.KEYDOWN: - if event.key == pygame.K_p: - self.manual_mode = not self.manual_mode - mode_str = "Manual" if self.manual_mode else "Auto" - self.world.hud.notification("Switched to {} mode".format(mode_str), seconds=2.0) - if event.type == pygame.KEYUP: - if self._is_quit_shortcut(event.key): - return True - - @staticmethod - def _is_quit_shortcut(key): - return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) - -# ============================================================================== -# -- HUD ----------------------------------------------------------------------- -# ============================================================================== - - -class HUD(object): - """Class for HUD text""" - - def __init__(self, width, height): - """Constructor method""" - self.dim = (width, height) - font = pygame.font.Font(pygame.font.get_default_font(), 20) - font_name = 'courier' if os.name == 'nt' else 'mono' - fonts = [x for x in pygame.font.get_fonts() if font_name in x] - default_font = 'ubuntumono' - mono = default_font if default_font in fonts else fonts[0] - mono = pygame.font.match_font(mono) - self._font_mono = pygame.font.Font(mono, 12 if os.name == 'nt' else 14) - self._notifications = FadingText(font, (width, 40), (0, height - 40)) - self.help = HelpText(pygame.font.Font(mono, 24), width, height) - self.server_fps = 0 - self.frame = 0 - self.simulation_time = 0 - self._show_info = True - self._info_text = [] - self._server_clock = pygame.time.Clock() - - def on_world_tick(self, timestamp): - """Gets informations from the world at every tick""" - self._server_clock.tick() - self.server_fps = self._server_clock.get_fps() - self.frame = timestamp.frame_count - self.simulation_time = timestamp.elapsed_seconds - - def tick(self, world, clock): - """HUD method for every tick""" - self._notifications.tick(world, clock) - if not self._show_info: - return - transform = world.player.get_transform() - vel = world.player.get_velocity() - control = world.player.get_control() - heading = 'N' if abs(transform.rotation.yaw) < 89.5 else '' - heading += 'S' if abs(transform.rotation.yaw) > 90.5 else '' - heading += 'E' if 179.5 > transform.rotation.yaw > 0.5 else '' - heading += 'W' if -0.5 > transform.rotation.yaw > -179.5 else '' - colhist = world.collision_sensor.get_collision_history() - collision = [colhist[x + self.frame - 200] for x in range(0, 200)] - max_col = max(1.0, max(collision)) - collision = [x / max_col for x in collision] - vehicles = world.world.get_actors().filter('vehicle.*') - - self._info_text = [ - 'Server: % 16.0f FPS' % self.server_fps, - 'Client: % 16.0f FPS' % clock.get_fps(), - '', - 'Vehicle: % 20s' % get_actor_display_name(world.player, truncate=20), - 'Map: % 20s' % world.map.name, - 'Simulation time: % 12s' % datetime.timedelta(seconds=int(self.simulation_time)), - '', - 'Speed: % 15.0f km/h' % (3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)), - u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (transform.rotation.yaw, heading), - 'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (transform.location.x, transform.location.y)), - 'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)), - 'Height: % 18.0f m' % transform.location.z, - ''] - if isinstance(control, carla.VehicleControl): - self._info_text += [ - ('Throttle:', control.throttle, 0.0, 1.0), - ('Steer:', control.steer, -1.0, 1.0), - ('Brake:', control.brake, 0.0, 1.0), - ('Reverse:', control.reverse), - ('Hand brake:', control.hand_brake), - ('Manual:', control.manual_gear_shift), - 'Gear: %s' % {-1: 'R', 0: 'N'}.get(control.gear, control.gear)] - elif isinstance(control, carla.WalkerControl): - self._info_text += [ - ('Speed:', control.speed, 0.0, 5.556), - ('Jump:', control.jump)] - self._info_text += [ - '', - 'Collision:', - collision, - '', - 'Number of vehicles: % 8d' % len(vehicles)] - - if len(vehicles) > 1: - self._info_text += ['Nearby vehicles:'] - - def dist(l): - return math.sqrt((l.x - transform.location.x)**2 + (l.y - transform.location.y) - ** 2 + (l.z - transform.location.z)**2) - vehicles = [(dist(x.get_location()), x) for x in vehicles if x.id != world.player.id] - - for dist, vehicle in sorted(vehicles): - if dist > 200.0: - break - vehicle_type = get_actor_display_name(vehicle, truncate=22) - self._info_text.append('% 4dm %s' % (dist, vehicle_type)) - - def toggle_info(self): - """Toggle info on or off""" - self._show_info = not self._show_info - - def notification(self, text, seconds=2.0): - """Notification text""" - self._notifications.set_text(text, seconds=seconds) - - def error(self, text): - """Error text""" - self._notifications.set_text('Error: %s' % text, (255, 0, 0)) - - def render(self, display): - """Render for HUD class""" - if self._show_info: - info_surface = pygame.Surface((220, self.dim[1])) - info_surface.set_alpha(100) - display.blit(info_surface, (0, 0)) - v_offset = 4 - bar_h_offset = 100 - bar_width = 106 - for item in self._info_text: - if v_offset + 18 > self.dim[1]: - break - if isinstance(item, list): - if len(item) > 1: - points = [(x + 8, v_offset + 8 + (1 - y) * 30) for x, y in enumerate(item)] - pygame.draw.lines(display, (255, 136, 0), False, points, 2) - item = None - v_offset += 18 - elif isinstance(item, tuple): - if isinstance(item[1], bool): - rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) - pygame.draw.rect(display, (255, 255, 255), rect, 0 if item[1] else 1) - else: - rect_border = pygame.Rect((bar_h_offset, v_offset + 8), (bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect_border, 1) - fig = (item[1] - item[2]) / (item[3] - item[2]) - if item[2] < 0.0: - rect = pygame.Rect( - (bar_h_offset + fig * (bar_width - 6), v_offset + 8), (6, 6)) - else: - rect = pygame.Rect((bar_h_offset, v_offset + 8), (fig * bar_width, 6)) - pygame.draw.rect(display, (255, 255, 255), rect) - item = item[0] - if item: # At this point has to be a str. - surface = self._font_mono.render(item, True, (255, 255, 255)) - display.blit(surface, (8, v_offset)) - v_offset += 18 - self._notifications.render(display) - self.help.render(display) - -# ============================================================================== -# -- FadingText ---------------------------------------------------------------- -# ============================================================================== - - -class FadingText(object): - """ Class for fading text """ - - def __init__(self, font, dim, pos): - """Constructor method""" - self.font = font - self.dim = dim - self.pos = pos - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - - def set_text(self, text, color=(255, 255, 255), seconds=2.0): - """Set fading text""" - text_texture = self.font.render(text, True, color) - self.surface = pygame.Surface(self.dim) - self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) - - def tick(self, _, clock): - """Fading text method for every tick""" - delta_seconds = 1e-3 * clock.get_time() - self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) - - def render(self, display): - """Render fading text method""" - display.blit(self.surface, self.pos) - -# ============================================================================== -# -- HelpText ------------------------------------------------------------------ -# ============================================================================== - - -class HelpText(object): - """ Helper class for text render""" - - def __init__(self, font, width, height): - """Constructor method""" - lines = __doc__.split('\n') - self.font = font - self.dim = (680, len(lines) * 22 + 12) - self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) - self.seconds_left = 0 - self.surface = pygame.Surface(self.dim) - self.surface.fill((0, 0, 0, 0)) - for i, line in enumerate(lines): - text_texture = self.font.render(line, True, (255, 255, 255)) - self.surface.blit(text_texture, (22, i * 22)) - self._render = False - self.surface.set_alpha(220) - - def toggle(self): - """Toggle on or off the render help""" - self._render = not self._render - - def render(self, display): - """Render help text method""" - if self._render: - display.blit(self.surface, self.pos) - -# ============================================================================== -# -- CollisionSensor ----------------------------------------------------------- -# ============================================================================== - - -class CollisionSensor(object): - """ Class for collision sensors""" - - def __init__(self, parent_actor, hud): - """Constructor method""" - self.sensor = None - self.history = [] - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - blueprint = world.get_blueprint_library().find('sensor.other.collision') - self.sensor = world.spawn_actor(blueprint, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to - # self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) - - def get_collision_history(self): - """Gets the history of collisions""" - history = collections.defaultdict(int) - for frame, intensity in self.history: - history[frame] += intensity - return history - - @staticmethod - def _on_collision(weak_self, event): - """On collision method""" - self = weak_self() - if not self: - return - actor_type = get_actor_display_name(event.other_actor) - self.hud.notification('Collision with %r' % actor_type) - impulse = event.normal_impulse - intensity = math.sqrt(impulse.x ** 2 + impulse.y ** 2 + impulse.z ** 2) - self.history.append((event.frame, intensity)) - if len(self.history) > 4000: - self.history.pop(0) - -# ============================================================================== -# -- LaneInvasionSensor -------------------------------------------------------- -# ============================================================================== - - -class LaneInvasionSensor(object): - """Class for lane invasion sensors""" - - def __init__(self, parent_actor, hud): - """Constructor method""" - self.sensor = None - self._parent = parent_actor - self.hud = hud - world = self._parent.get_world() - bp = world.get_blueprint_library().find('sensor.other.lane_invasion') - self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: LaneInvasionSensor._on_invasion(weak_self, event)) - - @staticmethod - def _on_invasion(weak_self, event): - """On invasion method""" - self = weak_self() - if not self: - return - lane_types = set(x.type for x in event.crossed_lane_markings) - text = ['%r' % str(x).split()[-1] for x in lane_types] - self.hud.notification('Crossed line %s' % ' and '.join(text)) - -# ============================================================================== -# -- GnssSensor -------------------------------------------------------- -# ============================================================================== - - -class GnssSensor(object): - """ Class for GNSS sensors""" - - def __init__(self, parent_actor): - """Constructor method""" - self.sensor = None - self._parent = parent_actor - self.lat = 0.0 - self.lon = 0.0 - world = self._parent.get_world() - blueprint = world.get_blueprint_library().find('sensor.other.gnss') - self.sensor = world.spawn_actor(blueprint, carla.Transform(carla.Location(x=1.0, z=2.8)), - attach_to=self._parent) - # We need to pass the lambda a weak reference to - # self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) - - @staticmethod - def _on_gnss_event(weak_self, event): - """GNSS method""" - self = weak_self() - if not self: - return - self.lat = event.latitude - self.lon = event.longitude - -# ============================================================================== -# -- CameraManager ------------------------------------------------------------- -# ============================================================================== - - -class CameraManager(object): - """ Class for camera management""" - - def __init__(self, parent_actor, hud, gamma_correction): - """Constructor method""" - self.sensor = None - self.surface = None - self._parent = parent_actor - self.hud = hud - self.recording = False - bound_y = 0.5 + self._parent.bounding_box.extent.y - attachment = carla.AttachmentType - self._camera_transforms = [ - (carla.Transform( - carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=1.6, z=1.7)), attachment.Rigid), - (carla.Transform( - carla.Location(x=5.5, y=1.5, z=1.5)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0)), attachment.SpringArm), - (carla.Transform( - carla.Location(x=-1, y=-bound_y, z=0.5)), attachment.Rigid)] - self.transform_index = 1 - self.sensors = [ - ['sensor.camera.rgb', cc.Raw, 'Camera RGB'], - ['sensor.camera.depth', cc.Raw, 'Camera Depth (Raw)'], - ['sensor.camera.depth', cc.Depth, 'Camera Depth (Gray Scale)'], - ['sensor.camera.depth', cc.LogarithmicDepth, 'Camera Depth (Logarithmic Gray Scale)'], - ['sensor.camera.semantic_segmentation', cc.Raw, 'Camera Semantic Segmentation (Raw)'], - ['sensor.camera.semantic_segmentation', cc.CityScapesPalette, - 'Camera Semantic Segmentation (CityScapes Palette)'], - ['sensor.lidar.ray_cast', None, 'Lidar (Ray-Cast)']] - world = self._parent.get_world() - bp_library = world.get_blueprint_library() - for item in self.sensors: - blp = bp_library.find(item[0]) - if item[0].startswith('sensor.camera'): - blp.set_attribute('image_size_x', str(hud.dim[0])) - blp.set_attribute('image_size_y', str(hud.dim[1])) - if blp.has_attribute('gamma'): - blp.set_attribute('gamma', str(gamma_correction)) - elif item[0].startswith('sensor.lidar'): - blp.set_attribute('range', '50') - item.append(blp) - self.index = None - - def toggle_camera(self): - """Activate a camera""" - self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) - self.set_sensor(self.index, notify=False, force_respawn=True) - - def set_sensor(self, index, notify=True, force_respawn=False): - """Set a sensor""" - index = index % len(self.sensors) - needs_respawn = True if self.index is None else ( - force_respawn or (self.sensors[index][0] != self.sensors[self.index][0])) - if needs_respawn: - if self.sensor is not None: - self.sensor.destroy() - self.surface = None - self.sensor = self._parent.get_world().spawn_actor( - self.sensors[index][-1], - self._camera_transforms[self.transform_index][0], - attach_to=self._parent, - attachment_type=self._camera_transforms[self.transform_index][1]) - - # We need to pass the lambda a weak reference to - # self to avoid circular reference. - weak_self = weakref.ref(self) - self.sensor.listen(lambda image: CameraManager._parse_image(weak_self, image)) - if notify: - self.hud.notification(self.sensors[index][2]) - self.index = index - - def next_sensor(self): - """Get the next sensor""" - self.set_sensor(self.index + 1) - - def toggle_recording(self): - """Toggle recording on or off""" - self.recording = not self.recording - self.hud.notification('Recording %s' % ('On' if self.recording else 'Off')) - - def render(self, display): - """Render method""" - if self.surface is not None: - display.blit(self.surface, (0, 0)) - - @staticmethod - def _parse_image(weak_self, image): - self = weak_self() - if not self: - return - if self.sensors[self.index][0].startswith('sensor.lidar'): - points = np.frombuffer(image.raw_data, dtype=np.dtype('f4')) - points = np.reshape(points, (int(points.shape[0] / 4), 4)) - lidar_data = np.array(points[:, :2]) - lidar_data *= min(self.hud.dim) / 100.0 - lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) - lidar_data = np.fabs(lidar_data) # pylint: disable=assignment-from-no-return - lidar_data = lidar_data.astype(np.int32) - lidar_data = np.reshape(lidar_data, (-1, 2)) - lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) - lidar_img = np.zeros(lidar_img_size) - lidar_img[tuple(lidar_data.T)] = (255, 255, 255) - self.surface = pygame.surfarray.make_surface(lidar_img) - else: - image.convert(self.sensors[self.index][1]) - array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - array = np.reshape(array, (image.height, image.width, 4)) - array = array[:, :, :3] - array = array[:, :, ::-1] - self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) - if self.recording: - image.save_to_disk('_out/%08d' % image.frame) - -# ============================================================================== -# -- Game Loop --------------------------------------------------------- -# ============================================================================== - - -def game_loop(args): - """ Main loop for agent""" - - pygame.init() - pygame.font.init() - world = None - tot_target_reached = 0 - num_min_waypoints = 21 - - try: - client = carla.Client(args.host, args.port) - client.set_timeout(4.0) - - display = pygame.display.set_mode( - (args.width, args.height), - pygame.HWSURFACE | pygame.DOUBLEBUF) - - hud = HUD(args.width, args.height) - world = World(client.get_world(), hud, args) - controller = KeyboardControl(world) - - if args.agent == "Roaming": - agent = RoamingAgent(world.player) - elif args.agent == "Basic": - agent = BasicAgent(world.player) - spawn_point = world.map.get_spawn_points()[0] - agent.set_destination((spawn_point.location.x, - spawn_point.location.y, - spawn_point.location.z)) - else: - agent = BehaviorAgent(world.player, behavior=args.behavior) - - spawn_points = world.map.get_spawn_points() - random.shuffle(spawn_points) - - if spawn_points[0].location != agent.vehicle.get_location(): - destination = spawn_points[0].location - else: - destination = spawn_points[1].location - - agent.set_destination(agent.vehicle.get_location(), destination, clean=True) - - clock = pygame.time.Clock() - - while True: - clock.tick_busy_loop(60) - if controller.parse_events(): - return - - # As soon as the server is ready continue! - if not world.world.wait_for_tick(10.0): - continue - - if args.agent == "Roaming" or args.agent == "Basic": - if controller.parse_events(): - return - - # as soon as the server is ready continue! - world.world.wait_for_tick(10.0) - - world.tick(clock) - world.render(display) - pygame.display.flip() - control = agent.run_step() - control.manual_gear_shift = False - - # 限速功能 - if args.max_speed > 0: - vel = world.player.get_velocity() - current_speed = 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) - if current_speed > args.max_speed: + if error >= 0: + throttle = base_throttle + Kp * error + control.throttle = max(0.0, min(throttle, 1.0)) + control.brake = 0.0 + else: control.throttle = 0.0 - overshoot = (current_speed - args.max_speed) / args.max_speed - control.brake = min(1.0, overshoot) - - world.player.apply_control(control) - else: - agent.update_information() - - world.tick(clock) - world.render(display) - pygame.display.flip() - - # Set new destination when target has been reached - if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop: - agent.reroute(spawn_points) - tot_target_reached += 1 - world.hud.notification("The target has been reached " + - str(tot_target_reached) + " times.", seconds=4.0) - - elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop: - print("Target reached, mission accomplished...") - break - - speed_limit = world.player.get_speed_limit() - agent.get_local_planner().set_speed(speed_limit) - - # 手动/自动模式切换 - if controller.manual_mode: - # 手动控制：读取键盘输入生成 control - keys = pygame.key.get_pressed() - control = carla.VehicleControl() - control.throttle = 1.0 if (keys[pygame.K_UP] or keys[pygame.K_w]) else 0.0 - control.brake = 1.0 if (keys[pygame.K_DOWN] or keys[pygame.K_s]) else 0.0 - # 转向：左负右正，范围 -1.0 到 1.0 - steer = 0.0 - if keys[pygame.K_LEFT] or keys[pygame.K_a]: - steer = -1.0 - elif keys[pygame.K_RIGHT] or keys[pygame.K_d]: - steer = 1.0 - control.steer = steer - control.reverse = keys[pygame.K_q] - control.hand_brake = keys[pygame.K_SPACE] - control.manual_gear_shift = False - else: - control = agent.run_step() - # 限速功能（新增） - if args.max_speed > 0: - vel = world.player.get_velocity() - current_speed = 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) - if current_speed > args.max_speed: - control.throttle = 0.0 - overshoot = (current_speed - args.max_speed) / args.max_speed - control.brake = min(1.0, overshoot) + control.brake = max(0.0, min(-Kp * error, 1.0)) world.player.apply_control(control) @@ -1819,4 +954,4 @@ def main(): if __name__ == '__main__': - main() + main() \ No newline at end of file From 4c4b799a930044c298d6dc67bd309bb0ee46a9c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=9E=A0=E8=BD=B2?= <3242212048@qq.com> Date: Fri, 8 May 2026 15:31:35 +0800 Subject: [PATCH 02/10] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=89=8D=E6=96=B9?= =?UTF-8?q?=E9=9A=9C=E7=A2=8D=E7=89=A9=E8=B7=9D=E7=A6=BB=E6=A3=80=E6=B5=8B?= =?UTF-8?q?=E4=B8=8E=E6=98=BE=E7=A4=BA=EF=BC=88=E5=9F=BA=E4=BA=8EActor?= =?UTF-8?q?=E8=BF=87=E6=BB=A4=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/automatic_control_revise/main.py | 37 ++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/automatic_control_revise/main.py b/src/automatic_control_revise/main.py index 4bfe953c2b..08fdfe600a 100644 --- a/src/automatic_control_revise/main.py +++ b/src/automatic_control_revise/main.py @@ -327,6 +327,43 @@ def dist(l): break vehicle_type = get_actor_display_name(vehicle, truncate=22) self._info_text.append('% 4dm %s' % (dist, vehicle_type)) + + # 新增：基于Actor的最近障碍物距离检测（车辆、行人、交通锥、路灯等） + obstacle_distance = None + vehicle_location = world.player.get_location() + vehicle_transform = world.player.get_transform() + forward_vector = vehicle_transform.get_forward_vector() + + all_actors = world.world.get_actors() + for actor in all_actors: + # 排除玩家车自身 + if actor.id == world.player.id: + continue + # 排除所有附着在玩家车上的传感器（安全检查父级） + if hasattr(actor, 'get_parent'): + if actor.get_parent() is not None and actor.get_parent().id == world.player.id: + continue + # 只考虑前方30米内的物体，提高性能 + if actor.get_location().distance(vehicle_location) > 30.0: + continue + # 计算相对向量 + delta = actor.get_location() - vehicle_location + # 判断是否在前方：点积 > 0 + dot = forward_vector.x * delta.x + forward_vector.y * delta.y + forward_vector.z * delta.z + if dot < 0: + continue + # 计算距离 + dist = math.sqrt(delta.x**2 + delta.y**2 + delta.z**2) + # 忽略距离小于 3.5 米的物体（自身传感器） + if dist < 3.5: + continue + if obstacle_distance is None or dist < obstacle_distance: + obstacle_distance = dist + + if obstacle_distance is not None: + self._info_text.append('Obstacle: % 5.1f m' % obstacle_distance) + else: + self._info_text.append('Obstacle: None') # 新增：显示下一个路径点的距离，并在到达新路径点时提示 if hasattr(world, 'agent') and world.agent is not None: From 1cf7a7e5daa17879c07764d03ba87d8883236e68 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=9E=A0=E8=BD=B2?= <3242212048@qq.com> Date: Sun, 10 May 2026 14:10:18 +0800 Subject: [PATCH 03/10] =?UTF-8?q?##=20=E6=A0=87=E9=A2=98=EF=BC=9A=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E6=8C=89=20V=20=E9=94=AE=E4=BF=9D=E5=AD=98=E5=BD=93?= =?UTF-8?q?=E5=89=8D=E7=9B=B8=E6=9C=BA=E7=94=BB=E9=9D=A2=E6=88=AA=E5=9B=BE?= =?UTF-8?q?=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/automatic_control_revise/main.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/automatic_control_revise/main.py b/src/automatic_control_revise/main.py index 08fdfe600a..34becc4d3b 100644 --- a/src/automatic_control_revise/main.py +++ b/src/automatic_control_revise/main.py @@ -218,6 +218,10 @@ def parse_events(self): if event.key == pygame.K_r: # 新增 self.reroute_requested = True self.world.hud.notification("Reroute requested", seconds=1.5) + if event.key == pygame.K_v: # 新增截图功能 + if self.world.camera_manager is not None: + self.world.camera_manager.request_screenshot = True + self.world.hud.notification("Screenshot requested", seconds=1.0) if event.type == pygame.KEYUP: if self._is_quit_shortcut(event.key): return True @@ -637,6 +641,7 @@ def __init__(self, parent_actor, hud, gamma_correction): self._parent = parent_actor self.hud = hud self.recording = False + self.request_screenshot = False # 新增：截图请求标志 bound_y = 0.5 + self._parent.bounding_box.extent.y attachment = carla.AttachmentType self._camera_transforms = [ @@ -741,6 +746,20 @@ def _parse_image(weak_self, image): array = array[:, :, :3] array = array[:, :, ::-1] self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + + # 截图请求处理 + if self.request_screenshot: + self.request_screenshot = False + # 生成时间戳文件名 + import datetime + import os + timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S") + # 创建 _screenshots 目录（如果不存在） + os.makedirs("_screenshots", exist_ok=True) + filename = f"_screenshots/screenshot_{timestamp}.png" + pygame.image.save(self.surface, filename) + self.hud.notification(f"Screenshot saved: {filename}", seconds=2.0) + if self.recording: image.save_to_disk('_out/%08d' % image.frame) From f540f8b28b3c42df524c08b4687aec860621f691 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=9E=A0=E8=BD=B2?= <3242212048@qq.com> Date: Mon, 11 May 2026 20:20:16 +0800 Subject: [PATCH 04/10] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=87=AA=E5=8A=A8?= =?UTF-8?q?=E7=B4=A7=E6=80=A5=E5=88=B6=E5=8A=A8=EF=BC=88AEB=EF=BC=89?= =?UTF-8?q?=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/automatic_control_revise/main.py | 37 ++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/automatic_control_revise/main.py b/src/automatic_control_revise/main.py index 34becc4d3b..b5579677be 100644 --- a/src/automatic_control_revise/main.py +++ b/src/automatic_control_revise/main.py @@ -257,6 +257,7 @@ def __init__(self, width, height): self._info_text = [] self._server_clock = pygame.time.Clock() self.last_waypoint = None + self.current_obstacle = None def on_world_tick(self, timestamp): """Gets informations from the world at every tick""" @@ -368,7 +369,11 @@ def dist(l): self._info_text.append('Obstacle: % 5.1f m' % obstacle_distance) else: self._info_text.append('Obstacle: None') + + # 将当前障碍物距离保存到实例变量，供 AEB 使用 + self.current_obstacle = obstacle_distance + # 新增：显示下一个路径点的距离，并在到达新路径点时提示 if hasattr(world, 'agent') and world.agent is not None: try: @@ -787,6 +792,7 @@ def game_loop(args): hud = HUD(args.width, args.height) world = World(client.get_world(), hud, args) + world.aeb_triggered = False # 初始化自动紧急制动触发标志 controller = KeyboardControl(world) if args.agent == "Roaming": @@ -845,6 +851,20 @@ def game_loop(args): control.throttle = 0.0 overshoot = (current_speed - args.max_speed) / args.max_speed control.brake = min(1.0, overshoot) + + # ===== 自动紧急制动 (AEB) 开始 ===== + if hasattr(world.hud, 'current_obstacle') and world.hud.current_obstacle is not None: + if world.hud.current_obstacle < args.aeb_distance: + # 强制刹车 + control.throttle = 0.0 + control.brake = 1.0 + control.reverse = False + if not world.aeb_triggered: + world.hud.notification("AEB engaged!", seconds=1.0) + world.aeb_triggered = True + else: + world.aeb_triggered = False + # ===== AEB 结束 ===== world.player.apply_control(control) else: @@ -916,6 +936,20 @@ def game_loop(args): else: control.throttle = 0.0 control.brake = max(0.0, min(-Kp * error, 1.0)) + + # ===== 自动紧急制动 (AEB) 开始 ===== + if hasattr(world.hud, 'current_obstacle') and world.hud.current_obstacle is not None: + if world.hud.current_obstacle < args.aeb_distance: + # 强制刹车 + control.throttle = 0.0 + control.brake = 1.0 + control.reverse = False + if not world.aeb_triggered: + world.hud.notification("AEB engaged!", seconds=3.0) + world.aeb_triggered = True + else: + world.aeb_triggered = False + # ===== AEB 结束 ===== world.player.apply_control(control) @@ -991,6 +1025,9 @@ def main(): '--max_speed', type=float, default=100.0, help='Maximum speed in km/h for the autonomous agent (default: 100.0)') + argparser.add_argument( + '--aeb_distance', type=float, default=5.0, + help='Distance in meters to trigger Automatic Emergency Braking (default: 5.0)') args = argparser.parse_args() args.width, args.height = [int(x) for x in args.res.split('x')] From 9d2b57ee0a116e4a11181b777538ae90138818f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=9E=A0=E8=BD=B2?= <3242212048@qq.com> Date: Fri, 15 May 2026 17:05:50 +0800 Subject: [PATCH 05/10] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=AE=9E=E6=97=B6?= =?UTF-8?q?=E9=80=9F=E5=BA=A6=E6=9B=B2=E7=BA=BF=E5=9B=BE=E6=98=BE=E7=A4=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/automatic_control_revise/main.py | 40 ++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/src/automatic_control_revise/main.py b/src/automatic_control_revise/main.py index b5579677be..ac6467f102 100644 --- a/src/automatic_control_revise/main.py +++ b/src/automatic_control_revise/main.py @@ -256,6 +256,8 @@ def __init__(self, width, height): self._show_info = True self._info_text = [] self._server_clock = pygame.time.Clock() + self.speed_history = [] # 存储最近的速度值 (km/h) + self.max_speed_points = 60 # 最多保存60个点 self.last_waypoint = None self.current_obstacle = None @@ -299,6 +301,13 @@ def tick(self, world, clock): 'Height: % 18.0f m' % transform.location.z, 'Speed limit: % 10.0f km/h' % world.player.get_speed_limit(), ''] + + # 记录速度历史（用于速度曲线） + current_speed = 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) + self.speed_history.append(current_speed) + if len(self.speed_history) > self.max_speed_points: + self.speed_history.pop(0) + if isinstance(control, carla.VehicleControl): self._info_text += [ ('Throttle:', control.throttle, 0.0, 1.0), @@ -458,6 +467,37 @@ def render(self, display): v_offset += 18 self._notifications.render(display) self.help.render(display) + + # 绘制速度曲线（右下角） + if len(self.speed_history) > 1: + graph_width = 200 + graph_height = 80 + graph_x = self.dim[0] - graph_width - 10 + graph_y = self.dim[1] - graph_height - 10 + # 背景框 + bg_rect = pygame.Rect(graph_x, graph_y, graph_width, graph_height) + pygame.draw.rect(display, (0, 0, 0, 100), bg_rect) + # 显示当前速度数值（曲线图左上角） + font = pygame.font.Font(None, 20) + speed_text = f"{self.speed_history[-1]:.0f} km/h" + text_surface = font.render(speed_text, True, (255, 255, 255)) + display.blit(text_surface, (graph_x + 5, graph_y + 5)) + # 边框 + pygame.draw.rect(display, (255, 255, 255), bg_rect, 1) + + # 计算纵坐标比例 + max_speed = max(self.speed_history) + if max_speed < 1: + max_speed = 1 + x_step = graph_width / (len(self.speed_history) - 1) + points = [] + for i, speed in enumerate(self.speed_history): + x = graph_x + i * x_step + y = graph_y + graph_height - (speed / max_speed) * graph_height + points.append((x, y)) + # 绘制折线（黄色） + pygame.draw.lines(display, (255, 255, 0), False, points, 2) + # ============================================================================== # -- FadingText ---------------------------------------------------------------- From a1d08e1c90ad5aef7d2dd0f7b3bc722b1d51d4aa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=9E=A0=E8=BD=B2?= <3242212048@qq.com> Date: Wed, 20 May 2026 00:00:15 +0800 Subject: [PATCH 06/10] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=87=AA=E9=80=82?= =?UTF-8?q?=E5=BA=94=E5=B7=A1=E8=88=AA=E6=8E=A7=E5=88=B6=EF=BC=88ACC?= =?UTF-8?q?=EF=BC=89=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/automatic_control_revise/main.py | 35 ++++++++++++++++++++++++---- 1 file changed, 30 insertions(+), 5 deletions(-) diff --git a/src/automatic_control_revise/main.py b/src/automatic_control_revise/main.py index ac6467f102..368f02f7f8 100644 --- a/src/automatic_control_revise/main.py +++ b/src/automatic_control_revise/main.py @@ -959,16 +959,30 @@ def game_loop(args): control.manual_gear_shift = False else: control = agent.run_step() - # ✅ 第四次修改：速度闭环控制（P控制） + + # 获取障碍物距离（用于 ACC） + obs_dist = None + if hasattr(world.hud, 'current_obstacle'): + obs_dist = world.hud.current_obstacle + + # 计算自适应巡航目标速度 + if args.acc_enable and obs_dist is not None: + if obs_dist < args.acc_min_dist: + target_speed = 0.0 + elif obs_dist < args.acc_max_dist: + ratio = (obs_dist - args.acc_min_dist) / (args.acc_max_dist - args.acc_min_dist) + target_speed = args.max_speed * ratio + else: + target_speed = args.max_speed + else: + target_speed = args.max_speed + + # P 控制器（速度闭环） vel = world.player.get_velocity() current_speed = 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) - - target_speed = args.max_speed error = target_speed - current_speed - Kp = 0.05 base_throttle = 0.25 - if error >= 0: throttle = base_throttle + Kp * error control.throttle = max(0.0, min(throttle, 1.0)) @@ -1068,6 +1082,17 @@ def main(): argparser.add_argument( '--aeb_distance', type=float, default=5.0, help='Distance in meters to trigger Automatic Emergency Braking (default: 5.0)') + + argparser.add_argument( + '--acc_enable', action='store_true', + help='Enable Adaptive Cruise Control (ACC)') + argparser.add_argument( + '--acc_max_dist', type=float, default=15.0, + help='Distance (m) above which ACC resumes full speed (default: 15.0)') + argparser.add_argument( + '--acc_min_dist', type=float, default=5.0, + help='Distance (m) below which ACC requests full stop (default: 5.0)') + args = argparser.parse_args() args.width, args.height = [int(x) for x in args.res.split('x')] From 762d99d86f60f63cb959fe90737311b9d0d74179 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=9E=A0=E8=BD=B2?= <3242212048@qq.com> Date: Wed, 27 May 2026 01:20:29 +0800 Subject: [PATCH 07/10] =?UTF-8?q?=E5=9F=BA=E4=BA=8EMLP=E7=A5=9E=E7=BB=8F?= =?UTF-8?q?=E7=BD=91=E7=BB=9C=E7=9A=84=E8=87=AA=E9=80=82=E5=BA=94=E5=B7=A1?= =?UTF-8?q?=E8=88=AA=E6=8E=A7=E5=88=B6=EF=BC=88MLP-ACC=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/automatic_control_revise/main.py | 69 +++++++++++++++++++++++----- 1 file changed, 57 insertions(+), 12 deletions(-) diff --git a/src/automatic_control_revise/main.py b/src/automatic_control_revise/main.py index 368f02f7f8..027c9413ea 100644 --- a/src/automatic_control_revise/main.py +++ b/src/automatic_control_revise/main.py @@ -357,6 +357,9 @@ def dist(l): if hasattr(actor, 'get_parent'): if actor.get_parent() is not None and actor.get_parent().id == world.player.id: continue + # 只考虑车辆和行人，忽略静态物体（路灯、路锥等） + if not (actor.type_id.startswith('vehicle.') or actor.type_id.startswith('walker.')): + continue # 只考虑前方30米内的物体，提高性能 if actor.get_location().distance(vehicle_location) > 30.0: continue @@ -818,6 +821,30 @@ def game_loop(args): pygame.init() pygame.font.init() + # 加载机器学习模型（如果启用） + ml_model = None + ml_scaler = None + if args.ml_acc: + try: + import joblib + ml_model = joblib.load('acc_mlp_model.pkl') + ml_scaler = joblib.load('acc_scaler.pkl') + print("Loaded MLP model for ACC") + except Exception as e: + print(f"Failed to load ML model: {e}. Falling back to rule-based ACC.") + args.ml_acc = False + # 数据记录初始化 + data_file = None + data_writer = None + if args.record_data: + import csv + data_file = open(args.record_data, 'w', newline='') + data_writer = csv.writer(data_file) + data_writer.writerow(['current_speed', 'obstacle_dist', 'target_speed']) + # 如果启用了数据记录，自动开启循环模式以保证持续行驶 + if args.record_data and not args.loop: + args.loop = True + print("Data recording enabled: forcing loop mode to keep vehicle running.") world = None tot_target_reached = 0 num_min_waypoints = 21 @@ -960,26 +987,39 @@ def game_loop(args): else: control = agent.run_step() + # 获取当前速度（提前计算） + vel = world.player.get_velocity() + current_speed = 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) + # 获取障碍物距离（用于 ACC） obs_dist = None if hasattr(world.hud, 'current_obstacle'): obs_dist = world.hud.current_obstacle - # 计算自适应巡航目标速度 + # 计算自适应巡航目标速度（MLP 或规则） if args.acc_enable and obs_dist is not None: - if obs_dist < args.acc_min_dist: - target_speed = 0.0 - elif obs_dist < args.acc_max_dist: - ratio = (obs_dist - args.acc_min_dist) / (args.acc_max_dist - args.acc_min_dist) - target_speed = args.max_speed * ratio + if args.ml_acc and ml_model is not None and ml_scaler is not None: + import numpy as np + features = np.array([[current_speed, obs_dist]]) + features_scaled = ml_scaler.transform(features) + target_speed = ml_model.predict(features_scaled)[0] + target_speed = max(0.0, min(target_speed, args.max_speed)) else: - target_speed = args.max_speed + if obs_dist < args.acc_min_dist: + target_speed = 0.0 + elif obs_dist < args.acc_max_dist: + ratio = (obs_dist - args.acc_min_dist) / (args.acc_max_dist - args.acc_min_dist) + target_speed = args.max_speed * ratio + else: + target_speed = args.max_speed else: target_speed = args.max_speed + # 记录数据（如果启用） + if data_writer is not None: + data_writer.writerow([current_speed, obs_dist if obs_dist is not None else -1.0, target_speed]) + # P 控制器（速度闭环） - vel = world.player.get_velocity() - current_speed = 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) error = target_speed - current_speed Kp = 0.05 base_throttle = 0.25 @@ -990,11 +1030,10 @@ def game_loop(args): else: control.throttle = 0.0 control.brake = max(0.0, min(-Kp * error, 1.0)) - + # ===== 自动紧急制动 (AEB) 开始 ===== if hasattr(world.hud, 'current_obstacle') and world.hud.current_obstacle is not None: if world.hud.current_obstacle < args.aeb_distance: - # 强制刹车 control.throttle = 0.0 control.brake = 1.0 control.reverse = False @@ -1092,7 +1131,13 @@ def main(): argparser.add_argument( '--acc_min_dist', type=float, default=5.0, help='Distance (m) below which ACC requests full stop (default: 5.0)') - + argparser.add_argument( + '--record_data', type=str, default=None, + help='Record ACC training data (features: current_speed, obstacle_dist; target: target_speed) to specified CSV file') + argparser.add_argument( + '--ml_acc', action='store_true', + help='Use MLP model for ACC (instead of rule-based linear interpolation)') + args = argparser.parse_args() args.width, args.height = [int(x) for x in args.res.split('x')] From cd90aef9966ddf5b17beadfebf131a5a5b3dade7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=9E=A0=E8=BD=B2?= <3242212048@qq.com> Date: Mon, 8 Jun 2026 16:56:35 +0800 Subject: [PATCH 08/10] =?UTF-8?q?HUD=E8=B6=85=E9=80=9F=E8=AD=A6=E7=A4=BA?= =?UTF-8?q?=EF=BC=88=E9=80=9F=E5=BA=A6=E6=95=B0=E5=AD=97=E5=8F=98=E7=BA=A2?= =?UTF-8?q?=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/automatic_control_revise/main.py | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/automatic_control_revise/main.py b/src/automatic_control_revise/main.py index 027c9413ea..b3b4cf7723 100644 --- a/src/automatic_control_revise/main.py +++ b/src/automatic_control_revise/main.py @@ -260,6 +260,7 @@ def __init__(self, width, height): self.max_speed_points = 60 # 最多保存60个点 self.last_waypoint = None self.current_obstacle = None + self.overspeed = False # 超速标志 def on_world_tick(self, timestamp): """Gets informations from the world at every tick""" @@ -285,6 +286,10 @@ def tick(self, world, clock): max_col = max(1.0, max(collision)) collision = [x / max_col for x in collision] vehicles = world.world.get_actors().filter('vehicle.*') + # 判断是否超速（当前速度 > 道路限速） + current_speed = 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) + road_limit = world.player.get_speed_limit() + self.overspeed = current_speed > road_limit self._info_text = [ 'Server: % 16.0f FPS' % self.server_fps, @@ -465,7 +470,12 @@ def render(self, display): pygame.draw.rect(display, (255, 255, 255), rect) item = item[0] if item: # At this point has to be a str. - surface = self._font_mono.render(item, True, (255, 255, 255)) + # 超速时速度文字变红 + if item.startswith('Speed:') and self.overspeed: + color = (255, 0, 0) + else: + color = (255, 255, 255) + surface = self._font_mono.render(item, True, color) display.blit(surface, (8, v_offset)) v_offset += 18 self._notifications.render(display) From d2acc8ee96a9dc75145d374f4e6e1caa2af7a6cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=9E=A0=E8=BD=B2?= <3242212048@qq.com> Date: Fri, 12 Jun 2026 01:26:18 +0800 Subject: [PATCH 09/10] =?UTF-8?q?=E6=8F=90=E4=BA=A4index.md=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docs/carla_autonomous_control/index.md | 239 +++++++++++++++++++++++++ 1 file changed, 239 insertions(+) create mode 100644 docs/carla_autonomous_control/index.md diff --git a/docs/carla_autonomous_control/index.md b/docs/carla_autonomous_control/index.md new file mode 100644 index 0000000000..0d375a2fc8 --- /dev/null +++ b/docs/carla_autonomous_control/index.md @@ -0,0 +1,239 @@ +# CARLA 自动驾驶控制客户端 + +## 目录 + +- [项目简介](#项目简介) +- [功能特性](#功能特性) +- [快速开始](#快速开始) + - [环境要求](#环境要求) + - [安装步骤](#安装步骤) + - [运行方式](#运行方式) +- [运行效果](#运行效果) +- [核心技术](#核心技术) + - [手动/自动模式切换](#手动自动模式切换) + - [最高速度限制](#最高速度限制) + - [自适应巡航控制（ACC）](#自适应巡航控制acc) + - [自动紧急制动（AEB）](#自动紧急制动aeb) + - [HUD 增强显示](#hud-增强显示) +- [命令行接口](#命令行接口) +- [项目结构](#项目结构) +- [数据输出](#数据输出) +- [参考资料](#参考资料) + +--- + +## 项目简介 + +本项目基于 CARLA 模拟器 0.9.11，实现了一个功能丰富的自动驾驶控制客户端。在官方示例的基础上，增加了手动/自动模式切换、最高速度限制、自适应巡航控制（ACC）、自动紧急制动（AEB）、增强型 HUD、数据记录以及机器学习集成（MLP 神经网络预测 ACC 目标速度）等多项功能。 + +**核心优势**： +- 🎮 **灵活控制**：一键切换手动/自动驾驶，适配测试与演示需求 +- 🚦 **安全优先**：AEB 主动紧急制动，ACC 动态跟车，限速双重保护 +- 📊 **数据驱动**：支持采集训练数据，集成 MLP 模型优化跟车平顺性 +- 🖥️ **信息丰富**：HUD 显示道路限速、障碍物距离、路径点距离、速度曲线等 + +--- + +## 功能特性 + +| 特性 | 描述 | +|------|------| +| 🎮 **手动/自动模式切换** | 按 `P` 键即时切换，手动控制支持 WASD/方向键、Q 倒车、空格手刹 | +| 🚦 **最高速度限制** | 通过 `--max_speed` 设置硬上限，超速时自动减小油门并线性刹车 | +| 🚗 **自适应巡航控制 (ACC)** | 根据前车距离自动调整速度，支持规则线性插值或 MLP 神经网络预测 | +| 🛑 **自动紧急制动 (AEB)** | 障碍物距离小于阈值时强制全力刹车，避免碰撞 | +| 📊 **HUD 增强显示** | 道路限速、下一个路径点距离、前方障碍物距离、实时速度曲线、超速红字警示 | +| ⌨️ **交互快捷键** | `R` 重规划路径，`V` 截图，`P` 模式切换，`ESC`/`Ctrl+Q` 退出 | +| 💾 **数据记录** | 记录当前车速、障碍物距离、目标速度，用于训练机器学习模型 | +| 🧠 **机器学习集成** | 训练好的 MLP 神经网络替代规则插值，实现更平滑的跟车行为 | + +--- + +## 快速开始 + +### 环境要求 + +- **Python**: 3.7 +- **CARLA Simulator**: 0.9.11 +- **操作系统**: Windows / Linux +- **依赖库**: pygame, numpy, scikit-learn, joblib, csv + +### 安装步骤 + +```bash +# 1. 将主程序 main.py 放入 CARLA 的 PythonAPI/examples 目录 +# 2. 安装 Python 依赖 +pip install pygame numpy scikit-learn joblib +``` + +**安装 CARLA 模拟器**： +- 下载 CARLA 0.9.11 版本，解压到本地。 +- 运行 `CarlaUE4.exe`（Windows）或 `./CarlaUE4.sh`（Linux）启动服务器。 + +### 运行方式 + +```bash +# 1. 启动 CARLA 模拟器（单独窗口） +# CarlaUE4.exe + +# 2. 运行自动驾驶客户端（默认 BehaviorAgent） +py -3.7 main.py --agent Behavior + +# 3. 启用 ACC 并使用 MLP 模型，限速 50 km/h +py -3.7 main.py --agent Behavior --max_speed 50 --acc_enable --ml_acc + +# 4. 手动驾驶模式（按 P 切换） +py -3.7 main.py --agent Behavior --max_speed 40 + +# 5. 开启循环行驶和数据记录 +py -3.7 main.py --agent Behavior --loop --record_data acc_data.csv +``` + +--- + +## 运行效果 + +下图展示了启用 MLP 模型后，车辆在自适应巡航控制下的平稳跟车表现（HUD 中可见跟车距离和速度曲线）。 + +![MLP-ACC 平稳跟车效果图1](image/mlp_acc_following_1.png) +![MLP-ACC 平稳跟车效果图2](image/mlp_acc_following_2.png) + +*图：车辆跟随前车时，速度曲线平滑，障碍物距离稳定在合理范围内，加减速平缓。* + +--- + +## 核心技术 + +### 手动/自动模式切换 + +- 实现：`KeyboardControl` 类监听 `P` 键，设置 `manual_mode` 标志。 +- 手动模式：读取键盘状态生成 `carla.VehicleControl`（油门、刹车、转向、倒车、手刹）。 +- 自动模式：调用 `agent.run_step()` 获取控制指令。 +- 切换时 HUD 显示当前模式。 + +### 最高速度限制 + +- 硬限速：`--max_speed` 参数，超速时将油门置零并线性施加刹车。 +- 道路限速显示：`world.player.get_speed_limit()` 实时获取。 +- 超速警示：HUD 速度数字自动变为红色。 + +### 自适应巡航控制（ACC） + +- 规则模式：根据障碍物距离线性插值计算目标速度： + - `obs_dist < acc_min_dist` → 目标速度 0 + - `acc_min_dist ≤ obs_dist < acc_max_dist` → 线性插值 + - `obs_dist ≥ acc_max_dist` → 目标速度 = `--max_speed` +- MLP 模式（`--ml_acc`）：使用预先训练的神经网络（输入：当前车速、障碍物距离；输出：目标速度），在测试集上 R² 达 0.9993。 +- P 控制器（`Kp=0.05`, `base_throttle=0.25`）实现速度闭环。 + +### 自动紧急制动（AEB） + +- 依赖障碍物检测（`world.hud.current_obstacle`）。 +- 当障碍物距离 < `--aeb_distance`（默认 5 米）时，强制设置 `throttle=0, brake=1.0`，并显示 HUD 通知。 +- 优先级高于 ACC 和限速。 + +### HUD 增强显示 + +- 使用 Pygame 绘制，`HUD.tick` 更新信息： + - 服务器/客户端 FPS、车辆型号、地图、仿真时间 + - 车速、航向、位置、GNSS、高度 + - **道路限速**（`Speed limit`） + - **下一个路径点距离**（`Next WP`） + - **前方障碍物距离**（仅车辆和行人） + - 实时速度曲线（右下角黄色折线，左上角显示当前速度） +- 超速时速度数字变为红色。 + +--- + +## 命令行接口 + +```bash +py -3.7 main.py [-h] [--host H] [-p P] [--res WIDTHxHEIGHT] + [--filter PATTERN] [--gamma GAMMA] [-l] [-b {cautious,normal,aggressive}] + [-a {Behavior,Roaming,Basic}] [-s SEED] [--max_speed MAX_SPEED] + [--aeb_distance AEB_DISTANCE] [--acc_enable] [--acc_max_dist ACC_MAX_DIST] + [--acc_min_dist ACC_MIN_DIST] [--record_data RECORD_DATA] [--ml_acc] +``` + +| 参数 | 说明 | 默认值 | +|------|------|--------| +| `--host` | CARLA 服务器 IP | 127.0.0.1 | +| `-p, --port` | 端口 | 2000 | +| `--res` | 窗口分辨率 | 1280x720 | +| `--filter` | 车辆过滤器 | vehicle.* | +| `--gamma` | 相机伽马校正 | 2.2 | +| `-l, --loop` | 循环行驶 | False | +| `-b, --behavior` | 行为风格 | normal | +| `-a, --agent` | 代理类型 | Behavior | +| `-s, --seed` | 随机种子 | None | +| `--max_speed` | 最高速度（km/h） | 100.0 | +| `--aeb_distance` | AEB 触发距离（米） | 5.0 | +| `--acc_enable` | 启用 ACC | False | +| `--acc_max_dist` | ACC 恢复全速距离（米） | 15.0 | +| `--acc_min_dist` | ACC 完全停车距离（米） | 5.0 | +| `--record_data` | 数据记录 CSV 文件 | None | +| `--ml_acc` | 使用 MLP 模型 | False | + +--- + +## 项目结构 + +``` +carla_autonomous_control/ +├── docs/ # 文档目录 +│ └── carla_autonomous_control/ +│ ├── index.md # 本 README +│ └── image/ # 效果截图 +│ ├── mlp_acc_following_1.png +│ └── mlp_acc_following_2.png +├── src/ # 源代码目录 +│ ├── main.py # 主程序（实际为 automatic_control_revise_11.py） +│ └── README.md # 项目说明（可链接到 docs） +├── train_mlp.py # MLP 模型训练脚本（可选） +├── acc_mlp_model.pkl # 训练好的 MLP 模型 +├── acc_scaler.pkl # 特征标准化器 +├── _screenshots/ # V 键截图保存目录 +├── acc_data.csv # 采集的 ACC 训练数据 +└── README.md # 项目说明（本文件） +``` + +### 核心文件说明 + +| 文件 | 职责 | +|------|------| +| `main.py` | 主程序，集成所有功能（模式切换、限速、ACC、AEB、HUD、快捷键等） | +| `train_mlp.py` | 独立训练脚本，使用 scikit-learn 训练 MLP 回归模型 | +| `acc_mlp_model.pkl` | 序列化的 MLP 模型 | +| `acc_scaler.pkl` | 特征标准化器（StandardScaler） | +| `_screenshots/` | 存放 V 键截图 | +| `acc_data.csv` | 数据记录输出（当使用 `--record_data` 时生成） | + +--- + +## 数据输出 + +当使用 `--record_data` 时，会生成 CSV 文件，包含以下字段： + +| 字段 | 单位 | 说明 | +|------|------|------| +| `current_speed` | km/h | 当前车速 | +| `obstacle_dist` | 米 | 前方最近障碍物距离（无前车时为 -1） | +| `target_speed` | km/h | ACC 计算的目标速度（规则或 MLP） | + +这些数据可用于后续模型训练或分析。 + +--- + +## 参考资料 + +- [CARLA Simulator Documentation](https://carla.readthedocs.io/) +- [CARLA Python API Reference](https://carla.readthedocs.io/en/latest/python_api/) +- [Behavior Agent](https://github.com/carla-simulator/carla/tree/master/PythonAPI/carla/agents/navigation) +- [Scikit-learn MLPRegressor](https://scikit-learn.org/stable/modules/generated/sklearn.neural_network.MLPRegressor.html) +- [Pygame Documentation](https://www.pygame.org/docs/) + +--- + +## 许可证 + +本项目基于 MIT 许可证，原始代码版权归 Intel Labs 所有。 From 18f26873767c681ed5f99fdb243c5831094d9fb4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=9E=A0=E8=BD=B2?= <3242212048@qq.com> Date: Wed, 24 Jun 2026 15:46:30 +0800 Subject: [PATCH 10/10] =?UTF-8?q?=E5=9C=A8index.md=E9=87=8C=E5=8A=A0?= =?UTF-8?q?=E4=B8=8A=E6=88=91=E7=9A=84=E9=A1=B9=E7=9B=AE=E4=BB=8B=E7=BB=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docs/index.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/index.md b/docs/index.md index 7d57f06acc..06a5eda14e 100644 --- a/docs/index.md +++ b/docs/index.md @@ -147,6 +147,8 @@ title: 主页 - [__人形机器人站立行走__](./mujoco_man/mujoco_manrun.md) - 基于 CPG + PD 的人形机器人稳定站立与行走仿真（MuJoCo） +- [__CARLA自动驾驶控制__](carla_autonomous_control/index.md) - 基于CARLA模拟器的自动驾驶控制项目 + - [__td3_carracing__](./td3_carracing/README.md) - 基于 TD3 + CNN 的 CarRacing 强化学习自动驾驶系统 - [__机器人仿真(MuJoCo)__](ant_robot/机器人仿真系统.md)