snd.h
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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
00040 class snd : public ThreadedDriver
00041 {
00042 public:
00043 snd(ConfigFile* cf, int section);
00044
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
00054 virtual void Main();
00055 int Odometry_Setup();
00056 int Laser_Setup();
00057
00058
00059 player_devaddr_t m_position_addr;
00060
00061 player_devaddr_t m_laser_addr;
00062
00063
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
00107
00108
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
00135
00136
00137 extern "C" int player_driver_init(DriverTable* table);
00138
00139
00140 #endif //snd_H