39 #include <pcl/io/io.h> 40 #include <pcl/PointIndices.h> 57 double cluster_tolerance;
58 if (!
pnh_->getParam (
"cluster_tolerance", cluster_tolerance))
60 NODELET_ERROR (
"[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!",
getName ().c_str ());
64 if (!
pnh_->getParam (
"spatial_locator", spatial_locator))
66 NODELET_ERROR (
"[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!",
getName ().c_str ());
79 srv_ = boost::make_shared <dynamic_reconfigure::Server<EuclideanClusterExtractionConfig> > (*pnh_);
81 srv_->setCallback (f);
83 NODELET_DEBUG (
"[%s::onInit] Nodelet successfully created with the following parameters:\n" 84 " - max_queue_size : %d\n" 85 " - use_indices : %s\n" 86 " - cluster_tolerance : %f\n",
92 impl_.setClusterTolerance (cluster_tolerance);
143 if (
impl_.getClusterTolerance () != config.cluster_tolerance)
145 impl_.setClusterTolerance (config.cluster_tolerance);
146 NODELET_DEBUG (
"[%s::config_callback] Setting new clustering tolerance to: %f.",
getName ().c_str (), config.cluster_tolerance);
148 if (
impl_.getMinClusterSize () != config.cluster_min_size)
150 impl_.setMinClusterSize (config.cluster_min_size);
151 NODELET_DEBUG (
"[%s::config_callback] Setting the minimum cluster size to: %d.",
getName ().c_str (), config.cluster_min_size);
153 if (
impl_.getMaxClusterSize () != config.cluster_max_size)
155 impl_.setMaxClusterSize (config.cluster_max_size);
156 NODELET_DEBUG (
"[%s::config_callback] Setting the maximum cluster size to: %d.",
getName ().c_str (), config.cluster_max_size);
161 NODELET_DEBUG (
"[%s::config_callback] Setting the maximum number of clusters to extract to: %d.",
getName ().c_str (), config.max_clusters);
181 if (indices && !
isValid (indices))
189 std_msgs::Header cloud_header =
fromPCL(cloud->header);
190 std_msgs::Header indices_header = indices->header;
192 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 193 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
195 cloud->width * cloud->height,
pcl::getFieldsList (*cloud).c_str (), cloud_header.stamp.toSec (), cloud_header.frame_id.c_str (),
pnh_->resolveName (
"input").c_str (),
196 indices->indices.size (), indices_header.stamp.toSec (), indices_header.frame_id.c_str (),
pnh_->resolveName (
"indices").c_str ());
198 NODELET_DEBUG (
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
getName ().c_str (), cloud->width * cloud->height,
fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (),
pnh_->resolveName (
"input").c_str ());
204 indices_ptr.reset (
new std::vector<int> (indices->indices));
206 impl_.setInputCloud (cloud);
207 impl_.setIndices (indices_ptr);
209 std::vector<pcl::PointIndices> clusters;
210 impl_.extract (clusters);
214 for (
size_t i = 0; i < clusters.size (); ++i)
219 pcl_msgs::PointIndices ros_pi;
225 NODELET_DEBUG (
"[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", clusters.size (),
pnh_->resolveName (
"output").c_str ());
229 for (
size_t i = 0; i < clusters.size (); ++i)
234 copyPointCloud (*cloud, clusters[i].indices, output);
241 toPCL(header, output.header);
244 NODELET_DEBUG (
"[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s",
245 i, clusters[i].indices.size (), 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
const std::string & getName() const
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
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...
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
pcl::PointCloud< pcl::PointXYZ > PointCloud
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)
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.
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
#define NODELET_DEBUG(...)
bool use_indices_
Set to true if point indices are used.
PointIndices::ConstPtr PointIndicesConstPtr
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
message_filters::Subscriber< PointCloud > sub_input_filter_
The message filter subscriber for PointCloud2.