amcl_laser.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: LASER sensor model for AMCL 00023 // Author: Andrew Howard 00024 // Date: 17 Aug 2003 00025 // CVS: $Id: amcl_laser.h 6443 2008-05-15 19:46:11Z gerkey $ 00026 // 00028 00029 #ifndef AMCL_LASER_H 00030 #define AMCL_LASER_H 00031 00032 #include "amcl_sensor.h" 00033 #include "map/map.h" 00034 #include "models/laser.h" 00035 00036 // Laser sensor data 00037 class AMCLLaserData : public AMCLSensorData 00038 { 00039 public: 00040 AMCLLaserData () {ranges=NULL;}; 00041 virtual ~AMCLLaserData() {delete [] ranges;}; 00042 // Laser range data (range, bearing tuples) 00043 public: int range_count; 00044 public: double range_max; 00045 public: double (*ranges)[2]; 00046 }; 00047 00048 00049 // Laseretric sensor model 00050 class AMCLLaser : public AMCLSensor 00051 { 00052 // Default constructor 00053 public: AMCLLaser(AdaptiveMCL & aAMCL, player_devaddr_t addr); 00054 00055 // Load the model 00056 public: virtual int Load(ConfigFile* cf, int section); 00057 00058 // Unload the model 00059 public: virtual int Unload(void); 00060 00061 // Initialize the model 00062 public: virtual int Setup(void); 00063 00064 // Finalize the model 00065 public: virtual int Shutdown(void); 00066 00067 // Process message for this interface 00068 public: virtual int ProcessMessage(QueuePointer &resp_queue, 00069 player_msghdr * hdr, 00070 void * data); 00071 // Check for new sensor measurements 00072 //private: virtual AMCLSensorData *GetData(void); 00073 00074 // Update the filter based on the sensor model. Returns true if the 00075 // filter has been updated. 00076 public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data); 00077 00078 // Determine the probability for the given pose 00079 private: static double SensorModel(AMCLLaserData *data, 00080 pf_sample_set_t* set); 00081 00082 // retrieve the map 00083 private: int SetupMap(void); 00084 00085 // Device info 00086 private: player_devaddr_t laser_addr; 00087 private: player_devaddr_t map_addr; 00088 private: Device *laser_dev; 00089 00090 // Current data timestamp 00091 private: double time; 00092 00093 // The laser map 00094 private: map_t *map; 00095 00096 // Laser offset relative to robot 00097 private: pf_vector_t laser_pose; 00098 00099 // Max beams to consider 00100 private: int max_beams; 00101 00102 // Laser range variance 00103 private: double range_var; 00104 00105 // Probability of bad range readings 00106 private: double range_bad; 00107 00108 #ifdef INCLUDE_RTKGUI 00109 // Setup the GUI 00110 private: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); 00111 00112 // Finalize the GUI 00113 private: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); 00114 00115 // Draw sensor data 00116 public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data); 00117 00118 // Figures 00119 private: rtk_fig_t *fig, *map_fig; 00120 #endif 00121 }; 00122 00123 00124 00125 00126 #endif