lasertransform.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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19  *
20  */
22 //
23 // Desc: Base class for laser transformations (i.e. cspace etc)
24 // Author: Andrew Howard
25 // Date: 1 Sep 2002
26 // CVS: $Id$
27 //
28 // Theory of operation -
29 //
30 // Requires - Laser device.
31 //
33 
34 
35 
36 #include <errno.h>
37 #include <string.h>
38 #include <math.h>
39 #include <stdlib.h> // for atoi(3)
40 #if !defined (WIN32)
41  #include <unistd.h>
42 #endif
43 
44 #include <libplayercore/playercore.h>
45 
46 // Driver for computing the free c-space from a laser scan.
47 class LaserTransform : public Driver
48 {
49  // Constructor
50  public: LaserTransform( ConfigFile* cf, int section);
51 
52  // MessageHandler
53  public: virtual int ProcessMessage(QueuePointer & resp_queue,
54  player_msghdr * hdr,
55  void * data);
56 
57  // Setup/shutdown routines.
58  public: virtual int Setup();
59  public: virtual int Shutdown();
60 
61  protected:
62  // Process laser data. Returns non-zero if the laser data has been
63  // updated.
64  virtual int UpdateLaser(player_laser_data_t * data) = 0;
65 
66  // Process requests. Returns 1 if the configuration has changed.
67  int HandleRequests();
68 
69  // Handle geometry requests.
70  void HandleGetGeom(void *client, void *req, int reqlen);
71 
72  // Laser stuff.
73  Device *laser_device;
74  player_devaddr_t laser_addr;
75  struct timeval laser_timestamp;
76 
77  // Fiducila stuff (the data we generate).
79  struct timeval time;
80 };
81 
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: toranger.cc:91
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:90
QueuePointer ret_queue
Last requester's queue.
Definition: driver.h:186
position 2d velocity command
Definition: player_interfaces.h:617
Request/reply: Get/set scan properties.
Definition: player_interfaces.h:961
#define PLAYER_RANGER_REQ_INTNS
Request/reply subtype: intensity data config.
Definition: player_interfaces.h:4965
player_ranger_geom_t geom
The geometry of the device at the time the scan was acquired.
Definition: player_interfaces.h:5038
double proll
roll [rad]
Definition: player.h:237
uint32_t intensities_count
Number of intensity readings.
Definition: player_interfaces.h:5051
virtual void Publish(player_devaddr_t addr, QueuePointer &queue, uint8_t type, uint8_t subtype, void *src=NULL, size_t deprecated=0, double *timestamp=NULL, bool copy=true)
Publish a message via one of this driver's interfaces.
uint32_t host
The "host" on which the device resides.
Definition: player.h:148
double py
Y [m].
Definition: player.h:199
#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
A rectangular bounding box, used to define the size of an object.
Definition: player.h:254
#define PLAYER_POINTCLOUD3D_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:4849
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
#define PLAYER_PTZ_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:1208
double px
X [m].
Definition: player.h:231
#define PLAYER_LASER_REQ_SET_CONFIG
Request/reply subtype: set configuration.
Definition: player_interfaces.h:857
A pose in the plane.
Definition: player.h:217
#define PLAYER_LASER_DATA_SCANPOSE
Data subtype: pose-stamped scan.
Definition: player_interfaces.h:848
float tilt
Tilt [rad].
Definition: player_interfaces.h:1233
player_pose3d_t pose
Laser pose, in robot cs (m, m, m, rad, rad, rad).
Definition: player_interfaces.h:948
Request/reply: Turn intensity data on/off for devices that provide it (PLAYER_RANGER_REQ_INTNS)
Definition: player_interfaces.h:5089
double ReadTupleLength(int section, const char *name, int index, double value)
Read a length from a tuple (includes units conversion)
player_ranger_data_intns_t data
The scan data.
Definition: player_interfaces.h:5063
Generic message header.
Definition: player.h:161
double pz
Z [m].
Definition: player.h:235
Device configuration request (PLAYER_RANGER_REQ_GET_CONFIG)
Definition: player_interfaces.h:4978
int Subscribe(QueuePointer &sub_queue)
Subscribe the given queue to this device.
float max_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:888
Request/reply: Turn power on/off.
Definition: player_interfaces.h:983
#define PLAYER_RANGER_DATA_INTNS
Data subtype: intensity scan.
Definition: player_interfaces.h:4950
Data: state (PLAYER_PTZ_DATA_STATE)
Definition: player_interfaces.h:1228
Data: Stamped range scan (PLAYER_RANGER_DATA_RANGESTAMPED)
Definition: player_interfaces.h:5031
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: lasertransform.cc:86
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:74
double px
X [m].
Definition: player.h:220
#define PLAYER_RANGER_REQ_GET_CONFIG
Request/reply subtype: get configuration.
Definition: player_interfaces.h:4971
Data: Get cloud (PLAYER_POINTCLOUD3D_DATA_STATE) The basic 3dcloudpoint data packet.
Definition: player_interfaces.h:4874
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
double pyaw
yaw [rad]
Definition: player.h:241
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
double ReadTupleFloat(int section, const char *name, int index, double value)
Read a float (double) from a tuple field.
Definition: lasertransform.h:47
double max_angle
End angle of scans [rad].
Definition: player_interfaces.h:4983
double min_angle
Start angle of scans [rad].
Definition: player_interfaces.h:4981
#define PLAYER_RANGER_DATA_RANGE
Data subtype: range scan.
Definition: player_interfaces.h:4944
double ReadAngle(int section, const char *name, double value)
Read an angle (includes unit conversion).
#define PLAYER_RANGER_DATA_INTNSSTAMPED
Data subtype: intensity scan with extra info.
Definition: player_interfaces.h:4953
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 ClearFilter(void)
Clear (i.e., turn off) message filter.
player_pose2d_t pose
The global pose of the laser at the time the scan was acquired.
Definition: player_interfaces.h:914
#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
#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
void PutMsg(QueuePointer &resp_queue, uint8_t type, uint8_t subtype, void *src, size_t deprecated, double *timestamp)
Send a message to this device.
double px
X [m].
Definition: player.h:197
uint32_t ranges_count
Number of range readings.
Definition: player_interfaces.h:5022
player_laser_data_t scan
The scan data.
Definition: player_interfaces.h:912
virtual int Setup()
Initialize the driver.
Definition: lasertransform.cc:52
virtual int Setup(void)
Initialize the driver.
Definition: toranger.cc:58
#define PLAYER_POSITION2D_REQ_GET_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:483
float * ranges
Range readings [m].
Definition: player_interfaces.h:896
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
static bool MatchDeviceAddress(player_devaddr_t addr1, player_devaddr_t addr2)
Compare two addresses.
Definition: device.h:201
Request/reply: Turn power on/off (PLAYER_RANGER_REQ_POWER)
Definition: player_interfaces.h:5077
double ppitch
pitch [rad]
Definition: player.h:239
double sh
Height [m].
Definition: player.h:261
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
uint8_t state
FALSE to turn laser off, TRUE to turn laser on.
Definition: player_interfaces.h:986
#define PLAYER_LASER_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:854
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:125
#define PLAYER_RANGER_REQ_POWER
Request/reply subtype: power config.
Definition: player_interfaces.h:4962
double sl
Length [m].
Definition: player.h:259
Request/reply: Get geometry.
Definition: player_interfaces.h:945
#define PLAYER_LASER_REQ_GET_CONFIG
Request/reply subtype: get configuration.
Definition: player_interfaces.h:860
double ReadTupleAngle(int section, const char *name, int index, double value)
Read an angle form a tuple (includes units conversion)
#define PLAYER_RANGER_REQ_SET_CONFIG
Request/reply subtype: set configuration.
Definition: player_interfaces.h:4968
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
int GetTupleCount(int section, const char *name)
Get the number of values in a tuple.
float resolution
Scan resolution [rad].
Definition: player_interfaces.h:968
Class for loading configuration file information.
Definition: configfile.h:196
double sw
Width [m].
Definition: player.h:257
virtual int Setup()
Initialize the driver.
Definition: driver.h:386
double pz
Z [m].
Definition: player.h:201
A device address.
Definition: player.h:145
virtual int Shutdown(void)
Finalize the driver.
Definition: toranger.cc:70
An autopointer for the message queue.
Definition: message.h:73
double py
Y [m].
Definition: player.h:233
virtual int Shutdown()
Finalize the driver.
Definition: lasertransform.cc:76
float min_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:964
double * intensities
Intensity readings.
Definition: player_interfaces.h:5053
#define PLAYER_RANGER_DATA_RANGESTAMPED
Data subtype: range scan with extra info.
Definition: player_interfaces.h:4947
void SetFilter(int host, int robot, int interf, int index, int type, int subtype)
Set filter values.
position2d data
Definition: player_interfaces.h:606
position2d geom
Definition: player_interfaces.h:655
A pose in space.
Definition: player.h:228
double range_res
Range resolution [m].
Definition: player_interfaces.h:4992
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
float min_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:886
player_devaddr_t device_addr
Default device address (single-interface drivers)
Definition: driver.h:269
double max_range
Maximum range [m].
Definition: player_interfaces.h:4990
float range_res
Range Resolution [m].
Definition: player_interfaces.h:972
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
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
float max_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:966
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
#define PLAYER_POSITION2D_CMD_POS
Command: position (PLAYER_POSITION2D_CMD_POS)
Definition: player_interfaces.h:588
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
player_bbox3d_t size
Laser dimensions (m, m, m).
Definition: player_interfaces.h:950
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
double * ranges
Range readings [m].
Definition: player_interfaces.h:5024
3D Pointcloud element structure An element as stored in a 3D pointcloud, containing a 3D position plu...
Definition: player_interfaces.h:4856
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
virtual int Shutdown()
Finalize the driver.
Definition: driver.h:393
double frequency
Scanning frequency [Hz].
Definition: player_interfaces.h:4994
double pa
yaw [rad]
Definition: player.h:224
Base class for all drivers.
Definition: driver.h:108
uint32_t intensity_count
Number of intensity readings.
Definition: player_interfaces.h:898
Definition: toranger.h:32
float max_range
Maximum range [m].
Definition: player_interfaces.h:970
Data: pose-stamped scan (PLAYER_LASER_DATA_SCANPOSE)
Definition: player_interfaces.h:909
#define PLAYER_MSG0(level, msg)
General messages.
Definition: error.h:105
float scanning_frequency
Scanning frequency [Hz].
Definition: player_interfaces.h:976
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.
#define PLAYER_MSGTYPE_SYNCH
A synch message.
Definition: player.h:117
Data: scan (PLAYER_LASER_DATA_SCAN)
Definition: player_interfaces.h:883
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:164
#define PLAYER_RANGER_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:4959
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76
position2d position command
Definition: player_interfaces.h:626
double angular_res
Scan resolution [rad].
Definition: player_interfaces.h:4985
#define PLAYER_LASER_REQ_POWER
Request/reply subtype: set power.
Definition: player_interfaces.h:863
Data: Stamped intensity scan (PLAYER_RANGER_DATA_INTNSSTAMPED)
Definition: player_interfaces.h:5060
uint8_t * intensity
Intensity readings.
Definition: player_interfaces.h:900
player_ranger_data_range_t data
The scan data.
Definition: player_interfaces.h:5034