22 #ifndef __PLUGINS_LASER_CLUSTER_LASER_CLUSTER_THREAD_H_ 23 #define __PLUGINS_LASER_CLUSTER_LASER_CLUSTER_THREAD_H_ 26 #include <pcl/point_cloud.h> 27 #include <pcl/point_types.h> 29 #include <core/threading/thread.h> 30 #include <aspect/clock.h> 31 #include <aspect/configurable.h> 32 #include <aspect/logging.h> 33 #include <aspect/blackboard.h> 34 #include <aspect/blocked_timing.h> 35 #include <aspect/tf.h> 36 #include <aspect/pointcloud.h> 38 #include <Eigen/StdVector> 39 #include <pcl/segmentation/sac_segmentation.h> 42 class Position3DInterface;
43 class SwitchInterface;
44 #ifdef USE_TIMETRACKER 47 class LaserClusterInterface;
66 virtual void finalize();
69 typedef pcl::PointXYZ PointType;
71 typedef Cloud::Ptr CloudPtr;
72 typedef Cloud::ConstPtr CloudConstPtr;
74 typedef pcl::PointXYZRGB ColorPointType;
76 typedef ColorCloud::Ptr ColorCloudPtr;
77 typedef ColorCloud::ConstPtr ColorCloudConstPtr;
79 typedef pcl::PointXYZL LabelPointType;
81 typedef LabelCloud::Ptr LabelCloudPtr;
82 typedef LabelCloud::ConstPtr LabelCloudConstPtr;
87 const Eigen::Vector4f ¢roid = Eigen::Vector4f(0, 0, 0, 0),
88 const Eigen::Quaternionf &rotation = Eigen::Quaternionf(1, 0, 0, 0));
90 float calc_line_length(CloudPtr cloud, pcl::PointIndices::Ptr inliers,
91 pcl::ModelCoefficients::Ptr coeff);
100 CloudConstPtr input_;
104 pcl::SACSegmentation<PointType> seg_;
106 std::vector<fawkes::Position3DInterface *> cluster_pos_ifs_;
116 std::string cfg_name_;
117 std::string cfg_prefix_;
119 bool cfg_line_removal_;
120 float cfg_depth_filter_min_x_;
121 float cfg_depth_filter_max_x_;
122 unsigned int cfg_segm_max_iterations_;
123 float cfg_segm_distance_threshold_;
124 unsigned int cfg_segm_min_inliers_;
125 float cfg_segm_sample_max_dist_;
126 float cfg_line_min_length_;
127 float cfg_cluster_tolerance_;
128 unsigned int cfg_cluster_min_size_;
129 unsigned int cfg_cluster_max_size_;
130 std::string cfg_input_pcl_;
131 std::string cfg_result_frame_;
132 float cfg_bbox_min_x_;
133 float cfg_bbox_max_x_;
134 float cfg_bbox_min_y_;
135 float cfg_bbox_max_y_;
137 float cfg_switch_tolerance_;
141 selection_mode_t cfg_selection_mode_;
142 unsigned int cfg_max_num_clusters_;
144 std::string output_cluster_name_;
145 std::string output_cluster_labeled_name_;
147 float current_max_x_;
149 unsigned int loop_count_;
153 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
158 Eigen::Vector4f centroid;
161 #ifdef USE_TIMETRACKER 163 unsigned int tt_loopcount_;
164 unsigned int ttc_full_loop_;
165 unsigned int ttc_msg_proc_;
166 unsigned int ttc_extract_lines_;
167 unsigned int ttc_clustering_;
Thread aspect to access to BlackBoard.
Thread aspect that allows to obtain the current time from the clock.
LaserClusterInterface Fawkes BlackBoard Interface.
Fawkes library namespace.
Thread aspect to provide and access point clouds.
virtual void run()
Code to execute in the thread.
Main thread of laser-cluster plugin.
Thread class encapsulation of pthreads.
virtual void run()
Stub to see name in backtrace for easier debugging.
Thread aspect to use blocked timing.
SwitchInterface Fawkes BlackBoard Interface.
Position3DInterface Fawkes BlackBoard Interface.
Thread aspect to log output.
Thread aspect to access configuration data.
RefPtr<> is a reference-counting shared smartpointer.