Fawkes API  Fawkes Development Version
tabletop_objects_thread.h
1 
2 /***************************************************************************
3  * tabletop_objects_thread.h - Thread to detect tabletop objects
4  *
5  * Created: Fri Nov 04 23:54:19 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_
23 #define __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_
24 
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>
32 
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>
38 
39 namespace fawkes {
40  class Position3DInterface;
41  class SwitchInterface;
42 #ifdef USE_TIMETRACKER
43  class TimeTracker;
44 #endif
45 }
46 
47 #ifdef HAVE_VISUAL_DEBUGGING
49 #endif
50 
52 : public fawkes::Thread,
53  public fawkes::ClockAspect,
54  public fawkes::LoggingAspect,
59 {
60  public:
62  virtual ~TabletopObjectsThread();
63 
64  virtual void init();
65  virtual void loop();
66  virtual void finalize();
67 
68 #ifdef HAVE_VISUAL_DEBUGGING
69  void set_visualization_thread(TabletopVisualizationThreadBase *visthread);
70 #endif
71 
72  private:
73  typedef pcl::PointXYZ PointType;
75 
76  typedef pcl::PointXYZRGB ColorPointType;
78  typedef Cloud::Ptr CloudPtr;
79  typedef Cloud::ConstPtr CloudConstPtr;
80 
81  typedef ColorCloud::Ptr ColorCloudPtr;
82  typedef ColorCloud::ConstPtr ColorCloudConstPtr;
83 
84  private:
85  void set_position(fawkes::Position3DInterface *iface,
86  bool is_visible, const Eigen::Vector4f &centroid = Eigen::Vector4f(0, 0, 0, 0),
87  const Eigen::Quaternionf &rotation = Eigen::Quaternionf(1, 0, 0, 0));
88 
89  CloudPtr simplify_polygon(CloudPtr polygon, float sqr_dist_threshold);
90 
91  CloudPtr generate_table_model(const float length, const float width,
92  const float thickness, const float step, const float max_error);
93 
94  CloudPtr generate_table_model(const float length, const float width,
95  const float step, const float max_error = 0.01);
96 
97  bool is_polygon_edge_better(PointType &cb_br_p1p, PointType &cb_br_p2p, PointType &br_p1p, PointType &br_p2p);
98 
99  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
100  protected: virtual void run() { Thread::run(); }
101 
102  private:
105  CloudConstPtr input_;
107 
108  pcl::VoxelGrid<PointType> grid_;
109  pcl::SACSegmentation<PointType> seg_;
110 
111  std::vector<fawkes::Position3DInterface *> pos_ifs_;
112  fawkes::Position3DInterface *table_pos_if_;
113 
114  fawkes::SwitchInterface *switch_if_;
115 
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_;
134 
135  fawkes::RefPtr<Cloud> ftable_model_;
136  CloudPtr table_model_;
137  fawkes::RefPtr<Cloud> fsimplified_polygon_;
138  CloudPtr simplified_polygon_;
139 
140  unsigned int loop_count_;
141 
142 #ifdef USE_TIMETRACKER
143  fawkes::TimeTracker *tt_;
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_;
162 #endif
163 
164 #ifdef HAVE_VISUAL_DEBUGGING
166 #endif
167 };
168 
169 #endif
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
virtual void run()
Stub to see name in backtrace for easier debugging.
Main thread of tabletop objects plugin.
Fawkes library namespace.
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:40
virtual void run()
Code to execute in the thread.
Definition: thread.cpp:918
Thread class encapsulation of pthreads.
Definition: thread.h:42
Base class for virtualization thread.
Thread aspect to access the transform system.
Definition: tf.h:38
SwitchInterface Fawkes BlackBoard Interface.
Position3DInterface Fawkes BlackBoard Interface.
Time tracking utility.
Definition: tracker.h:38
Thread aspect to log output.
Definition: logging.h:35
Thread aspect to access configuration data.
Definition: configurable.h:35
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49