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 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