“天不生我李淳罡”

1. 预知识
2. 发布GPS资料流程
- 2.1 建立发布者
- 2.2 读取GPS/IMU资料
- 2.3 发布
3. 执行
- 3.1从任务台可视化GPS数据
4. 源码
- 4.1 publish_utils.py
- 4.2 kitti_all.py

本节将在 3.2节的基础上,进行功能的添加(IMU发布),其他内容不变

工作空间是test3_autodrive_ws
python代码存在test3_autodrive_ws/src/demo1_kitti_pub_photo/scripts/kitti_all.py和publish_utils.py和data_utils.py中
数据文件存放在kitti_folder/2011_09_26/2011_09_26_drive_0005_sync/oxts/data下,可以看到是txt文件

1. 预知识

GPS/IMU实际记录的数据可以在浏览器键入 “kitti raw data”,找到gihub的readme.txt文档查看.这里给出链接,图如下:

上图可以看出IMU/GPS资料包含:经纬度,海拔,旋转角度roll,pitch,yaw,速度,加速度,角速度等资料.今天选取一部分数据进行展示.

2. 发布GPS资料流程

2.1 建立发布者

kitti_all.py中

from sensor_msgs.msg import NavSatFix...
gps_pub = rospy.Publisher("kitti_gps", NavSatFix, queue_size=10)

NavSatFix(Navigation satellite Fixed)固定的卫星的资料/是GPS的消息格式
先定义gps发布者
再读取gps数据,因为GPS和IMU数据都被存放在txt下,所有读取一边imu即可获取,由于上节已经读取了imu的全部数据,这里便不用再读取,而是直接使用

2.2 读取GPS/IMU资料

先看上节课读取的txt的数据内容:
这里的内容不仅包含IMU的,也包含GPS的,所以在IMU里读取的IMU_DATA可以直接用来提取出GPS数据,即这里无需再读

2.3 发布

publish_utils中写发布函数:

from sensor_msgs.msg import Imu

引入消息头文件

以下仍然是比较常规的,消息实例化, 消息头填充,数据填充
这里只给出比较关键的数据填充:

 def  publish_gps(gps_pub, imu_data):gps = NavSatFix()#头填充gps.header.frame_id = FRAME_IDgps.header.stamp = rospy.Time.now()#维度, 经度 和海拔gps.latitude = imu_data.latgps.longitude = imu_data.longps.altitude = imu_data.algps_pub.publish(gps)

其实imu包含很多参数都可以设定,但是这里只以旋转角度,线性加速度和角速度为例说明,其余参数可以参考之前的readme文档说明设定,f:forward, l:left, u:up

kitti_all.py中:
调用

publish_gps(gps_pub, imu_data)

3. 执行

打开roscore
rosrun 运行该文件
打开rviz并合理配置,可以看到如下界面:
- 问题是,rviz中没有,gps选项,因为它是不可视化的数据

3.1从任务台可视化GPS数据

GPS作为一组不可视的数据,可以通过如下指令看到消息内容:

rostopic info /kitti_gps

内容如下,数据一直变化

4. 源码

data_utils.py与上节一致

4.1 publish_utils.py

