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 
47 namespace ros {
48 class Publisher;
49 }
50 #endif
51 
52 namespace fawkes {
53 class Position3DInterface;
54 class SwitchInterface;
55 #ifdef USE_TIMETRACKER
56 class TimeTracker;
57 #endif
58 class LaserLineInterface;
59 } // namespace fawkes
60 
62  public fawkes::ClockAspect,
63  public fawkes::LoggingAspect,
69 #ifdef HAVE_VISUAL_DEBUGGING
70  public fawkes::ROSAspect,
71 #endif
73 {
74 public:
76  virtual ~LaserLinesThread();
77 
78  virtual void init();
79  virtual void loop();
80  virtual void finalize();
81 
82 private:
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() */
94 protected:
95  virtual void
96  run()
97  {
98  Thread::run();
99  }
100 
101 protected:
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 
122 private:
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 
147 private:
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
181  fawkes::TimeTracker *tt_;
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.