Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * tabletop_objects_thread.h - Thread to detect tabletop objects 00004 * 00005 * Created: Fri Nov 04 23:54:19 2011 00006 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #ifndef __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_ 00023 #define __PLUGINS_PERCEPTION_TABLETOP_OBJECTS_THREAD_H_ 00024 00025 #include <core/threading/thread.h> 00026 #include <aspect/clock.h> 00027 #include <aspect/configurable.h> 00028 #include <aspect/logging.h> 00029 #include <aspect/blackboard.h> 00030 #include <aspect/tf.h> 00031 #include <aspect/pointcloud.h> 00032 00033 #include <Eigen/StdVector> 00034 #include <pcl/point_types.h> 00035 #include <pcl/point_cloud.h> 00036 #include <pcl/segmentation/sac_segmentation.h> 00037 #include <pcl/filters/voxel_grid.h> 00038 00039 namespace fawkes { 00040 class Position3DInterface; 00041 class SwitchInterface; 00042 #ifdef USE_TIMETRACKER 00043 class TimeTracker; 00044 #endif 00045 } 00046 00047 #ifdef HAVE_VISUAL_DEBUGGING 00048 class TabletopVisualizationThreadBase; 00049 #endif 00050 00051 class TabletopObjectsThread 00052 : public fawkes::Thread, 00053 public fawkes::ClockAspect, 00054 public fawkes::LoggingAspect, 00055 public fawkes::ConfigurableAspect, 00056 public fawkes::BlackBoardAspect, 00057 public fawkes::TransformAspect, 00058 public fawkes::PointCloudAspect 00059 { 00060 public: 00061 TabletopObjectsThread(); 00062 virtual ~TabletopObjectsThread(); 00063 00064 virtual void init(); 00065 virtual void loop(); 00066 virtual void finalize(); 00067 00068 #ifdef HAVE_VISUAL_DEBUGGING 00069 void set_visualization_thread(TabletopVisualizationThreadBase *visthread); 00070 #endif 00071 00072 private: 00073 typedef pcl::PointXYZ PointType; 00074 typedef pcl::PointCloud<PointType> Cloud; 00075 00076 typedef pcl::PointXYZRGB ColorPointType; 00077 typedef pcl::PointCloud<ColorPointType> ColorCloud; 00078 typedef Cloud::Ptr CloudPtr; 00079 typedef Cloud::ConstPtr CloudConstPtr; 00080 00081 typedef ColorCloud::Ptr ColorCloudPtr; 00082 typedef ColorCloud::ConstPtr ColorCloudConstPtr; 00083 00084 private: 00085 void set_position(fawkes::Position3DInterface *iface, 00086 bool is_visible, const Eigen::Vector4f ¢roid = Eigen::Vector4f(0, 0, 0, 0), 00087 const Eigen::Quaternionf &rotation = Eigen::Quaternionf(1, 0, 0, 0)); 00088 00089 CloudPtr simplify_polygon(CloudPtr polygon, float sqr_dist_threshold); 00090 00091 CloudPtr generate_table_model(const float length, const float width, 00092 const float thickness, const float step, const float max_error); 00093 00094 CloudPtr generate_table_model(const float length, const float width, 00095 const float step, const float max_error = 0.01); 00096 00097 bool is_polygon_edge_better(PointType &cb_br_p1p, PointType &cb_br_p2p, PointType &br_p1p, PointType &br_p2p); 00098 00099 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ 00100 protected: virtual void run() { Thread::run(); } 00101 00102 private: 00103 fawkes::RefPtr<const pcl::PointCloud<PointType> > finput_; 00104 fawkes::RefPtr<pcl::PointCloud<ColorPointType> > fclusters_; 00105 CloudConstPtr input_; 00106 pcl::PointCloud<ColorPointType>::Ptr clusters_; 00107 00108 pcl::VoxelGrid<PointType> grid_; 00109 pcl::SACSegmentation<PointType> seg_; 00110 00111 std::vector<fawkes::Position3DInterface *> pos_ifs_; 00112 fawkes::Position3DInterface *table_pos_if_; 00113 00114 fawkes::SwitchInterface *switch_if_; 00115 00116 float cfg_depth_filter_min_x_; 00117 float cfg_depth_filter_max_x_; 00118 float cfg_voxel_leaf_size_; 00119 unsigned int cfg_segm_max_iterations_; 00120 float cfg_segm_distance_threshold_; 00121 float cfg_segm_inlier_quota_; 00122 float cfg_max_z_angle_deviation_; 00123 float cfg_table_min_height_; 00124 float cfg_table_max_height_; 00125 float cfg_table_model_length_; 00126 float cfg_table_model_width_; 00127 float cfg_table_model_step_; 00128 float cfg_horizontal_va_; 00129 float cfg_vertical_va_; 00130 float cfg_cluster_tolerance_; 00131 unsigned int cfg_cluster_min_size_; 00132 unsigned int cfg_cluster_max_size_; 00133 std::string cfg_result_frame_; 00134 00135 fawkes::RefPtr<Cloud> ftable_model_; 00136 CloudPtr table_model_; 00137 fawkes::RefPtr<Cloud> fsimplified_polygon_; 00138 CloudPtr simplified_polygon_; 00139 00140 unsigned int loop_count_; 00141 00142 #ifdef USE_TIMETRACKER 00143 fawkes::TimeTracker *tt_; 00144 unsigned int tt_loopcount_; 00145 unsigned int ttc_full_loop_; 00146 unsigned int ttc_msgproc_; 00147 unsigned int ttc_voxelize_; 00148 unsigned int ttc_plane_; 00149 unsigned int ttc_extract_plane_; 00150 unsigned int ttc_plane_downsampling_; 00151 unsigned int ttc_cluster_plane_; 00152 unsigned int ttc_convex_hull_; 00153 unsigned int ttc_simplify_polygon_; 00154 unsigned int ttc_find_edge_; 00155 unsigned int ttc_transform_; 00156 unsigned int ttc_transform_model_; 00157 unsigned int ttc_extract_non_plane_; 00158 unsigned int ttc_polygon_filter_; 00159 unsigned int ttc_table_to_output_; 00160 unsigned int ttc_cluster_objects_; 00161 unsigned int ttc_visualization_; 00162 #endif 00163 00164 #ifdef HAVE_VISUAL_DEBUGGING 00165 TabletopVisualizationThreadBase *visthread_; 00166 #endif 00167 }; 00168 00169 #endif