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 <pcl/point_cloud.h>
27 #include <pcl/point_types.h>
28 
29 #include "line_info.h"
30 
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>
39 
40 #include <Eigen/StdVector>
41 #include <pcl/ModelCoefficients.h>
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 
53 namespace fawkes {
54  class Position3DInterface;
55  class SwitchInterface;
56 #ifdef USE_TIMETRACKER
57  class TimeTracker;
58 #endif
59  class LaserLineInterface;
60 }
61 
63 : public fawkes::Thread,
64  public fawkes::ClockAspect,
65  public fawkes::LoggingAspect,
70 #ifdef HAVE_VISUAL_DEBUGGING
71  public fawkes::ROSAspect,
72 #endif
74 {
75  public:
77  virtual ~LaserLinesThread();
78 
79  virtual void init();
80  virtual void loop();
81  virtual void finalize();
82 
83  private:
84  typedef pcl::PointXYZ PointType;
86  typedef Cloud::Ptr CloudPtr;
87  typedef Cloud::ConstPtr CloudConstPtr;
88 
89  typedef pcl::PointXYZRGB ColorPointType;
91  typedef ColorCloud::Ptr ColorCloudPtr;
92  typedef ColorCloud::ConstPtr ColorCloudConstPtr;
93 
94  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
95  protected: virtual void run() { Thread::run(); }
96 
97  private:
98 
99  void set_line(unsigned int idx,
101  bool is_visible,
102  const std::string &frame_id = "",
103  const LineInfo &info = LineInfo());
104 
105 
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);
110 
111  void publish_visualization_add_line(visualization_msgs::MarkerArray &m,
112  unsigned int &idnum,
113  const LineInfo &info,
114  const size_t i,
115  const std::string &marker_namespace,
116  const std::string &name_suffix = "");
117 #endif
118 
119  private:
122  CloudConstPtr input_;
124 
125  std::vector<fawkes::LaserLineInterface *> line_ifs_;
126  std::vector<fawkes::LaserLineInterface *> line_avg_ifs_;
127  std::vector<TrackedLineInfo> known_lines_;
128 
129  fawkes::SwitchInterface *switch_if_;
130 
131  typedef enum {
132  SELECT_MIN_ANGLE,
133  SELECT_MIN_DIST
134  } selection_mode_t;
135 
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_;
148  float cfg_min_dist_;
149  float cfg_max_dist_;
150  bool cfg_moving_avg_enabled_;
151  unsigned int cfg_moving_avg_window_size_;
152  std::string cfg_tracking_frame_id_;
153 
154  unsigned int loop_count_;
155 
156 #ifdef USE_TIMETRACKER
157  fawkes::TimeTracker *tt_;
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_;
163 #endif
164 
165 #ifdef HAVE_VISUAL_DEBUGGING
166  ros::Publisher *vispub_;
167  size_t last_id_num_;
168 #endif
169 
170 };
171 
172 #endif
Line information container.
Definition: line_info.h:38
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
Thread aspect to get access to a ROS node handle.
Definition: ros.h:39
Fawkes library namespace.
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:40
Thread class encapsulation of pthreads.
Definition: thread.h:42
LaserLineInterface Fawkes BlackBoard Interface.
Main thread of laser-lines plugin.
Thread aspect to access the transform system.
Definition: tf.h:42
Thread aspect to use blocked timing.
SwitchInterface Fawkes BlackBoard Interface.
Time tracking utility.
Definition: tracker.h:38
Definition: act_thread.h:37
Thread aspect to log output.
Definition: logging.h:35
Thread aspect to access configuration data.
Definition: configurable.h:35
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
virtual void run()
Stub to see name in backtrace for easier debugging.