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 
55 double fiducial_map_calc_range( AMCLFiducialMap* fmap, double ox, double oy, double oa, double max_range, int id, int k);
56 double fiducial_map_calc_bearing( AMCLFiducialMap* fmap, double ox, double oy, double oa, double max_range, int id, int k);
57 
58 AMCLFiducialMap* fiducial_map_alloc();
59 
60 // Laseretric sensor model
61 class AMCLFiducial : public AMCLSensor
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_sensor.h:44
#define PLAYER_MSG1(level, msg, a)
Definition: error.h:106
virtual int Subscribe(player_devaddr_t addr)
Subscribe to this driver.
Definition: amcl_fiducial.h:62
Definition: pf_vector.h:42
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
Definition: amcl_sensor.h:106
double ReadTupleLength(int section, const char *name, int index, double value)
Read a length from a tuple (includes units conversion)
const char * ReadFilename(int section, const char *name, const char *value)
Read a filename.
int ReadInt(int section, const char *name, int value)
Read an integer value.
double ReadLength(int section, const char *name, double value)
Read a length (includes unit conversion, if any).
#define PLAYER_ERROR2(msg, a, b)
Definition: error.h:83
Definition: pf.h:111
double ReadTupleAngle(int section, const char *name, int index, double value)
Read an angle form a tuple (includes units conversion)
Definition: amcl_fiducial.h:46
Class for loading configuration file information.
Definition: configfile.h:197
A device address.
Definition: player.h:146
#define PLAYER_ERROR1(msg, a)
Definition: error.h:82
#define PLAYER_ERROR(msg)
Definition: error.h:81
Definition: localization/amcl/map/map.h:67
Definition: amcl_fiducial.h:38
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
Definition: localization/amcl/map/map.h:52
Base class for all drivers.
Definition: driver.h:109
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.