Go to the documentation of this file.00001 #pragma once
00002
00003
00004 #include <string.h>
00005 #include <math.h>
00006
00007
00008 #include <ros/ros.h>
00009 #include <actionlib/client/simple_action_client.h>
00010 #include <geometry_msgs/PoseStamped.h>
00011 #include <geometry_msgs/Point32.h>
00012 #include <sensor_msgs/PointCloud2.h>
00013
00014
00015 #include <pcl_ros/filters/passthrough.h>
00016 #include <pcl/segmentation/sac_segmentation.h>
00017 #include <pcl/sample_consensus/method_types.h>
00018 #include <pcl/sample_consensus/model_types.h>
00019 #include <pcl/surface/convex_hull.h>
00020 #include <pcl/filters/extract_indices.h>
00021 #include <pcl/filters/voxel_grid.h>
00022 #include <pcl_ros/transforms.h>
00023 #include <pcl/filters/statistical_outlier_removal.h>
00024
00025 #include <pcl/point_cloud.h>
00026 #include <pcl/point_types.h>
00027
00028 #include <pcl/filters/extract_indices.h>
00029 #include <pcl/filters/project_inliers.h>
00030 #include <pcl/segmentation/extract_clusters.h>
00031 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00032
00033 #include <pcl/surface/concave_hull.h>
00034 #include <visualization_msgs/Marker.h>
00035 #include <visualization_msgs/MarkerArray.h>
00036 #include <pcl/kdtree/kdtree.h>
00037 #include <pcl/kdtree/kdtree_flann.h>
00038 #include <pcl/features/integral_image_normal.h>
00039 #include <pcl/features/normal_3d_omp.h>
00040
00041
00042 #include <tf/transform_listener.h>
00043
00044
00045 #include <book_stacking_msgs/PlaneInfo.h>
00046 #include <book_stacking_msgs/PlaneInfos.h>
00047 #include <book_stacking_msgs/ObjectInfo.h>
00048 #include <book_stacking_msgs/ObjectInfos.h>
00049
00050 typedef pcl::PointXYZ Point;
00051 typedef pcl::KdTree<Point>::Ptr KdTreePtr;
00052
00053
00054 book_stacking_msgs::PlaneInfos getPlanesByNormals(const pcl::PointCloud<Point>& cloud,
00055 unsigned int max_planes,
00056 bool cluster_planes,
00057 bool use_concave,
00058 bool use_omp=false,
00059 double dist_thresh=0.03,
00060 double max_sac_iterations=1000,
00061 double sac_probability=0.99,
00062 unsigned int min_inliers=1000,
00063 double search_radius=0.1,
00064 double cluster_tolerance=0.1);
00065
00066
00067 void drawPlaneMarkers(const book_stacking_msgs::PlaneInfos& planes, const ros::Publisher& plane_pub, float r=0.0f, float g=1.0f, float b=0.0f);
00068
00069 void drawPlaneMarkers(const std::vector<book_stacking_msgs::PlaneInfo>& planes, const ros::Publisher& plane_pub, float r=0.0f, float g=1.0f, float b=0.0f);
00070
00071 void drawObjectPrisms(const book_stacking_msgs::ObjectInfos& objects, const ros::Publisher& object_pub, const book_stacking_msgs::PlaneInfo& plane, float r=0.0f, float g=0.0f, float b=1.0f);
00072
00073
00074 book_stacking_msgs::ObjectInfos getObjectsOverPlane(const book_stacking_msgs::PlaneInfo& plane, const pcl::PointCloud<Point>& cloud, double prism_min_height=0.0, double prism_max_height=1.0);
00075
00076 geometry_msgs::Point32 calcCentroid(const pcl::PointCloud<Point>& cloud);