ROS loads the KITTI dataset and displays it in rviz (2)

1. Reference materials

ROS releases the image and point cloud data of the kitti dataset at the same time. Rviz display, and loads a car model at the same time
ROS publishes the imu information of the kitti dataset, and uses rviz to display
the gps information of the kitti dataset released by ROS

2. ROS loads the image and point cloud data of the KITTI dataset

ROS loads the image and point cloud data of the KITTI dataset at the same time, and displays it in rviz, and loads a car model at the same time.

The overall process is as follows:

1. Prepare the dataset;
2. read data: data_utils.py;
3. post data: publish_utils.py;
4. publish node: kitti.py;
5. Download the car model dae;
6. Start rviz display.

1. Prepare the dataset

Download and unzip the RawData data of the KITTI dataset.

2011_10_03_drive_0027_sync.zip
2011_10_03_calib.zip

file directory structure

.
├── 2011_10_03
│   ├── 2011_10_03_drive_0027_sync
│   │   ├── image_00
│   │   ├── image_01
│   │   ├── image_02
│   │   ├── image_03
│   │   ├── oxts
│   │   └── velodyne_points
│   ├── calib_cam_to_cam.txt
│   ├── calib_imu_to_velo.txt
│   └── calib_velo_to_cam.txt

2. Read data

data_utils.py

#!/usr/bin/env python
# coding:utf-8

import cv2
import numpy as np

def read_camera(path):
    return cv2.imread(path)

def read_point_cloud(path):
    return np.fromfile(path,dtype=np.float32).reshap(-1,4)

3. Publish data

publish_utils.py

Four functions for publishing data are defined: publish_camera, publish_pcl, publish_ego_car, and publish_car_model, which are used to publish images, point clouds, maker marks (the two green lines in the figure) and vehicle models (the car model in the figure) respectively.

#!/usr/bin/env python
# coding:utf-8

from pyexpat import model
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
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import tf
import numpy as np
import tf_conversions

FRAME_ID='map'

def publish_camera(cam_pub,bridge,image):
    cam_pub.publish(bridge.cv2_to_imgmsg(image,'bgr8'))

def publish_pcl(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]))


def publish_ego_car(ego_car_pub):
    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

    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=[]
    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/Audi R8.dae"
    
    mesh_marker.mesh_resource="package://kitti_tutorial/Car-Model/Car.dae"

    mesh_marker.pose.position.x=0
    mesh_marker.pose.position.y=0
    mesh_marker.pose.position.z=-1.73
    q = tf_conversions.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

    model_pub.publish(mesh_marker)

4. Publish the node

kitti.py

#!/usr/bin/env python
import os
from data_utils import *
from publish_utils import *

DATA_PATH='/home/chen/Downloads/kittidata/2011_09_26/2011_09_26_drive_0005_sync/'

if __name__=='__main__':
    rospy.init_node('kitti_node',anonymous=True)
    cam_pub=rospy.Publisher('kitti_cam',Image,queue_size=10)
    pcl_pub=rospy.Publisher('kitti_pcl',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)
    bridge=CvBridge()
    rate=rospy.Rate(10)
    frame=0

    while not rospy.is_shutdown():
        img=cv2.imread(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
        pcl=np.fromfile(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame),dtype=np.float32).reshape(-1,4)

        publish_camera(cam_pub,bridge,img)
        publish_pcl(pcl_pub,pcl)
        publish_ego_car(ego_pub)
        publish_car_model(model_pub)
        rospy.loginfo('published')
        rate.sleep()
        frame+=1
        frame%=154

5. Download the car model dae

• Download the car model in dae format, car dae download link , the blogger uses the Low-Poly Car 3D model ;
• Unzip the car model, put it ~/catkin_ws/kitti_tutorial/Car-Modelunder the path, and name it: Car.dae;
• Modify the file path, modify the path in the publish_car_model function of publish_utils.py: mesh_marker.mesh_resource="package://kitti_tutorial/Car-Model/Car.dae", where the path starts with package://, kitti_tutorialwhich is a function package.

6. Start rviz display

# 如果没有设置环境变量，则source环境变量
source ~/catkin_ws/devel/setup.bash

# 启动rviz
rosrun rviz rviz

yoyo@yoyo:~/catkin_ws$ rosrun rviz rviz
[ INFO] [1690365388.073718375]: rviz version 1.12.17
[ INFO] [1690365388.073747781]: compiled against Qt version 5.5.1
[ INFO] [1690365388.073753943]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1690365388.210051811]: Stereo is NOT SUPPORTED
[ INFO] [1690365388.210108478]: OpenGl version: 3 (GLSL 1.3).

3. ROS loads the IMU data of the KITTI dataset

ROS loads the imu data of the kitti dataset and displays it in rviz.

The process of this chapter is similar to the previous chapter. This chapter only records the important and differences. For details, please refer to the previous chapter.

The overall process is as follows:

