Lab 4.1: Use Kinect and PCL

Overview

The goal of this lab is to write the script in c++ to use the data from Kinect and library from Point Cloud Library.

Subscribe point data from Kinect and filter the data

Now we are going to write a ROS node that filters the point cloud from the Kinect. This node will apply a filter to make a down sampling of point cloud data.

First create a .cpp file in your package folder.

rocs my_ex1/src
gedit KinectPCfilter.cpp

And then use following code:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
ros::Publisher pub;
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
sensor_msgs::PointCloud2 output;
pcl::PCLPointCloud2* cloud =new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
// Do data processing here…
pcl_conversions::toPCL(*input, *cloud);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize(0.05,0.05,0.05);
sor.filter (cloud_filtered);
pcl_conversions::fromPCL(cloud_filtered, output);
// Publish the data.
pub.publish (output);
}
int main (int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, “KinectPCfilter”);
ros::NodeHandle nh;
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe (“/camera/depth/points”, 1, cloud_cb);
// Create a ROS publisher for the output point cloud
pub = nh.advertise<sensor_msgs::PointCloud2> (“KinectFilter“, 1);
// Spin
ros::spin ();
return 0;
}

Since we use PCL, we have to include the library into CMakeLists.txt and also tell the manifest.xml the dependencies to compile this node. Open the CMakeLists.txt and add following lines before the first rosbuild_add_executable line:

find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

And then add following line for making the executable at the last line, after finishing editing, save the file :

rosbuild_add_executable(KinectPCfilter src/KinectPCfilter.cpp)
target_link_libraries (KinectPCfilter ${PCL_LIBRARIES})

Now open manifest.xml file to add dependencies, the complete one may be like following:

<package>
<description brief=”my_ex1″>
my_ex1
</description>
<author>stevas</author>
<license>BSD</license>
<review status=”unreviewed” notes=””/>
<url>http://ros.org/wiki/my_ex1</url>
<depend package=”std_msgs”/>
<depend package=”sensor_msgs”/>
<depend package=”tf”/>
<depend package=”nav_msgs”/>
<depend package=”geometry_msgs”/>
<depend package=”rospy”/>
<depend package=”roscpp”/>
<depend package=”pcl_msgs”/>
<depend package=”pcl_conversions”/>
<build_depend>libpcl-all-dev</build_depend>
<run_depend>libpcl-all</run_depend>
</package>

Noted that adding dependencies that are not used will be convenient though, it will increase compiling time. Save the manifest.xml file, the open terminal to compile the package using following command:

rosmake my_ex1 

Now we can use the node we build to down sample the point cloud data from Kinect.

Activate the Kinect:

roslaunch openni_launch openni.launch

Use the node we create:

rosrun my_ex1 KinectPCfilter

Open the rviz to visualize the result, add PointcCloud2 display and chose the topic as KinectFilter. Since we publish the filtered data, KinectFilter, as the topic (green text in the script), we can visualize the data. In following image we can find that the point cloud is less dense.

rvizforKinectfilter