Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * sensproc_thread.h - Laser HT sensor processing thread 00004 * 00005 * Created: Sat Jul 04 21:34:36 2009 (RoboCup 2009, Graz) 00006 * Copyright 2006-2008 Tim Niemueller [www.niemueller.de] 00007 * 00008 * $Id: sensor_thread.h 2627 2009-06-25 18:08:09Z tim $ 00009 * 00010 ****************************************************************************/ 00011 00012 /* This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU General Public License as published by 00014 * the Free Software Foundation; either version 2 of the License, or 00015 * (at your option) any later version. 00016 * 00017 * This program is distributed in the hope that it will be useful, 00018 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00019 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00020 * GNU Library General Public License for more details. 00021 * 00022 * Read the full text in the LICENSE.GPL file in the doc directory. 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 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ 00060 protected: virtual void run() { Thread::run(); } 00061 00062 private: 00063 /// @cond INTERNALS 00064 typedef struct { 00065 float angle; 00066 float dist; 00067 float x; 00068 float y; 00069 } laser_reading_t; 00070 /// @endcond 00071 00072 void fit_line(const std::vector<laser_reading_t> &points, 00073 const unsigned int first_index, 00074 float &a, float &b, float &least_square_error); 00075 00076 void line_points_from_params(float r, float phi, 00077 float &x1, float &y1, float &x2, float &y2); 00078 00079 private: 00080 fawkes::Laser360Interface *__laser360_if; 00081 fawkes::ObjectPositionInterface *__line_if; 00082 fawkes::VisualDisplay2DInterface *__visdisp_if; 00083 00084 unsigned int __cfg_num_samples; 00085 unsigned int __cfg_vote_threshold; 00086 float __cfg_r_scale; 00087 std::string __cfg_laser_ifid; 00088 bool __cfg_enable_disp; 00089 float __cfg_fitting_error_threshold; 00090 float __cfg_dist_threshold; 00091 00092 00093 HoughTransform *__ht; 00094 unsigned int __num_vals; 00095 int **__values; 00096 float __angle_step; 00097 float __r_scale; 00098 00099 #ifdef LASERHT_TIMETRACKER 00100 fawkes::TimeTracker *__tt; 00101 unsigned int __tt_loop; 00102 unsigned int __ttc_reset; 00103 unsigned int __ttc_process; 00104 unsigned int __ttc_fitting; 00105 unsigned int __ttc_total; 00106 #endif 00107 }; 00108 #endif