CARLA获得航向偏差

获得当前车辆与道路之间的夹角

def get_angle_error():
    vehicle_transform = vehicle.get_transform()  # 获得车辆的全局坐标
    waypoint_vehicle = map.get_waypoint(vehicle.get_location())  # 获得最近导航点坐标
    angle_error0 = waypoint.transform.rotation.yaw - vehicle_transform.rotation.yaw
    angle_error = math.radians(angle_error0)
    return angle_error