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_;
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.
virtual void config_value_changed(const fawkes::Configuration::ValueIterator *v)
Called whenever a watched value has changed.
Thread aspect to get access to a ROS node handle.
virtual void config_tag_changed(const char *)
Called whenever the tag has changed.
Fawkes library namespace.
Thread aspect to provide and access point clouds.
LaserLinesThread()
Constructor.
Interface for configuration change handling.
Thread class encapsulation of pthreads.
LaserLineInterface Fawkes BlackBoard Interface.
Main thread of laser-lines plugin.
virtual void loop()
Code to execute in the thread.
virtual void config_comment_changed(const fawkes::Configuration::ValueIterator *)
Called whenever a comment of a watched value has changed.
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 config_value_erased(const char *path)
Called whenever a value has been erased from the config.
Iterator interface to iterate over config values.
Container for a line with tracking and smoothing info.
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void read_config()
Read all configuration values.