#!/usr/bin/env python3
#coding:utf-8import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
from visualization_msgs.msg import Marker, MarkerArray
import sensor_msgs.point_cloud2 as pcl2
from geometry_msgs.msg import Point
from cv_bridge import CvBridge
import numpy as np
import tfFRAME_ID = "map"def publish_camera(cam_pub, bridge, image):cam_pub.publish(bridge.cv2_to_imgmsg(image, 'bgr8'))def publish_point_cloud(pcl_pub, point_cloud):header = Header()header.frame_id = FRAME_IDheader.stamp = rospy.Time.now()pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))def publish_ego_car(ego_car_pub):'publish left and right 45 degree FOV and ego car model mesh'#header部分marker = Marker()marker.header.frame_id = FRAME_IDmarker.header.stamp = rospy.Time.now()# marker的id marker.id = 0marker.action = Marker.ADD      # 加入一个markermarker.lifetime = rospy.Duration()  # 生存时间,()中无参数永远出现marker.type = Marker.LINE_STRIP     #marker 的type,有很多种,这里选择线条marker.color.r = 0.0marker.color.g = 1.0marker.color.b = 0.0            #这条线的颜色marker.color.a = 1.0            #透明度 1不透明marker.scale.x = 0.2            #大小,粗细#设定marker中的资料marker.points = []# 两条线,三个点即可#原点是0,0,0, 看左上角和右上角的数据要看kitti的设定,看坐标marker.points.append(Point(10, -10, 0))marker.points.append(Point(0, 0, 0))marker.points.append(Point(10, 10, 0))ego_car_pub.publish(marker) #设定完毕,发布def publish_car_model(model):#header部分mesh_marker = Marker()mesh_marker.header.frame_id = FRAME_IDmesh_marker.header.stamp = rospy.Time.now()# marker的id mesh_marker.id = -1mesh_marker.lifetime = rospy.Duration()  # 生存时间,()中无参数永远出现mesh_marker.type = Marker.MESH_RESOURCE     #marker 的type,有很多种,这里选择meshmesh_marker.mesh_resource = "package://demo1_kitti_pub_photo/mesh/car_model/car.DAE"#平移量设置mesh_marker.pose.position.x = 0.0mesh_marker.pose.position.y = 0.0#以为0,0,0 是velodyne的坐标(车顶),这里坐标是车底,所以是负数mesh_marker.pose.position.z = -1.73#旋转量设定q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi/2)# 这里的参数和下载模型的预设角度有关,旋转关系,根据显示效果而调整,转成四元数q#x轴旋转mesh_marker.pose.orientation.x = q[0]mesh_marker.pose.orientation.y = q[1]mesh_marker.pose.orientation.z = q[2]mesh_marker.pose.orientation.w = q[3]#颜色设定(白色)mesh_marker.color.r = 1.0mesh_marker.color.g = 1.0mesh_marker.color.b = 1.0mesh_marker.color.a = 1.0#设置体积:  x,y,z方向的膨胀程度mesh_marker.scale.x = 0.4mesh_marker.scale.y = 0.4mesh_marker.scale.z = 0.4model.publish(mesh_marker) #设定完毕,发布def publish_two_marker(kitti_two_marker):#建立markerarraymarkerarray = MarkerArray()# 绿线设定marker = Marker()marker.header.frame_id = FRAME_IDmarker.header.stamp = rospy.Time.now()# marker的id marker.id = 0marker.action = Marker.ADD      # 加入一个markermarker.lifetime = rospy.Duration()  # 生存时间,()中无参数永远出现marker.type = Marker.LINE_STRIP     #marker 的type,有很多种,这里选择线条marker.color.r = 0.0marker.color.g = 1.0marker.color.b = 0.0            #这条线的颜色marker.color.a = 1.0            #透明度 1不透明marker.scale.x = 0.2            #大小,粗细#设定marker中的资料marker.points = []# 两条线,三个点即可#原点是0,0,0, 看左上角和右上角的数据要看kitti的设定,看坐标marker.points.append(Point(10, -10, 0))marker.points.append(Point(0, 0, 0))marker.points.append(Point(10, 10, 0))#加入第一个markerarray.markers.append(marker)mesh_marker = Marker()mesh_marker.header.frame_id = FRAME_IDmesh_marker.header.stamp = rospy.Time.now()# marker的id mesh_marker.id = -1mesh_marker.lifetime = rospy.Duration()  # 生存时间,()中无参数永远出现mesh_marker.type = Marker.MESH_RESOURCE     #marker 的type,有很多种,这里选择meshmesh_marker.mesh_resource = "package://demo1_kitti_pub_photo/mesh/car_model/car.DAE"#平移量设置mesh_marker.pose.position.x = 0.0mesh_marker.pose.position.y = 0.0#以为0,0,0 是velodyne的坐标(车顶),这里坐标是车底,所以是负数mesh_marker.pose.position.z = -1.73#旋转量设定q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi/2)# 这里的参数和下载模型的预设角度有关,旋转关系,根据显示效果而调整,转成四元数q#x轴旋转mesh_marker.pose.orientation.x = q[0]mesh_marker.pose.orientation.y = q[1]mesh_marker.pose.orientation.z = q[2]mesh_marker.pose.orientation.w = q[3]#颜色设定(白色)mesh_marker.color.r = 1.0mesh_marker.color.g = 1.0mesh_marker.color.b = 1.0mesh_marker.color.a = 1.0#设置体积:  x,y,z方向的膨胀程度mesh_marker.scale.x = 0.4mesh_marker.scale.y = 0.4mesh_marker.scale.z = 0.4# 加入第二个:车子模型markerarray.markers.append(mesh_marker)#发布kitti_two_marker.publish(markerarray)def publish_imu(imu_pub, imu_data):# 消息建立imu = Imu()#头填充imu.header.frame_id = FRAME_IDimu.header.stamp = rospy.Time.now()#旋转角度q = tf.transformations.quaternion_from_euler(float(imu_data.roll), float(imu_data.pitch), float(imu_data.yaw))# 将roll, pitch, yaw转成可被电脑识别的四元数q,并设定出去imu.orientation.x = q[0]imu.orientation.y = q[1]imu.orientation.z = q[2]imu.orientation.w = q[3]#线性加速度imu.linear_acceleration.x = imu_data.afimu.linear_acceleration.y = imu_data.alimu.linear_acceleration.z = imu_data.au #角速度imu.angular_velocity.x = imu_data.wfimu.angular_velocity.y = imu_data.wlimu.angular_velocity.z = imu_data.wu#发布imu_pub.publish(imu)def  publish_gps(gps_pub, imu_data):gps = NavSatFix()#头填充gps.header.frame_id = FRAME_IDgps.header.stamp = rospy.Time.now()#维度, 经度 和海拔gps.latitude = imu_data.latgps.longitude = imu_data.longps.altitude = imu_data.algps_pub.publish(gps)

