Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
sensproc_thread.h
1 
2 /***************************************************************************
3  * sensproc_thread.h - Laser HT sensor processing thread
4  *
5  * Created: Sat Jul 04 21:34:36 2009 (RoboCup 2009, Graz)
6  * Copyright 2006-2008 Tim Niemueller [www.niemueller.de]
7  *
8  * $Id: sensor_thread.h 2627 2009-06-25 18:08:09Z tim $
9  *
10  ****************************************************************************/
11 
12 /* This program is free software; you can redistribute it and/or modify
13  * it under the terms of the GNU General Public License as published by
14  * the Free Software Foundation; either version 2 of the License, or
15  * (at your option) any later version.
16  *
17  * This program is distributed in the hope that it will be useful,
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20  * GNU Library General Public License for more details.
21  *
22  * Read the full text in the LICENSE.GPL file in the doc directory.
23  */
24 
25 #ifndef __PLUGINS_LASERHT_SENSPROC_THREAD_H_
26 #define __PLUGINS_LASERHT_SENSPROC_THREAD_H_
27 
28 #include <core/threading/thread.h>
29 #include <aspect/blocked_timing.h>
30 #include <aspect/logging.h>
31 #include <aspect/configurable.h>
32 #include <aspect/blackboard.h>
33 
34 #include <vector>
35 #include <string>
36 
37 namespace fawkes {
38  class Laser360Interface;
39  class ObjectPositionInterface;
40  class VisualDisplay2DInterface;
41  class TimeTracker;
42 }
43 class HoughTransform;
44 
46 : public fawkes::Thread,
48  public fawkes::LoggingAspect,
51 {
52  public:
54 
55  virtual void init();
56  virtual void finalize();
57  virtual void loop();
58 
59  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
60  protected: virtual void run() { Thread::run(); }
61 
62  private:
63  /// @cond INTERNALS
64  typedef struct {
65  float angle;
66  float dist;
67  float x;
68  float y;
69  } laser_reading_t;
70  /// @endcond
71 
72  void fit_line(const std::vector<laser_reading_t> &points,
73  const unsigned int first_index,
74  float &a, float &b, float &least_square_error);
75 
76  void line_points_from_params(float r, float phi,
77  float &x1, float &y1, float &x2, float &y2);
78 
79  private:
80  fawkes::Laser360Interface *__laser360_if;
83 
84  unsigned int __cfg_num_samples;
85  unsigned int __cfg_vote_threshold;
86  float __cfg_r_scale;
87  std::string __cfg_laser_ifid;
88  bool __cfg_enable_disp;
89  float __cfg_fitting_error_threshold;
90  float __cfg_dist_threshold;
91 
92 
93  HoughTransform *__ht;
94  unsigned int __num_vals;
95  int **__values;
96  float __angle_step;
97  float __r_scale;
98 
99 #ifdef LASERHT_TIMETRACKER
100  fawkes::TimeTracker *__tt;
101  unsigned int __tt_loop;
102  unsigned int __ttc_reset;
103  unsigned int __ttc_process;
104  unsigned int __ttc_fitting;
105  unsigned int __ttc_total;
106 #endif
107 };
108 #endif