amcl_fiducial.h

00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey et al.
00004  *
00005  *  This program is free software; you can redistribute it and/or modify
00006  *  it under the terms of the GNU General Public License as published by
00007  *  the Free Software Foundation; either version 2 of the License, or
00008  *  (at your option) any later version.
00009  *
00010  *  This program is distributed in the hope that it will be useful,
00011  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  *  GNU General Public License for more details.
00014  *
00015  *  You should have received a copy of the GNU General Public License
00016  *  along with this program; if not, write to the Free Software
00017  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018  *
00019  */
00021 //
00022 // Desc: FIDUCIAL sensor model for AMCL
00023 // Author: David Feil-Seifer
00024 // Date: 17 Aug 2003
00025 // CVS: $Id: amcl_fiducial.h 2951 2005-08-19 00:48:20Z gerkey $
00026 //
00028 
00029 #ifndef AMCL_FIDUCIAL_H
00030 #define AMCL_FIDUCIAL_H
00031 
00032 #include "amcl_sensor.h"
00033 #include "map/map.h"
00034 #include "models/laser.h"
00035 
00036 // Laser sensor data
00037 class AMCLFiducialData : public AMCLSensorData
00038 {
00039   // Laser range data (range, bearing tuples)
00040   public: int fiducial_count;
00041   public: double fiducials[PLAYER_FIDUCIAL_MAX_SAMPLES][3];
00042 };
00043 
00044 
00045 class AMCLFiducialMap
00046 {
00047   //x,y,id
00048   public: int fiducial_count;
00049   public: double origin_x;
00050   public: double origin_y;
00051   public: double scale;
00052   public: double fiducials[100][3];
00053 };
00054 
00055 double fiducial_map_calc_range( AMCLFiducialMap* fmap, double ox, double oy, double oa, double max_range, int id, int k);
00056 double fiducial_map_calc_bearing( AMCLFiducialMap* fmap, double ox, double oy, double oa, double max_range, int id, int k);
00057 
00058 AMCLFiducialMap* fiducial_map_alloc();
00059 
00060 // Laseretric sensor model
00061 class AMCLFiducial : public AMCLSensor
00062 {
00063   // Default constructor
00064   public: AMCLFiducial(player_devaddr_t addr);
00065   
00066   // Load the model
00067   public: virtual int Load(ConfigFile* cf, int section);
00068 
00069   // Unload the model
00070   public: virtual int Unload(void);
00071 
00072   // Initialize the model
00073   public: virtual int Setup(void);
00074 
00075   // Finalize the model
00076   public: virtual int Shutdown(void);
00077 
00078   // Check for new sensor measurements
00079   private: virtual AMCLSensorData *GetData(void);
00080   
00081   // Update the filter based on the sensor model.  Returns true if the
00082   // filter has been updated.
00083   public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
00084 
00085   // Determine the probability for the given pose
00086   private: static double SensorModel(AMCLFiducialData *data, pf_vector_t pose);
00087 
00088   // retrieve the map
00089   private: int SetupMap(void);
00090 
00091   private: int read_map_file(const char *);
00092 
00093   // Device info
00094   private: player_device_id_t fiducial_id;
00095   private: player_device_id_t map_id;
00096   private: Driver *driver;
00097 
00098   // Current data timestamp
00099   private: double time;
00100 
00101   // The laser map
00102   private: map_t *map;
00103 
00104   // The fiducial map
00105   private: AMCLFiducialMap *fmap;
00106 
00107   // Laser offset relative to robot
00108   private: pf_vector_t laser_pose;
00109   
00110   // Max valid laser range
00111   private: double range_max;
00112   
00113   // Laser range variance
00114   private: double range_var;
00115 
00116   // Laser angle variance
00117   private: double angle_var;
00118 
00119   // Probability of bad range readings
00120   private: double range_bad;
00121 
00122   // Probability of bad angle readings
00123   private: double angle_bad;
00124 
00125 #ifdef INCLUDE_RTKGUI
00126   // Setup the GUI
00127   private: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
00128 
00129   // Finalize the GUI
00130   private: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
00131 
00132   // Draw sensor data
00133   public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data);
00134 
00135   // Figures
00136   private: rtk_fig_t *fig, *map_fig;
00137 #endif
00138 
00139 };
00140 
00141 
00142 
00143 
00144 #endif

Last updated 12 September 2005 21:38:45