方法1:通过python API直接获取转换后坐标

这种方法是直接通过carla提供的pythonAPI建立客户端,直接从carla服务器中读取GNSS传感器消息.
要求你在本地启动carla服务器.并编写python脚本在线获取.
而如果通过carla-ros-bridge转发后的GNSS消息,是不能够存储转换关系的.所以这种方法是不适用与rosbag包的.

1.1 GNSS传感器消息-内容介绍

关于gnss传感器,CARLA官方文档-GNSS中展示了它输出的消息内容:
在这里插入图片描述
从 Output attributes可以看到,传感器输出的data中包含:

当前帧frame,
时间戳timestamp,
carla转换关系transform
经度longitude,纬度latitude和海拔altitude.

1.2 在线获取方法

因此,想要获取gnss到carla坐标系的转换,我们可以直接从transform中拿.这个transform的格式是carla.Transform,包含location和rotation.
在这里插入图片描述
下面的函数是将location和rotation中的坐标存储到txt中

def save_pose(ego_vehicle):
    # world_2_camera = camera.get_transform().get_matrix()
    t = ego_vehicle.get_transform()
    global_location_x = t.location.x
    global_location_y = t.location.y
    global_location_z = t.location.z
    global_rotation_yaw = t.rotation.yaw
    global_rotation_pitch = t.rotation.pitch
    global_rotation_roll = t.rotation.roll

    pose_file = cfg.save_root + f"{cfg.map}/poses/{save_idx:0>2d}.txt"

    context = f"{global_location_x} {global_location_y} {global_location_z} " \
              f"{global_rotation_yaw} {global_rotation_pitch} {global_rotation_roll}\n"
    # mkdir_folder(pose_file)
    with open(pose_file, 'a', encoding='utf-8') as f:
        f.write(context)

1.3 完整代码

import numpy as np
import carla

if __name__ == '__main__':
    # >>>>>获取地图并计算转换矩阵
    client=carla.Client('127.0.0.1',2000)
    # world=client.load_world('Town07')
    world=client.get_world()
    map=world.get_map()

    
    # >>>>>获取车辆当前的gnss并打印信息:
    actors=world.get_actors()
    gnss_actor_list = actors.filter('*gnss*') #找到gnss
    gnss_actor = gnss_actor_list[0]
    print(gnss_actor)
    car_longitude=0.0
    car_latitude=0.0
    car_altitude=0.0
    car_transform=None
    def callback(data):
        global car_longitude,car_latitude,car_altitude
        print(f">>>>>>gnss传感器geo坐标值: longitude 经度:{data.longitude},latitude 维度:{data.latitude},altitude 海拔:{data.altitude}")
        car_longitude=data.longitude
        car_latitude=data.latitude
        car_altitude=data.altitude
        car_transform=data.transform
        print(f">>>>>>gnss传感器中的carla坐标值 x:{car_transform.location.x},y:{car_transform.location.y},z:{car_transform.location.z}")
    gnss_actor.listen(lambda data: callback(data))
    

    times = 0
    while times < 1:
        print('----------------------------------------------------')
        times +=1
        world.tick()

运行结果:
在这里插入图片描述
自动打印车辆当前的geoLocation和转换后的carlaLocation.

方法2-通过离线读取转换关系的方式转换

这里我们不像方法1那样直接获取转换后的坐标,而是事先计算出转换关系矩阵然后离线存储起来.下一次用的时候就可以直接调用,而无需在线获取.

2.1 转换类代码和使用方法

我们先给出这个转换类:

