sensproc_thread.h
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef __PLUGINS_LASERHT_SENSPROC_THREAD_H_
00026 #define __PLUGINS_LASERHT_SENSPROC_THREAD_H_
00027
00028 #include <core/threading/thread.h>
00029 #include <aspect/blocked_timing.h>
00030 #include <aspect/logging.h>
00031 #include <aspect/configurable.h>
00032 #include <aspect/blackboard.h>
00033
00034 #include <vector>
00035 #include <string>
00036
00037 namespace fawkes {
00038 class Laser360Interface;
00039 class ObjectPositionInterface;
00040 class VisualDisplay2DInterface;
00041 class TimeTracker;
00042 }
00043 class HoughTransform;
00044
00045 class LaserHtSensorProcThread
00046 : public fawkes::Thread,
00047 public fawkes::BlockedTimingAspect,
00048 public fawkes::LoggingAspect,
00049 public fawkes::ConfigurableAspect,
00050 public fawkes::BlackBoardAspect
00051 {
00052 public:
00053 LaserHtSensorProcThread();
00054
00055 virtual void init();
00056 virtual void finalize();
00057 virtual void loop();
00058
00059
00060 protected: virtual void run() { Thread::run(); }
00061
00062 private:
00063 typedef struct {
00064 float angle;
00065 float dist;
00066 float x;
00067 float y;
00068 } laser_reading_t;
00069
00070 void fit_line(const std::vector<laser_reading_t> &points,
00071 const unsigned int first_index,
00072 float &a, float &b, float &least_square_error);
00073
00074 void line_points_from_params(float r, float phi,
00075 float &x1, float &y1, float &x2, float &y2);
00076
00077 private:
00078 fawkes::Laser360Interface *__laser360_if;
00079 fawkes::ObjectPositionInterface *__line_if;
00080 fawkes::VisualDisplay2DInterface *__visdisp_if;
00081
00082 unsigned int __cfg_num_samples;
00083 unsigned int __cfg_vote_threshold;
00084 float __cfg_r_scale;
00085 std::string __cfg_laser_ifid;
00086 bool __cfg_enable_disp;
00087 float __cfg_fitting_error_threshold;
00088 float __cfg_dist_threshold;
00089
00090
00091 HoughTransform *__ht;
00092 unsigned int __num_vals;
00093 int **__values;
00094 float __angle_step;
00095 float __r_scale;
00096
00097 #ifdef LASERHT_TIMETRACKER
00098 fawkes::TimeTracker *__tt;
00099 unsigned int __tt_loop;
00100 unsigned int __ttc_reset;
00101 unsigned int __ttc_process;
00102 unsigned int __ttc_fitting;
00103 unsigned int __ttc_total;
00104 #endif
00105 };
00106 #endif