自动驾驶carla模拟器坐标系转换为icv坐标系[完整版源码]

欢迎点赞转发、一起交流学习

#!/usr/bin/env python

#
# Copyright (c) 2018-2019 Intel Corporation
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
#
"""
Tool functions to convert transforms from carla to icv coordinate system
"""

import math
import numpy

import transformations
from geometry_msgs.msg import Vector3, Quaternion, Transform, Pose, Point, Twist, Accel


def carla_location_to_numpy_vector(carla_location):
    """
    Convert a carla location to a icv vector3
    Considers the conversion from left-handed system (unreal) to right-handed
    system (icv)
    :param carla_location: the carla location
    :type carla_location: carla.Location
    :return: a numpy.array with 3 elements
    :rtype: numpy.array
    """
    return numpy.array([
        carla_location.x,
        -carla_location.y,
        carla_location.z
    ])


def carla_location_to_icv_vector3(carla_location):
    """
    Convert a carla location to a icv vector3
    Considers the conversion from left-handed system (unreal) to right-handed
    system (icv)
    :param carla_location: the carla location
    :type carla_location: carla.Location
    :return: a icv vector3
    :rtype: geometry_msgs.msg.Vector3
    """
    icv_translation = Vector3()
    icv_translation.x = carla_location.x
    icv_translation.y = -carla_location.y
    icv_translation.z = carla_location.z

    return icv_translation


def carla_location_to_icv_point(carla_location):
    """
    Convert a carla location to a icv point
    Considers the conversion from left-handed system (unreal) to right-handed
    system (icv)
    :param carla_location: the carla location
    :type carla_location: carla.Location
    :return: a icv point
    :rtype: geometry_msgs.msg.Point
    """
    icv_point = Point()
    icv_point.x = carla_location.x
    icv_point.y = -carla_location.y
    icv_point.z = carla_location.z

    return icv_point


def