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_;
Main thread of laser-lines plugin.
virtual void config_comment_changed(const fawkes::Configuration::ValueIterator *)
Called whenever a comment of a watched value has changed.
virtual void config_value_changed(const fawkes::Configuration::ValueIterator *v)
Called whenever a watched value has changed.
virtual ~LaserLinesThread()
Destructor.
virtual void finalize()
Finalize the thread.
LaserLinesThread()
Constructor.
virtual void read_config()
Read all configuration values.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual void config_tag_changed(const char *)
Called whenever the tag has changed.
virtual void config_value_erased(const char *path)
Called whenever a value has been erased from the config.
virtual void run()
Stub to see name in backtrace for easier debugging.
Line information container.
Container for a line with tracking and smoothing info.
Thread aspect to access to BlackBoard.
Thread aspect to use blocked timing.
Thread aspect that allows to obtain the current time from the clock.
Thread aspect to access configuration data.
Interface for configuration change handling.
Iterator interface to iterate over config values.
LaserLineInterface Fawkes BlackBoard Interface.
Thread aspect to log output.
Thread aspect to provide and access point clouds.
Thread aspect to get access to a ROS node handle.
RefPtr<> is a reference-counting shared smartpointer.
SwitchInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
Fawkes library namespace.