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/surface/moving_least_squares.h"
00040 #include <pcl/io/io.h>
00042 void
00043 pcl_ros::MovingLeastSquares::onInit ()
00044 {
00045 PCLNodelet::onInit ();
00046
00047
00048 pub_output_ = pnh_->advertise<PointCloudIn> ("output", max_queue_size_);
00049 pub_normals_ = pnh_->advertise<NormalCloudOut> ("normals", max_queue_size_);
00050
00051
00052 if (!pnh_->getParam ("search_radius", search_radius_))
00053 {
00054
00055 NODELET_ERROR ("[%s::onInit] Need a 'search_radius' parameter to be set before continuing!", getName ().c_str ());
00056 return;
00057 }
00058 if (!pnh_->getParam ("spatial_locator", spatial_locator_type_))
00059 {
00060 NODELET_ERROR ("[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", getName ().c_str ());
00061 return;
00062 }
00063
00064
00065 srv_ = boost::make_shared<dynamic_reconfigure::Server<MLSConfig> > (*pnh_);
00066 dynamic_reconfigure::Server<MLSConfig>::CallbackType f = boost::bind (&MovingLeastSquares::config_callback, this, _1, _2 );
00067 srv_->setCallback (f);
00068
00069
00070 pnh_->getParam ("use_indices", use_indices_);
00071
00072
00073 if (use_indices_)
00074 {
00075
00076 sub_input_filter_.subscribe (*pnh_, "input", 1);
00077
00078 sub_indices_filter_.subscribe (*pnh_, "indices", 1);
00079
00080 if (approximate_sync_)
00081 {
00082 sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn, PointIndices> > >(max_queue_size_);
00083
00084 sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
00085 sync_input_indices_a_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
00086 }
00087 else
00088 {
00089 sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn, PointIndices> > >(max_queue_size_);
00090
00091 sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
00092 sync_input_indices_e_->registerCallback (bind (&MovingLeastSquares::input_indices_callback, this, _1, _2));
00093 }
00094 }
00095 else
00096
00097 sub_input_ = pnh_->subscribe<PointCloudIn> ("input", 1, bind (&MovingLeastSquares::input_indices_callback, this, _1, PointIndicesConstPtr ()));
00098
00099
00100 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00101 " - use_indices : %s",
00102 getName ().c_str (),
00103 (use_indices_) ? "true" : "false");
00104 }
00105
00107 void
00108 pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud,
00109 const PointIndicesConstPtr &indices)
00110 {
00111
00112 if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0)
00113 return;
00114
00115
00116 PointCloudIn output;
00117
00118
00119 NormalCloudOut::Ptr normals (new NormalCloudOut ());
00120
00121
00122 if (!isValid (cloud))
00123 {
00124 NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
00125 output.header = cloud->header;
00126 pub_output_.publish (output.makeShared ());
00127 return;
00128 }
00129
00130 if (indices && !isValid (indices, "indices"))
00131 {
00132 NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
00133 output.header = cloud->header;
00134 pub_output_.publish (output.makeShared ());
00135 return;
00136 }
00137
00139 if (indices)
00140 NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
00141 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00142 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00143 getName ().c_str (),
00144 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
00145 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
00146 else
00147 NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
00149
00150
00151 impl_.setInputCloud (cloud);
00152
00153 IndicesPtr indices_ptr;
00154 if (indices)
00155 indices_ptr.reset (new std::vector<int> (indices->indices));
00156
00157 impl_.setIndices (indices_ptr);
00158
00159
00160
00161
00162
00163
00164
00165
00166 output.header = cloud->header;
00167 pub_output_.publish (output.makeShared ());
00168 normals->header = cloud->header;
00169 pub_normals_.publish (normals);
00170 }
00171
00173 void
00174 pcl_ros::MovingLeastSquares::config_callback (MLSConfig &config, uint32_t level)
00175 {
00176
00177
00178
00179
00180
00181
00182 if (search_radius_ != config.search_radius)
00183 {
00184 search_radius_ = config.search_radius;
00185 NODELET_DEBUG ("[config_callback] Setting the search radius: %f.", search_radius_);
00186 impl_.setSearchRadius (search_radius_);
00187 }
00188 if (spatial_locator_type_ != config.spatial_locator)
00189 {
00190 spatial_locator_type_ = config.spatial_locator;
00191 NODELET_DEBUG ("[config_callback] Setting the spatial locator to type: %d.", spatial_locator_type_);
00192 }
00193 if (use_polynomial_fit_ != config.use_polynomial_fit)
00194 {
00195 use_polynomial_fit_ = config.use_polynomial_fit;
00196 NODELET_DEBUG ("[config_callback] Setting the use_polynomial_fit flag to: %d.", use_polynomial_fit_);
00197 impl_.setPolynomialFit (use_polynomial_fit_);
00198 }
00199 if (polynomial_order_ != config.polynomial_order)
00200 {
00201 polynomial_order_ = config.polynomial_order;
00202 NODELET_DEBUG ("[config_callback] Setting the polynomial order to: %d.", polynomial_order_);
00203 impl_.setPolynomialOrder (polynomial_order_);
00204 }
00205 if (gaussian_parameter_ != config.gaussian_parameter)
00206 {
00207 gaussian_parameter_ = config.gaussian_parameter;
00208 NODELET_DEBUG ("[config_callback] Setting the gaussian parameter to: %f.", gaussian_parameter_);
00209 impl_.setSqrGaussParam (gaussian_parameter_ * gaussian_parameter_);
00210 }
00211 }
00212
00213 typedef pcl_ros::MovingLeastSquares MovingLeastSquares;
00214 PLUGINLIB_DECLARE_CLASS (pcl, MovingLeastSquares, MovingLeastSquares, nodelet::Nodelet);