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 <pcl/io/io.h>
00039 #include "pcl_ros/transforms.h"
00040 #include "pcl_ros/filters/filter.h"
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00063 void
00064 pcl_ros::Filter::computePublish (const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
00065 {
00066 PointCloud2 output;
00067
00068 filter (input, indices, output);
00069
00070 PointCloud2::Ptr cloud_tf (new PointCloud2 (output));
00071
00072 if (!tf_output_frame_.empty () && output.header.frame_id != tf_output_frame_)
00073 {
00074 NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ());
00075
00076 PointCloud2 cloud_transformed;
00077 if (!pcl_ros::transformPointCloud (tf_output_frame_, output, cloud_transformed, tf_listener_))
00078 {
00079 NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_output_frame_.c_str ());
00080 return;
00081 }
00082 cloud_tf.reset (new PointCloud2 (cloud_transformed));
00083 }
00084 if (tf_output_frame_.empty () && output.header.frame_id != tf_input_orig_frame_)
00085
00086 {
00087 NODELET_DEBUG ("[%s::computePublish] Transforming output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ());
00088
00089 PointCloud2 cloud_transformed;
00090 if (!pcl_ros::transformPointCloud (tf_input_orig_frame_, output, cloud_transformed, tf_listener_))
00091 {
00092 NODELET_ERROR ("[%s::computePublish] Error converting output dataset from %s back to %s.", getName ().c_str (), output.header.frame_id.c_str (), tf_input_orig_frame_.c_str ());
00093 return;
00094 }
00095 cloud_tf.reset (new PointCloud2 (cloud_transformed));
00096 }
00097
00098
00099 pub_output_.publish (cloud_tf);
00100 }
00101
00103 void
00104 pcl_ros::Filter::onInit ()
00105 {
00106
00107 PCLNodelet::onInit ();
00108
00109
00110 bool has_service = false;
00111 if (!child_init (*pnh_, has_service))
00112 {
00113 NODELET_ERROR ("[%s::onInit] Initialization failed.", getName ().c_str ());
00114 return;
00115 }
00116
00117 pub_output_ = pnh_->advertise<PointCloud2> ("output", max_queue_size_);
00118
00119
00120 if (!has_service)
00121 {
00122 srv_ = boost::make_shared <dynamic_reconfigure::Server<pcl_ros::FilterConfig> > (*pnh_);
00123 dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind (&Filter::config_callback, this, _1, _2);
00124 srv_->setCallback (f);
00125 }
00126
00127
00128 if (use_indices_)
00129 {
00130
00131 sub_input_filter_.subscribe (*pnh_, "input", max_queue_size_);
00132 sub_indices_filter_.subscribe (*pnh_, "indices", max_queue_size_);
00133
00134 if (approximate_sync_)
00135 {
00136 sync_input_indices_a_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, pcl::PointIndices> > >(max_queue_size_);
00137 sync_input_indices_a_->connectInput (sub_input_filter_, sub_indices_filter_);
00138 sync_input_indices_a_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
00139 }
00140 else
00141 {
00142 sync_input_indices_e_ = boost::make_shared <message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, pcl::PointIndices> > >(max_queue_size_);
00143 sync_input_indices_e_->connectInput (sub_input_filter_, sub_indices_filter_);
00144 sync_input_indices_e_->registerCallback (bind (&Filter::input_indices_callback, this, _1, _2));
00145 }
00146 }
00147 else
00148
00149 sub_input_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_queue_size_, bind (&Filter::input_indices_callback, this, _1, pcl::PointIndicesConstPtr ()));
00150
00151 NODELET_DEBUG ("[%s::onInit] Nodelet successfully created.", getName ().c_str ());
00152 }
00153
00155 void
00156 pcl_ros::Filter::config_callback (pcl_ros::FilterConfig &config, uint32_t level)
00157 {
00158
00159 if (tf_input_frame_ != config.input_frame)
00160 {
00161 tf_input_frame_ = config.input_frame;
00162 NODELET_DEBUG ("[%s::config_callback] Setting the input TF frame to: %s.", getName ().c_str (), tf_input_frame_.c_str ());
00163 }
00164 if (tf_output_frame_ != config.output_frame)
00165 {
00166 tf_output_frame_ = config.output_frame;
00167 NODELET_DEBUG ("[%s::config_callback] Setting the output TF frame to: %s.", getName ().c_str (), tf_output_frame_.c_str ());
00168 }
00169 }
00170
00172 void
00173 pcl_ros::Filter::input_indices_callback (const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices)
00174 {
00175
00176 if (pub_output_.getNumSubscribers () <= 0)
00177 return;
00178
00179
00180 if (!isValid (cloud))
00181 {
00182 NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
00183 return;
00184 }
00185
00186 if (indices && !isValid (indices))
00187 {
00188 NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
00189 return;
00190 }
00191
00193 if (indices)
00194 NODELET_DEBUG ("[%s::input_indices_callback]\n"
00195 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
00196 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
00197 getName ().c_str (),
00198 cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str (),
00199 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), pnh_->resolveName ("indices").c_str ());
00200 else
00201 NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.frame_id.c_str (), pnh_->resolveName ("input").c_str ());
00203
00204
00205 tf_input_orig_frame_ = cloud->header.frame_id;
00206 PointCloud2::ConstPtr cloud_tf;
00207 if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
00208 {
00209 NODELET_DEBUG ("[%s::input_indices_callback] Transforming input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
00210
00211
00212 PointCloud2 cloud_transformed;
00213 if (!pcl_ros::transformPointCloud (tf_input_frame_, *cloud, cloud_transformed, tf_listener_))
00214 {
00215 NODELET_ERROR ("[%s::input_indices_callback] Error converting input dataset from %s to %s.", getName ().c_str (), cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
00216 return;
00217 }
00218 cloud_tf = boost::make_shared<PointCloud2> (cloud_transformed);
00219 }
00220 else
00221 cloud_tf = cloud;
00222
00223
00224 IndicesPtr vindices;
00225 if (indices)
00226 vindices.reset (new std::vector<int> (indices->indices));
00227
00228 computePublish (cloud_tf, vindices);
00229 }
00230