实现车辆自动定速巡航

src/carla_native_assisted_driving_system/soyo.1.py

@@ -1,9 +1,7 @@
 
-
-"""Example of automatic vehicle control from client side. (无agents纯净版)"""
-
 from __future__ import print_function
-
+"""Example of automatic vehicle control from client side. (无agents纯净版)"""
+"""CARLA-Native-Assisted-Driving-System - Version 1.0"""
 import argparse
 import collections
 import datetime
@@ -16,19 +14,9 @@
 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')
+import pygame
+from pygame.locals import *
+import numpy as np
 
 # ==============================================================================
 # -- Find CARLA module ---------------------------------------------------------
@@ -54,12 +42,27 @@
 
 
 # ==============================================================================
-# -- 已删除所有agents代码 ✅----------------------------------------------------
+# -- Global functions ----------------------------------------------------------
 # ==============================================================================
-
 # ==============================================================================
-# -- Global functions ----------------------------------------------------------
+# -- PID Controller (智能控制算法) ------------------------------------------------
 # ==============================================================================
+class PIDController:
+    def __init__(self, Kp=1.0, Ki=0.0, Kd=0.0):
+        self.Kp = Kp
+        self.Ki = Ki
+        self.Kd = Kd
+        self.error = 0.0
+        self.last_error = 0.0
+        self.integral = 0.0
+
+    def step(self, target, current, dt=0.05):
+        self.error = target - current
+        self.integral += self.error * dt
+        derivative = (self.error - self.last_error) / dt
+        output = self.Kp * self.error + self.Ki * self.integral + self.Kd * derivative
+        self.last_error = self.error
+        return max(0.0, min(1.0, output))
 def find_weather_presets():
     rgx = re.compile('.+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)')
 
@@ -168,30 +171,6 @@ def destroy(self):
             if actor is not None:
                 actor.destroy()
 
-
-# ==============================================================================
-# -- KeyboardControl -----------------------------------------------------------
-# ==============================================================================
-class KeyboardControl(object):
-    def __init__(self, world):
-        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.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):
     def __init__(self, width, height):
         self.dim = (width, height)
@@ -345,7 +324,7 @@ def render(self, display):
 
 class HelpText(object):
     def __init__(self, font, width, height):
-        lines = __doc__.split('\n')
+        lines = ["CARLA-Native-Assisted-Driving-System", "PID自动定速巡航 | ESC退出"]
         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])
@@ -378,6 +357,7 @@ def __init__(self, parent_actor, hud):
         weak_self = weakref.ref(self)
         self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event))
 
+
     def get_collision_history(self):
         history = collections.defaultdict(int)
         for frame, intensity in self.history:
@@ -546,53 +526,64 @@ def _parse_image(weak_self, image):
 # ==============================================================================
 # -- --------------------------------------------
 # ==============================================================================
+
 # ==============================================================================
-# --  --------------------------------------------
+# -- game_loop (PID定速巡航 自动控制版) ------------------------------------------
 # ==============================================================================
 def game_loop(args):
     pygame.init()
     pygame.font.init()
     world = None
 
+    # 初始化PID控制器（目标速度：30 km/h）
+    pid = PIDController(Kp=0.8, Ki=0.1, Kd=0.05)
+    target_speed = 30.0  # 目标巡航速度
+
     try:
         client = carla.Client(args.host, args.port)
-        client.set_timeout(4.0)
+        client.set_timeout(10.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)
         clock = pygame.time.Clock()
 
         while True:
             clock.tick_busy_loop(60)
-            if controller.parse_events():
-                return
+
+            # 仅处理退出事件，无任何手动控制
+            for event in pygame.event.get():
+                if event.type == pygame.QUIT or (event.type == pygame.KEYUP and event.key == K_ESCAPE):
+                    return
 
             world.world.wait_for_tick()
             world.tick(clock)
             world.render(display)
             pygame.display.flip()
 
-            # ===================== =====================
+            # ===================== PID算法自动控制 =====================
+            # 获取当前车速(km/h)
+            vel = world.player.get_velocity()
+            current_speed = 3.6 * math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2)
+
+            # PID计算油门
             control = carla.VehicleControl()
-            control.throttle = 0.5  # 满油门
-            control.steer = 0.0      # 直线
-            control.brake = 0.0       # 不刹车
-            control.hand_brake = False# 松手刹
-            control.reverse = False   # 不倒车
-            control.gear = 1          # 强制1档前进
-            # ====================================================
+            control.throttle = pid.step(target_speed, current_speed)
+            control.brake = 0.0
+            control.steer = 0.0
+            control.hand_brake = False
+            control.manual_gear_shift = False
+
+            # 算法输出控制指令
             world.player.apply_control(control)
 
     finally:
         if world is not None:
             world.destroy()
         pygame.quit()
-
 # ==============================================================================
 # -- main() --------------------------------------------------------------
 # ==============================================================================