00001 #define PCL_NO_PRECOMPILE 00002 #include "cloud_visualizer.h" 00003 #include <tf2_ros/buffer.h> 00004 #include <deque> 00005 #include <boost/thread/mutex.hpp> 00006 #include <nav_msgs/Path.h> 00007 #include <geometry_msgs/PoseStamped.h> 00008 #include <geometry_msgs/TransformStamped.h> 00009 #include <std_srvs/Empty.h> 00010 #include <pcl_ros/point_cloud.h> 00011 #include <pcl/point_types.h> 00012 #include <pcl/point_cloud.h> 00013 00014 typedef pcl::PointXYZRGB point_t; 00015 typedef pcl::PointCloud<point_t> pointcloud_t; 00016 00017 namespace rc 00018 { 00019 00024 class CloudAccumulator 00025 { 00026 00027 public: 00028 CloudAccumulator(double voxelgrid_size_display, 00029 double voxelgrid_size_save, 00030 double min_distance, 00031 double max_distance, 00032 std::string output_filename, 00033 bool keep_high_res, 00034 bool start_paused); 00035 00045 void pointCloudCallback(const pointcloud_t::ConstPtr& pointcloud); 00046 00048 void trajectoryCallback(const nav_msgs::PathConstPtr& path); 00049 00051 void poseCallback(const geometry_msgs::PoseStampedConstPtr& current_pose); 00052 00059 pointcloud_t::Ptr rebuildCloud(); 00060 00065 bool saveCloud(std_srvs::Empty::Request &req, std_srvs::Empty::Request &resp); 00066 00068 bool togglePause(std_srvs::Empty::Request &req, std_srvs::Empty::Request &resp); 00069 00070 private: 00071 Eigen::Affine3f lookupTransform(double timestamp); 00072 00073 //Members 00074 std::deque<pointcloud_t::Ptr> clouds_; 00075 boost::mutex tf_buffer_mutex_; 00076 tf2_ros::Buffer tf_buffer_; 00077 size_t num_poses_; 00078 boost::mutex display_cloud_mutex_; 00079 pointcloud_t::Ptr display_cloud_; 00080 double voxelgrid_size_display_; 00081 double voxelgrid_size_save_; 00082 double min_distance_; 00083 double max_distance_; 00084 std::string output_filename_; 00085 bool keep_high_res_; 00086 bool pause_; 00087 CloudVisualizer viewer_; 00088 boost::thread viewer_thread_; 00089 ros::Time last_pose_stamp_; 00090 }; 00091 00092 }//namespace rc