文章目录

• 方法1:通过python API直接获取转换后坐标
• • 1.1 GNSS传感器消息-内容介绍
  • 1.2 在线获取方法
  • 1.3 完整代码
• 方法2-通过离线读取转换关系的方式转换
• • 2.1 转换类代码和使用方法
  • 2.2 转换矩阵保存和读取
  • 2.3 运行结果
  • 2.5 注意事项
• 附件 所有地图的转换矩阵
• 参考链接:

方法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.xglobal_location_y = t.location.yglobal_location_z = t.location.zglobal_rotation_yaw = t.rotation.yawglobal_rotation_pitch = t.rotation.pitchglobal_rotation_roll = t.rotation.rollpose_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 carlaif __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*') #找到gnssgnss_actor = gnss_actor_list[0]print(gnss_actor)car_longitude=0.0car_latitude=0.0car_altitude=0.0car_transform=Nonedef callback(data):global car_longitude,car_latitude,car_altitudeprint(f">>>>>>gnss传感器geo坐标值: longitude 经度:{data.longitude},latitude 维度:{data.latitude},altitude 海拔:{data.altitude}")car_longitude=data.longitudecar_latitude=data.latitudecar_altitude=data.altitudecar_transform=data.transformprint(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 = 0while times < 1:print('----------------------------------------------------')times +=1world.tick()

运行结果:

自动打印车辆当前的geoLocation和转换后的carlaLocation.

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

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

2.1 转换类代码和使用方法

我们先给出这个转换类:

class Geo2Location(object):"""Helper class for homogeneous transform from geolocationThis 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.Locationloc1 = 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)^-1self._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._tformdef 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*') #找到gnssgnss_actor = gnss_actor_list[0]print(gnss_actor)car_longitude=0.0car_latitude=0.0car_altitude=0.0car_transform=Nonedef callback(data):global car_longitude,car_latitude,car_altitudeprint(f">>>>>>gnss传感器值:data.longitude 经度:{data.longitude},latitude 维度:{data.latitude},altitude 海拔:{data.altitude}")car_longitude=data.longitudecar_latitude=data.latitudecar_altitude=data.altitudecar_transform=data.transformprint(f"x:{car_transform.location.x},y:{car_transform.location.y},z:{car_transform.location.z}")gnss_actor.listen(lambda data: callback(data))times = 0while times < 1:print('----------------------------------------------------')times +=1world.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)

通过调用上述函数,就可以获取转换矩阵文件,对应每一个地图.

2.3 运行结果

我们通过carla-ros-bridge开启了一个节点,召唤了一台车,界面左侧展示了一些粗略的信息.

需要注意的是:这里的Location,GNSS和height都比较粗略,所以仅供参考.

然后我们运行2.1的main程序,会打印出4个结果,分别是:

1. gnss传感器中的geoLocation值
2. gnss传感器中的carla坐标值
3. 通过API读取的carla坐标值(也就是车辆当前的值)
4. 离线转换的carla坐标值(我们计算出来的值)

可以清楚的看到, 2,3,4得到的值完全一致,也就证明了我们转换矩阵的有效性.

2.5 注意事项

这里生成的GNSS传感器的坐标设置在(0,0,0)位置,也就是和当前车辆完全一致的位置,其他位置的GNSS传感器带进去计算的话,计算的就不是车辆本身的Location了.

附件 所有地图的转换矩阵

点击:我的gitte代码库
找到transferMatric_Geo2Loc文件夹

参考链接:

github 讨论贴: https://github.com/carla-simulator/carla/issues/2737

坐标转换代码来源:https://github.com/lian999111/carla-semantic-localization/blob/c4844f2f6b8bbc21c8e3e4962954cf01eb673e85/carlasim/data_collect.py

carla-geolocation:https://carla.readthedocs.io/en/latest/python_api/#carlageolocation

carla-location:https://carla.readthedocs.io/en/latest/python_api/#carla.Location

carla.GnssMeasurement:https://carla.readthedocs.io/en/latest/python_api/#carla.GnssMeasurement

GNSS sensorhttps://carla.readthedocs.io/en/latest/ref_sensors/#gnss-sensor

carla.tranform https://carla.readthedocs.io/en/latest/python_api/#carlatransform