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

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 Framemap修改为velodyne。如果是运行rosrun rviz rviz -f velodyne命令话就不用做这一步了。

ROS中使用VLP16激光雷达获取点云数据_第1张图片

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

ROS中使用VLP16激光雷达获取点云数据_第2张图片

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

ROS中使用VLP16激光雷达获取点云数据_第3张图片

录制和播放点云

在终端运行下面命令可以录制点云,点云记录会默认生成在运行目录。可以使用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

ROS中使用VLP16激光雷达获取点云数据_第4张图片

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

ROS中使用VLP16激光雷达获取点云数据_第5张图片

之后在终端中输入播放命令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的话,程序运行到这行代码就会卡死,不会往下执行其他代码,所以在实时可视化的时候不能使用。

你可能感兴趣的:(三维激光雷达,ROS,三维点云,VLP16)