snd.h
00001 /*  Smooth ND driver for Player/Stage
00002  *  
00003  *  SND Authors:  Joey Durham (algorithm) , 
00004  *                Luca Invernizzi (driver implementation)
00005  *
00006  *  Implemented on top of Player - One Hell of a Robot Server
00007  *  Copyright (C) 2003  (Brian Gerkey, Andrew Howard)
00008  *                      
00009  * 
00010  *  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU General Public License for more details.
00019  *
00020  *  You should have received a copy of the GNU General Public License
00021  *  along with this program; if not, write to the Free Software
00022  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00023  *
00024  */
00025 
00026 #ifndef snd_H
00027 #define snd_H
00028 
00029 
00030 #include <pthread.h>
00031 #include <semaphore.h>
00032 #include <vector>
00033 #include <libplayercore/playercore.h>
00034 
00035 
00036 class snd_Proxy; 
00037 
00039 // The class for the driver
00040 class snd : public ThreadedDriver
00041 {
00042 public:
00043     snd(ConfigFile* cf, int section);
00044     // Must implement the following methods.
00045     virtual int Setup();
00046     virtual int Shutdown();
00047     virtual int ProcessMessage(QueuePointer & resp_queue, 
00048                                player_msghdr * hdr, 
00049                                void * data);
00050 
00051 
00052 private:
00053     // Main function for device thread.
00054     virtual void Main();
00055     int Odometry_Setup();
00056     int Laser_Setup();
00057 
00058     // My position interface
00059     player_devaddr_t m_position_addr;
00060     // My laser interface
00061     player_devaddr_t m_laser_addr;
00062 
00063     // Address of and pointer to the laser device to which I'll subscribe
00064     player_devaddr_t laser_addr;
00065     player_devaddr_t odom_in_addr;
00066     player_devaddr_t odom_out_addr;
00067     Device* laser_dev;
00068     Device *odom_in_dev;
00069     Device *odom_out_dev;
00070     player_position2d_geom_t robot_geom;
00071     int first_goal_has_been_set_to_init_position;
00072 protected:
00073     void SetSpeedCmd(player_position2d_cmd_vel_t cmd);
00074 
00075     player_pose2d_t odom_pose;
00076 
00077     std::vector<double> laser__ranges;
00078     double laser__resolution;
00079     double laser__max_range;
00080     uint32_t laser__ranges_count;
00081 public:
00082         double robot_radius;
00083         double min_gap_width;
00084         double obstacle_avoid_dist;
00085         double max_speed;
00086         double max_turn_rate;
00087         double goal_position_tol;
00088         double goal_angle_tol;
00089     double goalX,goalY,goalA;
00090         pthread_t algorithm_thread;
00091     pthread_mutex_t goal_mutex;
00092     pthread_cond_t goal_changed_cond;
00093     double goal_changed;
00094     pthread_mutex_t data_mutex;
00095     pthread_cond_t data_changed_cond;
00096     double data_changed;
00097     int data_odometry_ready;
00098     int data_laser_ready;
00099  
00100     void WaitForNextGoal();
00101     void SignalNextGoal(double goalX,double goalY,double goalA);
00102     void Read();
00103     void ReadIfWaiting();
00104 };
00105 
00106 /*This class acts as a substitute for libplayerc++ proxies.
00107  * This has been done to reuse the code of the algorithm (which is in 
00108  * gap_nd_nav.cc)
00109  */
00110 class snd_Proxy : public snd
00111 {
00112 public:
00113     double   GetScanRes()  ;
00114     double   GetMaxRange() ;
00115     uint32_t GetCount()    ;
00116     double   range(const int index);
00117 
00118     void   SetMotorEnable(int turnkey);
00119     void   SetOdometry(double position_x0,
00120                                 double position_y0,
00121                                 double position_alpha0);
00122     double GetXPos() ;
00123     double GetYPos() ;
00124     double GetYaw()  ;
00125     void   RequestGeom();
00126     void   SetSpeed(double velocity_modulus,
00127                              double velocity_angle);
00128     snd_Proxy(ConfigFile* cf, int section):snd(cf,section){}
00129 };
00130 
00131 
00132 
00134 // Extra stuff for building a shared object.
00135 
00136 /* need the extern to avoid C++ name-mangling  */
00137 extern "C" int player_driver_init(DriverTable* table);
00138 
00139 
00140 #endif //snd_H