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/transforms.h"
00040 #include "pcl_ros/segmentation/extract_polygonal_prism_data.h"
00041 #include <pcl/io/io.h>
00042
00044 void
00045 pcl_ros::ExtractPolygonalPrismData::onInit ()
00046 {
00047
00048 PCLNodelet::onInit ();
00049
00050 sub_hull_filter_.subscribe (*pnh_, "planar_hull", max_queue_size_);
00051 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00052
00053
00054 srv_ = boost::make_shared <dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > (*pnh_);
00055 dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig>::CallbackType f = boost::bind (&ExtractPolygonalPrismData::config_callback, this, _1, _2);
00056 srv_->setCallback (f);
00057
00058
00059 if (approximate_sync_)
00060 sync_input_hull_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
00061 else
00062 sync_input_hull_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud, PointIndices> > > (max_queue_size_);
00063
00064 if (use_indices_)
00065 {
00066 sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00067 if (approximate_sync_)
00068 sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
00069 else
00070 sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, sub_indices_filter_);
00071 }
00072 else
00073 {
00074 sub_input_filter_.registerCallback (bind (&ExtractPolygonalPrismData::input_callback, this, _1));
00075
00076 if (approximate_sync_)
00077 sync_input_hull_indices_a_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
00078 else
00079 sync_input_hull_indices_e_->connectInput (sub_input_filter_, sub_hull_filter_, nf_);
00080 }
00081
00082 if (approximate_sync_)
00083 sync_input_hull_indices_a_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
00084 else
00085 sync_input_hull_indices_e_->registerCallback (bind (&ExtractPolygonalPrismData::input_hull_indices_callback, this, _1, _2, _3));
00086
00087
00088 pub_output_ = pnh_->advertise<PointIndices> ("output", max_queue_size_);
00089 }
00090
00092 void
00093 pcl_ros::ExtractPolygonalPrismData::config_callback (ExtractPolygonalPrismDataConfig &config, uint32_t level)
00094 {
00095 double height_min, height_max;
00096 impl_.getHeightLimits (height_min, height_max);
00097 if (height_min != config.height_min)
00098 {
00099 height_min = config.height_min;
00100 NODELET_DEBUG ("[%s::config_callback] Setting new minimum height to the planar model to: %f.", getName ().c_str (), height_min);
00101 impl_.setHeightLimits (height_min, height_max);
00102 }
00103 if (height_max != config.height_max)
00104 {
00105 height_max = config.height_max;
00106 NODELET_DEBUG ("[%s::config_callback] Setting new maximum height to the planar model to: %f.", getName ().c_str (), height_max);
00107 impl_.setHeightLimits (height_min, height_max);
00108 }
00109 }
00110
00112 void
00113 pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback (
00114 const PointCloudConstPtr &cloud,
00115 const PointCloudConstPtr &hull,
00116 const PointIndicesConstPtr &indices)
00117 {
00118
00119 if (pub_output_.getNumSubscribers () <= 0)
00120 return;
00121
00122
00123 pcl::PointIndices inliers;
00124 inliers.header = cloud->header;
00125
00126
00127 if (!isValid (cloud) || !isValid (hull, "planar_hull"))
00128 {
00129 NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid input!", getName ().c_str ());
00130 pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
00131 return;
00132 }
00133
00134 if (indices && !isValid (indices))
00135 {
00136 NODELET_ERROR ("[%s::input_hull_indices_callback] Invalid indices!", getName ().c_str ());
00137 pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
00138 return;
00139 }
00140
00142 if (indices)
00143 NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
00144 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00145 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00146 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00147 getName ().c_str (),
00148 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00149 hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str (),
00150 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00151 else
00152 NODELET_DEBUG ("[%s::input_indices_hull_callback]\n"
00153 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00154 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00155 getName ().c_str (),
00156 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00157 hull->width * hull->height, pcl::getFieldsList (*hull).c_str (), hull->header.stamp.toSec (), hull->header.frame_id.c_str (), pnh_->resolveName ("planar_hull").c_str ());
00159
00160 if (cloud->header.frame_id != hull->header.frame_id)
00161 {
00162 NODELET_DEBUG ("[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.", getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ());
00163 PointCloud planar_hull;
00164 if (!pcl_ros::transformPointCloud (cloud->header.frame_id, *hull, planar_hull, tf_listener_))
00165 {
00166
00167 pub_output_.publish (boost::make_shared<const pcl::PointIndices> (inliers));
00168 return;
00169 }
00170 impl_.setInputPlanarHull (planar_hull.makeShared ());
00171 }
00172 else
00173 impl_.setInputPlanarHull (hull);
00174
00175 IndicesPtr indices_ptr;
00176 if (indices && !indices->header.frame_id.empty ())
00177 indices_ptr.reset (new std::vector<int> (indices->indices));
00178
00179 impl_.setInputCloud (cloud);
00180 impl_.setIndices (indices_ptr);
00181
00182
00183 if (!cloud->points.empty ())
00184 impl_.segment (inliers);
00185
00186 inliers.header = cloud->header;
00187 pub_output_.publish (boost::make_shared<const PointIndices> (inliers));
00188 NODELET_DEBUG ("[%s::input_hull_callback] Publishing %zu indices.", getName ().c_str (), inliers.indices.size ());
00189 }
00190
00191 typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData;
00192 PLUGINLIB_DECLARE_CLASS (pcl, ExtractPolygonalPrismData, ExtractPolygonalPrismData, nodelet::Nodelet);
00193