turtlebot2-tutorials

PCL with ROS using C++

Using PCL with ROS is possible using the PCL_ROS and ROS_PERCEPTION libraries. This tutorial will show you how to get a message from a PointCloud2 topic in ROS, convert it to an pcl Point Cloud, and manipulate the point cloud.

Prequisites

This example requires an image stream on the /camera/rgb/image_raw topic.

  1. On the turtlebot, run 3dsensor.launch:
    1. roslaunch turtlebot_bringup 3dsensor.launch

This section requires the catkin_ws to be initialized and the turtlebot_dabit package created.
Please click here to learn how to initialize the catkin workspace

This section requires the roscpp example to be built in the turtlebot_dabit package.
Please click here to learn how to build turtlebot_dabit with roscpp

Getting a PointCloud from a ROS topic and using PCL

  1. Copy your example roscpp_hello_world.cpp code to roscpp_pcl.cpp:
    1. cp ~/catkin_ws/src/turtlebot_dabit/src/roscpp_hello_world.cpp ~/catkin_ws/src/turtlebot_dabit/src/roscpp_pcl.cpp
    2. gedit ~/catkin_ws/src/turtlebot_dabit/src/roscpp_pcl.cpp

       /*
        * Hello World Example using ROS and CPP
        */
      
       // Include the ROS library
       #include <ros/ros.h>
      
       // Main function
       int main(int argc, char** argv)
       {
         // Initialize the ROS Node "roscpp_example"
         ros::init(argc, argv, "roscpp_example");
      
         // Instantiate the ROS Node Handler as nh
         ros::NodeHandle nh;
      
         // Print "Hello ROS!" to the terminal and ROS log file
         ROS_INFO_STREAM("Hello from ROS node " << ros::this_node::getName());
      
         // Program succesful
         return 0;
       }
      
  2. Set up your package dependencies:
    1. gedit ~/CMakeLists.txt
      • Replace find_package(catkin REQUIRED COMPONENTS) with:

          find_package(catkin REQUIRED COMPONENTS
            roscpp
            pcl_conversions
            pcl_ros
          )
        
      • Replace catkin_package( with:

          catkin_package(
            INCLUDE_DIRS
            CATKIN_DEPENDS roscpp
                           pcl_conversions
                           pcl_ros
          )
        
      • Add your build target for roscpp_opencv.cpp:

          add_executable(roscpp_pcl_example src/roscpp_pcl_example.cpp)
          target_link_libraries(roscpp_pcl_example ${catkin_LIBRARIES})
        
    2. gedit package.xml
      • Add the build_depend and run_depends under roscpp:

          <build_depend>roscpp</build_depend>
          <build_depend>pcl_conversions</build_depend>
          <build_depend>pcl_ros</build_depend>
          <build_depend>libpcl-all-dev</build_depend>
          <run_depend>roscpp</run_depend>
          <run_depend>pcl_conversions</run_depend>
          <run_depend>pcl_ros</run_depend>
          <run_depend>libpcl-all</run_depend>
        
  3. Edit roscpp_pcl_example.cpp in your src folder
    1. gedit ~/catkin_ws/src/turtlebot_dabit/src/roscpp_pcl_example.cpp
    2. Replace the Hello ROS code with the following PCL code:

       /*
                * PCL Example using ROS and CPP
                */
      
               // Include the ROS library
               #include <ros/ros.h>
      
               // Include pcl
               #include <pcl_conversions/pcl_conversions.h>
               #include <pcl/point_cloud.h>
               #include <pcl/point_types.h>
               #include <pcl/filters/voxel_grid.h>
      
               // Include PointCloud2 message
               #include <sensor_msgs/PointCloud2.h>
      
               // Topics
               static const std::string IMAGE_TOPIC = "/camera/depth/points";
               static const std::string PUBLISH_TOPIC = "/pcl/points";
      
               // ROS Publisher
               ros::Publisher pub;
      
               void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
               {
                   // Container for original & filtered data
                   pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
                   pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
                   pcl::PCLPointCloud2 cloud_filtered;
      
                   // Convert to PCL data type
                   pcl_conversions::toPCL(*cloud_msg, *cloud);
      
                   // Perform the actual filtering
                   pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
                   sor.setInputCloud (cloudPtr);
                   sor.setLeafSize (0.1, 0.1, 0.1);
                   sor.filter (cloud_filtered);
      
                   // Convert to ROS data type
                   sensor_msgs::PointCloud2 output;
                   pcl_conversions::moveFromPCL(cloud_filtered, output);
      
                   // Publish the data
                   pub.publish (output);
               }
      
               int main (int argc, char** argv)
               {
                   // Initialize the ROS Node "roscpp_pcl_example"
                   ros::init (argc, argv, "roscpp_pcl_example");
                   ros::NodeHandle nh;
      
                   // Print "Hello" message with node name to the terminal and ROS log file
                   ROS_INFO_STREAM("Hello from ROS Node: " << ros::this_node::getName());
      
                   // Create a ROS Subscriber to IMAGE_TOPIC with a queue_size of 1 and a callback function to cloud_cb
                   ros::Subscriber sub = nh.subscribe(IMAGE_TOPIC, 1, cloud_cb);
      
                   // Create a ROS publisher to PUBLISH_TOPIC with a queue_size of 1
                   pub = nh.advertise<sensor_msgs::PointCloud2>(PUBLISH_TOPIC, 1);
      
                   // Spin
                   ros::spin();
      
                   // Success
                   return 0;
               }
      
    3. Save and exit
  4. Build and run your new code:
    1. catkin_make --directory ~/catkin_ws --pkg turtlebot_dabit
    2. source ~/devel/setup.sh
    3. rosrun turtlebot_dabit roscpp_pcl_example

First Example Complete

Additional Resources

ROSCPP Tutorials
ROSPCL Tutorials

Return to the main README page