00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef _PIONEER2PACKETMANAGER_H_
00018 #define _PIONEER2PACKETMANAGER_H_
00019
00020 #include "PSOSPacket.h"
00021 #include <pthread.h>
00022 #include <list>
00023 #include <string>
00024 #include "Object.h"
00025 #include "pseudosem.h"
00026
00027 namespace RobotFlow {
00028
00029
00030 #define P2OS_CYCLETIME_USEC 50000
00031
00032
00033 #define DX_AngleConvFactor 0.001534
00034 #define DX_DistConvFactor 0.826
00035 #define DX_VelConvFactor 1.0
00036 #define DX_RobotRadius 250.0
00037 #define DX_RobotDiagonal 120.0
00038 #define DX_Holonomic true
00039 #define DX_RangeConvFactor 0.268
00040 #define DX_RobotAxleLength 320.0
00041
00042
00043 #define AT_AngleConvFactor 0.001534
00044 #define AT_DistConvFactor 1.303
00045 #define AT_VelConvFactor 1.0
00046 #define AT_RobotRadius 500.0
00047 #define AT_RobotDiagonal 120.0
00048 #define AT_Holonomic true
00049 #define AT_RangeConvFactor 0.268
00050 #define AT_RobotAxleLength 320.0
00051
00052
00053
00054 class Pioneer2Data : public FD::Object {
00055
00056 public:
00057
00058 unsigned char status;
00059 float xpos;
00060 float ypos;
00061 float thpos;
00062 float lvel;
00063 float rvel;
00064 float battery;
00065 unsigned short bumpers;
00066 float control;
00067 unsigned short ptu;
00068 float sonars[16];
00069 unsigned short timer;
00070 float analog;
00071 unsigned char digin;
00072 unsigned char digout;
00073
00074 void printOn(std::ostream &out) const {
00075 print(out);
00076 }
00077
00078 void print (std::ostream &out) const {
00079
00080 out<<"status : "<<(int)status<<std::endl;
00081 out<<"xpos : "<<xpos<<std::endl;
00082 out<<"ypos : "<<ypos<<std::endl;
00083 out<<"thpos : "<<thpos<<std::endl;
00084 out<<"lvel : "<<lvel<<std::endl;
00085 out<<"rvel : "<<rvel<<std::endl;
00086 out<<"battery : "<<battery<<std::endl;
00087 out<<"bumpers : "<<bumpers<<std::endl;
00088 out<<"control : "<<control<<std::endl;
00089 out<<"ptu : "<<ptu<<std::endl;
00090 for (int i = 0; i < 16; i++) {
00091 out<<"sonar "<<i<<" : "<<sonars[i]<<std::endl;
00092 }
00093 out<<"timer : "<<timer<<std::endl;
00094 out<<"analog : "<<analog<<std::endl;
00095 out<<"digin : "<<(int)digin<<std::endl;
00096 out<<"digout : "<<(int)digout<<std::endl;
00097 }
00098
00099 };
00100
00101 class Pioneer2PacketManager {
00102
00103 friend void* Pioneer2PacketManager_recv_thread (void *user_data);
00104 friend void* Pioneer2PacketManager_send_thread (void *user_data);
00105
00106 public:
00107
00108 static Pioneer2PacketManager* instance();
00109 static void destroyInstance();
00110 Pioneer2PacketManager();
00111 virtual ~Pioneer2PacketManager();
00112 void initialize(const std::string &tty);
00113
00114
00115 unsigned char get_status();
00116 float get_Xpos();
00117 float get_Ypos();
00118 float get_Thpos();
00119 float get_Lvel();
00120 float get_Rvel();
00121 float get_battery();
00122 unsigned short get_bumpers();
00123 float get_control();
00124 unsigned short get_ptu();
00125 float get_compass();
00126 float get_sonar(int s_number);
00127 unsigned short get_timer();
00128 float get_analog();
00129 unsigned char get_digin();
00130 unsigned char get_digout();
00131 void get_all(Pioneer2Data &data);
00132
00133
00134 void send_motor_command(short command);
00135 void send_reset_translational_acc(short command);
00136 void send_reset_translational_vel(short command);
00137 void send_reset_server();
00138 void send_reset_rotational_vel(short command);
00139 void send_velocity_command(short command);
00140 void send_heading_command(short command);
00141 void send_rel_heading_command(short command);
00142 void send_say_command(const std::string &command);
00143 void send_config_command(short command);
00144 void send_encoder_command(short command);
00145 void send_rotational_vel(short command);
00146 void send_colbert_relative_heading(short command);
00147 void send_rotational_acc(short command);
00148 void send_sonar_command(short command);
00149 void send_front_sonar_command();
00150 void send_rear_sonar_command();
00151 void send_stop_command();
00152 void send_digout_command(short command);
00153 void send_velocity2_command(short command);
00154 void send_gripper_command(short command);
00155 void send_ad_select_command(short command);
00156 void send_gripper_val_command(short command);
00157 void send_ptupos_command(short command);
00158 void send_tty2_command(const std::string& data);
00159 void send_tty2_command(const unsigned char* data, int size);
00160 void send_getaux_command(short command);
00161 void send_bumpstall_command(short command);
00162 void send_tcm2_command(short command);
00163 void send_emergency_stop();
00164 void send_pulse();
00165 void send_step();
00166
00167
00168
00169 protected:
00170
00171 unsigned char m_status;
00172 unsigned short m_xpos;
00173 unsigned short m_ypos;
00174 short m_thpos;
00175 short m_lvel;
00176 short m_rvel;
00177 unsigned char m_battery;
00178 short m_bumpers;
00179 short m_control;
00180 unsigned short m_ptu;
00181 unsigned char m_compass;
00182 unsigned short m_sonars[16];
00183 unsigned short m_timer;
00184 unsigned char m_analog;
00185 unsigned char m_digin;
00186 unsigned char m_digout;
00187
00188
00189 float m_AngleConvFactor;
00190 float m_DistConvFactor;
00191 float m_VelConvFactor;
00192 float m_RobotRadius;
00193 float m_RobotDiagonal;
00194 bool m_Holonomic;
00195 float m_RangeConvFactor;
00196 float m_RobotAxleLength;
00197
00198
00199 private:
00200
00201 int open_serial(const char *tty);
00202
00203 void lock_send_thread();
00204 void unlock_send_thread();
00205 void lock_recv_thread();
00206 void unlock_recv_thread();
00207 void send_int_command(unsigned char command, short value);
00208 void send_signed_int_command(unsigned char command, short value);
00209 void send_noarg_command(unsigned char command);
00210 void send_string_command(unsigned char command, const std::string& data);
00211 void send_unsigned_char_command(unsigned char command, const unsigned char* data, int size);
00212 void configure_parameters(const std::string &robotType, const std::string &robotSubType);
00213 void reinitialize();
00214
00215
00216 static Pioneer2PacketManager *m_instance;
00217 pseudosem_t m_semaphore;
00218 pthread_t m_send_thread;
00219 pthread_t m_recv_thread;
00220 pthread_mutex_t m_send_mutex;
00221 pthread_mutex_t m_recv_mutex;
00222
00223 volatile bool m_send_running;
00224 volatile bool m_recv_running;
00225
00226 int m_file_descriptor;
00227 std::list<PSOSPacket> m_send_list;
00228 enum {NO_SYNC, AFTER_FIRST_SYNC, AFTER_SECOND_SYNC, READY} m_psos_state;
00229
00230 public:
00231
00232
00233 static const unsigned char SYNC0;
00234 static const unsigned char SYNC1;
00235 static const unsigned char SYNC2;
00236 static const unsigned char PULSE;
00237 static const unsigned char OPEN;
00238 static const unsigned char CLOSE;
00239 static const unsigned char POLLING;
00240 static const unsigned char ENABLE;
00241 static const unsigned char SETA;
00242 static const unsigned char SETV;
00243 static const unsigned char SET0;
00244 static const unsigned char SETRV;
00245 static const unsigned char VEL;
00246 static const unsigned char HEAD;
00247 static const unsigned char DHEAD;
00248 static const unsigned char SAY;
00249 static const unsigned char CONFIG;
00250 static const unsigned char ENCODER;
00251 static const unsigned char RVEL;
00252 static const unsigned char DCHEAD;
00253 static const unsigned char SETRA;
00254 static const unsigned char SONAR;
00255 static const unsigned char STOP;
00256 static const unsigned char DIGOUT;
00257 static const unsigned char VEL2;
00258 static const unsigned char GRIPPER;
00259 static const unsigned char ADSEL;
00260 static const unsigned char GRIPPERVAL;
00261 static const unsigned char PTUPOS;
00262 static const unsigned char TTY2;
00263 static const unsigned char GETAUX;
00264 static const unsigned char BUMPSTALL;
00265 static const unsigned char TCM2;
00266 static const unsigned char E_STOP;
00267 static const unsigned char STEP;
00268
00269
00270 static const short GRIPOPEN;
00271 static const short GRIPCLOSE;
00272 static const short GRIPSTOP;
00273 static const short LIFTUP;
00274 static const short LIFTDOWN;
00275 static const short LIFTSTOP;
00276 static const short GRIPSTORE;
00277 static const short GRIPDEPLOY;
00278 static const short GRIPHALT;
00279 static const short GRIPPRESS;
00280 static const short LIFTCARRY;
00281
00282
00283 static const int MAX_QUEUE_SIZE;
00284
00285 };
00286
00287
00288
00289 }
00290
00291 #endif