snd.h
1 /* Smooth ND driver for Player/Stage
2  *
3  * SND Authors: Joey Durham (algorithm) ,
4  * Luca Invernizzi (driver implementation)
5  *
6  * Implemented on top of Player - One Hell of a Robot Server
7  * Copyright (C) 2003 (Brian Gerkey, Andrew Howard)
8  *
9  *
10  * This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License
21  * along with this program; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 #ifndef snd_H
27 #define snd_H
28 
29 
30 #include <pthread.h>
31 #include <semaphore.h>
32 #include <vector>
33 #include <libplayercore/playercore.h>
34 
35 
36 class snd_Proxy;
37 
39 // The class for the driver
40 class snd : public ThreadedDriver
41 {
42 public:
43  snd(ConfigFile* cf, int section);
44  // Must implement the following methods.
45  virtual int Setup();
46  virtual int Shutdown();
47  virtual int ProcessMessage(QueuePointer & resp_queue,
48  player_msghdr * hdr,
49  void * data);
50 
51 
52 private:
53  // Main function for device thread.
54  virtual void Main();
55  int Odometry_Setup();
56  int Laser_Setup();
57 
58  // My position interface
59  player_devaddr_t m_position_addr;
60  // My laser interface
61  player_devaddr_t m_laser_addr;
62 
63  // Address of and pointer to the laser device to which I'll subscribe
64  player_devaddr_t laser_addr;
65  player_devaddr_t odom_in_addr;
66  player_devaddr_t odom_out_addr;
67  Device* laser_dev;
68  Device *odom_in_dev;
69  Device *odom_out_dev;
70  player_position2d_geom_t robot_geom;
71  int first_goal_has_been_set_to_init_position;
72 protected:
73  void SetSpeedCmd(player_position2d_cmd_vel_t cmd);
74 
75  player_pose2d_t odom_pose;
76 
77  std::vector<double> laser__ranges;
78  double laser__resolution;
79  double laser__max_range;
80  uint32_t laser__ranges_count;
81 public:
82  double robot_radius;
83  double min_gap_width;
84  double obstacle_avoid_dist;
85  double max_speed;
86  double max_turn_rate;
87  double goal_position_tol;
88  double goal_angle_tol;
89  double goalX,goalY,goalA;
90  pthread_t algorithm_thread;
91  pthread_mutex_t goal_mutex;
92  pthread_cond_t goal_changed_cond;
93  double goal_changed;
94  pthread_mutex_t data_mutex;
95  pthread_cond_t data_changed_cond;
96  double data_changed;
97  int data_odometry_ready;
98  int data_laser_ready;
99 
100  void WaitForNextGoal();
101  void SignalNextGoal(double goalX,double goalY,double goalA);
102  void Read();
103  void ReadIfWaiting();
104 };
105 
106 /*This class acts as a substitute for libplayerc++ proxies.
107  * This has been done to reuse the code of the algorithm (which is in
108  * gap_nd_nav.cc)
109  */
110 class snd_Proxy : public snd
111 {
112 public:
113  double GetScanRes() ;
114  double GetMaxRange() ;
115  uint32_t GetCount() ;
116  double range(const int index);
117 
118  void SetMotorEnable(int turnkey);
119  void SetOdometry(double position_x0,
120  double position_y0,
121  double position_alpha0);
122  double GetXPos() ;
123  double GetYPos() ;
124  double GetYaw() ;
125  void RequestGeom();
126  void SetSpeed(double velocity_modulus,
127  double velocity_angle);
128  snd_Proxy(ConfigFile* cf, int section):snd(cf,section){}
129 };
130 
131 
132 
134 // Extra stuff for building a shared object.
135 
136 /* need the extern to avoid C++ name-mangling */
137 extern "C" int player_driver_init(DriverTable* table);
138 
139 
140 #endif //snd_H
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:89
position 2d velocity command
Definition: player_interfaces.h:617
#define PLAYER_RFID_REQ_READTAG
Request/reply: read data from the RFID tag - to be implemented.
Definition: player_interfaces.h:4304
#define PLAYER_MSG3(level, msg, a, b, c)
Error message macros.
Definition: error.h:107
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.
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:620
#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:158
A pose in the plane.
Definition: player.h:216
Generic message header.
Definition: player.h:160
int Subscribe(QueuePointer &sub_queue)
Subscribe the given queue to this device.
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:657
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:663
virtual int Shutdown()
Finalize the driver.
Definition: snd.cc:325
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:165
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:73
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
double px
X [m].
Definition: player.h:219
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:167
virtual void Main(void)=0
Main method for driver thread.
double ReadAngle(int section, const char *name, double value)
Read an angle (includes unit conversion).
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:187
void ProcessMessages(void)
Process pending messages.
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:94
QueuePointer InQueue
Queue for all incoming messages for this driver.
Definition: driver.h:284
double py
Y [m].
Definition: player.h:221
#define PLAYER_ERROR2(msg, a, b)
Error message macros.
Definition: error.h:82
void PutMsg(QueuePointer &resp_queue, uint8_t type, uint8_t subtype, void *src, size_t deprecated, double *timestamp)
Send a message to this device.
bool Wait(double TimeOut=0.0)
Wait for new data to arrive on the driver's queue.
#define PLAYER_RFID_DATA_TAGS
Data subtype.
Definition: player_interfaces.h:4298
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: snd.h:40
virtual int Setup()
Initialize the driver.
Definition: snd.cc:307
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:105
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:124
Definition: snd.h:110
virtual void Main()
Main method for driver thread.
Definition: snd.cc:343
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
#define PLAYER_RFID_REQ_WRITETAG
Request/reply: write data to the RFID tag - to be implemented.
Definition: player_interfaces.h:4307
#define PLAYER_RFID_REQ_LOCKTAG
Request/reply: lock data blocks of a RFID tag - to be implemented.
Definition: player_interfaces.h:4310
Class for loading configuration file information.
Definition: configfile.h:195
A device address.
Definition: player.h:144
An autopointer for the message queue.
Definition: message.h:72
#define PLAYER_RFID_REQ_POWER
Request/reply: put the reader in sleep mode (0) or wake it up (1).
Definition: player_interfaces.h:4301
position2d data
Definition: player_interfaces.h:606
position2d geom
Definition: player_interfaces.h:655
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:80
player_devaddr_t device_addr
Default device address (single-interface drivers)
Definition: driver.h:268
Base class for drivers which oeprate with a thread.
Definition: driver.h:551
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: snd.cc:423
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:169
virtual void StopThread(void)
Cancel (and wait for termination) of the driver thread.
#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:173
Reference-counted message objects.
Definition: message.h:131
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:88
Structure describing a single RFID tag.
Definition: player_interfaces.h:4316
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
double pa
yaw [rad]
Definition: player.h:223
Base class for all drivers.
Definition: driver.h:107
#define PLAYER_MSG0(level, msg)
General messages.
Definition: error.h:104
int Unsubscribe(QueuePointer &sub_queue)
Unsubscribe the given queue from this device.
Data.
Definition: player_interfaces.h:4329
Data: scan (PLAYER_LASER_DATA_SCAN)
Definition: player_interfaces.h:883
player_msghdr_t * GetHeader()
Get pointer to header.
Definition: message.h:185
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:163
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:75
position2d position command
Definition: player_interfaces.h:626
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:629