CARLA两种预瞄模型的搭建方式

1、第一种预瞄模型的搭建方式（适用于简单的单点预瞄）

# 获得状态量
def get_observation():
    vehicle_transform = vehicle.get_transform()  # 获得车辆的全局坐标

    # 获得0m处的横向误差
    waypoint_vehicle = map.get_waypoint(vehicle.get_location())
    error_0 = ((vehicle_transform.location.x - waypoint_vehicle.transform.location.x) ** 2 +
               (vehicle_transform.location.y - waypoint_vehicle.transform.location.y) ** 2) ** 0.5

    # 获得3m处的预瞄误差
    vehicle_future = vehicle_transform.transform(carla.Vector3D(x=3, y=0, z=0))
    waypoint_future = map.get_waypoint(vehicle_future)

    # 判断车子坐标在前进方向的左侧还是右侧
    s0 = (waypoint_vehicle.transform.location.x - vehicle_transform.location.x) * (
            waypoint_future.transform.location.y - vehicle_transform.location.y) - (
                 waypoint_vehicle.transform.location.y - vehicle_transform.location.y) * (
                 waypoint_future.transform.location.x - vehicle_transform.location.x)
    if s0 >= 0:
        error_0 = error_0
    if s0 < 0:
        error_0 = -error_0

    # 判断预瞄点在前进方向的左侧还是右侧（左正右负）
    s_y = (waypoint_vehicle.transform.location.x -
           vehicle_future.x) * (waypoint_future.transform.location.y - vehicle_future.y) - (
                waypoint_vehicle.transform.location.y - vehicle_future.y) * (
                waypoint_future.transform.location.x - vehicle_future.x)
    error_2 = ((vehicle_future.x - waypoint_future.transform.location.x) ** 2 + (
            vehicle_future.y - waypoint_future.transform.location.y) ** 2) ** 0.5
    if s_y >= 0:
        error_2 = error_2
    if s_y < 0:
        error_2 = -error_2

    return error_0, error_2   # error_0为当前位置的横向误差， error_2为预瞄点处的横向误差 

多点预瞄用这种方法也行，但偶尔会发生返回的导航点是相邻车道的导航点

2、第二种预瞄模型的搭建方式（适用于多点预瞄模型，涉及坐标变换）

#  定义坐标变换
def coordinate_transformation(point_t):
    vehicle_transform = vehicle.get_transform()  # 获得车辆的全局坐标
    yaw = -vehicle_transform.rotation.yaw
    yaw = yaw * math.pi / 180
    a = vehicle_transform.location.x
    b = vehicle_transform.location.y
    x0 = point_t.transform.location.x
    y0 = point_t.transform.location.y
    error = x0*math.sin(yaw)+y0*math.cos(yaw)-a*math.sin(yaw)-b*math.cos(yaw)
    # x = x0*math.cos(yaw)-y0*math.sin(yaw)-a*math.cos(yaw)+b*math.sin(yaw)
    return error


# 获得状态量
def get_observation():
    waypoint_vehicle = map.get_waypoint(vehicle.get_location())  # 获得最近导航点坐标
    # 获得0m处的横向误差
    error_0 = coordinate_transformation(waypoint_vehicle )
    # 获得2m处的横向误差
    w_2 = waypoint_vehicle.next(2)
    error_2 = coordinate_transformation(w_2[0])
    # 获得4m处的横向误差
    w_4 = waypoint_vehicle.next(4)
    error_4 = coordinate_transformation(w_4[0])
    # 获得6m处的横向误差
    w_6 = waypoint_vehicle.next(6)
    error_6 = coordinate_transformation(w_6[0])
    # 获得8m处的横向误差
    w_8 = waypoint_vehicle.next(8)
    error_8 = coordinate_transformation(w_8[0])

    return error_0, error_2, error_4, error_6, error_8

第二种预瞄模型不是常规的预瞄模型，获得的是前面道路相对于车身坐标系的坐标