kitti数据集【图片、点云、IMU、GPS】话题发布（kitti2bag方式+python源码方式）

liiiuzy

已于 2024-03-08 16:55:53 修改

阅读量969

点赞数 10

分类专栏： ROS学习文章标签： python

于 2024-01-23 20:32:13 首次发布

本文链接：https://blog.csdn.net/liiiuzy/article/details/135763134

版权

ROS学习专栏收录该内容

7 篇文章 1 订阅

订阅专栏

kitti数据集传感器话题发布

一、前期准备工作

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传感器话题发布完成

liiiuzy

关注

10
点赞
踩
13

收藏

觉得还不错? 一键收藏
1
评论
kitti数据集【图片、点云、IMU、GPS】话题发布（kitti2bag方式+python源码方式）

打开rviz,点击Add,在By topic中订阅 /kitti_cam下的Image,即可显示发布的图片。这个教程配置环境变量那非常好用，避免了在vscode的 import中出现大量无法解释的问题。可能是pip版本过低，先检查pip指令版本。运行可执行文件即开始发布照片话题。直接在发布图片的代码上进行新增。保存并退出后，生成可执行文件。下新建kitti.py文件。再执行下述指令更新pip。
复制链接

扫一扫