39 #include <pcl/common/io.h> 41 #include <geometry_msgs/PolygonStamped.h> 55 NODELET_DEBUG (
"[%s::onInit] Nodelet successfully created with the following parameters:\n" 56 " - use_indices : %s",
124 output.header = cloud->header;
129 if (indices && !
isValid (indices,
"indices"))
133 output.header = cloud->header;
141 " - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n" 142 " - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
147 NODELET_DEBUG (
"[%s::input_indices_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 (),
getMTPrivateNodeHandle ().
resolveName (
"input").c_str ());
152 indices_ptr.reset (
new std::vector<int> (indices->indices));
154 impl_.setInputCloud (cloud);
155 impl_.setIndices (indices_ptr);
158 impl_.reconstruct (output);
161 if (output.points.size () >= 3)
163 geometry_msgs::PolygonStamped poly;
164 poly.header =
fromPCL(output.header);
165 poly.polygon.points.resize (output.points.size ());
167 pcl::Vector4fMap O = output.points[1].getVector4fMap ();
168 pcl::Vector4fMap B = output.points[0].getVector4fMap ();
169 pcl::Vector4fMap A = output.points[2].getVector4fMap ();
171 Eigen::Vector4f OA = A - O;
172 Eigen::Vector4f OB = B - O;
173 Eigen::Vector4f N = OA.cross3 (OB);
174 double theta = N.dot (O);
175 bool reversed =
false;
176 if (theta < (M_PI / 2.0))
178 for (
size_t i = 0; i < output.points.size (); ++i)
182 size_t j = output.points.size () - i - 1;
183 poly.polygon.points[i].x = output.points[j].x;
184 poly.polygon.points[i].y = output.points[j].y;
185 poly.polygon.points[i].z = output.points[j].z;
189 poly.polygon.points[i].x = output.points[i].x;
190 poly.polygon.points[i].y = output.points[i].y;
191 poly.polygon.points[i].z = output.points[i].z;
194 pub_plane_.
publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly));
197 output.header = cloud->header;
virtual void unsubscribe()
std::string getFieldsList(const sensor_msgs::PointCloud2 &cloud)
#define NODELET_ERROR(...)
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ExactTime< PointCloud, PointIndices > > > sync_input_indices_e_
Synchronized input, and indices.
virtual void onInit()
Nodelet initialization routine. Reads in global parameters used by all nodelets.
void publish(const boost::shared_ptr< M > &message) const
ros::Subscriber sub_input_
The input PointCloud subscriber.
std::string resolveName(const std::string &name, bool remap=true) const
const std::string & getName() const
ros::Publisher pub_plane_
Publisher for PolygonStamped.
ConvexHull2D represents a 2D ConvexHull implementation.
message_filters::Subscriber< PointIndices > sub_indices_filter_
The message filter subscriber for PointIndices.
void input_indices_callback(const PointCloudConstPtr &cloud, const PointIndicesConstPtr &indices)
Input point cloud callback.
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...
ros::NodeHandle & getMTPrivateNodeHandle() const
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
virtual void subscribe()
LazyNodelet connection routine.
pcl::ConvexHull< pcl::PointXYZ > impl_
The PCL implementation used.
pcl::PointCloud< pcl::PointXYZ > PointCloud
virtual void onInit()
Nodelet initialization routine.
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.
boost::shared_ptr< message_filters::Synchronizer< sync_policies::ApproximateTime< PointCloud, PointIndices > > > sync_input_indices_a_
#define NODELET_DEBUG(...)
pcl_ros::ConvexHull2D ConvexHull2D
bool use_indices_
Set to true if point indices are used.
PointIndices::ConstPtr PointIndicesConstPtr
message_filters::Subscriber< PointCloud > sub_input_filter_
The message filter subscriber for PointCloud2.