I am currently applying path planning to my robotic arm (in Gazebo) and have chosen to use an RRT. In order to detect points of collision, I was thinking of getting a Point Cloud from a Kinect subscriber and feeding it to something like an Octomap to have a collision map I could import into Gazebo. However, there is no Gazebo plugin to import Octomap files and I do not have enough experience to write my own. The next idea would be to instead feed this point cloud to a mesh generator (like Meshlab) and turn that into a URDF, but before starting I'd rather get the input of somebody far more experienced. Is this the right way to go? Keep in mind the environment is static, and the only things moving are the arms. Thank you. Below is just a picture of an octomap.enter image description here

  • $\begingroup$ I think converting to URDF (STL meshes from MeshLab should work) is the best option, but I have limited experience with Gazebo. $\endgroup$ Commented Nov 26, 2015 at 10:08
  • $\begingroup$ Thanks, as always! Just wanted to do a check with more experienced people to see if there was an established framework for what I'm trying to do. I'll post my results within the next few days in case anybody's interested. $\endgroup$
    – Iche
    Commented Nov 26, 2015 at 18:09
  • $\begingroup$ Yes please, I don't know if there are any examples of this out there but interesting to see how well it works. $\endgroup$ Commented Nov 26, 2015 at 18:13
  • $\begingroup$ Posted the generated STL. $\endgroup$
    – Iche
    Commented Nov 27, 2015 at 3:33

1 Answer 1


For the most part I was able to get this done. The only difference I chose was to use the Point Cloud library's triangular mesh generator instead of Meshlab's mesh generator mainly because Meshlab's generator is interactive. Below is the image of the Kinect (on top of the Baxter) taking a picture of the shelf, and a picture of the STL put together from the PointCloud the Kinect took.enter image description here

enter image description here

The code I used is as below:

#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/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/vtk_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/io/io.h>
int n = 0;
cloud_cb ()
 // Load input file into a PointCloud<T> with an appropriate type
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PCLPointCloud2 cloud_blob;
  pcl::io::loadPCDFile ("mesh.pcd", cloud_blob);
  pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
  //* the data should be available in cloud

  // Normal estimation*
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud);
  n.setInputCloud (cloud);
  n.setSearchMethod (tree);
  n.setKSearch (20);
  n.compute (*normals);
  //* normals should not contain the point normals + surface curvatures

  // Concatenate the XYZ and normal fields*
  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
  pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
  //* cloud_with_normals = cloud + normals

  // Create search tree*
  pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
  tree2->setInputCloud (cloud_with_normals);

  // Initialize objects
  pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
  pcl::PolygonMesh triangles;

  // Set the maximum distance between connected points (maximum edge length)
  gp3.setSearchRadius (0.025);

  // Set typical values for the parameters
  gp3.setMu (2.5);
  gp3.setMaximumNearestNeighbors (100);
  gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
  gp3.setMinimumAngle(M_PI/18); // 10 degrees
  gp3.setMaximumAngle(2*M_PI/3); // 120 degrees

  // Get result
  gp3.setInputCloud (cloud_with_normals);
  gp3.setSearchMethod (tree2);
  gp3.reconstruct (triangles);

  // Additional vertex information
  std::vector<int> parts = gp3.getPartIDs();
  std::vector<int> states = gp3.getPointStates();
pcl::io::savePolygonFileSTL("mesh.stl", triangles);

void saveFile(const sensor_msgs::PointCloud2::ConstPtr& input)
if(n <= 1)
 pcl::PCLPointCloud2::Ptr pcl_input_cloud(new pcl::PCLPointCloud2);
     pcl_conversions::toPCL(*input, *pcl_input_cloud);
    pcl::io::savePCDFile("mesh.pcd", *pcl_input_cloud);


main (int argc, char** argv)
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;
    ros::Rate loop_late(1);
  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe("/cameras/kinect/depth/points", 1, saveFile);

return 0;

Most of the code is copied exactly from http://pointclouds.org/documentation/tutorials/greedy_projection.php . I'll have to find a way to get a better 3D reconstruction of the shelf considering there are gaps in the levels.

  • 1
    $\begingroup$ Awesome, thanks for sharing! Since the Kinect has a dense gridded point cloud, you can definitely fill in those gaps with ease. Not sure how to do it with PCL (probably a way), but it isn't hard to create ASCII STL files automatically from a pointcloud with your own code -- I have done it in MATLAB and can share that if you want. $\endgroup$ Commented Nov 27, 2015 at 3:36
  • $\begingroup$ Whoops, uploaded the wrong code earlier. This one is what worked. And thank you, that would be really helpful! $\endgroup$
    – Iche
    Commented Nov 27, 2015 at 3:52
  • $\begingroup$ Will do, I do notice that you define the maximum distance between points: gp3.setSearchRadius (0.025); -- try making that some big number like 10. $\endgroup$ Commented Nov 27, 2015 at 19:04

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.