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
Class for loading configuration file information.
Definition: configfile.h:196
position 2d velocity command
Definition: player_interfaces.h:617
Generic message header.
Definition: player.h:161
virtual int Setup()
Initialize the driver.
Definition: snd.cc:307
Definition: snd.h:110
A pose in the plane.
Definition: player.h:217
virtual void Main()
Main method for driver thread.
Definition: snd.cc:343
A device address.
Definition: player.h:145
Base class for drivers which oeprate with a thread.
Definition: driver.h:552
Definition: snd.h:40
position2d geom
Definition: player_interfaces.h:655
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:74
An autopointer for the message queue.
Definition: message.h:73
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: snd.cc:423
virtual int Shutdown()
Finalize the driver.
Definition: snd.cc:325