amcl_odom.h
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2000 Brian Gerkey et al.
4  *
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
18  *
19  */
21 //
22 // Desc: Odometry sensor model for AMCL
23 // Author: Andrew Howard
24 // Date: 17 Aug 2003
25 // CVS: $Id$
26 //
28 
29 #ifndef AMCL_ODOM_H
30 #define AMCL_ODOM_H
31 
32 #include "amcl_sensor.h"
33 #include "pf/pf_pdf.h"
34 
35 
36 // Odometric sensor data
38 {
39  // Odometric pose
40  public: pf_vector_t pose;
41 
42  // Change in odometric pose
43  public: pf_vector_t delta;
44 };
45 
46 
47 // Odometric sensor model
48 class AMCLOdom : public AMCLSensor
49 {
50  // Default constructor
51  public: AMCLOdom(AdaptiveMCL & aAMCL, player_devaddr_t addr);
52 
53  // Load the model
54  public: virtual int Load(ConfigFile* cf, int section);
55 
56  // Unload the model
57  public: virtual int Unload(void);
58 
59  // Initialize the model
60  public: virtual int Setup(void);
61 
62  // Finalize the model
63  public: virtual int Shutdown(void);
64 
65  // Process message for this interface
66  public: virtual int ProcessMessage(QueuePointer &resp_queue,
67  player_msghdr * hdr,
68  void * data);
69  // Check for new sensor measurements
70  //private: virtual AMCLSensorData *GetData(void);
71 
72  // Update the filter based on the action model. Returns true if the filter
73  // has been updated.
74  public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
75 
76  // The action model callback (static method)
77  public: static void ActionModel(AMCLOdom *self, pf_sample_set_t* set);
78 
79  // Device info
80  private: player_devaddr_t odom_addr;
81  private: Device *odom_dev;
82 
83  // Current data timestamp
84  private: double time;
85 
86  // Drift model
87  private: pf_matrix_t drift;
88 
89  // PDF used to generate action samples
90  private: pf_pdf_gaussian_t *action_pdf;
91 
92 #ifdef INCLUDE_RTKGUI
93  // Setup the GUI
94  private: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
95 
96  // Finalize the GUI
97  private: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
98 #endif
99 };
100 
101 
102 
103 
104 #endif
Definition: amcl_sensor.h:43
Definition: pf_vector.h:41
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
Definition: amcl_sensor.h:105
Definition: amcl.h:70
Generic message header.
Definition: player.h:161
int Subscribe(QueuePointer &sub_queue)
Subscribe the given queue to this device.
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:74
double px
X [m].
Definition: player.h:220
Definition: pf.h:93
double ReadTupleFloat(int section, const char *name, int index, double value)
Read a float (double) from a tuple field.
Definition: pf_pdf.h:48
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
QueuePointer InQueue
Queue for all incoming messages for this driver.
Definition: driver.h:285
double py
Y [m].
Definition: player.h:222
Definition: pf.h:110
Class for loading configuration file information.
Definition: configfile.h:196
A device address.
Definition: player.h:145
An autopointer for the message queue.
Definition: message.h:73
Definition: pf.h:62
Definition: amcl_odom.h:37
position2d data
Definition: player_interfaces.h:606
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:170
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
Definition: pf_vector.h:48
double pa
yaw [rad]
Definition: player.h:224
int Unsubscribe(QueuePointer &sub_queue)
Unsubscribe the given queue from this device.
Definition: amcl_odom.h:48