如何在 Carla 上实施变道操作
How to implement a lane change manoeuver on Carla
我正在尝试在 Carla 模拟器上为自动驾驶汽车实施简单的变道操作。特别是左变道。
但是，当从左车道检索 waypoints 时（使用 carla.Waypoint.get_left_lane() 函数），我得到的 waypoints 在左右车道之间振荡。下面是我使用的脚本：
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])
我刚刚找出问题的原因。原因是我将 Carla waypoints 作为无向点进行操作。然而，在 Carla 中，每个航路点都是由道路方向指向的。
在场景中，我正在测试我的代码，所有的道路都有两条车道，每条车道都在相反的方向。因此，每条车道的左车道为剩余车道。
之前代码中的问题是我没有改变我的视角来匹配车道的方向。我假设有一个全局参考框架，但在 Carla 中，waypoints 是相对于连接到它们各自车道的框架的。而且由于两个坐标系中只有一个（对于每个车道）与我想象的全局参考系相匹配，我得到了一个振荡行为。
另一个问题是我更新目标 waypoints 太早了。这导致车辆移动了很短的距离而没有穿过所有目标waypoints。我将其更改为继续跟踪相同的目标航路点，直到它与我的车辆之间的距离变得太近，然后再移动到下一个航路点。这有助于执行变道行为。
下面是我使用的脚本：
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])
我正在尝试在 Carla 模拟器上为自动驾驶汽车实施简单的变道操作。特别是左变道。 但是，当从左车道检索 waypoints 时（使用 carla.Waypoint.get_left_lane() 函数），我得到的 waypoints 在左右车道之间振荡。下面是我使用的脚本：
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])
我刚刚找出问题的原因。原因是我将 Carla waypoints 作为无向点进行操作。然而，在 Carla 中，每个航路点都是由道路方向指向的。
在场景中，我正在测试我的代码，所有的道路都有两条车道，每条车道都在相反的方向。因此，每条车道的左车道为剩余车道。
之前代码中的问题是我没有改变我的视角来匹配车道的方向。我假设有一个全局参考框架，但在 Carla 中，waypoints 是相对于连接到它们各自车道的框架的。而且由于两个坐标系中只有一个（对于每个车道）与我想象的全局参考系相匹配，我得到了一个振荡行为。
另一个问题是我更新目标 waypoints 太早了。这导致车辆移动了很短的距离而没有穿过所有目标waypoints。我将其更改为继续跟踪相同的目标航路点，直到它与我的车辆之间的距离变得太近，然后再移动到下一个航路点。这有助于执行变道行为。
下面是我使用的脚本：
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])