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 #include <pluginlib/class_list_macros.h>
00039 #include <pcl/io/io.h>
00040 #include "pcl_ros/transforms.h"
00041 #include "pcl_ros/io/concatenate_data.h"
00042
00044 void
00045 pcl_ros::PointCloudConcatenateDataSynchronizer::onInit ()
00046 {
00047 private_nh_ = getMTPrivateNodeHandle ();
00048
00049 private_nh_.getParam ("output_frame", output_frame_);
00050 private_nh_.getParam ("approximate_sync", approximate_sync_);
00051
00052 if (output_frame_.empty ())
00053 {
00054 NODELET_ERROR ("[onInit] Need an 'output_frame' parameter to be set before continuing!");
00055 return;
00056 }
00057
00058
00059 private_nh_.getParam ("max_queue_size", maximum_queue_size_);
00060
00061
00062 pub_output_ = private_nh_.advertise<PointCloud2> ("output", maximum_queue_size_);
00063
00064 XmlRpc::XmlRpcValue input_topics;
00065 if (!private_nh_.getParam ("input_topics", input_topics))
00066 {
00067 NODELET_ERROR ("[onInit] Need a 'input_topics' parameter to be set before continuing!");
00068 return;
00069 }
00070
00071 switch (input_topics.getType ())
00072 {
00073 case XmlRpc::XmlRpcValue::TypeArray:
00074 {
00075 if (input_topics.size () == 1)
00076 {
00077 NODELET_ERROR ("[onInit] Only one topic given. Need at least two topics to continue.");
00078 return;
00079 }
00080
00081 if (input_topics.size () > 8)
00082 {
00083 NODELET_ERROR ("[onInit] More than 8 topics passed!");
00084 return;
00085 }
00086
00087 ROS_INFO_STREAM ("[onInit] Subscribing to " << input_topics.size () << " user given topics as inputs:");
00088 for (int d = 0; d < input_topics.size (); ++d)
00089 ROS_INFO_STREAM (" - " << (std::string)(input_topics[d]));
00090
00091
00092 filters_.resize (input_topics.size ());
00093
00094
00095 if (approximate_sync_)
00096 ts_a_.reset (new message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, PointCloud2, PointCloud2,
00097 PointCloud2, PointCloud2, PointCloud2,
00098 PointCloud2, PointCloud2>
00099 > (maximum_queue_size_));
00100 else
00101 ts_e_.reset (new message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, PointCloud2, PointCloud2,
00102 PointCloud2, PointCloud2, PointCloud2,
00103 PointCloud2, PointCloud2>
00104 > (maximum_queue_size_));
00105
00106
00107 for (int d = 0; d < input_topics.size (); ++d)
00108 {
00109 filters_[d].reset (new message_filters::Subscriber<PointCloud2> ());
00110 filters_[d]->subscribe (private_nh_, (std::string)(input_topics[d]), maximum_queue_size_);
00111 }
00112
00113
00114 filters_[0]->registerCallback (bind (&PointCloudConcatenateDataSynchronizer::input_callback, this, _1));
00115
00116 switch (input_topics.size ())
00117 {
00118 case 2:
00119 {
00120 if (approximate_sync_)
00121 ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
00122 else
00123 ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
00124 break;
00125 }
00126 case 3:
00127 {
00128 if (approximate_sync_)
00129 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
00130 else
00131 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
00132 break;
00133 }
00134 case 4:
00135 {
00136 if (approximate_sync_)
00137 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
00138 else
00139 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
00140 break;
00141 }
00142 case 5:
00143 {
00144 if (approximate_sync_)
00145 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
00146 else
00147 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
00148 break;
00149 }
00150 case 6:
00151 {
00152 if (approximate_sync_)
00153 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
00154 else
00155 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
00156 break;
00157 }
00158 case 7:
00159 {
00160 if (approximate_sync_)
00161 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
00162 else
00163 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
00164 break;
00165 }
00166 case 8:
00167 {
00168 if (approximate_sync_)
00169 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
00170 else
00171 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
00172 break;
00173 }
00174 default:
00175 {
00176 NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
00177 return;
00178 }
00179 }
00180 break;
00181 }
00182 default:
00183 {
00184 NODELET_ERROR ("[onInit] Invalid 'input_topics' parameter given!");
00185 return;
00186 }
00187 }
00188
00189 if (approximate_sync_)
00190 ts_a_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
00191 else
00192 ts_e_->registerCallback (boost::bind (&PointCloudConcatenateDataSynchronizer::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
00193 }
00194
00196 void
00197 pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (const PointCloud2 &in1, const PointCloud2 &in2, PointCloud2 &out)
00198 {
00199
00200 PointCloud2::Ptr in1_t (new PointCloud2 ());
00201 PointCloud2::Ptr in2_t (new PointCloud2 ());
00202
00203
00204 if (output_frame_ != in1.header.frame_id)
00205 pcl_ros::transformPointCloud (output_frame_, in1, *in1_t, tf_);
00206 else
00207 in1_t = boost::make_shared<PointCloud2> (in1);
00208
00209 if (output_frame_ != in2.header.frame_id)
00210 pcl_ros::transformPointCloud (output_frame_, in2, *in2_t, tf_);
00211 else
00212 in2_t = boost::make_shared<PointCloud2> (in2);
00213
00214
00215 pcl::concatenatePointCloud (*in1_t, *in2_t, out);
00216 }
00217
00219 void
00220 pcl_ros::PointCloudConcatenateDataSynchronizer::input (
00221 const PointCloud2::ConstPtr &in1, const PointCloud2::ConstPtr &in2,
00222 const PointCloud2::ConstPtr &in3, const PointCloud2::ConstPtr &in4,
00223 const PointCloud2::ConstPtr &in5, const PointCloud2::ConstPtr &in6,
00224 const PointCloud2::ConstPtr &in7, const PointCloud2::ConstPtr &in8)
00225 {
00226 PointCloud2::Ptr out1 (new PointCloud2 ());
00227 PointCloud2::Ptr out2 (new PointCloud2 ());
00228 pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*in1, *in2, *out1);
00229 if (in3)
00230 {
00231 pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in3, *out2);
00232 out1 = out2;
00233 if (in4)
00234 {
00235 pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in4, *out1);
00236 if (in5)
00237 {
00238 pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in5, *out2);
00239 out1 = out2;
00240 if (in6)
00241 {
00242 pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out2, *in6, *out1);
00243 if (in7)
00244 {
00245 pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds (*out1, *in7, *out2);
00246 out1 = out2;
00247 }
00248 }
00249 }
00250 }
00251 }
00252 pub_output_.publish (boost::make_shared<PointCloud2> (*out1));
00253 }
00254
00255 typedef pcl_ros::PointCloudConcatenateDataSynchronizer PointCloudConcatenateDataSynchronizer;
00256 PLUGINLIB_DECLARE_CLASS (pcl, PointCloudConcatenateDataSynchronizer, PointCloudConcatenateDataSynchronizer, nodelet::Nodelet);
00257
