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
36class snd_Proxy;
37
39// The class for the driver
40class snd : public ThreadedDriver
41{
42public:
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
52private:
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;
72protected:
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;
81public:
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 */
110class snd_Proxy : public snd
111{
112public:
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 */
137extern "C" int player_driver_init(DriverTable* table);
138
139
140#endif //snd_H
Class for loading configuration file information.
Definition configfile.h:197
Encapsulates a device (i.e., a driver bound to an interface)
Definition device.h:75
An autopointer for the message queue.
Definition message.h:74
Base class for drivers which oeprate with a thread.
Definition driver.h:553
Definition snd.h:111
Definition snd.h:41
virtual int Shutdown()
Finalize the driver.
Definition snd.cc:325
virtual int Setup()
Initialize the driver.
Definition snd.cc:307
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition snd.cc:423
virtual void Main()
Main method for driver thread.
Definition snd.cc:343
A device address.
Definition player.h:146
Generic message header.
Definition player.h:162
A pose in the plane.
Definition player.h:218