4.2 kitti_all.py

#!/usr/bin/env python3
#coding:utf-8
from data_utils import *
from publish_utils import *DATA_PATH = '/home/qinsir/kitti_folder/2011_09_26/2011_09_26_drive_0005_sync/'if __name__ == '__main__':frame = 0rospy.init_node('kitti_node', anonymous=True)   #默认节点可以重名cam_pub = rospy.Publisher('kitti_cam', Image, queue_size=10)pcl_pub = rospy.Publisher("kitti_point_cloud", PointCloud2, queue_size=10)#ego_pub = rospy.Publisher('kitti_ego_car', Marker, queue_size=10)#model_pub = rospy.Publisher("kitti_car_model", Marker, queue_size=10)two_marker_pub = rospy.Publisher("kitti_two_mark", MarkerArray, queue_size=10)imu_pub = rospy.Publisher("kitti_imu", Imu, queue_size=10)  #IMU发布者gps_pub = rospy.Publisher("kitti_gps", NavSatFix, queue_size=10)bridge = CvBridge()      #opencv支持的图片和ROS可以读取的图片之间转换的桥梁rate = rospy.Rate(10)while not rospy.is_shutdown():#使用OS,路径串接,%010d,这个字串有10个数字(比如0000000001).pngimg = read_camera(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame)) point_cloud = read_point_cloud(os.path.join(DATA_PATH, "velodyne_points/data/%010d.bin"%frame))imu_data = read_imu(os.path.join(DATA_PATH, "oxts/data/%010d.txt"%frame))publish_camera(cam_pub, bridge, img)publish_point_cloud(pcl_pub, point_cloud)#publish_ego_car(ego_pub)#publish_car_model(model_pub)publish_two_marker(two_marker_pub)publish_imu(imu_pub, imu_data)publish_gps(gps_pub, imu_data)rospy.loginfo('new file publish')rate.sleep()frame += 1frame %= 154

【从kitti开始自动驾驶】--5. GPS资料发布相关推荐

【cartographer】园区场景自动驾驶-利用GPS实现ENU坐标系下的建图与定位
园区场景自动驾驶-利用GPS实现当地地理坐标系下的建图与定位概述主要实现如下的功能: 1.利用gps信息,修正题图为当地地理ENU坐标系: 2.在纯定位模式下,利用gps与imu(rtk)信息,实 ...
kitti百度网盘分享 kitti百度云盘，全套kitti分享自动驾驶
kitti百度网盘分享 kitti百度云盘其实,早在一年前,我就写了下面这篇文章,分享了自动驾驶相关任务基准数据集kitti全套的百度网盘,并且先后有几百个朋友从StrongerTang 这里拿到了 ...
自动驾驶汽车GPS系统数字孪生建模（一）
标注:本文来自本实验室胡宗盛的研究成果. 定位的技术发展第一为 GNSS 定位,该定位依托于卫星定位技术,定位精度在 10 米以下: 第二为惯导定位: 第三为高精度定位,该种定位已经实现了对传感器的 ...
自动驾驶学习过程及资料整理
本文用于个人自动驾驶学习收藏整理目前的想法是通过学习开源的自动驾驶项目(apollo和autoware)的路径,来完成自动驾驶技术的入门. 在博客.公众号上了解apollo和autoware的框架和 ...
自动驾驶汽车GPS系统数字孪生建模（二）
标注:本文来自本实验室胡宗盛的研究成果. GPS系统数字孪生建模的仿真模型实时同步数据中,实时与仿真时间之间存在差异,模拟世界拥有自己的时钟和时间,由服务器来执行, 计算两个模拟步骤需要一些实时的时间 ...
自动驾驶方程式赛车，微软发布机器学习开源框架 | AI一周学术
大数据文摘专栏作品作者:Christopher Dossman 编译:笪洁琼.conrad.云舟呜啦啦啦啦啦啦啦大家好,本周的AI Scholar Weekly栏目又和大家见面啦! AI Scho ...
【从kitti开始自动驾驶】--9.1 利用IMU/GPS测距并比较效果(jupyter)
"义勇添青史几段" 1 基础知识 1.1 角度计算 1.2 距离计算 2 python程序编写 2.1 大圆距离公式函数 2.2-a GPS测距离 2.2-b IMU测距离 2.3 ...
【从kitti开始自动驾驶】--6.2 全场景2D检测(移植至ROS,显示于RVIZ)
"飞扬的少年最动人心" 1. python程序完善 1.1 多目标的tracking 1.2 区分不同object的颜色 2. 移植至ROS 2.1 读取tracking 资料 2 ...
【从kitti开始自动驾驶】--3.1 机器人模型,相机视野可视化(RVIZ)
"愿世界和平无战争" 1. 本节内容 2. 加入相机视野 2.1 python文件新增代码 2.2 执行,查看视野效果 3. 加入机器人模型 3.1 下载3D模型 3.2为pyth ...

【从kitti开始自动驾驶】--5. GPS资料发布