Working with Point Cloud Library in ROS & Gazebo [PCL Part 1]

Quick start to work with Point Cloud Library (PCL) and Velodyne lidar sensor in Robot Operating System & Gazebo Simulator. In this part, we will setup a catkin workspace that include Velodyne lidar sensor model and subscribe to its point cloud message data.

The Point Cloud Library (PCL) is an open source library for 2D/3D image and point cloud processing. It is a cross-platform library and written in C++ language. It has bindings in Python. It contains algorithms for filtering, feature estimation, surface reconstruction, 3D registration, model fitting, object recognition, and segmentation. Link: https://pointclouds.org/

Velodyne Lidar is a Silicon Valley-based company which produce of sensor technology that used for autonomous vehicles, advanced driver assistance systems (ADASs), mapping, security, and unmanned aerial vehicles. Google, Nokia, Microsoft Bing Maps, Ford are partners and customers. Link: https://velodynelidar.com/

Ubuntu 20.04

ROS Noetic & Gazebo 11

GitHub Link: https://github.com/Tevhit/pcl_velodyne_ws

Velodyne lidar model and its point cloud data

This link include the ROS & Gazebo model of Velodyne Lidar sensor model: https://bitbucket.org/DataspeedInc/velodyne_simulator/src/master/

Let’s create a catkin workspace and clone it into a workspace.

mkdir -p pcl_velodyne_ws/src
cd pcl_velodyne_ws
catkin_make
cd src
git clone https://bitbucket.org/DataspeedInc/velodyne_simulator.git
sudo mv velodyne_description/ velodyne_gazebo_plugins/ ..
cd ..
rm -rf velodyne_simulator/
cd ..
catkin_make

Now, our pcl_velodyne_ws workspace has Velodyne lidar sensor model. We can launch already existing example which is in velodyne_description package.

source devel/setup.bash
roslaunch velodyne_description example.launch

So, we can see the lidar models in Gazebo and point cloud data of theirs in RViz. There are two different models which are VLP-16 and HDL-32E.

ROS topics of lidar sensors

When we list the ros topics, we can see /velodyne_points and /velodyne_points2 topics as above. One of them belong to VLP-16 and the other one to HDL-32E.

The sensor publishes type with sensor_msgs/PointCloud2 When we run “rostopic echo /velodyne_points”, we can see real time point clouds.

Let’s continue with developing our custom subscriber node.

cd src
catkin_create_pkg basic_pcl std_msgs rospy roscpp
cd ..
catkin_make

Then, open the pcl_velodyne_ws workspace with CLion or any C++ IDE. (CLion is an IDE for C and C++ by JetBrains. To installation: “sudo snap install clion — classic”)

<buildtool_depend>catkin</buildtool_depend>

<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>libpcl-all-dev</build_depend>

<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>pcl_conversions</build_export_depend>
<build_export_depend>pcl_ros</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>libpcl-all</build_export_depend>

<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>pcl_conversions</exec_depend>
<exec_depend>pcl_ros</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>libpcl-all</exec_depend>

Add the dependencies to the package.xml file in our basic_pcl package as above.

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

find_package(catkin REQUIRED COMPONENTS
        roscpp
        rospy
        std_msgs
        pcl_conversions
        pcl_ros
)
add_executable(SubscrivePointCloud2Message src/SubscribePointCloud2Message.cpp)
target_link_libraries(SubscrivePointCloud2Message ${catkin_LIBRARIES})
target_link_libraries(SubscrivePointCloud2Message ${PCL_LIBRARIES})

To develop our custom subscriber node, create a C++ class named SubscribePointCloud2Message. And add the dependencies to the CMakeLists.txt file in our basic_pcl package as above.

Workspace Structure

So, the workspace is shown above.

//
// Created by tevhit on 27.06.2023.
//

#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

// PCL includes
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>

using namespace std;

class SubscribePointCloud2Message
{
public:
    SubscribePointCloud2Message()
    {
        // velodyne_points  -> VLP-16
        // velodyne_points2 -> HDL-32E
        sub_ = nh_.subscribe ("/velodyne_points2", 1,  &SubscribePointCloud2Message::cloudCallback, this);
    }

private:
    // Callback
    void cloudCallback(const pcl::PCLPointCloud2::ConstPtr& callback)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromPCLPointCloud2(*callback, *cloud);

        std::cout << "Real Time Point Cloud Data:" << std::endl;
        std::cerr << *cloud << std::endl;
        std::cout << "Point Cloud data has: " << cloud->points.size () << " data points." << std::endl;
        std::cout << "----------------------------" << std::endl;
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;
};

int main(int argc, char** argv)
{
    ros::init (argc, argv, "Subscribe_PointCloud2_Message", ros::init_options::AnonymousName);

    SubscribePointCloud2Message subscribePointCloud2Message;
    ros::spin();

    return (0);
}

The SubscribePointCloud2Message class provides to subscribe the topic of /velodyne_points2. Run the node with;

cd ~/pcl_velodyne_ws/
catkin_make
source devel/setup.bash
rosrun basic_pcl SubscrivePointCloud2Message

SubscribePointCloud2Message.cpp

Real Time Point Cloud Data:
header: seq: 372 stamp: 38469000 frame_id: velodyne2

points[]: 5137
width: 5137
height: 1
is_dense: 1
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]

Point Cloud data has: 5137 data points.
----------------------------

We obtain the terminal outputs as above.