Fix dependency for hydro compliance

Author: plnegre

pointcloud_tools/CMakeLists.txt

@@ -1,19 +1,24 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(pointcloud_tools)
 
-find_package(catkin REQUIRED COMPONENTS roscpp pcl_ros std_msgs sensor_msgs )
-find_package(PCL REQUIRED )
+find_package(catkin REQUIRED COMPONENTS roscpp pcl_ros std_msgs sensor_msgs nav_msgs)
+find_package(PCL REQUIRED)
+
+find_package(Boost REQUIRED COMPONENTS signals filesystem system)
+include_directories(${Boost_INCLUDE_DIRS})
 
 catkin_package()
 
 include_directories(/usr/include/vtk-5.8 ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
 
 add_executable(pointcloud_mapper src/pointcloud_mapper.cpp)
+add_executable(pointcloud_mapper_for_slam src/pointcloud_mapper_for_slam.cpp)
 add_executable(pointcloud_filtering src/pointcloud_filtering.cpp)
 add_executable(pcd_publisher src/pcd_publisher.cpp)
 add_executable(pointcloud_viewer src/pointcloud_viewer.cpp)
 
-target_link_libraries(pointcloud_viewer ${PCL_LIBRARIES} ${catkin_LIBRARIES} libvtkCommon.so libvtkFiltering.so libvtkRendering.so)
+target_link_libraries(pointcloud_viewer ${Boost_LIBRARIES} ${PCL_LIBRARIES} ${catkin_LIBRARIES} libvtkCommon.so libvtkFiltering.so libvtkRendering.so)
 target_link_libraries(pcd_publisher ${PCL_LIBRARIES} ${catkin_LIBRARIES})
 target_link_libraries(pointcloud_mapper ${PCL_LIBRARIES} ${catkin_LIBRARIES})
+target_link_libraries(pointcloud_mapper_for_slam ${PCL_LIBRARIES} ${catkin_LIBRARIES})
 target_link_libraries(pointcloud_filtering ${PCL_LIBRARIES} ${catkin_LIBRARIES})

pointcloud_tools/package.xml

@@ -16,10 +16,12 @@
   <build_depend>pcl_ros</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>pcl_ros</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
 
   <export>
 

pointcloud_tools/src/pointcloud_viewer.cpp

@@ -42,11 +42,14 @@
 #include <boost/thread/mutex.hpp>
 #include <boost/thread.hpp>
 // PCL includes
+#include <pcl/common/centroid.h>
 #include <pcl/point_types.h>
 #include <pcl_ros/point_cloud.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/features/feature.h>
-#include <pcl/common/centroid.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/passthrough.h>
 
 using pcl::visualization::PointCloudColorHandlerGenericField;
 
@@ -59,6 +62,8 @@ typedef pcl::PointCloud<PointRGB> PointCloudRGB;
 sensor_msgs::PointCloud2ConstPtr cloud_, cloud_old_;
 boost::mutex m;
 bool viewer_initialized_;
+bool save_cloud_;
+std::string pcd_filename_;
 int counter_;
 
 void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& cloud)
@@ -71,6 +76,30 @@ void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& cloud)
   m.unlock();
 }
 
