22 #ifndef __PLUGINS_LASER_LINES_LASER_LINES_THREAD_H_ 23 #define __PLUGINS_LASER_LINES_LASER_LINES_THREAD_H_ 26 #include <pcl/point_cloud.h> 27 #include <pcl/point_types.h> 29 #include "line_info.h" 31 #include <core/threading/thread.h> 32 #include <aspect/clock.h> 33 #include <aspect/configurable.h> 34 #include <aspect/logging.h> 35 #include <aspect/blackboard.h> 36 #include <aspect/blocked_timing.h> 37 #include <aspect/tf.h> 38 #include <aspect/pointcloud.h> 40 #include <Eigen/StdVector> 41 #include <pcl/ModelCoefficients.h> 43 #ifdef HAVE_VISUAL_DEBUGGING 44 # include <plugins/ros/aspect/ros.h> 45 # include <visualization_msgs/MarkerArray.h> 54 class Position3DInterface;
55 class SwitchInterface;
56 #ifdef USE_TIMETRACKER 59 class LaserLineInterface;
70 #ifdef HAVE_VISUAL_DEBUGGING 84 typedef pcl::PointXYZ PointType;
86 typedef Cloud::Ptr CloudPtr;
87 typedef Cloud::ConstPtr CloudConstPtr;
89 typedef pcl::PointXYZRGB ColorPointType;
91 typedef ColorCloud::Ptr ColorCloudPtr;
92 typedef ColorCloud::ConstPtr ColorCloudConstPtr;
95 protected:
virtual void run() { Thread::run(); }
99 void set_line(
unsigned int idx,
102 const std::string &frame_id =
"",
106 #ifdef HAVE_VISUAL_DEBUGGING 107 void publish_visualization(
const std::vector<TrackedLineInfo> &linfos,
108 const std::string &marker_namespace,
109 const std::string &avg_marker_namespace);
111 void publish_visualization_add_line(visualization_msgs::MarkerArray &m,
115 const std::string &marker_namespace,
116 const std::string &name_suffix =
"");
122 CloudConstPtr input_;
125 std::vector<fawkes::LaserLineInterface *> line_ifs_;
126 std::vector<fawkes::LaserLineInterface *> line_avg_ifs_;
127 std::vector<TrackedLineInfo> known_lines_;
136 unsigned int cfg_segm_max_iterations_;
137 float cfg_segm_distance_threshold_;
138 float cfg_segm_sample_max_dist_;
139 float cfg_min_length_;
140 float cfg_max_length_;
141 unsigned int cfg_segm_min_inliers_;
142 std::string cfg_input_pcl_;
143 std::string cfg_result_frame_;
144 unsigned int cfg_max_num_lines_;
145 float cfg_switch_tolerance_;
146 float cfg_cluster_tolerance_;
147 float cfg_cluster_quota_;
150 bool cfg_moving_avg_enabled_;
151 unsigned int cfg_moving_avg_window_size_;
152 std::string cfg_tracking_frame_id_;
154 unsigned int loop_count_;
156 #ifdef USE_TIMETRACKER 158 unsigned int tt_loopcount_;
159 unsigned int ttc_full_loop_;
160 unsigned int ttc_msg_proc_;
161 unsigned int ttc_extract_lines_;
162 unsigned int ttc_clustering_;
165 #ifdef HAVE_VISUAL_DEBUGGING 166 ros::Publisher *vispub_;
Line information container.
virtual void init()
Initialize the thread.
Thread aspect to access to BlackBoard.
Thread aspect that allows to obtain the current time from the clock.
virtual void finalize()
Finalize the thread.
Thread aspect to get access to a ROS node handle.
Fawkes library namespace.
Thread aspect to provide and access point clouds.
LaserLinesThread()
Constructor.
Thread class encapsulation of pthreads.
LaserLineInterface Fawkes BlackBoard Interface.
Main thread of laser-lines plugin.
virtual void loop()
Code to execute in the thread.
Thread aspect to use blocked timing.
SwitchInterface Fawkes BlackBoard Interface.
virtual ~LaserLinesThread()
Destructor.
Thread aspect to log output.
Thread aspect to access configuration data.
RefPtr<> is a reference-counting shared smartpointer.
virtual void run()
Stub to see name in backtrace for easier debugging.