class Geo2Location(object):
    """
    Helper class for homogeneous transform from geolocation

    This class is used by GNSS class to transform from carla.GeoLocation to carla.Location.
    This transform is not provided by Carla, but it can be solved using 4 chosen points.
    Note that carla.Location is in the left-handed coordinate system.
    """

    def __init__(self, carla_map):
        """ Constructor method """
        self._map = carla_map
        # Pick 4 points of carla.Location
        loc1 = carla.Location(0, 0, 0)
        loc2 = carla.Location(1, 0, 0)
        loc3 = carla.Location(0, 1, 0)
        loc4 = carla.Location(0, 0, 1)
        # Get the corresponding carla.GeoLocation points using carla's transform_to_geolocation()
        geoloc1 = self._map.transform_to_geolocation(loc1)
        geoloc2 = self._map.transform_to_geolocation(loc2)
        geoloc3 = self._map.transform_to_geolocation(loc3)
        geoloc4 = self._map.transform_to_geolocation(loc4)
        # Solve the transform from geolocation to location (geolocation_to_location)
        l = np.array([[loc1.x, loc2.x, loc3.x, loc4.x],
                      [loc1.y, loc2.y, loc3.y, loc4.y],
                      [loc1.z, loc2.z, loc3.z, loc4.z],
                      [1, 1, 1, 1]], dtype=float)
        g = np.array([[geoloc1.latitude, geoloc2.latitude, geoloc3.latitude, geoloc4.latitude],
                      [geoloc1.longitude, geoloc2.longitude,
                          geoloc3.longitude, geoloc4.longitude],
                      [geoloc1.altitude, geoloc2.altitude,
                          geoloc3.altitude, geoloc4.altitude],
                      [1, 1, 1, 1]], dtype=float)
        # Tform = (G*L^-1)^-1
        self._tform = np.linalg.inv(g.dot(np.linalg.inv(l)))
        # self._tform = np.linalg.inv(g).dot(l)

    def transform(self, geolocation):
        """
        Transform from carla.GeoLocation to carla.Location (left_handed z-up).

        Numerical error may exist. Experiments show error is about under 1 cm in Town03.
        """
        geoloc = np.array(
            [geolocation.latitude, geolocation.longitude, geolocation.altitude, 1])
        loc = self._tform.dot(geoloc.T)
        return carla.Location(loc[0], loc[1], loc[2])

    def get_matrix(self):
        """ Get the 4-by-4 transform matrix """
        return self._tform
    def set_matrix(self,matrix):
        """ set the 4-by-4 transform matrix """
        self._tform = matrix

再给出类的使用方法的demo,在这个demo中.我们以一台车的坐标作为测试对象,验证我们的转换方法是否正确.


if __name__ == '__main__':
    # >>>>>获取地图并计算转换矩阵
    client=carla.Client('127.0.0.1',2000)
    # world=client.load_world('Town07')
    world=client.get_world()
    map=world.get_map()

    
    # >>>>>获取车辆当前的gnss并打印信息:
    actors=world.get_actors()
    gnss_actor_list = actors.filter('*gnss*') #找到gnss
    gnss_actor = gnss_actor_list[0]
    print(gnss_actor)
    car_longitude=0.0
    car_latitude=0.0
    car_altitude=0.0
    car_transform=None
    def callback(data):
        global car_longitude,car_latitude,car_altitude
        print(f">>>>>>gnss传感器值:data.longitude 经度:{data.longitude},latitude 维度:{data.latitude},altitude 海拔:{data.altitude}")
        car_longitude=data.longitude
        car_latitude=data.latitude
        car_altitude=data.altitude
        car_transform=data.transform
        print(f"x:{car_transform.location.x},y:{car_transform.location.y},z:{car_transform.location.z}")
    gnss_actor.listen(lambda data: callback(data))
    

    times = 0
    while times < 1:
        print('----------------------------------------------------')
        times +=1
        world.tick()
        # >>>>>>获取车辆的carla.Location并打印信息:
        vehicle_actor_list=actors.filter('*vehicle*')
        vehicle_actor=vehicle_actor_list[0]
        vehicle_location=vehicle_actor.get_location()
        print(f">>>>>>车辆坐标读取值:vehicle location x:{vehicle_location.x},y:{vehicle_location.y},z:{vehicle_location.z}")
        
        # >>>>>carla.GeoLocation -> carla.Location:  
        # 使用Geo2Location类转换
        g2l_obj=Geo2Location(map) #通过加入地图map计算转换矩阵
        trans_matrix=g2l_obj.get_matrix() #转换矩阵
        save_numpy(trans_matrix, 'town07')
        matrix_val=load_numpy_txt('town07')
        g2l_obj.set_matrix(matrix_val)
        
        car_gnss=carla.GeoLocation(longitude=car_longitude,latitude=car_latitude,altitude=car_altitude)
        car_location=carla.Location()
        car_location=g2l_obj.transform(car_gnss)
        print(f'>>>>>>转换后值车辆坐标值:car_location x:{car_location.x},y:{car_location.y},z:{car_location.z}')
        print('----------------------------------------------------')

2.2 转换矩阵保存和读取

2.1中还涉及到一个保存和载入转换矩阵的函数,将转换矩阵保存成txt和npy两种格式.

def save_numpy(array,name):
    path_txt=f'./transferMatric_G2L/{name}.txt'
    path_np=f'./transferMatric_G2L/{name}.npy'
    np.savetxt(path_txt, array)
    np.save(path_np, array)

def load_numpy_txt(name):
    path_txt=f'./transferMatric_G2L/{name}.txt'
    # path_np=f'./transferMatric_G2L/{name}.npy'
    return np.loadtxt(path_txt)
    # np.load(path_np)

def load_numpy_np(name):
    # path_txt=f'./transferMatric_G2L/{name}.txt'
    path_np=f'./transferMatric_G2L/{name}.npy'
    # np.load(path_txt)
    return np.load(path_np)