ROS中使用VLP16激光雷达获取点云数据

个人博客地址

本文测试环境为：Ubuntu20.04 ROS Noetic

---

需要将激光雷达与PC连接，然后在设置=>网络=>有线中将IPv4改为手动，并且地址为192.168.1.100，子网掩码为255.255.255.0，点击应用。然后在浏览器输入192.168.1.201，如果成功进入配置界面则表示连接成功。

安装VLP驱动

cd catkin_ws/src
git clone https://github.com/ros-drivers/velodyne.git

继续执行下面命令。如果在执行第二条命令的时候报错了，需要根据终端的提示安装一些包或者可以看看这篇文章点我

cd ..
rosdep install --from-paths src --ignore-src --rosdistro noetic -y
catkin_make

报错处理：

如果catkin_make命令找不到，就需要看安装ROS依赖最后source脚本部分有没有完成。

编译中间如果出现找不到文件的报错，大概率是没有安装对应的包。

例如出现“Could NOT find image_transport (missing: image_transport_DIR)”错误，那就需要在终端输入sudo apt install ros-noetic-image-transport.

如果遇到“Could NOT find PY_em (missing: PY_EM)”错误，就需要运行catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3。

报错参考文章：

ROS报错：-- Could NOT find PY_em (missing: PY_EM)

ROS报错：Command ‘catkin_make‘ not found

【ROS】解决rosdep无法更新、缺少依赖问题

可视化测试

运行rosrun找不到命令的话，可以使用sudo apt install ros-noetic-rosbash安装。

sudo apt-get install ros-noetic-rviz

在终端输入下面命令，这个命令会一直运行，可以用Ctrl+C结束。要在catkin_velodyne目录下运行。

roslaunch velodyne_pointcloud VLP16_points.launch

运行后会出现一个窗口，将其左上角Fixed Frame的map修改为velodyne。如果是运行rosrun rviz rviz -f velodyne命令话就不用做这一步了。

再点击左下角的Add，在弹出的窗口中选择By topic，再选择PointCloud2，最后点击OK

之后就可以在右侧看到可视化点云了。

录制和播放点云

在终端运行下面命令可以录制点云，点云记录会默认生成在运行目录。可以使用Crtl+C结束录制。

rosbag record out /velodyne_points

在终端运行下面命令可以播放点云，第一个参数填写要播放的点云记录。可以在rviz中看到播放的结果。按空格键可以暂停播放。

rosbag play 2023-03-28-22-49-58.bag

但是如果是在一直可视化显示点云的rviz下播放记录会出现重叠现象，看起来非常不舒服，因此最好是再重新开一个rviz窗口进行播放。

在终端输入roscore命令，然后再重新开一个终端输入rosrun rviz rviz -f velodyne命令。

点击左下角的Add，在弹出的窗口中选择PointCloud2，最后点击OK

点开左侧的PointCloud2，将Topic的内容改为*/velodyne_points*

之后在终端中输入播放命令rosbag play 2023-03-28-22-49-58.bag，就可以在该窗口的右侧看到播放画面了。

将bag文件转化成pcd文件

录制点云会生成bag文件，可以将该文件记录的每一帧点云转换成pcd格式并保存下来。

首先在终端中输入roscore命令

