amcl.h
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00004  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00022 //
00023 // Desc: Adaptive Monte-Carlo localization
00024 // Author: Andrew Howard
00025 // Date: 6 Feb 2003
00026 // CVS: $Id: amcl.h 7208 2008-12-19 02:10:56Z gbiggs $
00027 //
00028 // Theory of operation:
00029 //  TODO
00030 //
00031 // Requires: position (odometry), laser, sonar, gps
00032 // Provides: localization
00033 //
00035 
00036 #ifndef AMCL_H
00037 #define AMCL_H
00038 
00039 #include <pthread.h>
00040 
00041 #include <playerconfig.h>
00042 #ifdef INCLUDE_RTKGUI
00043 #include "rtk.h"
00044 #endif
00045 
00046 #include <libplayercore/playercore.h>
00047 
00048 #include "pf/pf.h"
00049 //#include "amcl_sensor.h"
00050 class AMCLSensor;
00051 class AMCLSensorData;
00052 
00053 // Pose hypothesis
00054 typedef struct
00055 {
00056   // Total weight (weights sum to 1)
00057   double weight;
00058 
00059   // Mean of pose esimate
00060   pf_vector_t pf_pose_mean;
00061 
00062   // Covariance of pose estimate
00063   pf_matrix_t pf_pose_cov;
00064 
00065 } amcl_hyp_t;
00066 
00067 
00068 
00069 // Incremental navigation driver
00070 class AdaptiveMCL : public ThreadedDriver
00071 {
00073   // Top half methods; these methods run in the server thread (except for
00074   // the sensor Init and Update functions, which run in the driver thread).
00076 
00077   // Constructor
00078   public: AdaptiveMCL(ConfigFile* cf, int section);
00079 
00080   // Destructor
00081   public: virtual ~AdaptiveMCL(void);
00082 
00083   // Setup/shutdown routines.
00084   public: virtual int MainSetup(void);
00085 
00087   // Middle methods: these methods facilitate communication between the top
00088   // and bottom halfs.
00090 
00092   // Bottom half methods; these methods run in the device thread
00094 
00095   // Push data onto the queue
00096   public: void Push(AMCLSensorData *data);
00097 
00098   // Take a peek at the queue
00099   private: AMCLSensorData *Peek(void);
00100 
00101   // Pop data from the queue
00102   private: AMCLSensorData *Pop(void);
00103 
00104   // MessageHandler
00105   public: virtual int ProcessMessage(QueuePointer &resp_queue,
00106                                      player_msghdr * hdr,
00107                                      void * data);
00108 
00109   // Check for updated sensor data
00110   public: virtual void UpdateSensorData(void);
00111 
00112   // Main function for device thread.
00113   private: virtual void Main(void);
00114 
00115   // Device thread finalization
00116   private: virtual void MainQuit();
00117 
00118   // Initialize the filter
00119   private: void InitFilter(void);
00120 
00121   // Update/initialize the filter with new sensor data
00122   private: bool UpdateFilter();
00123 
00124   // Put new localization data
00125   private: void PutDataLocalize(double time);
00126 
00127   // Put new position data
00128   private: void PutDataPosition(pf_vector_t delta, double time);
00129 
00130   // Send back geometry data
00131   private: void ProcessGeom(QueuePointer &resp_queue, player_msghdr_t* hdr);
00132 
00133 #ifdef INCLUDE_RTKGUI
00134   // Set up the GUI
00135   private: int SetupGUI(void);
00136 
00137   // Shut down the GUI
00138   private: int ShutdownGUI(void);
00139 
00140   // Update the GUI
00141   private: void UpdateGUI(void);
00142 
00143   // Draw the current best pose estimate
00144   private: void DrawPoseEst();
00145 #endif
00146 
00148   // Properties
00150 
00151   // interfaces we might be using
00152   private: player_devaddr_t position_addr;
00153   private: player_devaddr_t localize_addr;
00154 
00155   // List of all sensors
00156   private: int sensor_count;
00157   private: AMCLSensor *sensors[16];
00158 
00159   // Index of sensor providing initialization model
00160   private: int init_sensor;
00161 
00162   // Index of sensor providing action model
00163   private: int action_sensor;
00164 
00165   // Particle filter
00166   private: pf_t *pf;
00167   private: int pf_min_samples, pf_max_samples;
00168   private: double pf_err, pf_z;
00169 
00170   // Sensor data queue
00171   private: int q_size, q_start, q_len;
00172   private: AMCLSensorData **q_data;
00173 
00174   // Current particle filter pose estimates
00175   private: int hyp_count;
00176   private: int hyp_alloc;
00177   private: amcl_hyp_t *hyps;
00178   private: pf_vector_t best_hyp;
00179   private: pthread_mutex_t best_hyp_lock;
00180 
00181   // Has the filter been initialized?
00182   private: bool pf_init;
00183   private: bool pf_init_internal;
00184 
00185   // Initial pose estimate; used for filter initialization
00186   private: pf_vector_t pf_init_pose_mean;
00187   private: pf_matrix_t pf_init_pose_cov;
00188 
00189   // Last odometric pose value used to update filter
00190   private: pf_vector_t pf_odom_pose;
00191 
00192   // Minimum update distances
00193   private: double min_dr, min_da;
00194 
00195 #ifdef INCLUDE_RTKGUI
00196   // RTK stuff; for testing only
00197   private: int enable_gui;
00198   private: rtk_app_t *app;
00199   private: rtk_canvas_t *canvas;
00200   private: rtk_fig_t *map_fig;
00201   private: rtk_fig_t *pf_fig;
00202   private: rtk_fig_t *robot_fig;
00203 #endif
00204 
00205 #ifdef INCLUDE_OUTFILE
00206   private: FILE *outfile;
00207 #endif
00208 };
00209 
00210 #endif