22 #ifndef _PLUGINS_LASER_CLUSTER_LASER_CLUSTER_THREAD_H_
23 #define _PLUGINS_LASER_CLUSTER_LASER_CLUSTER_THREAD_H_
26 #include <aspect/blackboard.h>
27 #include <aspect/blocked_timing.h>
28 #include <aspect/clock.h>
29 #include <aspect/configurable.h>
30 #include <aspect/logging.h>
31 #include <aspect/pointcloud.h>
32 #include <aspect/tf.h>
33 #include <core/threading/thread.h>
34 #include <pcl/point_cloud.h>
35 #include <pcl/point_types.h>
36 #include <pcl/segmentation/sac_segmentation.h>
38 #include <Eigen/StdVector>
41 class Position3DInterface;
42 class SwitchInterface;
43 #ifdef USE_TIMETRACKER
46 class LaserClusterInterface;
67 typedef pcl::PointXYZ PointType;
69 typedef Cloud::Ptr CloudPtr;
70 typedef Cloud::ConstPtr CloudConstPtr;
72 typedef pcl::PointXYZRGB ColorPointType;
74 typedef ColorCloud::Ptr ColorCloudPtr;
75 typedef ColorCloud::ConstPtr ColorCloudConstPtr;
77 typedef pcl::PointXYZL LabelPointType;
79 typedef LabelCloud::Ptr LabelCloudPtr;
80 typedef LabelCloud::ConstPtr LabelCloudConstPtr;
85 const Eigen::Vector4f & centroid = Eigen::Vector4f(0, 0, 0, 0),
86 const Eigen::Quaternionf & rotation = Eigen::Quaternionf(1, 0, 0, 0));
88 float calc_line_length(CloudPtr cloud,
89 pcl::PointIndices::Ptr inliers,
90 pcl::ModelCoefficients::Ptr coeff);
104 CloudConstPtr input_;
108 pcl::SACSegmentation<PointType> seg_;
110 std::vector<fawkes::Position3DInterface *> cluster_pos_ifs_;
115 typedef enum { SELECT_MIN_ANGLE, SELECT_MIN_DIST } selection_mode_t;
117 std::string cfg_name_;
118 std::string cfg_prefix_;
120 bool cfg_line_removal_;
121 unsigned int cfg_segm_max_iterations_;
122 float cfg_segm_distance_threshold_;
123 unsigned int cfg_segm_min_inliers_;
124 float cfg_segm_sample_max_dist_;
125 float cfg_line_min_length_;
126 float cfg_cluster_tolerance_;
127 unsigned int cfg_cluster_min_size_;
128 unsigned int cfg_cluster_max_size_;
129 std::string cfg_input_pcl_;
130 std::string cfg_result_frame_;
131 float cfg_bbox_min_x_;
132 float cfg_bbox_max_x_;
133 float cfg_bbox_min_y_;
134 float cfg_bbox_max_y_;
136 float cfg_switch_tolerance_;
140 selection_mode_t cfg_selection_mode_;
141 unsigned int cfg_max_num_clusters_;
143 std::string output_cluster_name_;
144 std::string output_cluster_labeled_name_;
146 float current_max_x_;
148 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_;