22 #ifndef _PLUGINS_LASER_LINES_LASER_LINES_THREAD_H_
23 #define _PLUGINS_LASER_LINES_LASER_LINES_THREAD_H_
26 #include "line_info.h"
28 #include <aspect/blackboard.h>
29 #include <aspect/blocked_timing.h>
30 #include <aspect/clock.h>
31 #include <aspect/configurable.h>
32 #include <aspect/logging.h>
33 #include <aspect/pointcloud.h>
34 #include <aspect/tf.h>
35 #include <config/change_handler.h>
36 #include <core/threading/thread.h>
37 #include <pcl/ModelCoefficients.h>
38 #include <pcl/point_cloud.h>
39 #include <pcl/point_types.h>
41 #include <Eigen/StdVector>
43 #ifdef HAVE_VISUAL_DEBUGGING
44 # include <plugins/ros/aspect/ros.h>
45 # include <visualization_msgs/MarkerArray.h>
53 class Position3DInterface;
54 class SwitchInterface;
55 #ifdef USE_TIMETRACKER
58 class LaserLineInterface;
69 #ifdef HAVE_VISUAL_DEBUGGING
83 typedef pcl::PointXYZ PointType;
85 typedef Cloud::Ptr CloudPtr;
86 typedef Cloud::ConstPtr CloudConstPtr;
88 typedef pcl::PointXYZRGB ColorPointType;
90 typedef ColorCloud::Ptr ColorCloudPtr;
91 typedef ColorCloud::ConstPtr ColorCloudConstPtr;
123 void update_lines(std::vector<LineInfo> &);
124 void publish_known_lines();
126 void set_interface(
unsigned int idx,
130 const std::string & frame_id =
"");
134 #ifdef HAVE_VISUAL_DEBUGGING
135 void publish_visualization(
const std::vector<TrackedLineInfo> &linfos,
136 const std::string & marker_namespace,
137 const std::string & avg_marker_namespace);
139 void publish_visualization_add_line(visualization_msgs::MarkerArray &m,
140 unsigned int & idnum,
143 const std::string & marker_namespace,
144 const std::string & name_suffix =
"");
150 CloudConstPtr input_;
153 std::vector<fawkes::LaserLineInterface *> line_ifs_;
154 std::vector<fawkes::LaserLineInterface *> line_avg_ifs_;
155 std::vector<TrackedLineInfo> known_lines_;
159 typedef enum { SELECT_MIN_ANGLE, SELECT_MIN_DIST } selection_mode_t;
161 unsigned int cfg_segm_max_iterations_;
162 float cfg_segm_distance_threshold_;
163 float cfg_segm_sample_max_dist_;
164 float cfg_min_length_;
165 float cfg_max_length_;
166 unsigned int cfg_segm_min_inliers_;
167 std::string cfg_input_pcl_;
168 unsigned int cfg_max_num_lines_;
169 float cfg_switch_tolerance_;
170 float cfg_cluster_tolerance_;
171 float cfg_cluster_quota_;
174 bool cfg_moving_avg_enabled_;
175 unsigned int cfg_moving_avg_window_size_;
176 std::string cfg_tracking_frame_id_;
178 unsigned int loop_count_;
180 #ifdef USE_TIMETRACKER
182 unsigned int tt_loopcount_;
183 unsigned int ttc_full_loop_;
184 unsigned int ttc_msg_proc_;
185 unsigned int ttc_extract_lines_;
186 unsigned int ttc_clustering_;
189 #ifdef HAVE_VISUAL_DEBUGGING
190 ros::Publisher *vispub_;