sensproc_thread.h

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

Generated on Tue Feb 22 13:32:30 2011 for Fawkes API by  doxygen 1.4.7