40 #include <pcl/io/io.h> 51 if (!
pnh_->getParam (
"model_type", model_type))
53 NODELET_ERROR (
"[%s::onInit] Need a 'model_type' parameter to be set before continuing!",
getName ().c_str ());
58 bool copy_all_data =
false;
61 bool copy_all_fields =
true;
63 pnh_->getParam (
"copy_all_data", copy_all_data);
64 pnh_->getParam (
"copy_all_fields", copy_all_fields);
71 NODELET_DEBUG (
"[%s::onInit] Nodelet successfully created with the following parameters:\n" 72 " - model_type : %d\n" 73 " - copy_all_data : %s\n" 74 " - copy_all_fields : %s",
76 model_type, (copy_all_data) ?
"true" :
"false", (copy_all_fields) ?
"true" :
"false");
79 impl_.setModelType (model_type);
80 impl_.setCopyAllFields (copy_all_fields);
81 impl_.setCopyAllData (copy_all_data);
142 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 143 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.\n" 144 " - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.",
146 cloud->width * cloud->height,
pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (),
pnh_->resolveName (
"input").c_str (),
147 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (),
pnh_->resolveName (
"inliers").c_str (),
148 model->values.size (), model->header.stamp.toSec (), model->header.frame_id.c_str (),
pnh_->resolveName (
"model").c_str ());
154 vindices.reset (
new std::vector<int> (indices->indices));
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
#define NODELET_ERROR(...)
message_filters::Subscriber< ModelCoefficients > sub_model_
The message filter subscriber for model coefficients.
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
pcl::ProjectInliers< pcl::PCLPointCloud2 > impl_
The PCL filter implementation used.
void unsubscribe()
Lazy transport unsubscribe routine.
const std::string & getName() const
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloud2, PointIndices, ModelCoefficients > > > sync_input_indices_model_a_
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
std::string tf_input_orig_frame_
The original data input TF frame.
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloud2, PointIndices, ModelCoefficients > > > sync_input_indices_model_e_
Synchronized input, indices, and model coefficients.
bool isValid(const PointCloud2::ConstPtr &cloud, const std::string &topic_name="input")
Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-ze...
virtual void onInit()
Nodelet initialization routine.
ModelCoefficientsConstPtr model_
A pointer to the vector of model coefficients.
uint32_t getNumSubscribers() const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
message_filters::Subscriber< PointCloud2 > sub_input_filter_
int max_queue_size_
The maximum queue size (default: 3).
ModelCoefficients::ConstPtr ModelCoefficientsConstPtr
ros::Publisher pub_output_
The output PointCloud publisher.
void input_indices_model_callback(const PointCloud2::ConstPtr &cloud, const PointIndicesConstPtr &indices, const ModelCoefficientsConstPtr &model)
PointCloud2 + Indices + Model data callback.
void subscribe()
NodeletLazy connection routine.
#define NODELET_DEBUG(...)
void computePublish(const PointCloud2::ConstPtr &input, const IndicesPtr &indices)
Call the child filter () method, optionally transform the result, and publish it. ...
PLUGINLIB_EXPORT_CLASS(ProjectInliers, nodelet::Nodelet)
PointIndices::ConstPtr PointIndicesConstPtr
ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a sepa...