Player
Frontpage
Contents
User
Installation
Quick start
Supported devices
Tutorials
Utilities
Client libraries
FAQ
Help
Developer
Architecture
libplayercore
interfaces
libplayerdrivers
drivers
libplayercommon
libplayerutils
libplayersd
libplayertcp
libplayerxdr
TODO
Online
Homepage
Download
Project
Bugs
Help
server
drivers
localization
amcl
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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19
*
20
*/
22
//
23
// Desc: Adaptive Monte-Carlo localization
24
// Author: Andrew Howard
25
// Date: 6 Feb 2003
26
// CVS: $Id: amcl.h 7208 2008-12-19 02:10:56Z gbiggs $
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"
50
class
AMCLSensor
;
51
class
AMCLSensorData
;
52
53
// Pose hypothesis
54
typedef
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
65
}
amcl_hyp_t
;
66
67
68
69
// Incremental navigation driver
70
class
AdaptiveMCL
:
public
ThreadedDriver
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
Last updated 12 September 2005 21:38:45