python - 如何在 Carla 上实现变道操作

Question

我正在尝试在 Carla 模拟器上为自动驾驶汽车实现简单的变道操作。具体来说是左变道。 然而，当从左车道检索路点时(使用 carla.Waypoint.get_left_lane() 函数)，我得到的路点在左车道和右车道之间振荡。以下是我使用的脚本:

import sys
import glob
import os

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 time
import math
import numpy as np
import random
from agents.navigation.controller import VehiclePIDController



dt = 1.0 / 20.0
args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': dt}
args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': dt}
max_throt = 0.75
max_brake = 0.3
max_steer = 0.8
offset = 0
VEHICLE_VEL = 5

actorList = []
try:
    client = carla.Client("localhost",2000)
    client.set_timeout(10.0)
    world = client.load_world("Town02")

    spectator = world.get_spectator()
    actorList.append(spectator)

    settings = world.get_settings()
    settings.synchronous_mode = True
    settings.fixed_delta_seconds = 1/20
    world.apply_settings(settings)

    blueprint_library = world.get_blueprint_library()
    vehicle_bp = blueprint_library.filter("cybertruck")[0]
    
    vehicle = None
    while(vehicle is None):
        spawn_points = world.get_map().get_spawn_points()
        spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
        vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
    actorList.append(vehicle)
    
    vehicle_controller = VehiclePIDController(vehicle,
                                        args_lateral=args_lateral_dict,
                                        args_longitudinal=args_longitudinal_dict,
                                        offset=offset,
                                        max_throttle=max_throt,
                                        max_brake=max_brake,
                                        max_steering=max_steer)
    
    old_yaw = math.radians(vehicle.get_transform().rotation.yaw)
    old_x = vehicle.get_transform().location.x
    old_y = vehicle.get_transform().location.y