1. Prepare the dataset;
2. read data: data_utils.py;
3. post data: publish_utils.py;
4. publish node: kitti.py;

1. Read data

data_utils.py

#!/usr/bin/env python

import cv2
import numpy as np
import pandas as pd
from sensor_msgs.msg import Imu


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_camera(path):
    return cv2.imread(path)

def read_point_cloud(path):
    return np.fromfile(path,dtype=np.float32).reshape(-1,4)

def read_imu(path):
    df=pd.read_csv(path,header=None,sep=' ')
    df.columns=IMU_COLUMN_NAMES
    return df

2. Publish data

publish_utils.py

#!/usr/bin/env python

from pyexpat import model
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image,PointCloud2,Imu
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
from visualization_msgs.msg import Marker,MarkerArray
from geometry_msgs.msg import Point
import tf
import numpy as np
import tf_conversions



FRAME_ID='map'

def publish_camera(cam_pub,bridge,image):
    cam_pub.publish(bridge.cv2_to_imgmsg(image,'bgr8'))

def publish_pcl(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]))


def publish_ego_car(ego_car_pub):

    marker_array=MarkerArray()

    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

    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=[]
    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)
#######################################################
    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/Car-Model/Car.dae"

    mesh_marker.pose.position.x=0
    mesh_marker.pose.position.y=0
    mesh_marker.pose.position.z=-1.73
    q = tf_conversions.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)


    ego_car_pub.publish(marker_array)

def publish_imu(imu_pub,imu_data):
    imu=Imu()
    imu.header.frame_id=FRAME_ID
    imu.header.stamp=rospy.Time.now()

    q = tf_conversions.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)

3. Publish the node

kitti.py

#!/usr/bin/env python



import os

from data_utils import *
from publish_utils import *



DATA_PATH='/home/chen/Downloads/kittidata/2011_09_26/2011_09_26_drive_0005_sync/'

if __name__=='__main__':
    rospy.init_node('kitti_node',anonymous=True)
    cam_pub=rospy.Publisher('kitti_cam',Image,queue_size=10)
    pcl_pub=rospy.Publisher('kitti_pcl',PointCloud2,queue_size=10)
    ego_pub=rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)
    imu_pub=rospy.Publisher('kitti_imu',Imu,queue_size=10)
    bridge=CvBridge()
    rate=rospy.Rate(10)
    frame=0

    while not rospy.is_shutdown():
        img=read_camera(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
        pcl=read_point_cloud(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame))
        imu=read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))
        publish_camera(cam_pub,bridge,img)
        publish_pcl(pcl_pub,pcl)
        publish_ego_car(ego_pub)
        publish_imu(imu_pub,imu)
        rospy.loginfo('published')
        rate.sleep()
        frame+=1
        frame%=154

4. Start rviz display

4. ROS loads the GPS data of the KITTI dataset

ROS loads the GPS data of the kitti dataset and displays it in rviz.

The process of this chapter is similar to the previous chapter. This chapter only records the important and differences. For details, please refer to the previous chapter.

The overall process is as follows:

1. Prepare the dataset;
2. read data: data_utils.py;
3. post data: publish_utils.py;
4. publish node: kitti.py;
5. Print out the gps information.

1. Read data

data_utils.py

#!/usr/bin/env python

import cv2
import numpy as np
import pandas as pd
from sensor_msgs.msg import Imu


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_camera(path):
    return cv2.imread(path)

def read_point_cloud(path):
    return np.fromfile(path,dtype=np.float32).reshape(-1,4)

def read_imu(path):
    df=pd.read_csv(path,header=None,sep=' ')
    df.columns=IMU_COLUMN_NAMES
    return df

2. Publish data

publish_utils.py

#!/usr/bin/env python



import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image,PointCloud2,Imu,NavSatFix
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
from visualization_msgs.msg import Marker,MarkerArray
from geometry_msgs.msg import Point
import tf
import numpy as np
import tf_conversions



FRAME_ID='map'

def publish_camera(cam_pub,bridge,image):
    cam_pub.publish(bridge.cv2_to_imgmsg(image,'bgr8'))

def publish_pcl(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]))


def publish_ego_car(ego_car_pub):

    marker_array=MarkerArray()

    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

    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=[]
    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)
#######################################################
    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/Car-Model/Car.dae"

    mesh_marker.pose.position.x=0
    mesh_marker.pose.position.y=0
    mesh_marker.pose.position.z=-1.73
    q = tf_conversions.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)


    ego_car_pub.publish(marker_array)

def publish_imu(imu_pub,imu_data):
    imu=Imu()
    imu.header.frame_id=FRAME_ID
    imu.header.stamp=rospy.Time.now()

    q = tf_conversions.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)
    
    
def publish_gps(gps_pub,gps_data):
    gps=NavSatFix()
    gps.header.frame_id=FRAME_ID
    gps.header.stamp=rospy.Time.now()

    gps.latitude=gps_data.lat
    gps.longitude=gps_data.lon
    gps.altitude=gps_data.alt

    gps_pub.publish(gps)

