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