56 NODELET_ERROR (
"[onInit] Need an 'output_frame' parameter to be set before continuing!");
62 NODELET_ERROR (
"[onInit] Need a 'input_topics' parameter to be set before continuing!");
67 NODELET_ERROR (
"[onInit] Invalid 'input_topics' parameter given!");
72 NODELET_ERROR (
"[onInit] Only one topic given. Need at least two topics to continue.");
94 ROS_INFO_STREAM (
"Subscribing to " << input_topics_.size () <<
" user given topics as inputs:");
95 for (
int d = 0;
d < input_topics_.size (); ++
d)
99 filters_.resize (input_topics_.size ());
102 if (approximate_sync_)
106 > (maximum_queue_size_));
111 > (maximum_queue_size_));
114 for (
int d = 0;
d < input_topics_.size (); ++
d)
117 filters_[
d]->subscribe (*pnh_, (std::string)(input_topics_[
d]), maximum_queue_size_);
123 switch (input_topics_.size ())
127 if (approximate_sync_)
128 ts_a_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
130 ts_e_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
135 if (approximate_sync_)
136 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
138 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
143 if (approximate_sync_)
144 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
146 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
151 if (approximate_sync_)
152 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
154 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
159 if (approximate_sync_)
160 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
162 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
167 if (approximate_sync_)
168 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
170 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
175 if (approximate_sync_)
176 ts_a_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
178 ts_e_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
188 if (approximate_sync_)
198 for (
size_t d = 0;
d < filters_.size (); ++
d)
200 filters_[
d]->unsubscribe ();
213 if (output_frame_ != in1.header.frame_id)
217 NODELET_ERROR (
"[%s::combineClouds] Error converting first input dataset from %s to %s.",
getName ().c_str (), in1.header.frame_id.c_str (), output_frame_.c_str ());
223 in1_t = boost::make_shared<PointCloud2> (in1);
226 if (output_frame_ != in2.header.frame_id)
230 NODELET_ERROR (
"[%s::combineClouds] Error converting second input dataset from %s to %s.",
getName ().c_str (), in2.header.frame_id.c_str (), output_frame_.c_str ());
236 in2_t = boost::make_shared<PointCloud2> (in2);
242 out.header.stamp = in1.header.stamp;
248 const PointCloud2::ConstPtr &in1,
const PointCloud2::ConstPtr &in2,
249 const PointCloud2::ConstPtr &in3,
const PointCloud2::ConstPtr &in4,
250 const PointCloud2::ConstPtr &in5,
const PointCloud2::ConstPtr &in6,
251 const PointCloud2::ConstPtr &in7,
const PointCloud2::ConstPtr &in8)
256 if (in3 && in3->width * in3->height > 0)
260 if (in4 && in4->width * in4->height > 0)
263 if (in5 && in5->width * in5->height > 0)
267 if (in6 && in6->width * in6->height > 0)
270 if (in7 && in7->width * in7->height > 0)
274 if (in8 && in8->width * in8->height > 0)
283 pub_output_.publish (boost::make_shared<PointCloud2> (*out1));