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