Go to the documentation of this file.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
00031 #ifndef _CLASS_ATLASMV_H_
00032 #define _CLASS_ATLASMV_H_
00033
00034 #define _DO_NOT_USE_OPENCV_ 0
00035 #define _DO_NOT_USE_CARMEN_ 0
00036
00037
00038
00039
00040
00041
00042 #include <pthread.h>
00043 #include <string.h>
00044 #include <termios.h>
00045 #include <sys/ioctl.h>
00046 #include <stdlib.h>
00047 #include <stdio.h>
00048 #include <sys/types.h>
00049 #include <sys/stat.h>
00050 #include <fcntl.h>
00051 #include <unistd.h>
00052 #include <ros/ros.h>
00053 #include <termios.h>
00054 #include <sys/ioctl.h>
00055 #include <signal.h>
00056 #include <atlasmv_base/timer.h>
00057
00058 #include <hitec5980sg/hitec5980sg.h>
00059 #include <atlasmv_base/class_dioc.h>
00060 #include <atlasmv_base/des70_10.h>
00061 #include <iostream>
00062
00063 #include <atlasmv_base/AtlasmvLightsCommand.h>
00064 #include <atlasmv_base/AtlasmvMotionCommand.h>
00065 #include <atlasmv_base/AtlasmvVertSignsCommand.h>
00066 #include <atlasmv_base/AtlasmvStatus.h>
00067
00068 using namespace std;
00069
00070 typedef enum
00071 {
00072 ENUM_ERR_NONE = 0,
00073 ENUM_ERR_READ_DIR = 3,
00074 ENUM_ERR_OVER_CURRENT = 4,
00075 ENUM_ERR_OVER_VOLTAGE = 5,
00076 ENUM_ERR_OVER_SPEED = 6,
00077 ENUM_ERR_SUPPLY_VOLT_LOW = 7,
00078 ENUM_ERR_ANG_DET = 8,
00079 ENUM_ERR_ENC_RES = 9
00080 }ENUM_ERR;
00081
00082 typedef enum
00083 {
00084 MANDATORY_MEDIUM_LIGTHS = 1,
00085 MANDATORY_BUS_LANE = 2,
00086 WARNING_ROAD_NARROWS = 3,
00087 WARNING_DIP_AHEAD = 4,
00088 INFORMATION_HOSPITAL = 5,
00089 INFORMATION_REC_SPEED_60 = 6,
00090 NO_VERT_SIGN=0,
00091 }TYPE_ENUM_SIGNS;
00092
00096 typedef struct details{
00097 int cycle_freq;
00098 double sleep_time;
00099
00100 double max_forward_speed;
00101 double max_backward_speed;
00102 double brake_speed_dif;
00103
00104 int servo_direction_id;
00105 int servo_brake_id;
00106
00107 std::string com_servos;
00108 std::string com_dioc;
00109 std::string com_des;
00110
00111 double length;
00112 double width;
00113 double back_wheel_diam;
00114 double wheelaxisdistance;
00115 double maximum_dir;
00116 double minimum_dir;
00117 double maximum_brk;
00118 double minimum_brk;
00119 double transmission_relation;
00120 int min_pulse_brk;
00121 int max_pulse_brk;
00122 int min_pulse_dir;
00123 int max_pulse_dir;
00124 double brk_time_no_maxon;
00125
00126 TYPE_des_sysparam sDES_sysparam;
00127 int des_frq_chk_errors;
00128
00129 friend ostream& operator<< (ostream &o, const details &i)
00130 {
00131 return o
00132 << "cycle_freq: "<< i.cycle_freq <<endl
00133 << "sleep_time: "<< i.sleep_time<<endl
00134 << "com_servos: "<< i.com_servos<<endl
00135 << "com_dioc: "<< i.com_dioc<<endl
00136 << "com_des: "<< i.com_des<<endl;
00137 }
00138 }TYPE_atlasmv_public_params;
00139
00143 class c_atlasmv{
00144 private:
00145
00146
00147 pthread_mutex_t c_atlasmv_robot_details_mutex;
00148 TYPE_atlasmv_public_params *robot_details;
00149 pthread_mutex_t flags_mutex;
00150 bool debug_mode;
00151
00152 char get_executionflags_debug();
00153 c_timer *timer;
00154 pthread_mutex_t errors_mutex;
00155 int get_errors();
00156 int set_errors(int);
00157 int errors;
00158
00159
00160
00161 pthread_mutex_t max_forward_speed_mutex;
00162 double max_forward_speed;
00163 pthread_mutex_t max_backward_speed_mutex;
00164 double max_backward_speed;
00165 pthread_mutex_t brake_speed_dif_mutex;
00166 double brake_speed_dif;
00167
00168 pthread_mutex_t com_des_mutex;
00169 std::string com_des;
00170 pthread_mutex_t porthandler_des_mutex;
00171 int* porthandler_des;
00172
00173 pthread_mutex_t length_mutex;
00174 double length;
00175 pthread_mutex_t width_mutex;
00176 double width;
00177 pthread_mutex_t back_wheel_diam_mutex;
00178 double back_wheel_diam;
00179 pthread_mutex_t wheelaxisdistance_mutex;
00180 double wheelaxisdistance;
00181 pthread_mutex_t transmission_relation_mutex;
00182 double transmission_relation;
00183 pthread_mutex_t sDES_sysparam_mutex;
00184 TYPE_des_sysparam sDES_sysparam;
00185 pthread_mutex_t rpm_mutex;
00186 int actual_rpm;
00187 int req_rpm;
00188 pthread_mutex_t linear_speed_mutex;
00189 double actual_speed;
00190 double req_speed;
00191 pthread_mutex_t des_initialized_mutex;
00192 char des_initialized;
00193 pthread_mutex_t brk_time_no_maxon_mutex;
00194 double brk_time_no_maxon;
00195
00196
00197
00198
00199
00200 pthread_mutex_t maximum_dir_mutex;
00201 double maximum_dir;
00202 pthread_mutex_t minimum_dir_mutex;
00203 double minimum_dir;
00204 pthread_mutex_t maximum_brk_mutex;
00205 double maximum_brk;
00206 pthread_mutex_t minimum_brk_mutex;
00207 double minimum_brk;
00208 pthread_mutex_t com_servos_mutex;
00209 std::string com_servos;
00210 pthread_mutex_t actual_dir_mutex;
00211 double actual_dir;
00212 pthread_mutex_t actual_brk_mutex;
00213 double actual_brk;
00214 pthread_mutex_t servo_initialized_mutex;
00215 char servo_initialized;
00216 pthread_mutex_t min_pulse_brk_mutex;
00217 int min_pulse_brk;
00218 pthread_mutex_t max_pulse_brk_mutex;
00219 int max_pulse_brk;
00220 pthread_mutex_t min_pulse_dir_mutex;
00221 int min_pulse_dir;
00222 pthread_mutex_t max_pulse_dir_mutex;
00223 int max_pulse_dir;
00224
00225 pthread_mutex_t servo_direction_id_mutex;
00226 int servo_direction_id;
00227 pthread_mutex_t servo_brake_id_mutex;
00228 int servo_brake_id;
00229
00230
00231 float slope_rad2pulse;
00232 float bias_rad2pulse;
00233 float slope_pulse2rad;
00234 float bias_pulse2rad;
00235 hitec_5980SG *servo;
00236
00237
00238
00239
00240 int init_com_servo();
00241
00242
00243
00244
00245
00246 pthread_mutex_t com_dioc_mutex;
00247 std::string com_dioc;
00248 class_dioc *dioc;
00249 pthread_mutex_t dioc_initialized_mutex;
00250 char dioc_initialized;
00251 pthread_mutex_t sign_mutex;
00252 int prev_sign;
00253 pthread_mutex_t lights_mutex;
00254 int prev_lights;
00255 pthread_mutex_t cross_sensor_mutex;
00256 int cross_sensor;
00257
00258 int init_com_dioc();
00259 pthread_mutex_t robot_status_mutex;
00260 int turnright;
00261 int turnleft;
00262 int headlights;
00263 int taillights;
00264 int reverselights;
00265 int vert_sign;
00266
00267
00268
00269
00270
00271 bool init;
00272
00273 double velocity;
00274
00275
00276 int init_com_des();
00277
00278 int rad2pulses(double rad);
00279 double pulses2rad(int pulses);
00280
00281 public:
00282 c_atlasmv(bool debug_);
00283 int set_atlasmv_details(TYPE_atlasmv_public_params *);
00284 int initialize_robot(void);
00285 int new_robot_state(atlasmv_base::AtlasmvMotionCommand&,atlasmv_base::AtlasmvVertSignsCommand&,atlasmv_base::AtlasmvLightsCommand&);
00286 int update_robot_status(atlasmv_base::AtlasmvStatus&);
00287
00289
00291 int check_des(void);
00292
00293
00294
00295
00296 double map_linear(double in,std::vector<std::pair<double,double> >& cp)
00297 {
00298
00299
00300
00301 double out=NAN;
00302
00303 for(uint i=0;i<cp.size()-1;i++)
00304 {
00305 if( in >= cp[i].first && in <= cp[i+1].first)
00306 {
00307
00308 double m = (cp[i+1].second - cp[i].second)/(cp[i+1].first - cp[i].first);
00309 double b = cp[i+1].second -m*cp[i+1].first;
00310
00311 out=m*in+b;
00312 break;
00313 }
00314 }
00315
00316 if(isnan(out)){
00317 cout<<"Warning: Input argument: "<<in<<" is out of bounds, ["<<cp[0].first<<","<< cp[cp.size()-1].first<<"]"<<endl;
00318 return 0;
00319 }
00320 else{
00321 return out;
00322 }
00323 }
00324
00325
00326
00327 int restart_des();
00328 int set_speed(double *);
00329 int read_speed(int);
00330
00331 void set_com_des(std::string);
00332 std::string get_com_des();
00333 void set_porthandler_des(int );
00334 int get_porthandler_des();
00335 void set_linear_speed(double*, double *);
00336 void get_linear_speed(double *, double *);
00337 void set_rpm(int *, int *);
00338 void get_rpm(int *, int *);
00339 void set_max_forward_speed(double *);
00340 double get_max_forward_speed();
00341 void set_max_backward_speed(double *);
00342 double get_max_backward_speed();
00343 void set_brake_speed_dif(double *);
00344 double get_brake_speed_dif();
00345 void set_length(double *);
00346 double get_length();
00347 void set_width(double *);
00348 double get_width();
00349 void set_back_wheel_diam(double *);
00350 double get_back_wheel_diam();
00351 void set_wheelaxisdistance(double *);
00352 double get_wheelaxisdistance();
00353 void set_transmission_relation(double *);
00354 double get_transmission_relation();
00355 void set_sDES_sysparam(TYPE_des_sysparam *);
00356 void get_sDES_sysparam(TYPE_des_sysparam *);
00357 void set_brk_time_no_maxon(double *);
00358 double get_brk_time_no_maxon();
00359
00360
00361
00362
00363
00364
00365
00366
00367 void set_des_initialized(char);
00368 char get_des_initialized();
00369
00370
00371 int set_direction(double *);
00372 int read_direction(double *);
00373 double get_actual_dir();
00374 void set_actual_dir(double *);
00375 double get_actual_brk();
00376 void set_actual_brk(double *);
00377 int set_brake(double *);
00378
00379 int read_brake(double *brk, int *num_pulses=NULL);
00380
00381
00382 void set_com_servo(std::string);
00383 std::string get_com_servo();
00384 void set_dir_srv_id(unsigned char);
00385 char get_dir_srv_id();
00386 void set_brk_srv_id(unsigned char);
00387 char get_brk_srv_id();
00388 void set_maximum_brk_angle(double *);
00389 double get_maximum_brk_angle();
00390 void set_minimum_brk_angle(double *);
00391 double get_minimum_brk_angle();
00392 void set_maximum_dir_angle(double *);
00393 double get_maximum_dir_angle();
00394 void set_minimum_dir_angle(double *);
00395 double get_minimum_dir_angle();
00396 void set_min_pulse_brk(int *);
00397 int get_min_pulse_brk();
00398 void set_max_pulse_brk(int *);
00399 int get_max_pulse_brk();
00400 void set_min_pulse_dir(int *);
00401 int get_min_pulse_dir();
00402 void set_max_pulse_dir(int *);
00403 int get_max_pulse_dir();
00404 void set_servo_initialized(char);
00405 char get_servo_initialized();
00406
00407
00408 int set_lights(atlasmv_base::AtlasmvVertSignsCommand&,atlasmv_base::AtlasmvLightsCommand&);
00409 int read_cross_sensor();
00410
00411 void set_com_dioc(std::string);
00412 std::string get_com_dioc();
00413 void set_dioc_initialized(char);
00414 char get_dioc_initialized();
00415 void set_cross_sensor(int);
00416 int get_cross_sensor();
00417 int get_lights(atlasmv_base::AtlasmvStatus&);
00418
00419
00420 ~c_atlasmv();
00421 };
00422
00423 #endif