amcl_laser.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: LASER sensor model for AMCL
23 // Author: Andrew Howard
24 // Date: 17 Aug 2003
25 // CVS: $Id$
26 //
28 
29 #ifndef AMCL_LASER_H
30 #define AMCL_LASER_H
31 
32 #include "amcl_sensor.h"
33 #include "map/map.h"
34 #include "models/laser.h"
35 
36 // Laser sensor data
38 {
39  public:
40  AMCLLaserData () {ranges=NULL;};
41  virtual ~AMCLLaserData() {delete [] ranges;};
42  // Laser range data (range, bearing tuples)
43  public: int range_count;
44  public: double range_max;
45  public: double (*ranges)[2];
46 };
47 
48 
49 // Laseretric sensor model
50 class AMCLLaser : public AMCLSensor
51 {
52  // Default constructor
53  public: AMCLLaser(AdaptiveMCL & aAMCL, player_devaddr_t addr);
54 
55  // Load the model
56  public: virtual int Load(ConfigFile* cf, int section);
57 
58  // Unload the model
59  public: virtual int Unload(void);
60 
61  // Initialize the model
62  public: virtual int Setup(void);
63 
64  // Finalize the model
65  public: virtual int Shutdown(void);
66 
67  // Process message for this interface
68  public: virtual int ProcessMessage(QueuePointer &resp_queue,
69  player_msghdr * hdr,
70  void * data);
71  // Check for new sensor measurements
72  //private: virtual AMCLSensorData *GetData(void);
73 
74  // Update the filter based on the sensor model. Returns true if the
75  // filter has been updated.
76  public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
77 
78  // Determine the probability for the given pose
79  private: static double SensorModel(AMCLLaserData *data,
80  pf_sample_set_t* set);
81 
82  // retrieve the map
83  private: int SetupMap(void);
84 
85  // Device info
86  private: player_devaddr_t laser_addr;
87  private: player_devaddr_t map_addr;
88  private: Device *laser_dev;
89 
90  // Current data timestamp
91  private: double time;
92 
93  // The laser map
94  private: map_t *map;
95 
96  // Laser offset relative to robot
97  private: pf_vector_t laser_pose;
98 
99  // Max beams to consider
100  private: int max_beams;
101 
102  // Laser range variance
103  private: double range_var;
104 
105  // Probability of bad range readings
106  private: double range_bad;
107 
108 #ifdef INCLUDE_RTKGUI
109  // Setup the GUI
110  private: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
111 
112  // Finalize the GUI
113  private: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
114 
115  // Draw sensor data
116  public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data);
117 
118  // Figures
119  private: rtk_fig_t *fig, *map_fig;
120 #endif
121 };
122 
123 
124 
125 
126 #endif
Definition: player_interfaces.h:3034
Definition: amcl_sensor.h:43
uint32_t data_count
The number of cells.
Definition: player_interfaces.h:3066
#define PLAYER_MSG1(level, msg, a)
Error message macros.
Definition: error.h:106
#define PLAYER_MSG3(level, msg, a, b, c)
Error message macros.
Definition: error.h:108
Definition: pf_vector.h:41
#define PLAYER_LASER_DATA_SCAN
Data subtype: scan.
Definition: player_interfaces.h:845
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
Definition: amcl_sensor.h:105
double px
X [m].
Definition: player.h:231
player_pose3d_t pose
Laser pose, in robot cs (m, m, m, rad, rad, rad).
Definition: player_interfaces.h:948
Definition: amcl.h:70
int8_t * data
Cell occupancy value.
Definition: player_interfaces.h:3070
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
Request/reply: get grid map tile.
Definition: player_interfaces.h:3055
double pyaw
yaw [rad]
Definition: player.h:241
uint32_t height
The size of the map [pixels].
Definition: player_interfaces.h:3041
Definition: amcl_laser.h:37
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).
void * GetPayload()
Get pointer to payload.
Definition: message.h:188
#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
float scale
The scale of the map [m/pixel].
Definition: player_interfaces.h:3037
Definition: pf.h:110
float * ranges
Range readings [m].
Definition: player_interfaces.h:896
uint32_t row
The tile origin [pixels].
Definition: player_interfaces.h:3060
uint32_t width
The size of the tile [pixels].
Definition: player_interfaces.h:3062
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
uint32_t height
The size of the tile [pixels].
Definition: player_interfaces.h:3064
#define PLAYER_LASER_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:854
Request/reply: Get geometry.
Definition: player_interfaces.h:945
uint32_t width
The size of the map [pixels].
Definition: player_interfaces.h:3039
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Class for loading configuration file information.
Definition: configfile.h:196
#define PLAYER_MAP_REQ_GET_DATA
Request/reply subtype: get grid map tile
Definition: player_interfaces.h:3024
uint32_t col
The tile origin [pixels].
Definition: player_interfaces.h:3058
A device address.
Definition: player.h:145
An autopointer for the message queue.
Definition: message.h:73
double py
Y [m].
Definition: player.h:233
Definition: pf.h:62
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
Definition: localization/amcl/map/map.h:66
float min_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:886
float max_range
Maximum range [m].
Definition: player_interfaces.h:892
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:170
player_pose2d_t origin
The origin of the map [m, m, rad].
Definition: player_interfaces.h:3044
Reference-counted message objects.
Definition: message.h:132
Message * Request(QueuePointer &resp_queue, uint8_t type, uint8_t subtype, void *src, size_t deprecated, double *timestamp, bool threaded=true)
Make a request of another device.
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
Definition: localization/amcl/map/map.h:51
#define PLAYER_MSG0(level, msg)
General messages.
Definition: error.h:105
float resolution
Angular resolution [rad].
Definition: player_interfaces.h:890
uint32_t ranges_count
Number of range readings.
Definition: player_interfaces.h:894
int Unsubscribe(QueuePointer &sub_queue)
Unsubscribe the given queue from this device.
Data: scan (PLAYER_LASER_DATA_SCAN)
Definition: player_interfaces.h:883
uint16_t index
Which device of that interface.
Definition: player.h:155
#define PLAYER_MAP_REQ_GET_INFO
Request/reply subtype: get grid map metadata
Definition: player_interfaces.h:3021
Definition: amcl_laser.h:50