41 #include <pcl/io/io.h> 56 srv_ = boost::make_shared <dynamic_reconfigure::Server<ExtractPolygonalPrismDataConfig> > (*pnh_);
58 srv_->setCallback (f);
118 double height_min, height_max;
119 impl_.getHeightLimits (height_min, height_max);
120 if (height_min != config.height_min)
122 height_min = config.height_min;
123 NODELET_DEBUG (
"[%s::config_callback] Setting new minimum height to the planar model to: %f.",
getName ().c_str (), height_min);
124 impl_.setHeightLimits (height_min, height_max);
126 if (height_max != config.height_max)
128 height_max = config.height_max;
129 NODELET_DEBUG (
"[%s::config_callback] Setting new maximum height to the planar model to: %f.",
getName ().c_str (), height_max);
130 impl_.setHeightLimits (height_min, height_max);
146 pcl_msgs::PointIndices inliers;
147 inliers.header =
fromPCL(cloud->header);
157 if (indices && !
isValid (indices))
167 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 168 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 169 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
171 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 (),
172 hull->width * hull->height,
pcl::getFieldsList (*hull).c_str (),
fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (),
pnh_->resolveName (
"planar_hull").c_str (),
173 indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (),
pnh_->resolveName (
"indices").c_str ());
176 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 177 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
179 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 (),
180 hull->width * hull->height,
pcl::getFieldsList (*hull).c_str (),
fromPCL(hull->header).stamp.toSec (), hull->header.frame_id.c_str (),
pnh_->resolveName (
"planar_hull").c_str ());
183 if (cloud->header.frame_id != hull->header.frame_id)
185 NODELET_DEBUG (
"[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.",
getName ().c_str (), hull->header.frame_id.c_str (), cloud->header.frame_id.c_str ());
193 impl_.setInputPlanarHull (planar_hull.makeShared ());
196 impl_.setInputPlanarHull (hull);
199 if (indices && !indices->header.frame_id.empty ())
200 indices_ptr.reset (
new std::vector<int> (indices->indices));
202 impl_.setInputCloud (cloud);
203 impl_.setIndices (indices_ptr);
206 if (!cloud->points.empty ()) {
207 pcl::PointIndices pcl_inliers;
209 impl_.segment (pcl_inliers);
213 inliers.header =
fromPCL(cloud->header);
215 NODELET_DEBUG (
"[%s::input_hull_callback] Publishing %zu indices.",
getName ().c_str (), inliers.indices.size ());
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
tf::TransformListener tf_listener_
TF listener object.
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)
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
Apply a rigid transform defined by a 3D offset and a quaternion.
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).
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
#define NODELET_DEBUG(...)
bool use_indices_
Set to true if point indices are used.
void moveToPCL(sensor_msgs::Image &image, pcl::PCLImage &pcl_image)
PointIndices::ConstPtr PointIndicesConstPtr
message_filters::Subscriber< PointCloud > sub_input_filter_
The message filter subscriber for PointCloud2.
Connection registerCallback(const C &callback)