amcl_odom.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: Odometry sensor model for AMCL 00023 // Author: Andrew Howard 00024 // Date: 17 Aug 2003 00025 // CVS: $Id: amcl_odom.h 4135 2007-08-23 19:58:48Z gerkey $ 00026 // 00028 00029 #ifndef AMCL_ODOM_H 00030 #define AMCL_ODOM_H 00031 00032 #include "amcl_sensor.h" 00033 #include "pf/pf_pdf.h" 00034 00035 00036 // Odometric sensor data 00037 class AMCLOdomData : public AMCLSensorData 00038 { 00039 // Odometric pose 00040 public: pf_vector_t pose; 00041 00042 // Change in odometric pose 00043 public: pf_vector_t delta; 00044 }; 00045 00046 00047 // Odometric sensor model 00048 class AMCLOdom : public AMCLSensor 00049 { 00050 // Default constructor 00051 public: AMCLOdom(AdaptiveMCL & aAMCL, player_devaddr_t addr); 00052 00053 // Load the model 00054 public: virtual int Load(ConfigFile* cf, int section); 00055 00056 // Unload the model 00057 public: virtual int Unload(void); 00058 00059 // Initialize the model 00060 public: virtual int Setup(void); 00061 00062 // Finalize the model 00063 public: virtual int Shutdown(void); 00064 00065 // Process message for this interface 00066 public: virtual int ProcessMessage(QueuePointer &resp_queue, 00067 player_msghdr * hdr, 00068 void * data); 00069 // Check for new sensor measurements 00070 //private: virtual AMCLSensorData *GetData(void); 00071 00072 // Update the filter based on the action model. Returns true if the filter 00073 // has been updated. 00074 public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); 00075 00076 // The action model callback (static method) 00077 public: static void ActionModel(AMCLOdom *self, pf_sample_set_t* set); 00078 00079 // Device info 00080 private: player_devaddr_t odom_addr; 00081 private: Device *odom_dev; 00082 00083 // Current data timestamp 00084 private: double time; 00085 00086 // Drift model 00087 private: pf_matrix_t drift; 00088 00089 // PDF used to generate action samples 00090 private: pf_pdf_gaussian_t *action_pdf; 00091 00092 #ifdef INCLUDE_RTKGUI 00093 // Setup the GUI 00094 private: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); 00095 00096 // Finalize the GUI 00097 private: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); 00098 #endif 00099 }; 00100 00101 00102 00103 00104 #endif