Fawkes API Fawkes Development Version
laser-lines-thread.h
1
2/***************************************************************************
3 * laser-lines-thread.h - Thread to detect a lines in 2D laser data
4 *
5 * Created: Fri May 23 18:10:54 2014
6 * Copyright 2011-2014 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#ifndef _PLUGINS_LASER_LINES_LASER_LINES_THREAD_H_
23#define _PLUGINS_LASER_LINES_LASER_LINES_THREAD_H_
24
25// must be first for reliable ROS detection
26#include "line_info.h"
27
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>
40
41#include <Eigen/StdVector>
42
43#ifdef HAVE_VISUAL_DEBUGGING
44# include <plugins/ros/aspect/ros.h>
45# include <visualization_msgs/MarkerArray.h>
46
47namespace ros {
48class Publisher;
49}
50#endif
51
52namespace fawkes {
53class Position3DInterface;
54class SwitchInterface;
55#ifdef USE_TIMETRACKER
56class TimeTracker;
57#endif
58class LaserLineInterface;
59} // namespace fawkes
60
69#ifdef HAVE_VISUAL_DEBUGGING
70 public fawkes::ROSAspect,
71#endif
73{
74public:
76 virtual ~LaserLinesThread();
77
78 virtual void init();
79 virtual void loop();
80 virtual void finalize();
81
82private:
83 typedef pcl::PointXYZ PointType;
85 typedef Cloud::Ptr CloudPtr;
86 typedef Cloud::ConstPtr CloudConstPtr;
87
88 typedef pcl::PointXYZRGB ColorPointType;
90 typedef ColorCloud::Ptr ColorCloudPtr;
91 typedef ColorCloud::ConstPtr ColorCloudConstPtr;
92
93 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
94protected:
95 virtual void
97 {
98 Thread::run();
99 }
100
101protected:
102 virtual void read_config();
103 virtual void
104 config_tag_changed(const char *)
105 {
106 }
107 virtual void
109 {
110 }
111 virtual void
113 {
114 read_config();
115 }
116 virtual void
117 config_value_erased(const char *path)
118 {
119 read_config();
120 }
121
122private:
123 void update_lines(std::vector<LineInfo> &);
124 void publish_known_lines();
125
126 void set_interface(unsigned int idx,
128 bool moving_average,
129 const TrackedLineInfo & tinfo,
130 const std::string & frame_id = "");
131
132 void set_empty_interface(fawkes::LaserLineInterface *iface) const;
133
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);
138
139 void publish_visualization_add_line(visualization_msgs::MarkerArray &m,
140 unsigned int & idnum,
141 const LineInfo & info,
142 const size_t i,
143 const std::string & marker_namespace,
144 const std::string & name_suffix = "");
145#endif
146
147private:
150 CloudConstPtr input_;
152
153 std::vector<fawkes::LaserLineInterface *> line_ifs_;
154 std::vector<fawkes::LaserLineInterface *> line_avg_ifs_;
155 std::vector<TrackedLineInfo> known_lines_;
156
157 fawkes::SwitchInterface *switch_if_;
158
159 typedef enum { SELECT_MIN_ANGLE, SELECT_MIN_DIST } selection_mode_t;
160
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_;
172 float cfg_min_dist_;
173 float cfg_max_dist_;
174 bool cfg_moving_avg_enabled_;
175 unsigned int cfg_moving_avg_window_size_;
176 std::string cfg_tracking_frame_id_;
177
178 unsigned int loop_count_;
179
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_;
187#endif
188
189#ifdef HAVE_VISUAL_DEBUGGING
190 ros::Publisher *vispub_;
191 size_t last_id_num_;
192#endif
193};
194
195#endif
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.
Definition: line_info.h:40
Container for a line with tracking and smoothing info.
Definition: line_info.h:60
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect to use blocked timing.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Interface for configuration change handling.
Iterator interface to iterate over config values.
Definition: config.h:75
LaserLineInterface Fawkes BlackBoard Interface.
Thread aspect to log output.
Definition: logging.h:33
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:38
Thread aspect to get access to a ROS node handle.
Definition: ros.h:39
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
SwitchInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
Definition: thread.h:46
Time tracking utility.
Definition: tracker.h:37
Thread aspect to access the transform system.
Definition: tf.h:39
Fawkes library namespace.