+PointCloudRGB::Ptr filter(PointCloudRGB::Ptr cloud, double voxel_size)
+{
+  PointCloudRGB::Ptr cloud_filtered_ptr(new PointCloudRGB);
+
+  // Downsampling using voxel grid
+  pcl::VoxelGrid<PointRGB> grid_;
+  PointCloudRGB::Ptr cloud_downsampled_ptr(new PointCloudRGB);
+
+  grid_.setLeafSize(voxel_size,
+                    voxel_size,
+                    voxel_size);
+  grid_.setDownsampleAllData(true);
+  grid_.setInputCloud(cloud);
+  grid_.filter(*cloud_downsampled_ptr);
+
+  return cloud_downsampled_ptr;
+}
+
+void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* nothing)
+{
+  if (event.getKeySym() == "space" && event.keyDown())
+    save_cloud_ = true;
+}
+
 void updateVisualization()
 {
   PointCloud                    cloud_xyz;
@@ -80,13 +109,15 @@ void updateVisualization()
 
   ros::WallDuration d(0.01);
   bool rgb = false;
+  //std::vector<sensor_msgs::PointField> fields;
   std::vector<pcl::PCLPointField> fields;
 
   // Create the visualizer
   pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer");
 
   // Add a coordinate system to screen
   viewer.addCoordinateSystem(0.1);
+  viewer.registerKeyboardCallback(&keyboardEventOccurred);
 
   while(true)
   {
@@ -127,25 +158,35 @@ void updateVisualization()
       // Initialize the camera view
       if(!viewer_initialized_)
       {
-        computeMeanAndCovarianceMatrix(cloud_xyz_rgb, covariance_matrix, xyz_centroid);
+        pcl::computeMeanAndCovarianceMatrix(cloud_xyz_rgb, covariance_matrix, xyz_centroid);
         viewer.initCameraParameters();
         viewer.setCameraPosition(xyz_centroid(0), xyz_centroid(1), xyz_centroid(2)+3.0, 0, -1, 0);
         ROS_INFO_STREAM("[PointCloudViewer:] Point cloud viewer camera initialized in: [" << 
           xyz_centroid(0) << ", " << xyz_centroid(1) << ", " << xyz_centroid(2)+3.0 << "]");
         viewer_initialized_ = true;
       }
-
       // Show the point cloud
       pcl::visualization::PointCloudColorHandlerRGBField<PointRGB> color_handler(
         cloud_xyz_rgb.makeShared());
       viewer.addPointCloud(cloud_xyz_rgb.makeShared(), color_handler, "cloud");
+
+      // Save pcd
+      if (save_cloud_ && cloud_xyz_rgb.size() > 0)
+      {
+        if (pcl::io::savePCDFile(pcd_filename_, cloud_xyz_rgb) == 0)
+          ROS_INFO_STREAM("[PointCloudViewer:] Pointcloud saved into: " << pcd_filename_);
+        else 
+          ROS_ERROR_STREAM("[PointCloudViewer:] Problem saving " << pcd_filename_.c_str());
+        save_cloud_ = false;
+      }
+
     }
     else
     {
       // Initialize the camera view
       if(!viewer_initialized_)
       {
-        computeMeanAndCovarianceMatrix(cloud_xyz_rgb, covariance_matrix, xyz_centroid);
+        pcl::computeMeanAndCovarianceMatrix(cloud_xyz_rgb, covariance_matrix, xyz_centroid);
         viewer.initCameraParameters();
         viewer.setCameraPosition(xyz_centroid(0), xyz_centroid(1), xyz_centroid(2)+3.0, 0, -1, 0);
         ROS_INFO_STREAM("[PointCloudViewer:] Point cloud viewer camera initialized in: [" << 
@@ -170,8 +211,17 @@ void updateVisualization()
         pcl::visualization::PointCloudColorHandlerCustom<Point> color_handler(
         cloud_xyz.makeShared(), 255, 0, 255);
       }
-
       viewer.addPointCloud(cloud_xyz.makeShared(), color_handler, "cloud");
+
+      // Save pcd
+      if (save_cloud_ && cloud_xyz.size() > 0)
+      {
+        if (pcl::io::savePCDFile(pcd_filename_, cloud_xyz) == 0)
+          ROS_INFO_STREAM("[PointCloudViewer:] Pointcloud saved into: " << pcd_filename_);
+        else 
+          ROS_ERROR_STREAM("[PointCloudViewer:] Problem saving " << pcd_filename_.c_str());
+        save_cloud_ = false;
+      }
     }
 
     counter_++;
@@ -188,9 +238,14 @@ int main(int argc, char** argv)
 {
   ros::init(argc, argv, "pointcloud_viewer", ros::init_options::NoSigintHandler);
   ros::NodeHandle nh;
+  ros::NodeHandle nh_priv("~");
   viewer_initialized_ = false;
+  save_cloud_ = false;
   counter_ = 0;
 
+  // Read parameters
+  nh_priv.param("pcd_filename", pcd_filename_, std::string("pointcloud_file.pcd"));
+
   // Create a ROS subscriber
   ros::Subscriber sub = nh.subscribe("input", 30, cloud_cb);