carla自动驾驶车辆变道路径规划
参考链接：
Carla自动驾驶仿真九：车辆变道路径规划_车辆变道函数-CSDN博客
完整代码：
import carla
import time
import math
import sys
import sys
# 修改成自己的carla路径
sys.path.append(r'D:\qcc\UnrealEngine\carla\PythonAPI\carla')
from agents.navigation.global_route_planner import GlobalRoutePlanner
from agents.navigation.controller import VehiclePIDController,PIDLongitudinalController
from agents.tools.misc import draw_waypoints, distance_vehicle, vector, is_within_distance, get_speed
class CarlaWorld:
def __init__(self):
self.client = carla.Client('localhost', 2000)
# self.world = self.client.load_world('Town06')
#self.world = client.load_world(r'D:\qcc\UnrealEngine\carla\Unreal\CarlaUE4\Content\Package76\Maps\wujiaochang_xianyu_76\wujiaochang_xianyu_76')
self.world = self.client.get_world()
self.map = self.world.get_map()
# 开启同步模式
settings = self.world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.05
def spawm_ego_by_point(self,ego_spawn_point):
vehicle_bp = self.world.get_blueprint_library().filter('vehicle.tesla.*')[0]
ego_vehicle = self.world.try_spawn_actor(vehicle_bp,ego_spawn_point)
return ego_vehicle
def spawn_npc_by_offset(self,ego_spawn_point,offset):
vehicle_bp = self.world.get_blueprint_library().filter('vehicle.tesla.*')[0]
# 计算新的生成点
rotation = ego_spawn_point.rotation
location = ego_spawn_point.location
location.x += offset.x
location.y += offset.y
location.z += offset.z
npc_transform = carla.Transform(location, rotation)
npc_vehicle = self.world.spawn_actor(vehicle_bp, npc_transform)
return npc_vehicle
def get_spawn_point(self, target_road_id, target_lane_id):
# 每隔5m生成1个waypoint
waypoints = self.map.generate_waypoints(5.0) # 以5米的距离生成整张地图的waypoints列表
# ========== [Added code] ==========
# 在此处打印所有道路ID和车道ID，以便调试或查看
print("Printing all waypoints' road_id and lane_id:")
for i, wp in enumerate(waypoints):
print(f"Waypoint {i}: road_id={wp.road_id}, lane_id={wp.lane_id}")
# ========== [End of added code] ==========
# 遍历路点
for waypoint in waypoints: # 遍历每个waypoint
if waypoint.road_id == target_road_id: # 判断当前road_id是否匹配
lane_id = waypoint.lane_id # 获取当前waypoint的车道ID
# 检查是否已经找到了特定车道ID的路点
if lane_id == target_lane_id: # 如果匹配到目标车道ID
location = waypoint.transform.location # 获取该waypoint的世界坐标
location.z = 1 # 将Z设置为1，以避免生成在地面以下或其他位置
ego_spawn_point = carla.Transform(location, waypoint.transform.rotation) # 构造Transform
break # 找到后跳出循环
return ego_spawn_point # 返回符合条件的Transform
def cal_target_route(self,vehicle=None,lanechange="left",target_dis=20):
#实例化道路规划模块
grp = GlobalRoutePlanner(self.map, 2)
#获取npc车辆当前所在的waypoint
current_location = vehicle.get_transform().location
current_waypoint = self.map.get_waypoint(current_location)
#选择变道方向
if "left" in lanechange:
target_org_waypoint = current_waypoint.get_left_lane()
elif "right" in lanechange:
target_org_waypoint = current_waypoint.get_right_lane()
#获取终点的位置
target_location = target_org_waypoint.next(target_dis)[0].transform.location
#根据起点和重点生成规划路径
route = grp.trace_route(current_location, target_location)
return route
def draw_target_line(self,waypoints):
# 获取世界和调试助手
debug = self.world.debug
# 设置绘制参数
life_time = 60.0 # 点和线将持续显示的时间（秒）
color = carla.Color(255, 0, 0)
thickness = 0.3 # 线的厚度
for i in range(len(waypoints) - 1):
debug.draw_line(waypoints[i][0].transform.location + carla.Location(z=0.5),
waypoints[i + 1][0].transform.location + carla.Location(z=0.5),
thickness=thickness,
color=color,
life_time=life_time)
def draw_current_point(self,current_point):
self.world.debug.draw_point(current_point,size=0.1, color=carla.Color(b=255), life_time=60)
def speed_con_by_pid(self,vehilce=None,pid=None,target_speed=30):
control_signal = pid.run_step(target_speed=target_speed, debug=False)
throttle = max(min(control_signal, 1.0), 0.0) # 确保油门值在0到1之间
brake = 0.0 # 根据需要设置刹车值
if control_signal < 0:
throttle = 0.0
brake = abs(control_signal) # 假设控制器输出的负值可以用来刹车
vehilce.apply_control(carla.VehicleControl(throttle=throttle, brake=brake))
def set_spectator(self,vehicle):
self.world.get_spectator().set_transform(
carla.Transform(vehicle.get_transform().location +
carla.Location(z=50), carla.Rotation(pitch=-90))
)
def should_cut_in(self,npc_vehicle, ego_vehicle, dis_to_cut=5):
location1 = npc_vehicle.get_transform().location
location2 = ego_vehicle.get_transform().location
rel_x = location1.x - location2.x
rel_y = location1.y - location2.y
distance = math.sqrt(rel_x * rel_x + rel_y * rel_y)
print("relative dis",distance)
if rel_x >= 0:
distance = distance
else:
distance = -distance
if distance >= dis_to_cut:
print("The conditions for changing lanes are met.")
cut_in_flag = True
else:
cut_in_flag = False
return cut_in_flag
if __name__ == '__main__':
try:
CARLA = CarlaWorld()
#根据road_id和lane_id选择出生点
start_point = CARLA.get_spawn_point(target_road_id=14, target_lane_id=2)
#生成自车
ego_vehicle = CARLA.spawm_ego_by_point(start_point)
#设置初始的观察者视角
CARLA.set_spectator(ego_vehicle)
#相对ego生成目标车
relative_ego = carla.Location(x=-10, y=3.75, z=0)
npc_vehicle = CARLA.spawn_npc_by_offset(start_point, relative_ego)
# 设置ego自动巡航
ego_vehicle.set_autopilot(True)
#设置目标车初始速度的纵向控制PID
initspd_pid = PIDLongitudinalController(npc_vehicle, K_P=1.0, K_I=0.1, K_D=0.05)
#设置目标车的cut_in的横纵向控制PID
args_lateral_dict = {'K_P': 0.8, 'K_D': 0.8, 'K_I': 0.70, 'dt': 1.0 / 10.0}
args_long_dict = {'K_P': 1, 'K_D': 0.0, 'K_I': 0.75, 'dt': 1.0 / 10.0}
PID = VehiclePIDController(npc_vehicle, args_lateral_dict, args_long_dict)
waypoints = None
waypoint_index = 0
need_cal_route = True
cut_in_flag = False
arrive_target_point = False
target_distance_threshold = 2.0 # 切换waypoint的距离
start_sim_time = time.time()
while not arrive_target_point:
CARLA.world.tick()
# 更新观察者的视野
CARLA.set_spectator(ego_vehicle)
#计算目标车的初始速度
ego_speed = (ego_vehicle.get_velocity().x * 3.6) #km/h
target_speed = ego_speed + 8 #目标车的目标速度
#是否满足cut_in条件
if cut_in_flag:
if need_cal_route:
#生成车侧车道前方30m的waypoint
waypoints = CARLA.cal_target_route(npc_vehicle,lanechange= "left",target_dis=30)
CARLA.draw_target_line(waypoints)
need_cal_route = False
# 如果已经计算了路线
if waypoints is not None and waypoint_index < len(waypoints):
# 获取当前目标路点
target_waypoint = waypoints[waypoint_index][0]
# 获取车辆当前位置
transform = npc_vehicle.get_transform()
#绘制当前运行的点
CARLA.draw_current_point(transform.location)
# 计算车辆与当前目标路点的距离
distance_to_waypoint = distance_vehicle(target_waypoint, transform)
# 如果车辆距离当前路点的距离小于阈值，则更新到下一个路点
if distance_to_waypoint < target_distance_threshold:
waypoint_index += 1 # 移动到下一个路点
if waypoint_index >= len(waypoints):
arrive_target_point = True
print("npc_vehicle had arrive target point.")
break # 如果没有更多的路点，退出循环
else:
# 计算控制命令
control = PID.run_step(target_speed, target_waypoint)
# 应用控制命令
npc_vehicle.apply_control(control)
else:
#设置NPC的初始速度
CARLA.speed_con_by_pid(npc_vehicle,initspd_pid,target_speed)
#判断是否可以cut in
cut_in_flag = CARLA.should_cut_in(npc_vehicle,ego_vehicle,dis_to_cut=8)
# 判断是否达到模拟时长
if time.time() - start_sim_time > 60:
print("Simulation ended due to time limit.")
break
#到达目的地停车
npc_vehicle.apply_control(carla.VehicleControl(throttle=0, steer=0, brake=-0.5))
print("Control the target car to brake.")
time.sleep(10)
except Exception as e:
print(f"An error occurred: {e}")
finally:
# 清理资源
print("Cleaning up the simulation...")
if ego_vehicle is not None:
ego_vehicle.destroy()
if npc_vehicle is not None:
npc_vehicle.destroy()
settings = CARLA.world.get_settings()
settings.synchronous_mode = False # 禁用同步模式
settings.fixed_delta_seconds = None
浙公网安备 33010602011771号