实现效果:
图一
1.准备数据
kitti数据集:
图二(图像资源)
图三(点云资源)
2.发布话题
创建工作空间与功能包
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd ..
catkin_make
cd src
catkin_create_pkg kitti_tutorial rospy std_msgs
cd ..
catkin_make
在kitti_tutorial的src中创建python文件(kitti.py)内容如下:
#!/usr/bin/env python3
# -*- coding:utf-8 -*-
import os
import cv2
from cv_bridge import CvBridge
import rospy
import numpy as np
from std_msgs.msg import Header
from sensor_msgs.msg import Image,PointCloud2
import sensor_msgs.point_cloud2 as pcl2
DATA_PATH = "/home/yh/rawdata/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))
cam_pub.publish(bridge.cv2_to_imgmsg(img,"bgr8"))
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]))
rospy.loginfo("kitti published")
rate.sleep()
frame += 1
frame %= 154
下来,在上面创建功能包与工作空间的基础上:
注意:DATA_PATH为自己的文件路径,需要自己去更改
cd ~/catkin_ws/src
roscd kitti_tutorial/src
chmod +x kitti.py
下面开三个终端分别输入:
roscore
rosrun kitti_tutorial kitti.py
rosrun rviz rviz
3.在rviz中加载话题
到这里就结束了最后结果和上面的一样。