如何在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()

最后：

print("Destroying actors")

client.apply_batch([carla.command.DestroyActor(x) for x in actorList])

Answer 1

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

在场景中，我正在测试我的代码，所有的道路都有两条车道，每条都在相反的方向上。因此，每条行车线的左边行车线是余下的行车线。

以前的代码中的问题是，我并没有改变我的观点，以配合车道的方向。我当时假设的是一个全球参考框架，但在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])