40 #include <pcl/io/io.h> 51 NODELET_DEBUG (
"[%s::onInit] Nodelet successfully created with the following parameters:\n" 52 " - max_queue_size : %d",
68 srv_ = boost::make_shared <dynamic_reconfigure::Server<SegmentDifferencesConfig> > (*pnh_);
70 srv_->setCallback (f);
98 if (
impl_.getDistanceThreshold () != config.distance_threshold)
100 impl_.setDistanceThreshold (config.distance_threshold);
101 NODELET_DEBUG (
"[%s::config_callback] Setting new distance threshold to: %f.",
getName ().c_str (), config.distance_threshold);
118 output.header = cloud->header;
124 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 125 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
127 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 (),
128 cloud_target->width * cloud_target->height,
pcl::getFieldsList (*cloud_target).c_str (),
fromPCL(cloud_target->header).stamp.toSec (), cloud_target->header.frame_id.c_str (),
pnh_->resolveName (
"target").c_str ());
130 impl_.setInputCloud (cloud);
131 impl_.setTargetCloud (cloud_target);
134 impl_.segment (output);
137 NODELET_DEBUG (
"[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s",
getName ().c_str (),
138 output.points.size (),
fromPCL(output.header).stamp.toSec (),
pnh_->resolveName (
"output").c_str ());
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
#define NODELET_ERROR(...)
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
void publish(const boost::shared_ptr< M > &message) const
void config_callback(SegmentDifferencesConfig &config, uint32_t level)
Dynamic reconfigure callback.
const std::string & getName() const
SegmentDifferences()
Empty constructor.
void onInit()
Nodelet initialization routine.
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...
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloud, PointCloud > > > sync_input_target_a_
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloud, PointCloud > > > sync_input_target_e_
Synchronized input, and planar hull.
pcl::PointCloud< pcl::PointXYZ > PointCloud
void input_target_callback(const PointCloudConstPtr &cloud, const PointCloudConstPtr &cloud_target)
Input point cloud callback.
pcl::SegmentDifferences< pcl::PointXYZ > impl_
The PCL implementation used.
boost::shared_ptr< dynamic_reconfigure::Server< SegmentDifferencesConfig > > srv_
Pointer to a dynamic reconfigure service.
PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet)
PointCloud::ConstPtr PointCloudConstPtr
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)
message_filters::Subscriber< PointCloud > sub_target_filter_
The message filter subscriber for PointCloud2.
void subscribe()
LazyNodelet connection routine.
bool approximate_sync_
True if we use an approximate time synchronizer versus an exact one (false by default).
int max_queue_size_
The maximum queue size (default: 3).
ros::Publisher pub_output_
The output PointCloud publisher.
#define NODELET_DEBUG(...)
SegmentDifferences obtains the difference between two spatially aligned point clouds and returns the ...
message_filters::Subscriber< PointCloud > sub_input_filter_
The message filter subscriber for PointCloud2.