自动驾驶carla模拟器坐标系转换为icv坐标系[完整版源码]_carla中世界坐标系转为主车坐标系-CSDN博客

本文链接：https://blog.csdn.net/weixin_41194129/article/details/112580247

本文详细介绍了如何将自动驾驶模拟器Carla中的坐标系转换为ICV（智能车辆信息系统）坐标系，提供了完整的源码实现，旨在帮助读者深入理解坐标转换在自动驾驶中的应用。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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

#!/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