amcl.h
1/*
2 * Player - One Hell of a Robot Server
3 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
4 * gerkey@usc.edu kaspers@robotics.usc.edu
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19 *
20 */
22//
23// Desc: Adaptive Monte-Carlo localization
24// Author: Andrew Howard
25// Date: 6 Feb 2003
26// CVS: $Id$
27//
28// Theory of operation:
29// TODO
30//
31// Requires: position (odometry), laser, sonar, gps
32// Provides: localization
33//
35
36#ifndef AMCL_H
37#define AMCL_H
38
39#include <pthread.h>
40
41#include <playerconfig.h>
42#ifdef INCLUDE_RTKGUI
43#include "rtk.h"
44#endif
45
46#include <libplayercore/playercore.h>
47
48#include "pf/pf.h"
49//#include "amcl_sensor.h"
50class AMCLSensor;
51class AMCLSensorData;
52
53// Pose hypothesis
54typedef struct
55{
56 // Total weight (weights sum to 1)
57 double weight;
58
59 // Mean of pose esimate
60 pf_vector_t pf_pose_mean;
61
62 // Covariance of pose estimate
63 pf_matrix_t pf_pose_cov;
64
66
67
68
69// Incremental navigation driver
71{
73 // Top half methods; these methods run in the server thread (except for
74 // the sensor Init and Update functions, which run in the driver thread).
76
77 // Constructor
78 public: AdaptiveMCL(ConfigFile* cf, int section);
79
80 // Destructor
81 public: virtual ~AdaptiveMCL(void);
82
83 // Setup/shutdown routines.
84 public: virtual int MainSetup(void);
85
87 // Middle methods: these methods facilitate communication between the top
88 // and bottom halfs.
90
92 // Bottom half methods; these methods run in the device thread
94
95 // Push data onto the queue
96 public: void Push(AMCLSensorData *data);
97
98 // Take a peek at the queue
99 private: AMCLSensorData *Peek(void);
100
101 // Pop data from the queue
102 private: AMCLSensorData *Pop(void);
103
104 // MessageHandler
105 public: virtual int ProcessMessage(QueuePointer &resp_queue,
106 player_msghdr * hdr,
107 void * data);
108
109 // Check for updated sensor data
110 public: virtual void UpdateSensorData(void);
111
112 // Main function for device thread.
113 private: virtual void Main(void);
114
115 // Device thread finalization
116 private: virtual void MainQuit();
117
118 // Initialize the filter
119 private: void InitFilter(void);
120
121 // Update/initialize the filter with new sensor data
122 private: bool UpdateFilter();
123
124 // Put new localization data
125 private: void PutDataLocalize(double time);
126
127 // Put new position data
128 private: void PutDataPosition(pf_vector_t delta, double time);
129
130 // Send back geometry data
131 private: void ProcessGeom(QueuePointer &resp_queue, player_msghdr_t* hdr);
132
133#ifdef INCLUDE_RTKGUI
134 // Set up the GUI
135 private: int SetupGUI(void);
136
137 // Shut down the GUI
138 private: int ShutdownGUI(void);
139
140 // Update the GUI
141 private: void UpdateGUI(void);
142
143 // Draw the current best pose estimate
144 private: void DrawPoseEst();
145#endif
146
148 // Properties
150
151 // interfaces we might be using
152 private: player_devaddr_t position_addr;
153 private: player_devaddr_t localize_addr;
154
155 // List of all sensors
156 private: int sensor_count;
157 private: AMCLSensor *sensors[16];
158
159 // Index of sensor providing initialization model
160 private: int init_sensor;
161
162 // Index of sensor providing action model
163 private: int action_sensor;
164
165 // Particle filter
166 private: pf_t *pf;
167 private: int pf_min_samples, pf_max_samples;
168 private: double pf_err, pf_z;
169
170 // Sensor data queue
171 private: int q_size, q_start, q_len;
172 private: AMCLSensorData **q_data;
173
174 // Current particle filter pose estimates
175 private: int hyp_count;
176 private: int hyp_alloc;
177 private: amcl_hyp_t *hyps;
178 private: pf_vector_t best_hyp;
179 private: pthread_mutex_t best_hyp_lock;
180
181 // Has the filter been initialized?
182 private: bool pf_init;
183 private: bool pf_init_internal;
184
185 // Initial pose estimate; used for filter initialization
186 private: pf_vector_t pf_init_pose_mean;
187 private: pf_matrix_t pf_init_pose_cov;
188
189 // Last odometric pose value used to update filter
190 private: pf_vector_t pf_odom_pose;
191
192 // Minimum update distances
193 private: double min_dr, min_da;
194
195#ifdef INCLUDE_RTKGUI
196 // RTK stuff; for testing only
197 private: int enable_gui;
198 private: rtk_app_t *app;
199 private: rtk_canvas_t *canvas;
200 private: rtk_fig_t *map_fig;
201 private: rtk_fig_t *pf_fig;
202 private: rtk_fig_t *robot_fig;
203#endif
204
205#ifdef INCLUDE_OUTFILE
206 private: FILE *outfile;
207#endif
208};
209
210#endif
Definition amcl_sensor.h:106
Definition amcl_sensor.h:44
Definition amcl.h:71
virtual void Main(void)
Main method for driver thread.
Definition amcl.cc:625
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition amcl.cc:508
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition amcl.cc:717
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition amcl.cc:1092
Class for loading configuration file information.
Definition configfile.h:197
An autopointer for the message queue.
Definition message.h:74
Base class for drivers which oeprate with a thread.
Definition driver.h:553
Definition pf.h:111
Definition amcl.h:55
Definition pf_vector.h:49
Definition pf_vector.h:42
A device address.
Definition player.h:146
Generic message header.
Definition player.h:162