How to implement a lane change manoeuver on Carla

Question

I am trying to implement a simple lane change manoeuver for a self driving car on Carla simulator. Specifically a left lane change. However when retrieving waypoints from the left lane ( using carla.Waypoint.get_left_lane() function ), the waypoints I get are oscillating between left and right lanes. Below is the script I used :

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

I just figured out the cause of the problem. The reason is that I was manipulating Carla waypoints as undirected points. However, in Carla, each waypoint is directed by the road direction.

In the scene, I was testing my code in, all the roads have two lanes, each one in an opposite direction. Hence, the left lane of each lane is the remaining lane.

The issue in the previous code was that I was not changing my view to match the direction of the lane. I was assuming a global reference frame, but in Carla, the waypoints are relative to the frames attached to their respective lanes. And since only one of the two coordinate frames (for each lane) was matching my imagined global reference frame, I was getting an oscillatory behavior.

Another issue was that I was updating the target waypoints to track too early. This caused the vehicle to move a very short distance without going through all the target waypoints. I changed this to keep tracking the same target waypoint until the distance separating it to my vehicle becomes too close before moving to the next waypoint. This helped to perform the lane change behavior.

Below is the script I used :

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])