3. Publish the node

kitti.py

#!/usr/bin/env python



import os

from data_utils import *
from publish_utils import *



DATA_PATH='/home/chen/Downloads/kittidata/2011_09_26/2011_09_26_drive_0005_sync/'

if __name__=='__main__':
    rospy.init_node('kitti_node',anonymous=True)
    cam_pub=rospy.Publisher('kitti_cam',Image,queue_size=10)
    pcl_pub=rospy.Publisher('kitti_pcl',PointCloud2,queue_size=10)
    ego_pub=rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)
    imu_pub=rospy.Publisher('kitti_imu',Imu,queue_size=10)
    gps_pub=rospy.Publisher('kitti_gps',NavSatFix,queue_size=10)
    bridge=CvBridge()
    rate=rospy.Rate(10)
    frame=0

    while not rospy.is_shutdown():
        img=read_camera(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
        pcl=read_point_cloud(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame))
        imu=read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))

        publish_camera(cam_pub,bridge,img)
        publish_pcl(pcl_pub,pcl)
        publish_ego_car(ego_pub)
        publish_imu(imu_pub,imu)
        publish_gps(gps_pub,imu)
        rospy.loginfo('published')
        rate.sleep()
        frame+=1
        frame%=154

4. Print out gps information

GPS data cannot be visualized, only latitude, longitude and altitude data. Need to print it out in terminal.

# 查看话题
rostopic list

# 获取话题的信息
rostopic info /kitti_gps

# 终端打印话题信息
rostopic echo /kitti_gps

yoyo@yoyo:~/catkin_ws$ rostopic list
/kitti_cam
/kitti_ego_car
/kitti_gps
/kitti_imu
/kitti_pcl
/rosout
/rosout_agg

yoyo@yoyo:~/catkin_ws$ rostopic info /kitti_gps
Type: sensor_msgs/NavSatFix

Publishers: 
 * /kitti_node_5283_1690376577528 (http://yoyo:43411/)

Subscribers: None

yoyo@yoyo:~/catkin_ws$ rostopic echo /kitti_gps
header: 
  seq: 1
  stamp: 
    secs: 1690377869
    nsecs: 446662902
  frame_id: "map"
status: 
  status: 0
  service: 0
latitude: 48.982823219
longitude: 8.39058595049
altitude: 116.419876099
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
---
header: 
  seq: 2
  stamp: 
    secs: 1690377869
    nsecs: 548213958
  frame_id: "map"
status: 
  status: 0
  service: 0
latitude: 48.9828321777
longitude: 8.39059294785
altitude: 116.457099915
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
---
header: 
  seq: 3
  stamp: 
    secs: 1690377869
    nsecs: 648731946
  frame_id: "map"
status: 
  status: 0
  service: 0
latitude: 48.9828402246
longitude: 8.39059920153
altitude: 116.482711792
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
---
header: 
  seq: 4
  stamp: 
    secs: 1690377869
    nsecs: 750245094
  frame_id: "map"
status: 
  status: 0
  service: 0
latitude: 48.9828490886
longitude: 8.39060608495
altitude: 116.50869751
position_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
position_covariance_type: 0
---
...
...
...

5. FAQ

Q：[package://kitti_tutorial/Car-Model/Car.dae]: Unable to open file "package://kitti_tutorial/Car-Model/Car.dae".

yoyo@yoyo:~/catkin_ws$ rosrun rviz rviz
[ INFO] [1690365120.133667513]: rviz version 1.12.17
[ INFO] [1690365120.133702479]: compiled against Qt version 5.5.1
[ INFO] [1690365120.133711106]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1690365120.256604362]: Stereo is NOT SUPPORTED
[ INFO] [1690365120.256674124]: OpenGl version: 3 (GLSL 1.3).
[rospack] Error: package 'kitti_tutorial' not found
[librospack]: error while executing command
[ERROR] [1690365143.160433866]: Could not load resource [package://kitti_tutorial/Car-Model/Car.dae]: Unable to open file "package://kitti_tutorial/Car-Model/Car.dae".

错误原因：
运行rviz的终端没有将环境变量添加进来

解决办法：
source环境变量
source ~/catkin_ws/devel/setup.bash

Q：[rosrun] Couldn't find executable named kitti.py

yoyo@yoyo:~/catkin_ws$ rosrun kitti_tutorial kitti.py
[rosrun] Couldn't find executable named kitti.py below /home/yoyo/catkin_ws/src/kitti_tutorial
[rosrun] Found the following, but they're either not files,
[rosrun] or not executable:
[rosrun]   /home/yoyo/catkin_ws/src/kitti_tutorial/scripts/kitti.py

错误原因：
没有给 kitti.py 文件添加权限

解决办法：
给 kitti.py 文件添加权限
chmod +x kitti.py