```
    spectator_transform =  vehicle.get_transform()
    spectator_transform.location += carla.Location(x = 10, y= 0, z = 5.0)

    
    
    control = carla.VehicleControl()
    control.throttle = 1.0
    vehicle.apply_control(control)

    while True:

    
            ####### Im suspecting the bug is within this portion of code ########################
            current_waypoint = world.get_map().get_waypoint(vehicle.get_location())
                # if not turned_left :
            left_waypoint = current_waypoint.get_left_lane()
            if(left_waypoint is not None and left_waypoint.lane_type == carla.LaneType.Driving) :
                target_waypoint = left_waypoint.previous(0.3)[0]
                turned_left = True
            else : 
                if(turned_left) :
                    target_waypoint = waypoint.previous(0.3)[0]
                else : # I tryed commenting this else section but the bug is still present so I dont suspect the bug relates to this else part
                    target_waypoint = waypoint.next(0.3)[0]
            ################### End of suspected bug ############################################


            world.debug.draw_string(target_waypoint.transform.location, 'O', draw_shadow=False,
                                       color=carla.Color(r=255, g=0, b=0), life_time=120.0,
                                       persistent_lines=True)
            control_signal = vehicle_controller.run_step(VEHICLE_VEL,target_waypoint)
            
            vehicle.apply_control(control_signal)
            
            new_yaw = math.radians(vehicle.get_transform().rotation.yaw)
            spectator_transform =  vehicle.get_transform()
            spectator_transform.location += carla.Location(x = -10*math.cos(new_yaw), y= -10*math.sin(new_yaw), z = 5.0)
            
            spectator.set_transform(spectator_transform)
            
            world.tick()
finally:
    print("Destroying actors")
    client.apply_batch([carla.command.DestroyActor(x) for x in actorList])

Answer 1

我刚刚找出问题的原因。原因是我将卡拉航点作为无向点进行操作。然而，在卡拉，每个航路点都是由道路方向指示的。

在场景中，我正在测试我的代码，所有道路都有两条车道，每条车道的方向相反。因此，每条车道的左侧车道就是剩余车道。

前面代码中的问题是我没有改变 View 以匹配车道的方向。我假设有一个全局引用系，但在 Carla 中，航路点是相对于附加到各自车道的引用系的。由于(每个车道)两个坐标系中只有一个与我想象的全局引用系相匹配，因此我出现了振荡行为。

另一个问题是我太早更新目标航路点以进行跟踪。这导致车辆移动了很短的距离，而没有经过所有目标航路点。我更改了此设置以继续跟踪相同的目标航路点，直到它与我的车辆之间的距离变得太近，然后再移动到下一个航路点。这有助于执行变道行为。

下面是我使用的脚本:

import sys
import glob
import os

#The added path depends on where the carla binaries are stored
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 time
import math
import numpy as np
import random
from agents.navigation.controller import VehiclePIDController

VEHICLE_VEL = 5
class Player():
    def __init__(self, world, bp, vel_ref = VEHICLE_VEL, max_throt = 0.75, max_brake = 0.3, max_steer = 0.8):
        self.world = world
        self.max_throt = max_throt
        self.max_brake = max_brake
        self.max_steer = max_steer
        self.vehicle = None
        self.bp = bp 
        while(self.vehicle is None):
            spawn_points = world.get_map().get_spawn_points()
            spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
            self.vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
        
        self.spectator = world.get_spectator()
        
        dt = 1.0 / 20.0
        args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': dt}
        args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': dt}
        offset = 0
        
        self.controller = VehiclePIDController(self.vehicle,
                                        args_lateral=args_lateral_dict,
                                        args_longitudinal=args_longitudinal_dict,
                                        offset=offset,
                                        max_throttle=max_throt,
                                        max_brake=max_brake,
                                        max_steering=max_steer)
        self.vel_ref = vel_ref
        self.waypointsList = []
        self.current_pos = self.vehicle.get_transform().location
        self.past_pos = self.vehicle.get_transform().location
    
    def dist2Waypoint(self, waypoint):
        vehicle_transform = self.vehicle.get_transform()
        vehicle_x = vehicle_transform.location.x
        vehicle_y = vehicle_transform.location.y
        waypoint_x = waypoint.transform.location.x
        waypoint_y = waypoint.transform.location.y
        return math.sqrt((vehicle_x - waypoint_x)**2 + (vehicle_y - waypoint_y)**2)
    
    def go2Waypoint(self, waypoint, draw_waypoint = True, threshold = 0.3):
        if draw_waypoint :
            # print(" I draw") 
            self.world.debug.draw_string(waypoint.transform.location, 'O', draw_shadow=False,
                                                       color=carla.Color(r=255, g=0, b=0), life_time=10.0,
                                                       persistent_lines=True)
        
        current_pos_np = np.array([self.current_pos.x,self.current_pos.y])
        past_pos_np = np.array([self.past_pos.x,self.past_pos.y])
        waypoint_np = np.array([waypoint.transform.location.x,waypoint.transform.location.y])
        vec2wp = waypoint_np - current_pos_np
        motion_vec = current_pos_np - past_pos_np
        dot = np.dot(vec2wp,motion_vec)
        if (dot >=0):
            while(self.dist2Waypoint(waypoint) > threshold) :
        
                control_signal = self.controller.run_step(self.vel_ref,waypoint)
                    
                self.vehicle.apply_control(control_signal)
                
                self.update_spectator()

    def getLeftLaneWaypoints(self, offset = 2*VEHICLE_VEL, separation = 0.3):
        current_waypoint = self.world.get_map().get_waypoint(self.vehicle.get_location())
        left_lane = current_waypoint.get_left_lane()
        self.waypointsList = left_lane.previous(offset)[0].previous_until_lane_start(separation)

    def getRightLaneWaypoints(self, offset = 2*VEHICLE_VEL, separation = 0.3):
        current_waypoint = self.world.get_map().get_waypoint(self.vehicle.get_location())
        right_lane = current_waypoint.get_left_lane()
        self.waypointsList = right_lane.next(offset)[0].next_until_lane_end(separation)
    
    def do_left_lane_change(self):
        
        self.getLeftLaneWaypoints()
        for i in range(len(self.waypointsList)-1):
            self.current_pos = self.vehicle.get_location()
            self.go2Waypoint(self.waypointsList[i])
            self.past_pos = self.current_pos
            self.update_spectator()

    def do_right_lane_change(self):

        self.getRightLaneWaypoints()
        for i in range(len(self.waypointsList)-1)
            self.current_pos = self.vehicle.get_location()
            self.go2Waypoint(self.waypointsList[i])
            self.past_pos = self.current_pos
            self.update_spectator()

    def update_spectator(self):
        new_yaw = math.radians(self.vehicle.get_transform().rotation.yaw)
        spectator_transform =  self.vehicle.get_transform()
        spectator_transform.location += carla.Location(x = -10*math.cos(new_yaw), y= -10*math.sin(new_yaw), z = 5.0)
        
        self.spectator.set_transform(spectator_transform)
        self.world.tick()

    def is_waypoint_in_direction_of_motion(self,waypoint):
        current_pos = self.vehicle.get_location()

    def draw_waypoints(self):
        for waypoint in self.waypointsList:
            self.world.debug.draw_string(waypoint.transform.location, 'O', draw_shadow=False,
                                                       color=carla.Color(r=255, g=0, b=0), life_time=10.0,
                                                       persistent_lines=True)

dt = 1.0 / 20.0
args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': dt}
args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': dt}

offset = 0
VEHICLE_VEL = 10

actorList = []
try:
    client = carla.Client("localhost",2000)
    client.set_timeout(10.0)
    world = client.load_world("Town02")

    spectator = world.get_spectator()
    actorList.append(spectator)

    settings = world.get_settings()
    settings.synchronous_mode = True
    settings.fixed_delta_seconds = 1/20
    world.apply_settings(settings)

    blueprint_library = world.get_blueprint_library()
    vehicle_bp = blueprint_library.filter("cybertruck")[0]
    
    player = Player(world, vehicle_bp)
    actorList.append(player.vehicle)
    actorList.append(player.spectator)

    while(True):
        player.update_spectator()
        manoeuver = input("Enter manoeuver: ")
        if ( manoeuver == "l"): #Perform left lane change
            player.do_left_lane_change()
        elif (manoeuver == "r"): #Perform right lane change
            player.do_right_lane_change()
        elif (manoeuver == "d"): #Just draws the waypoints for debugging purpose
            player.getLeftLaneWaypoints()
            player.draw_waypoints()
    
finally:
    print("Destroying actors")
    client.apply_batch([carla.command.DestroyActor(x) for x in actorList])