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/segmentation/segment_differences.h"
00040 #include <pcl/io/io.h>
00041
00043 void
00044 pcl_ros::SegmentDifferences::onInit ()
00045 {
00046
00047 PCLNodelet::onInit ();
00048
00049 pub_output_ = pnh_->advertise<PointCloud> ("output", max_queue_size_);
00050
00051
00052 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00053 sub_target_filter_.subscribe (*pnh_, "target", max_queue_size_);
00054
00055
00056 srv_ = boost::make_shared <dynamic_reconfigure::Server<SegmentDifferencesConfig> > (*pnh_);
00057 dynamic_reconfigure::Server<SegmentDifferencesConfig>::CallbackType f = boost::bind (&SegmentDifferences::config_callback, this, _1, _2);
00058 srv_->setCallback (f);
00059
00060 if (approximate_sync_)
00061 {
00062 sync_input_target_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud, PointCloud> > > (max_queue_size_);
00063 sync_input_target_a_->connectInput (sub_input_filter_, sub_target_filter_);
00064 sync_input_target_a_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
00065 }
00066 else
00067 {
00068 sync_input_target_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud, PointCloud> > > (max_queue_size_);
00069 sync_input_target_e_->connectInput (sub_input_filter_, sub_target_filter_);
00070 sync_input_target_e_->registerCallback (bind (&SegmentDifferences::input_target_callback, this, _1, _2));
00071 }
00072
00073 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created with the following parameters:\n"
00074 " - max_queue_size : %d",
00075 getName ().c_str (),
00076 max_queue_size_);
00077 }
00078
00080 void
00081 pcl_ros::SegmentDifferences::config_callback (SegmentDifferencesConfig &config, uint32_t level)
00082 {
00083 if (impl_.getDistanceThreshold () != config.distance_threshold)
00084 {
00085 impl_.setDistanceThreshold (config.distance_threshold);
00086 NODELET_DEBUG ("[%s::config_callback] Setting new distance threshold to: %f.", getName ().c_str (), config.distance_threshold);
00087 }
00088 }
00089
00090
00092 void
00093 pcl_ros::SegmentDifferences::input_target_callback (const PointCloudConstPtr &cloud,
00094 const PointCloudConstPtr &cloud_target)
00095 {
00096 if (pub_output_.getNumSubscribers () <= 0)
00097 return;
00098
00099 if (!isValid (cloud) || !isValid (cloud_target, "target"))
00100 {
00101 NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
00102 PointCloud output;
00103 output.header = cloud->header;
00104 pub_output_.publish (output.makeShared ());
00105 return;
00106 }
00107
00108 NODELET_DEBUG ("[%s::input_indices_callback]\n"
00109 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00110 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
00111 getName ().c_str (),
00112 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00113 cloud_target->width * cloud_target->height, pcl::getFieldsList (*cloud_target).c_str (), cloud_target->header.stamp.toSec (), cloud_target->header.frame_id.c_str (), pnh_->resolveName ("target").c_str ());
00114
00115 impl_.setInputCloud (cloud);
00116 impl_.setTargetCloud (cloud_target);
00117
00118 PointCloud output;
00119 impl_.segment (output);
00120
00121 pub_output_.publish (output.makeShared ());
00122 NODELET_DEBUG ("[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", getName ().c_str (),
00123 output.points.size (), output.header.stamp.toSec (), pnh_->resolveName ("output").c_str ());
00124 }
00125
00126 typedef pcl_ros::SegmentDifferences SegmentDifferences;
00127 PLUGINLIB_DECLARE_CLASS (pcl, SegmentDifferences, SegmentDifferences, nodelet::Nodelet);
00128