localize.hh
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2010
4  * Alejandro R. Mosteo
5  *
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public
9  * License as published by the Free Software Foundation; either
10  * version 2.1 of the License, or (at your option) any later version.
11  *
12  * This library is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15  * Lesser General Public License for more details.
16  *
17  * You should have received a copy of the GNU Lesser General Public
18  * License along with this library; if not, write to the Free Software
19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20  */
21 
22 #ifndef LOCALIZE_HH_
23 #define LOCALIZE_HH_
24 
25 #include "robot_location.hh"
26 #include "scan.hh"
27 #include "segment_map.hh"
28 
29 class Localize
30 {
31 public:
33  Localize(double laser_max_range,
34  double laser_pose_x,
35  double laser_pose_y,
36  double laser_pose_angle,
37  double laser_noise_range, // uncertainty in distance measured (2sigma)
38  double laser_noise_bearing, // uncertainty in sample bearing (2sigma)
39  double odom_noise_x,
40  double odom_noise_y,
41  double odom_noise_angle // uncertainty for odometry model
42  );
43 
45  void SetRobotPose(double x, double y, double angle);
47  void SetPoses(double ox, double oy, double oth,
48  double gx, double gy, double gth);
50  void SetRobotPoseError(double ex, double ey, double eth);
51 
54  void SetLaserPose(double x, double y, double a);
55 
56  void LoadMap(const char* filename);
57  void SetMap(const SegmentsVector& map);
58  void AddSegment(const Segment& segment);
59  const SegmentMap &Map(void) const;
60 
63  bool Update(double robot_x, double robot_y, double robot_angle,
64  DoublesVector ranges, DoublesVector bearings);
65 
66  Pose pose(void) const;
67  MatrixXd GetCovariance(void) const { return robot_.Covariance(); };
68 private:
69  Scan scan_;
70  RobotLocation robot_;
71  bool has_pose_;
72 };
73 
74 #endif /* LOCALIZE_HH_ */
Definition: robot_location.hh:31
void SetRobotPose(double x, double y, double angle)
Initialize robot map pose, and reset odometry to zero.
Definition: localize.cc:49
uint32_t host
The "host" on which the device resides.
Definition: player.h:148
uint32_t timestamp_usec
Time of entry creation.
Definition: player.h:286
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
uint32_t key_count
Length of key in bytes.
Definition: player.h:268
void SetLaserPose(double x, double y, double a)
Update laser pose (limited to x, y ,a for now, though) m m rad.
Definition: localize.cc:44
uint8_t * data
Entry data.
Definition: player.h:282
void SetRobotPoseError(double ex, double ey, double eth)
Initial pose error.
Definition: localize.cc:61
Definition: localize.hh:30
Generic message header.
Definition: player.h:162
char * group
Second identifier.
Definition: player.h:274
void SetPoses(double ox, double oy, double oth, double gx, double gy, double gth)
Inform the current odometric and global (map) pose.
Definition: robot_location.cc:40
Definition: types.hh:43
uint32_t robot
The "robot" or device collection in which the device resides.
Definition: player.h:151
uint16_t type
Entry data type.
Definition: player.h:276
Definition: types.hh:30
uint32_t group_count
Second identifier count.
Definition: player.h:272
char * key
Identifier for the entry.
Definition: player.h:270
Localize(double laser_max_range, double laser_pose_x, double laser_pose_y, double laser_pose_angle, double laser_noise_range, double laser_noise_bearing, double odom_noise_x, double odom_noise_y, double odom_noise_angle)
Initialize parameters.
Definition: localize.cc:26
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
#define PLAYER_ERROR2(msg, a, b)
Error message macros.
Definition: error.h:83
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: scan.hh:31
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
bool Update(double robot_x, double robot_y, double robot_angle, DoublesVector ranges, DoublesVector bearings)
Compute actualization from robot accumulated odometry and laser reading Returns true if the update wa...
Definition: localize.cc:93
Class for loading configuration file information.
Definition: configfile.h:197
virtual int Setup()
Initialize the driver.
Definition: driver.h:386
#define PLAYER_CAPABILITIES_REQ
Capability request message type.
Definition: player.h:397
void SetPoses(double ox, double oy, double oth, double gx, double gy, double gth)
Initialize both odometry (if it is not zero) and global (map) pose.
Definition: localize.cc:54
A device address.
Definition: player.h:146
An autopointer for the message queue.
Definition: message.h:74
void SetLastScan(const DoublesVector &ranges, const DoublesVector &bearings)
Set last laser reading Removes out of range values and attaches the uncertainty model.
Definition: scan.cc:87
void SetCurrentError(double ex, double ey, double eth)
Inform the error for the initial pose Robot is expected to be at x +/- (ex/2)
Definition: robot_location.cc:51
uint32_t timestamp_sec
Time of entry creation.
Definition: player.h:284
Definition: gridmap.h:112
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
virtual int Shutdown()
Finalize the driver.
Definition: driver.h:393
Base class for all drivers.
Definition: driver.h:109
Vectormap feature data.
Definition: player.h:266
#define PLAYER_MSG0(level, msg)
General messages.
Definition: error.h:105
int entries
Total number of entries in the device table using this driver.
Definition: driver.h:273
struct player_blackboard_entry player_blackboard_entry_t
Vectormap feature data.
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
uint32_t data_count
Entry data length.
Definition: player.h:280
uint16_t subtype
Entry data subtype.
Definition: player.h:278
void SetLaserPose(double x, double y, double a)
Update laser pose.
Definition: scan.cc:41
uint16_t index
Which device of that interface.
Definition: player.h:155
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:164
uint16_t interf
The interface provided by the device; must be one of PLAYER_*_CODE.
Definition: player.h:153
Definition: transf.hh:36
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76
Definition: segment_map.hh:30