kitti数据集【图片、点云、IMU、GPS】话题发布（kitti2bag方式+python源码方式）
kitti数据集传感器话题发布
-
- 一、前期准备工作
-
- kitti数据集转bag
- 安装vscode
- 新建工作环境
- 安装opencv-python
- 二、发布图片
- 三、发布点云数据
- 四、整理前两次的代码
- 五、添加汽车图片和摄像头视角
-
- 常规写法
- 优化写法
- 六、发布IMU
- 七、发布GPS
一、前期准备工作
kitti数据集转bag
如果只是想把kitti数据集转成bag，直接用kiiti2bag指令就可以完成，教程在下面链接中。后文是详细的代码发布过程，可忽略
用kitti2bag包将kitti数据集转换成bag
安装vscode
vscode的安装及配置
这个教程配置环境变量那非常好用，避免了在vscode的 import中出现大量无法解释的问题。
新建工作环境
mkdir -p ~/catkin_ws/src
cd ~catkin_ws/src
catkin_init_workspace
cd ~/catkin_ws/
catkin_make
cd ~catkin_ws/src
catkin_create_pkg kitti_tutorial rospy
cd ..
catkin_make
source ./devel/setup.bash ## 放入环境变量
source /.bashrc
安装opencv-python
pip install opencv-python
报错ImportError: No module named ‘skbuild‘
opencv官网的一些解释
可能是pip版本过低，先检查pip指令版本
pip -V
再执行下述指令更新pip
pip install opencv-python
二、发布图片
在catkon_ws/src/kitti_tutorial/src下新建kitti.py文件
#!/usr/bin/env python
import cv2
import os
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
DATA_PATH = '/home/lzy/KITTI/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync'
if __name__ == '__main__':
frame = 0
rospy.init_node('kitti_node',anonymous=True)
cam_pub = rospy.Publisher('kitti_cam',Image,queue_size=10)
bridge = CvBridge()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
img = cv2.imread(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
cam_pub.publish(bridge.cv2_to_imgmsg(img,"bgr8"))
rospy.loginfo("camera image published")
rate.sleep()
frame += 1
frame %= 154
保存并退出后，生成可执行文件
chmod +x kitti.py
运行可执行文件即开始发布照片话题
rosrun kitti_tutorial kitti.py
打开rviz,点击Add,在By topic中订阅 /kitti_cam下的Image,即可显示发布的图片
三、发布点云数据
直接在发布图片的代码上进行新增
#!/usr/bin/env python
#-- coding: UTF-8 --
import cv2
import os
import numpy as np
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image, PointCloud2
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
DATA_PATH = '/home/lzy/KITTI/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync'
if __name__ == '__main__':
frame = 0
rospy.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)
bridge = CvBridge()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
img = cv2.imread(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
point_cloud = np.fromfile(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame), dtype = np.float32).reshape(-1, 4)
cam_pub.publish(bridge.cv2_to_imgmsg(img,"bgr8"))
rospy.loginfo("camera image published")
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:,:3])) #[:,:3} 表示取所有的点，前三行
rospy.loginfo("point_cloud image published")
rate.sleep()
frame += 1
frame %= 154
四、整理前两次的代码
到此把前面的代码整理下，分成三部分，
1、kitti.py 主程序
2、publish_utils.py 话题发布
3、data_utils.py 文件定位
以点云发布为例：
原kitti.py中文件定位和发布分别是：
#文件定位
point_cloud = np.fromfile(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame), dtype = np.float32).reshape(-1, 4)
# 话题发布
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:,:3])) #[:,:3} 表示取所有的点，前三行
ospy.loginfo("point_cloud image published")
修改到data_utils.py中是
def read_point_cloud(path):
return np.fromfile(path,dtype=np.float32).reshape(-1,4)
修改到publish_utils.py中是
FRAME_ID = 'map'
def publish_point_cloud(pcl_pub,point_cloud):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = FRAME_ID
pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:,:3]))
rospy.loginfo("point_cloud pub!")
五、添加汽车图片和摄像头视角
常规写法
这里主要是用到了Marker 模块。
# publish_utils.py
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import tf
# 发布相机视角的话题
def publish_ego_car(ego_car_pub):
# publish left and right 45 degree FOV lines and ego car model mesh
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.LINE_STRIP
# line
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0 # 透明度
marker.scale.x = 0.2 # 线的尺寸
marker.points = []
# check the kitti axis model
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_pub):
mesh_marker = Marker()
mesh_marker.header.frame_id = FRAME_ID
mesh_marker.header.stamp = rospy.Time.now()
mesh_marker.id = -1
mesh_marker.lifetime = rospy.Duration() #存活时间，默认无限
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.mesh_resource = "package://kitti_tutorial/Audi R8/Models/Car.dae"
mesh_marker.pose.position.x = 0.0
mesh_marker.pose.position.y = 0.0
mesh_marker.pose.position.z = -1.73
q = tf.transformations.quaternion_from_euler(0,0,np.pi/2) # 这一开始可以设3个0，根据模型实际现实再做修改
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.0
mesh_marker.color.g = 1.0
mesh_marker.color.b = 1.0
mesh_marker.color.a = 1.0
mesh_marker.scale.x = 0.9
mesh_marker.scale.y = 0.9
mesh_marker.scale.z = 0.9
model_pub.publish(mesh_marker)
# kiiti.py
# 新建节点
ego_pub = rospy.Publisher('ego_pub',Marker,queue_size=10)
model_pub = rospy.Publisher('model_pub',Marker,queue_size=10)
while not rospy.is_shutdown(): #放在之前代码的while中即可
publish_ego_car(ego_pub)
publish_car_model(model_pub)
运行kitti.py并在rviz中订阅并可实现如下结果：
优化写法
由于上面两个Mark需要同时发布，一前以后会出现一点问题，现给两个Mark组合起来一起发布：
# publish_utils.py
from visualization_msgs.msg import Marker, MarkerArray # 引入 MarkerArray 模块
def publish_ego_car(ego_car_pub):
marker_array = MarkerArray() # 新建了 marker_array
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.LINE_STRIP
# line
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0 # 透明度
marker.scale.x = 0.2 # 线的尺寸
marker.points = []
# check the kitti axis model
marker.points.append(Point(10,-10,0)) # 右线
marker.points.append(Point(0,0,0)) # 中心
marker.points.append(Point(10, 10,0)) # 左线
marker_array.markers.append(marker) # 把摄像头视野的marker放入marker_array
mesh_marker = Marker()
mesh_marker.header.frame_id = FRAME_ID
mesh_marker.header.stamp = rospy.Time.now()
mesh_marker.id = -1
mesh_marker.lifetime = rospy.Duration()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.mesh_resource = "package://kitti_tutorial/Audi R8/Models/Car.dae"
mesh_marker.pose.position.x = 0.0
mesh_marker.pose.position.y = 0.0
mesh_marker.pose.position.z = -1.73
q = tf.transformations.quaternion_from_euler(0,0,np.pi/2)
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.0
mesh_marker.color.g = 1.0
mesh_marker.color.b = 1.0
mesh_marker.color.a = 1.0
mesh_marker.scale.x = 0.9
mesh_marker.scale.y = 0.9
mesh_marker.scale.z = 0.9
marker_array.markers.append(mesh_marker) # 把车辆模型mesh_marker放入marker_array
ego_car_pub.publish(marker_array) # 只用发布marker_array
主程序的发布格式也要改，除了删去 publish_car_model(model_pub)
还要修改新建节点的格式
#kitti.py
# 这里改成了MarkerArray
ego_pub = rospy.Publisher('ego_pub',MarkerArray,queue_size=10)
六、发布IMU
这里主要是注意imu的发布格式
https://github.com/pratikac/kitti/blob/master/readme.raw.txt
http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html
# data_utils.py
# 使用padas模块获取表内数据
import pandas as pd
IMU_COLUMN_NAMES = ['lat', 'lon', 'alt', 'roll', 'pitch', 'yaw', 'vn', 've', 'vf', 'vl', 'vu', 'ax', 'ay', 'az', 'af','al', 'au', 'wx', 'wy', 'wz', 'wf', 'wl', 'wu', 'posacc', 'velacc', 'navstat', 'numsats', 'posmode','velmode', 'orimode']
def read_imu(path):
df = pd.read_csv(path, header=None, sep=' ')
df.columns = IMU_COLUMN_NAMES
return df
# publish_utils.py
from sensor_msgs.msg import Image, PointCloud2, Imu
def publish_imu(imu_pub, imu_data):
imu = Imu()
imu.header.frame_id = FRAME_ID
imu.header.stamp = rospy.Time.now()
# 格式可见 http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html
q = tf.transformations.quaternion_from_euler(float(imu_data.roll), float(imu_data.pitch), float(imu_data.yaw))
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.af
imu.linear_acceleration.y = imu_data.al
imu.linear_acceleration.z = imu_data.au
imu.angular_velocity.x = imu_data.wf
imu.angular_velocity.y = imu_data.wl
imu.angular_velocity.z = imu_data.wu
imu_pub.publish(imu)
# kitti.py
#!/usr/bin/env python
#-- coding: UTF-8 --
from publish_utils import *
from data_utils import *
DATA_PATH = '/home/lzy/KITTI/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync'
if __name__ == '__main__':
frame = 0
rospy.init_node('kitti_node',anonymous=True)
imu_pub = rospy.Publisher('kitti_imu',Imu, queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
imu_data = read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame)) # imu读取
publish_imu(imu_pub,imu_data) # imu发布
rate.sleep()
frame += 1
frame %= 154
七、发布GPS
类似IMU
# data_utils.py
# 使用padas模块获取表内数据
import pandas as pd
IMU_COLUMN_NAMES = ['lat', 'lon', 'alt', 'roll', 'pitch', 'yaw', 'vn', 've', 'vf', 'vl', 'vu', 'ax', 'ay', 'az', 'af','al', 'au', 'wx', 'wy', 'wz', 'wf', 'wl', 'wu', 'posacc', 'velacc', 'navstat', 'numsats', 'posmode','velmode', 'orimode']
def read_imu(path):
df = pd.read_csv(path, header=None, sep=' ')
df.columns = IMU_COLUMN_NAMES
return df
# publish_utils.py
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
def publish_gps(gps_pub, imu_data ):
gps = NavSatFix()
gps.header.frame_id = FRAME_ID
gps.header.stamp = rospy.Time.now()
gps.latitude = imu_data.lat
gps.longitude = imu_data.lon
gps.altitude = imu_data.alt
gps_pub.publish(gps)
# kitti.py
#!/usr/bin/env python
#-- coding: UTF-8 --
from publish_utils import *
from data_utils import *
DATA_PATH = '/home/lzy/KITTI/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync'
if __name__ == '__main__':
frame = 0
rospy.init_node('kitti_node',anonymous=True)
gps_pub = rospy.Publisher('kitti_gps',NavSatFix, queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
imu_data = read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame)) # imu读取
publish_gps(gps_pub, imu_data ) # GPS发布
rate.sleep()
frame += 1
frame %= 154
GPS数据在rviz无法可视化，在终端用rostopic打印出来
rostopic echo /kitti_gps
至此kitti传感器话题发布完成