Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_IO_CONCATENATE_DATA_H_
00039 #define PCL_IO_CONCATENATE_DATA_H_
00040
00041
00042 #include <tf/transform_listener.h>
00043 #include <nodelet/nodelet.h>
00044 #include <message_filters/subscriber.h>
00045 #include <message_filters/synchronizer.h>
00046 #include <message_filters/pass_through.h>
00047 #include <message_filters/sync_policies/exact_time.h>
00048 #include <message_filters/sync_policies/approximate_time.h>
00049
00050 namespace pcl_ros
00051 {
00052 namespace sync_policies = message_filters::sync_policies;
00053
00060 class PointCloudConcatenateDataSynchronizer: public nodelet::Nodelet
00061 {
00062 public:
00063 typedef sensor_msgs::PointCloud2 PointCloud2;
00064 typedef PointCloud2::Ptr PointCloud2Ptr;
00065 typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
00066
00068 PointCloudConcatenateDataSynchronizer () : maximum_queue_size_ (3) {};
00069
00073 PointCloudConcatenateDataSynchronizer (int queue_size) : maximum_queue_size_(queue_size), approximate_sync_(false) {};
00074
00076 virtual ~PointCloudConcatenateDataSynchronizer () {};
00077
00078 void onInit ();
00079
00080 private:
00082 ros::NodeHandle private_nh_;
00083
00085 ros::Publisher pub_output_;
00086
00088 int maximum_queue_size_;
00089
00091 bool approximate_sync_;
00092
00094 std::vector<boost::shared_ptr<message_filters::Subscriber<PointCloud2> > > filters_;
00095
00097 std::string output_frame_;
00098
00100 tf::TransformListener tf_;
00101
00104 message_filters::PassThrough<PointCloud2> nf_;
00105
00109 boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2> > > ts_a_;
00110 boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2, PointCloud2> > > ts_e_;
00111
00116 inline void
00117 input_callback (const PointCloud2ConstPtr &input)
00118 {
00119 PointCloud2 cloud;
00120 cloud.header.stamp = input->header.stamp;
00121 nf_.add (boost::make_shared<PointCloud2> (cloud));
00122 }
00123
00125 void input (const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2,
00126 const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4,
00127 const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6,
00128 const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8);
00129
00130 void combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out);
00131 };
00132 }
00133
00134 #endif //#ifndef PCL_ROS_IO_CONCATENATE_H_