再开启一个终端输入下面的命令。output.bag为要处理的bag文件；*pcd/*为最终pcd文件的输出目录

rosrun pcl_ros bag_to_pcd output.bag /velodyne_points pcd/

python获取实时点云数据并保存成pcd文件

使用python获取VLP16返回的实时点云数据，并把每一帧数据保存成pcd文件。下面介绍两种方式：

方式一：

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2


counts = 0
def callback(data):
    global counts
    counts += 1
    gen = pc2.read_points(data, skip_nans=True)
    cloud = []
    for p in gen:
        # p是元组类型，代表一个点云数据，一共有6个元素，分别为x, y, z, intensity, ring, time
        cloud.append([p[0], p[1], p[2]])
    # pcd_name = str(counts) + ".pcd"
    with open('output.pcd', 'w') as f:
        f.write('# .PCD v.7 - Point Cloud Data file format\n')
        f.write('VERSION .7\n')
        f.write('FIELDS x y z\n')
        f.write('SIZE 4 4 4\n')
        f.write('TYPE F F F\n')
        f.write('COUNT 1 1 1\n')
        f.write('WIDTH %d\n' % len(cloud))
        f.write('HEIGHT 1\n')
        f.write('VIEWPOINT 0 0 0 1 0 0 0\n')
        f.write('POINTS %d\n' % len(cloud))
        f.write('DATA ascii\n')
        for p in cloud:
            f.write('%f %f %f\n' % (p[0], p[1], p[2]))
    print(counts)


def listener():
    rospy.init_node('velodyne_listener', anonymous=True)
    rospy.Subscriber('/velodyne_points', PointCloud2, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

方式二：

import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2


counts = 0
def callback(data):
    global counts
    counts += 1
    # cloud_list是列表类型，其存放着每个点云数据
    # 每个点云数据是sensor_msgs.point_cloud2.Point类型，共有6个元素，分别为x, y, z, intensity, ring, time
    cloud_list = pc2.read_points_list(data, skip_nans=True)
    # pcd_name = str(counts) + ".pcd"
    with open('output_list.pcd', 'w') as f:
        f.write('# .PCD v.7 - Point Cloud Data file format\n')
        f.write('VERSION .7\n')
        f.write('FIELDS x y z\n')
        f.write('SIZE 4 4 4\n')
        f.write('TYPE F F F\n')
        f.write('COUNT 1 1 1\n')
        f.write('WIDTH %d\n' % len(cloud_list))
        f.write('HEIGHT 1\n')
        f.write('VIEWPOINT 0 0 0 1 0 0 0\n')
        f.write('POINTS %d\n' % len(cloud_list))
        f.write('DATA ascii\n')
        for i in range(len(cloud_list)):
            f.write('%f %f %f\n' % (cloud_list[i][0], cloud_list[i][1], cloud_list[i][2]))
    print(counts)


def listener():
    rospy.init_node('velodyne_listener', anonymous=True)
    rospy.Subscriber('/velodyne_points', PointCloud2, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

实时可视化激光雷达点云数据

from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import numpy as np
import open3d as o3d
import rospy
import cv2


# 按下B结束运行
def key_callback(vis):
    global stop_flag
    stop_flag = True
    print("Stop")


if __name__ == '__main__':
    # 创建可视化窗口和点云
    vis = o3d.visualization.VisualizerWithKeyCallback()
    vis.create_window(width=1080, height=720)
    vis_render = vis.get_render_option()
    vis_render.background_color = (0, 0, 0)
    vis.register_key_callback(ord('B'), key_callback)
    pcd = o3d.geometry.PointCloud()

    # 标记
    add_flag = False    # 首次vis.add标记
    stop_flag = False    # 停止标记

    # 初始化ros节点
    rospy.init_node('velodyne_listener', anonymous=True)

    while not stop_flag:      
        # 等待一次消息
        data = rospy.wait_for_message('/velodyne_points', PointCloud2, timeout=None)
        gen = pc2.read_points(data, skip_nans=True)
        cloud = []
        for p in gen:
            cloud.append([p[0], p[1], p[2], p[3]])
        points = np.array(cloud).astype(np.float32)
        # 提取xyz，暂时不需要强度
        points_xyz = points[:, :3]

        pcd.points = o3d.utility.Vector3dVector(points_xyz)
        if not add_flag:
            vis.add_geometry(pcd)
            add_flag = True
        vis.update_geometry(pcd)
        vis.poll_events()
        vis.update_renderer()
    vis.destroy_window()

在这里使用的是rospy.wait_for_message用于接收一次消息，该函数会返回接收到的消息。可以设置timeout=rospy.Duration(1)，代表超时时间为1秒钟。如果在1秒内没有收到消息，则函数会返回None。

如果使用rospy.spin的话，程序运行到这行代码就会卡死，不会往下执行其他代码，所以在实时可视化的时候不能使用。