Fawkes API Fawkes Development Version
laser-cluster-thread.h
1
2/***************************************************************************
3 * laser-cluster-thread.h - Thread to detect a cluster in 2D laser data
4 *
5 * Created: Sun Apr 21 01:17:09 2013
6 * Copyright 2011-2015 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_CLUSTER_LASER_CLUSTER_THREAD_H_
23#define _PLUGINS_LASER_CLUSTER_LASER_CLUSTER_THREAD_H_
24
25// must be first for reliable ROS detection
26#include <aspect/blackboard.h>
27#include <aspect/blocked_timing.h>
28#include <aspect/clock.h>
29#include <aspect/configurable.h>
30#include <aspect/logging.h>
31#include <aspect/pointcloud.h>
32#include <aspect/tf.h>
33#include <core/threading/thread.h>
34#include <pcl/point_cloud.h>
35#include <pcl/point_types.h>
36#include <pcl/segmentation/sac_segmentation.h>
37
38#include <Eigen/StdVector>
39
40namespace fawkes {
41class Position3DInterface;
42class SwitchInterface;
43#ifdef USE_TIMETRACKER
44class TimeTracker;
45#endif
46class LaserClusterInterface;
47} // namespace fawkes
48
57{
58public:
59 LaserClusterThread(std::string &cfg_name, std::string &cfg_prefix);
60 virtual ~LaserClusterThread();
61
62 virtual void init();
63 virtual void loop();
64 virtual void finalize();
65
66private:
67 typedef pcl::PointXYZ PointType;
69 typedef Cloud::Ptr CloudPtr;
70 typedef Cloud::ConstPtr CloudConstPtr;
71
72 typedef pcl::PointXYZRGB ColorPointType;
74 typedef ColorCloud::Ptr ColorCloudPtr;
75 typedef ColorCloud::ConstPtr ColorCloudConstPtr;
76
77 typedef pcl::PointXYZL LabelPointType;
79 typedef LabelCloud::Ptr LabelCloudPtr;
80 typedef LabelCloud::ConstPtr LabelCloudConstPtr;
81
82private:
83 void set_position(fawkes::Position3DInterface *iface,
84 bool is_visible,
85 const Eigen::Vector4f & centroid = Eigen::Vector4f(0, 0, 0, 0),
86 const Eigen::Quaternionf & rotation = Eigen::Quaternionf(1, 0, 0, 0));
87
88 float calc_line_length(CloudPtr cloud,
89 pcl::PointIndices::Ptr inliers,
90 pcl::ModelCoefficients::Ptr coeff);
91
92 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
93protected:
94 virtual void
96 {
97 Thread::run();
98 }
99
100private:
104 CloudConstPtr input_;
106 pcl::PointCloud<LabelPointType>::Ptr clusters_labeled_;
107
108 pcl::SACSegmentation<PointType> seg_;
109
110 std::vector<fawkes::Position3DInterface *> cluster_pos_ifs_;
111
112 fawkes::SwitchInterface * switch_if_;
114
115 typedef enum { SELECT_MIN_ANGLE, SELECT_MIN_DIST } selection_mode_t;
116
117 std::string cfg_name_;
118 std::string cfg_prefix_;
119
120 bool cfg_line_removal_;
121 unsigned int cfg_segm_max_iterations_;
122 float cfg_segm_distance_threshold_;
123 unsigned int cfg_segm_min_inliers_;
124 float cfg_segm_sample_max_dist_;
125 float cfg_line_min_length_;
126 float cfg_cluster_tolerance_;
127 unsigned int cfg_cluster_min_size_;
128 unsigned int cfg_cluster_max_size_;
129 std::string cfg_input_pcl_;
130 std::string cfg_result_frame_;
131 float cfg_bbox_min_x_;
132 float cfg_bbox_max_x_;
133 float cfg_bbox_min_y_;
134 float cfg_bbox_max_y_;
135 bool cfg_use_bbox_;
136 float cfg_switch_tolerance_;
137 float cfg_offset_x_;
138 float cfg_offset_y_;
139 float cfg_offset_z_;
140 selection_mode_t cfg_selection_mode_;
141 unsigned int cfg_max_num_clusters_;
142
143 std::string output_cluster_name_;
144 std::string output_cluster_labeled_name_;
145
146 float current_max_x_;
147
148 unsigned int loop_count_;
149
150 class ClusterInfo
151 {
152 public:
153 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
154
155 double angle;
156 double dist;
157 unsigned int index;
158 Eigen::Vector4f centroid;
159 };
160
161#ifdef USE_TIMETRACKER
163 unsigned int tt_loopcount_;
164 unsigned int ttc_full_loop_;
165 unsigned int ttc_msg_proc_;
166 unsigned int ttc_extract_lines_;
167 unsigned int ttc_clustering_;
168#endif
169};
170
171#endif
Main thread of laser-cluster plugin.
virtual ~LaserClusterThread()
Destructor.
virtual void run()
Stub to see name in backtrace for easier debugging.
LaserClusterThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
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
LaserClusterInterface Fawkes BlackBoard Interface.
Thread aspect to log output.
Definition: logging.h:33
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:38
Position3DInterface Fawkes BlackBoard Interface.
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.