We are going to add 3D object recognition to our robot so I have been trying to implement PCL 1.7 on ubuntu 12.04 these two weeks, where the ROS version is fuerte. As we know, even we only want to have a small function in ROS, we need to look into a lot of documents and it is a pity that there is very few step by step tutorial. I simply record what I have done for the past two weeks for my own reference and I hope it can help some one who want to do the same thing.
If you are using Groovy or Hydro, this article may not be helpful on some technical details. However, the pipeline should be the same.
I. PCL17
Currently PCL is independent from ROS. And the original ros wiki
http://wiki.ros.org/pcl does not provide up-to-date info for PCL installation. We need to refer to
wiki.ros.org/pcl17 and you can get basic ideas how to do that through reading the following two posts.
http://answers.ros.org/question/58409/problems-with-repository-pcl17/
http://answers.ros.org/question/44821/updating-to-pcl-17-in-ros-fuerte/
For me, I have downloaded
pcl17, but for
pcl_ros I am using the original one with fuerte. The package
pcl_ros provides functions to convert different data types between PCL and ROS and transformation for point cloud.
It takes quite a looooooong time to compile
pcl17. Over 3000s on my Thinkpad T410. So please be patient or just AFK..
II. pcl/Tutorial
Though the compilation of the
pcl17 itself is not a problem, it is not that straight forward to include PCL in your own ROS code. Firstly, we try to go through the pcl/Tutorial. This is a very important tutorial and you should read the whole page line by line and try not to miss a single word!
http://wiki.ros.org/pcl/Tutorials
This section is just following and explaining the ros tutorial, solving the problems during compilation.
1.Code skeleton
I am not familiar with catkin so that I am using rosbuild. Because in this version of PCL,
the used namespace is pcl17. That means we need to change all the
pcl:: to
pcl17::, and use
#include<pcl17/***> instead of
#include<pcl/***>. Also, remember to change the
manifest.xml
<depend package="pcl17"/>
I stick to the dependency on
pcl_ros package because I am not using
pcl17_ros.
<depend package="pcl_ros"/>
You should be able to compile the
example.cpp successfully.
2.sensor_msgs/PointCloud2
Proceed on to ros pcl tutorial section 4.1, simply add the 5 lines of codes into function
cloud_cb(),
pcl::VoxelGrid
. Now you have a function which can down sample the point cloud.
rosrun my_pcl_tutorial example input:= <PointCloud2>
The input must be a PointCloud2 type. If you have an RGB-D device and openni installed, you can try
/camera/depth/points
/camera/depth_registered/points
The results can be visualized in rviz.
But, if you do not have an RGB-D device at present? You can download some .pcd file, the extension means
point
cloud
document. For example
http://svn.pointclouds.org/data/tutorials/correspondence_grouping/milk_cartoon_all_small_clorox.pcd
After you have the .pcd file, we can use the function in
pcl_ros to publish a point cloud. Refer to
http://wiki.ros.org/pcl_ros 4.4.3 Usage. For example
rosrun pcl_ros pcd_to_pointcloud milk_cartoon_all_small_clorox.pcd 0.5
So the
pcd_to_pointcloud function can read the .pcd file and publish it as point cloud every o.5 sec. The topic is
/cloud_pcd
Finally, using
rosrun my_pcl_tutorial example input:=/cloud_pcd
Now we can see some results in rviz.
3. pcl/PointCloud<T>
I did not really compile and execute this tutorial. But the data type conversion mentioned here is very important. The cloud using in ROS is
sensor_msgs::PointCloud2, but in PCL it is
pcl17::PointCloud. We need to perform conversions when needed. The functions are
fromROSMsg and
toROSMsg, can be checked in
http://docs.pointclouds.org/1.0.0/namespacepcl.html
Pay attention to the format whether your input argument is a pointer or not? We will give detailed examples in the next section.
III. Correspondence Grouping
After trying out some examples, now we are going to integrate the object recognition with ROS. Please refer to the following link. Now let's try to realize the function in ROS.
http://pointclouds.org/documentation/tutorials/correspondence_grouping.php#correspondence-grouping
1. Code Modification
Read through the PCL tutorial, get familiar with the functions. As we already have the ROS skeleton for PCL, what we need to do is to embed the
main() function into
cloud_cb(). It is not necessary to have
showHelp() and
parseCommandLine(). We simply modify the parameters in the program. If you want use command line to control the parameters, ROS provides powerful tools to do that..
Similarly, change all the
pcl:: to
pcl17::. Pay attention to
PCL_VISUALIZER_POINT_SIZE, this parameter in
visualization:: class need to change to
PCL17_VISUALIZER_POINT_SIZE.
To use the
.pcd file provided by the tutorial, you can simply read the
.pcd file, or using
pcl_ros::fromROSMsg, take in the parameters are arguments for
cloud_cb(). Here, we read the model from the file, and takes in the scene from ROS topic. In this way, it is easier to change the scene from
.pcd file.
To read the
.pcd file, we can use the
loadPCDFile() provided by PCL. However, it seems that the loaded point cloud does not contain the transformation/reference information, we need to define it in either
camera_link or
base_link. Or else it cannot be shown in rviz because rviz cannot perform a transformation.
// The model is loaded from pcd file, the scene is directly obtained from the camera, rostopic, which is an input of the cloud_cb.
std::string model_path=ros::package::getPath("my_pcl_tutorial");
model_path+="/pcd/milk.pcd";
if (pcl17::io::loadPCDFile (model_path, *model) < 0)
{
std::cout << "Error loading model cloud." << std::endl;
return;
}
model->header.frame_id= "camera_link";
model->header.stamp = ros::Time::now ();
for the scene cloud
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl17::fromROSMsg (*input, *scene);
}
And we publish the rotated_model
pcl17::toROSMsg(*rotated_model,cloud_filtered);
pub.publish (cloud_filtered);
If you are still not clear how to modify the code, leave me a message with your email.
Finally, we are not going to use the visualization module. Because the visualization will spin itself and the call back function stops there...Instead, we need to publish the topic and visualize the results in rviz. So, we can comment out all the
visualization:: class related code. The specific lines which need to be commented are listed below.
//pcl17::visualization::PCLVisualizer viewer ("Correspondence Grouping");
//viewer.addPointCloud (scene, "scene_cloud");
//viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
//viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
//viewer.setPointCloudRenderingProperties (pcl17::visualization::PCL17_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");
//viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
//viewer.setPointCloudRenderingProperties (pcl17::visualization::PCL17_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");
//viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());
//viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ());
// while (!viewer.wasStopped ())
// {
// viewer.spinOnce ();
// }
2. Compilation
The executable for our program is
correspondence_grouping. Here several errors are listed. However, some errors may not be a problem if you have already changed your code according to my suggestion.
fatal error: vtkMatrix4x4.h: No such file or directory
Add in two lines into CMakelists.txt
include_directories(/usr/include/vtk-5.8)
target_link_libraries(correspondence_grouping ${PCL_LIBRARIES} libvtkCommon.so libvtkFiltering.so libvtkRendering.so)
Please refer to
http://answers.ros.org/question/39444/pcl-visualizer-error/?answer=39744#post-id-39744
libboost error
/usr/bin/ld: note: 'boost::system::system_category()' is defined in DSO /usr/lib/libboost_system.so.1.46.1 so try adding it to the linker command line
/usr/lib/libboost_system.so.1.46.1: could not read symbols: Invalid operation
You need to add in one line into CMakelist.txt
rosbuild_link_boost(correspondence_grouping system)
PCL_VISUALIZER_POINT_SIZE not declared in
common.h
This is introduced in the previous section. Change it to
PCL17_VISUALIZER_POINT_SIZE
Please refer to
http://www.pcl-users.org/Correspondence-Grouping-Tutorial-ROS-td4021994.html
3. Show results
Hopefully you can compile you code successfully.
First, publish the point cloud from a .pcd file. Pay attention to the file path
rosrun pcl_ros pcd_to_pointcloud pcd/milk_cartoon_all_small_clorox.pcd 0.2
So the reference for the cloud is by default /base_link.
rosrun my_pcl_tutorial correspondence_grouping input:=/cloud_pcd
And now we can have a look at the results in rviz.
