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_ros/filters/project_inliers.h"
00040 #include <pcl/io/io.h>
00041
00043 void
00044 pcl_ros::ProjectInliers::onInit ()
00045 {
00046
00047
00048 PCLNodelet::onInit ();
00049
00050
00051
00052 int model_type;
00053 if (!pnh_->getParam ("model_type", model_type))
00054 {
00055 NODELET_ERROR ("[%s::onInit] Need a 'model_type' parameter to be set before continuing!", getName ().c_str ());
00056 return;
00057 }
00058
00059
00060 bool copy_all_data = false;
00061
00062
00063 bool copy_all_fields = true;
00064
00065 pnh_->getParam ("copy_all_data", copy_all_data);
00066 pnh_->getParam ("copy_all_fields", copy_all_fields);
00067
00068 pub_output_ = pnh_->advertise<PointCloud2> ("output", max_queue_size_);
00069
00070
00071 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00072
00073
00074
00075
00076
00077
00078 sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00079
00080 sub_model_.subscribe (*pnh_, "model", max_queue_size_);
00081
00082 if (approximate_sync_)
00083 {
00084 sync_input_indices_model_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_);
00085 sync_input_indices_model_a_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_);
00086 sync_input_indices_model_a_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3));
00087 }
00088 else
00089 {
00090 sync_input_indices_model_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2, PointIndices, ModelCoefficients> > > (max_queue_size_);
00091 sync_input_indices_model_e_->connectInput (sub_input_filter_, sub_indices_filter_, sub_model_);
00092 sync_input_indices_model_e_->registerCallback (bind (&ProjectInliers::input_indices_model_callback, this, _1, _2, _3));
00093 }
00094
00095 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00096 " - model_type : %d\n"
00097 " - copy_all_data : %s\n"
00098 " - copy_all_fields : %s",
00099 getName ().c_str (),
00100 model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false");
00101
00102
00103 impl_.setModelType (model_type);
00104 impl_.setCopyAllFields (copy_all_fields);
00105 impl_.setCopyAllData (copy_all_data);
00106 }
00107
00109 void
00110 pcl_ros::ProjectInliers::input_indices_model_callback (const PointCloud2::ConstPtr &cloud,
00111 const PointIndicesConstPtr &indices,
00112 const ModelCoefficientsConstPtr &model)
00113 {
00114 if (pub_output_.getNumSubscribers () <= 0)
00115 return;
00116
00117 if (!isValid (model) || !isValid (indices) || !isValid (cloud))
00118 {
00119 NODELET_ERROR ("[%s::input_indices_model_callback] Invalid input!", getName ().c_str ());
00120 return;
00121 }
00122
00123 NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
00124 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00125 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n"
00126 " - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.",
00127 getName ().c_str (),
00128 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00129 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("inliers").c_str (),
00130 model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (), pnh_->resolveName ("model").c_str ());
00131
00132 tf_input_orig_frame_ = cloud->header.frame_id;
00133
00134 IndicesPtr vindices;
00135 if (indices)
00136 vindices.reset (new std::vector<int> (indices->indices));
00137
00138 model_ = model;
00139 computePublish (cloud, vindices);
00140 }
00141
00142 typedef pcl_ros::ProjectInliers ProjectInliers;
00143 PLUGINLIB_EXPORT_CLASS(ProjectInliers,nodelet::Nodelet);
00144