cloud_accumulator.h

Go to the documentation of this file.

#define PCL_NO_PRECOMPILE
#include "cloud_visualizer.h"
#include <tf2_ros/buffer.h>
#include <deque>
#include <boost/thread/mutex.hpp>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <std_srvs/Empty.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

typedef pcl::PointXYZRGB point_t;
typedef pcl::PointCloud<point_t> pointcloud_t;

namespace rc
{

class CloudAccumulator
{

  public:
  CloudAccumulator(double voxelgrid_size_display,
                   double voxelgrid_size_save,
                   double min_distance,
                   double max_distance,
                   std::string output_filename,
                   bool keep_high_res,
                   bool start_paused);

  void pointCloudCallback(const pointcloud_t::ConstPtr& pointcloud);

  void trajectoryCallback(const nav_msgs::PathConstPtr& path);

  void poseCallback(const geometry_msgs::PoseStampedConstPtr& current_pose);

  pointcloud_t::Ptr rebuildCloud();

  bool saveCloud(std_srvs::Empty::Request &req, std_srvs::Empty::Request &resp);

  bool togglePause(std_srvs::Empty::Request &req, std_srvs::Empty::Request &resp);

  private:
  Eigen::Affine3f lookupTransform(double timestamp);

  //Members
  std::deque<pointcloud_t::Ptr> clouds_;
  boost::mutex tf_buffer_mutex_;
  tf2_ros::Buffer tf_buffer_;      
  size_t num_poses_;               
  boost::mutex display_cloud_mutex_;
  pointcloud_t::Ptr display_cloud_;
  double voxelgrid_size_display_;  
  double voxelgrid_size_save_;     
  double min_distance_;            
  double max_distance_;            
  std::string output_filename_;    
  bool keep_high_res_;             
  bool pause_;                     
  CloudVisualizer viewer_;
  boost::thread viewer_thread_;
  ros::Time last_pose_stamp_;
};

}//namespace rc