amcl_fiducial.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: FIDUCIAL sensor model for AMCL
23// Author: David Feil-Seifer
24// Date: 17 Aug 2003
25// CVS: $Id$
26//
28
29#ifndef AMCL_FIDUCIAL_H
30#define AMCL_FIDUCIAL_H
31
32#include "amcl_sensor.h"
33#include "map/map.h"
34#include "models/laser.h"
35
36// Laser sensor data
38{
39 // Laser range data (range, bearing tuples)
40 public: int fiducial_count;
41 public: double fiducials[PLAYER_FIDUCIAL_MAX_SAMPLES][3];
42};
43
44
46{
47 //x,y,id
48 public: int fiducial_count;
49 public: double origin_x;
50 public: double origin_y;
51 public: double scale;
52 public: double fiducials[100][3];
53};
54
55double fiducial_map_calc_range( AMCLFiducialMap* fmap, double ox, double oy, double oa, double max_range, int id, int k);
56double fiducial_map_calc_bearing( AMCLFiducialMap* fmap, double ox, double oy, double oa, double max_range, int id, int k);
57
58AMCLFiducialMap* fiducial_map_alloc();
59
60// Laseretric sensor model
62{
63 // Default constructor
64 public: AMCLFiducial(player_devaddr_t addr);
65
66 // Load the model
67 public: virtual int Load(ConfigFile* cf, int section);
68
69 // Unload the model
70 public: virtual int Unload(void);
71
72 // Initialize the model
73 public: virtual int Setup(void);
74
75 // Finalize the model
76 public: virtual int Shutdown(void);
77
78 // Check for new sensor measurements
79 private: virtual AMCLSensorData *GetData(void);
80
81 // Update the filter based on the sensor model. Returns true if the
82 // filter has been updated.
83 public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
84
85 // Determine the probability for the given pose
86 private: static double SensorModel(AMCLFiducialData *data, pf_vector_t pose);
87
88 // retrieve the map
89 private: int SetupMap(void);
90
91 private: int read_map_file(const char *);
92
93 // Device info
94 private: player_device_id_t fiducial_id;
95 private: player_device_id_t map_id;
96 private: Driver *driver;
97
98 // Current data timestamp
99 private: double time;
100
101 // The laser map
102 private: map_t *map;
103
104 // The fiducial map
105 private: AMCLFiducialMap *fmap;
106
107 // Laser offset relative to robot
108 private: pf_vector_t laser_pose;
109
110 // Max valid laser range
111 private: double range_max;
112
113 // Laser range variance
114 private: double range_var;
115
116 // Laser angle variance
117 private: double angle_var;
118
119 // Probability of bad range readings
120 private: double range_bad;
121
122 // Probability of bad angle readings
123 private: double angle_bad;
124
125#ifdef INCLUDE_RTKGUI
126 // Setup the GUI
127 private: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
128
129 // Finalize the GUI
130 private: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
131
132 // Draw sensor data
133 public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data);
134
135 // Figures
136 private: rtk_fig_t *fig, *map_fig;
137#endif
138
139};
140
141
142
143
144#endif
Definition amcl_fiducial.h:38
Definition amcl_fiducial.h:46
Definition amcl_fiducial.h:62
Definition amcl_sensor.h:106
Definition amcl_sensor.h:44
Class for loading configuration file information.
Definition configfile.h:197
Base class for all drivers.
Definition driver.h:109
Definition pf.h:111
Definition localization/amcl/map/map.h:67
Definition pf_vector.h:42
A device address.
Definition player.h:146