一、参考资料
ROS 同时发布kitti数据集的图像和点云数据rviz显示，同时加载一个小车模型
ROS 发布kitti数据集的imu信息，并用rviz显示
ROS 发布kitti数据集的gps信息
二、ROS加载KITTI数据集的图像和点云数据
ROS同时加载KITTI数据集的图像和点云数据，并在rviz中显示，同时加载一个小车模型。
总体流程如下：
- 准备数据集；
- 读取数据：
data_utils.py； - 发布数据：
publish_utils.py； - 发布节点：
kitti.py； - 下载小车模型dae；
- 启动rviz显示。
1. 准备数据集
下载并解压KITTI数据集的 RawData数据。
2011_10_03_drive_0027_sync.zip
2011_10_03_calib.zip
文件目录结构
.
├── 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. 读取数据
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_utils.py
定义了四个发布数据的函数：publish_camera、publish_pcl、publish_ego_car和publish_car_model，分别用于发布图像、点云、maker标记（图中的两条绿色的线）以及车辆模型（图中的小车模型）。
#!/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. 发布节点
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. 下载小车模型dae
- 下载dae格式的小车模型， car dae下载链接，博主使用的是 Low-Poly Car 3D模型；
- 解压小车模型，并放到
~/catkin_ws/kitti_tutorial/Car-Model路径下，命名为：Car.dae； - 修改文件路径，publish_utils.py的publish_car_model函数中修改路径:
mesh_marker.mesh_resource="package://kitti_tutorial/Car-Model/Car.dae"，其中路径开头是package://，kitti_tutorial是功能包。
6. 启动rviz显示
# 如果没有设置环境变量，则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).
三、ROS加载KITTI数据集的IMU数据
ROS加载kitti数据集的imu数据，并在rviz中显示。
该章节流程与上一章节类似，本章节仅记录重要且有差异的地方，详细内容请参阅上一章节。
总体流程如下：
- 准备数据集；
- 读取数据：
data_utils.py； - 发布数据：
publish_utils.py； - 发布节点：
kitti.py；
1. 读取数据
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_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. 发布节点
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. 启动rviz显示
四、ROS加载KITTI数据集的GPS数据
ROS加载kitti数据集的GPS数据，在rviz中显示。
该章节流程与上一章节类似，本章节仅记录重要且有差异的地方，详细内容请参阅上一章节。
总体流程如下：
- 准备数据集；
- 读取数据：
data_utils.py； - 发布数据：
publish_utils.py； - 发布节点：
kitti.py； - 打印输出gps信息。
1. 读取数据
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_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. 发布节点
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. 打印输出gps信息
gps数据不能可视化，只有经纬度和海拔数据。需要在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
---
...
...
...
五、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