Fawkes API  Fawkes Development Version
amcl_odom.h
1 /***************************************************************************
2  * amcl_odom.h: Odometry sensor model for AMCL
3  *
4  * Created: Thu May 24 18:51:17 2012
5  * Copyright 2000 Brian Gerkey
6  * 2000 Kasper Stoy
7  * 2012 Tim Niemueller [www.niemueller.de]
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 /* From:
24  * Player - One Hell of a Robot Server (LGPL)
25  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
26  * gerkey@usc.edu kaspers@robotics.usc.edu
27  */
28 ///////////////////////////////////////////////////////////////////////////
29 // Desc: Odometry sensor model for AMCL
30 // Author: Andrew Howard
31 // Date: 17 Aug 2003
32 ///////////////////////////////////////////////////////////////////////////
33 
34 #ifndef AMCL_ODOM_H
35 #define AMCL_ODOM_H
36 
37 #include "../pf/pf_pdf.h"
38 #include "amcl_sensor.h"
39 
40 /// @cond EXTERNAL
41 
42 namespace amcl {
43 
44 typedef enum { ODOM_MODEL_DIFF, ODOM_MODEL_OMNI } odom_model_t;
45 
46 // Odometric sensor data
47 class AMCLOdomData : public AMCLSensorData
48 {
49  // Odometric pose
50 public:
51  pf_vector_t pose;
52 
53  // Change in odometric pose
54 public:
55  pf_vector_t delta;
56 };
57 
58 // Odometric sensor model
59 class AMCLOdom : public AMCLSensor
60 {
61  // Default constructor
62 public:
63  AMCLOdom();
64 
65 public:
66  void SetModelDiff(double alpha1, double alpha2, double alpha3, double alpha4);
67 
68 public:
69  void SetModelOmni(double alpha1, double alpha2, double alpha3, double alpha4, double alpha5);
70 
71  // Update the filter based on the action model. Returns true if the filter
72  // has been updated.
73 public:
74  virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
75 
76  // Current data timestamp
77 private:
78  double time;
79 
80  // Model type
81 private:
82  odom_model_t model_type;
83 
84  // Drift parameters
85 private:
86  double alpha1, alpha2, alpha3, alpha4, alpha5;
87 };
88 
89 } // namespace amcl
90 
91 /// @endcond
92 
93 #endif