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