22 #ifndef __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_
23 #define __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_
25 #include <core/threading/thread.h>
26 #include <aspect/clock.h>
27 #include <aspect/configurable.h>
28 #include <aspect/logging.h>
29 #include <aspect/blackboard.h>
30 #include <aspect/tf.h>
31 #include <aspect/pointcloud.h>
33 #include <Eigen/StdVector>
34 #include <pcl/point_types.h>
35 #include <pcl/point_cloud.h>
36 #include <pcl/segmentation/sac_segmentation.h>
37 #include <pcl/filters/voxel_grid.h>
40 class Position3DInterface;
41 class SwitchInterface;
42 #ifdef USE_TIMETRACKER
47 #ifdef HAVE_VISUAL_DEBUGGING
68 #ifdef HAVE_VISUAL_DEBUGGING
73 typedef pcl::PointXYZ PointType;
74 typedef pcl::PointCloud<PointType> Cloud;
76 typedef pcl::PointXYZRGB ColorPointType;
77 typedef pcl::PointCloud<ColorPointType> ColorCloud;
78 typedef Cloud::Ptr CloudPtr;
79 typedef Cloud::ConstPtr CloudConstPtr;
81 typedef ColorCloud::Ptr ColorCloudPtr;
82 typedef ColorCloud::ConstPtr ColorCloudConstPtr;
86 bool is_visible,
const Eigen::Vector4f ¢roid = Eigen::Vector4f(0, 0, 0, 0),
87 const Eigen::Quaternionf &rotation = Eigen::Quaternionf(1, 0, 0, 0));
89 CloudPtr simplify_polygon(CloudPtr polygon,
float sqr_dist_threshold);
91 CloudPtr generate_table_model(
const float length,
const float width,
92 const float thickness,
const float step,
const float max_error);
94 CloudPtr generate_table_model(
const float length,
const float width,
95 const float step,
const float max_error = 0.01);
97 bool is_polygon_edge_better(PointType &cb_br_p1p, PointType &cb_br_p2p, PointType &br_p1p, PointType &br_p2p);
105 CloudConstPtr input_;
106 pcl::PointCloud<ColorPointType>::Ptr clusters_;
108 pcl::VoxelGrid<PointType> grid_;
109 pcl::SACSegmentation<PointType> seg_;
111 std::vector<fawkes::Position3DInterface *> pos_ifs_;
116 float cfg_depth_filter_min_x_;
117 float cfg_depth_filter_max_x_;
118 float cfg_voxel_leaf_size_;
119 unsigned int cfg_segm_max_iterations_;
120 float cfg_segm_distance_threshold_;
121 float cfg_segm_inlier_quota_;
122 float cfg_max_z_angle_deviation_;
123 float cfg_table_min_height_;
124 float cfg_table_max_height_;
125 float cfg_table_model_length_;
126 float cfg_table_model_width_;
127 float cfg_table_model_step_;
128 float cfg_horizontal_va_;
129 float cfg_vertical_va_;
130 float cfg_cluster_tolerance_;
131 unsigned int cfg_cluster_min_size_;
132 unsigned int cfg_cluster_max_size_;
133 std::string cfg_result_frame_;
136 CloudPtr table_model_;
138 CloudPtr simplified_polygon_;
140 unsigned int loop_count_;
142 #ifdef USE_TIMETRACKER
144 unsigned int tt_loopcount_;
145 unsigned int ttc_full_loop_;
146 unsigned int ttc_msgproc_;
147 unsigned int ttc_voxelize_;
148 unsigned int ttc_plane_;
149 unsigned int ttc_extract_plane_;
150 unsigned int ttc_plane_downsampling_;
151 unsigned int ttc_cluster_plane_;
152 unsigned int ttc_convex_hull_;
153 unsigned int ttc_simplify_polygon_;
154 unsigned int ttc_find_edge_;
155 unsigned int ttc_transform_;
156 unsigned int ttc_transform_model_;
157 unsigned int ttc_extract_non_plane_;
158 unsigned int ttc_polygon_filter_;
159 unsigned int ttc_table_to_output_;
160 unsigned int ttc_cluster_objects_;
161 unsigned int ttc_visualization_;
164 #ifdef HAVE_VISUAL_DEBUGGING