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
00032 #include "maneuvers.h"
00033
00034 namespace atlascar_base
00035 {
00036 void Maneuvers::init()
00037 {
00038 maneuver=0;
00039
00040 int ret = gamepad.startComm("/dev/input/js0");
00041
00042
00043 if(ret<0)
00044 {
00045 cout<<"Cannot open communications with gamepad on: "<<"/dev/input/js0"<<endl;
00046 return;
00047 }
00048
00049 current_state = INIT;
00050
00051 command_msg.lifetime = INFINITY;
00052 command_msg.priority = 1;
00053
00054
00055 gamepad.buttons(0)->callback = sigc::mem_fun<int>(this,&Maneuvers::positive_velocity);
00056 gamepad.buttons(2)->callback = sigc::mem_fun<int>(this,&Maneuvers::neutral_velocity_parking);
00057 gamepad.buttons(1)->callback = sigc::mem_fun<int>(this,&Maneuvers::neutral_velocity);
00058 gamepad.buttons(3)->callback = sigc::mem_fun<int>(this,&Maneuvers::negative_velocity);
00059 gamepad.buttons(12)->callback = sigc::mem_fun<int>(this,&Maneuvers::parallel_parking);
00060 gamepad.buttons(14)->callback = sigc::mem_fun<int>(this,&Maneuvers::reversing);
00061 }
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072 void Maneuvers::setupMessaging()
00073 {
00074
00075 status_sub = nh.subscribe("status", 1, &Maneuvers::statusCallback, this);
00076
00077 command_pub = nh.advertise<atlascar_base::ManagerCommand>("command_msg", 1);
00078 }
00079
00080 void Maneuvers::statusCallback(const atlascar_base::ManagerStatusPtr& status)
00081 {
00082
00083 status_msg.velocity=status->velocity;
00084 status_msg.count=status->count;
00085 status_msg.pulses_sec=status->pulses_sec;
00086 status_msg.revolutions_sec=status->revolutions_sec;
00087 status_msg.header.stamp=status->header.stamp;
00088 status_msg.gear=status->gear;
00089 status_msg.clutch=status->clutch;
00090 status_msg.brake=status->brake;
00091 status_msg.steering_wheel=status->steering_wheel;
00092 status_msg.rpm=status->rpm;
00093 status_msg.throttle_pedal=status->throttle_pedal;
00094 status_msg.velocity=status->velocity;
00095 status_msg.count=status->count;
00096 status_msg.throttle_pressure=status->throttle_pressure;
00097 status_msg.brake_pressure=status->brake_pressure;
00098 status_msg.clutch_pressure=status->clutch_pressure;
00099
00100
00101
00102
00103
00104
00105 }
00106
00107 void Maneuvers::positive_velocity(int value)
00108 {
00109
00110 maneuver=1;
00111
00112
00113 if(value==0)
00114 return;
00115
00116 if (current_state==INIT)
00117 current_state = POSITIVE_VELOCITY;
00118 }
00119
00120
00121 void Maneuvers::negative_velocity(int value)
00122 {
00123
00124 maneuver=2;
00125
00126
00127 if(value==0)
00128 return;
00129
00130 if (current_state==INIT)
00131 current_state = NEGATIVE_VELOCITY;
00132 }
00133
00134 void Maneuvers::parallel_parking(int value)
00135 {
00136
00137 maneuver=3;
00138
00139
00140 if(value==0)
00141 return;
00142
00143 if (current_state==INIT)
00144 {
00145 status_memory.count = status_msg.count;
00146 pparking=FIRST;
00147 current_state = PARALLEL_PARKING;
00148 }
00149 }
00150
00151 void Maneuvers::reversing(int value)
00152 {
00153
00154 maneuver=4;
00155
00156
00157 if(value==0)
00158 return;
00159
00160 if (current_state==INIT)
00161 {
00162 status_memory.count = status_msg.count;
00163 rreverse=FIRST;
00164 current_state = REVERSING;
00165 }
00166 }
00167
00168 void Maneuvers::neutral_velocity(int value)
00169 {
00170
00171 maneuver=0;
00172
00173
00174 if(value==0)
00175 return;
00176
00177 current_state = NEUTRAL_VELOCITY;
00178 }
00179
00180 void Maneuvers::neutral_velocity_parking(int value)
00181 {
00182
00183 maneuver=0;
00184
00185
00186 if(value==0)
00187 return;
00188
00189 if (current_state = INIT)
00190 current_state = NEUTRAL_VELOCITY_PARKING;
00191 }
00192
00193 void Maneuvers::reverse()
00194 {
00195 switch(rreverse)
00196 {
00197
00198 case FIRST:
00199 command_msg.velocity=2;
00200 command_msg.steering_wheel=0.45;
00201
00202
00203 if (status_msg.count>status_memory.count+10)
00204 rreverse=SECOND;
00205
00206 break;
00207
00208
00209 case SECOND:
00210 command_msg.velocity=0;
00211 command_msg.steering_wheel=0.45;
00212
00213
00214 if (abs(status_msg.velocity)<0.01)
00215 {
00216 status_memory.count=status_msg.count;
00217 rreverse=THIRD;
00218 }
00219
00220 break;
00221
00222
00223 case THIRD:
00224 command_msg.velocity=-2;
00225 command_msg.steering_wheel=-0.45;
00226
00227
00228 if (status_msg.count<status_memory.count-2)
00229 rreverse=FOURTH;
00230
00231 break;
00232
00233
00234 case FOURTH:
00235 command_msg.velocity=0;
00236 command_msg.steering_wheel=-0.45;
00237
00238
00239 if (abs(status_msg.velocity)<0.01)
00240 {
00241 status_memory.count=status_msg.count;
00242 rreverse=FIFTH;
00243 }
00244
00245 break;
00246
00247
00248 case FIFTH:
00249 command_msg.velocity=2;
00250 command_msg.steering_wheel=0.45;
00251
00252
00253 if (status_msg.count>status_memory.count+2)
00254 rreverse=SIXTH;
00255
00256 break;
00257
00258
00259 case SIXTH:
00260 command_msg.velocity=0;
00261 command_msg.steering_wheel=0.45;
00262
00263
00264 if (abs(status_msg.velocity)<0.01)
00265 {
00266 status_memory.count=status_msg.count;
00267 rreverse=SEVENTH;
00268 }
00269
00270 break;
00271
00272
00273 case SEVENTH:
00274
00275 command_msg.velocity=-2;
00276 command_msg.steering_wheel=-0.45;
00277
00278
00279 if (status_msg.count<status_memory.count-2)
00280 rreverse=EIGHTH;
00281
00282 break;
00283
00284
00285 case EIGHTH:
00286 command_msg.velocity=0;
00287 command_msg.steering_wheel=-0.45;
00288
00289
00290 if (abs(status_msg.velocity)<0.01)
00291 {
00292 status_memory.count=status_msg.count;
00293 rreverse=NINETH;
00294 }
00295
00296 break;
00297
00298
00299 case NINETH:
00300 command_msg.velocity=2;
00301 command_msg.steering_wheel=0.45;
00302
00303
00304 if (status_msg.count>status_memory.count+5)
00305 rreverse=TENTH;
00306
00307 break;
00308
00309
00310 case TENTH:
00311 command_msg.velocity=0;
00312 command_msg.steering_wheel=0;
00313
00314
00315 if (abs(status_msg.velocity)<0.01)
00316 {
00317 maneuver=0;
00318
00319 current_state=INIT;
00320 }
00321
00322 break;
00323 }
00324 }
00325
00326 void Maneuvers::parallel()
00327 {
00328 switch(pparking)
00329 {
00330
00331 case FIRST:
00332 command_msg.velocity=-2;
00333 command_msg.steering_wheel=-0.40;
00334
00335
00336 if (status_msg.count<status_memory.count-10)
00337 {
00338 status_memory.count=status_msg.count;
00339 pparking=SECOND;
00340 }
00341 break;
00342
00343
00344 case SECOND:
00345 command_msg.velocity=0;
00346 command_msg.steering_wheel=-0.40;
00347
00348
00349 if (abs(status_msg.velocity)<0.01)
00350 {
00351 status_memory.count=status_msg.count;
00352 pparking=THIRD;
00353 }
00354
00355 break;
00356
00357
00358 case THIRD:
00359 command_msg.velocity=-2;
00360 command_msg.steering_wheel=0.40;
00361
00362
00363 if (status_msg.count<status_memory.count-2)
00364 pparking=FOURTH;
00365
00366 break;
00367
00368
00369 case FOURTH:
00370 command_msg.velocity=0;
00371 command_msg.steering_wheel=0.40;
00372
00373
00374 if (abs(status_msg.velocity)<0.01)
00375 {
00376 status_memory.count=status_msg.count;
00377 pparking=FIFTH;
00378 }
00379
00380 break;
00381
00382
00383 case FIFTH:
00384 command_msg.velocity=2;
00385 command_msg.steering_wheel=-0.40;
00386
00387
00388 if (status_msg.count>status_memory.count+1)
00389 pparking=SIXTH;
00390
00391 break;
00392
00393
00394 case SIXTH:
00395 command_msg.velocity=0;
00396 command_msg.steering_wheel=0;
00397
00398
00399 if (abs(status_msg.velocity)<0.01)
00400 {
00401 maneuver=0;
00402
00403 current_state=INIT;
00404 }
00405
00406 break;
00407 }
00408 }
00409
00410 void Maneuvers::stateManager()
00411 {
00412
00413 switch(current_state)
00414 {
00415 case UNKNOWN:
00416 break;
00417
00418 case INIT:
00419 break;
00420
00421 case POSITIVE_VELOCITY:
00422
00423 command_msg.parking=0;
00424 command_msg.velocity = 2;
00425 command_msg.steering_wheel=0;
00426
00427
00428 current_state=INIT;
00429 break;
00430
00431 case NEUTRAL_VELOCITY:
00432
00433 command_msg.parking=0;
00434 command_msg.velocity = 0;
00435 command_msg.steering_wheel=0;
00436
00437
00438 current_state=INIT;
00439 break;
00440
00441 case NEGATIVE_VELOCITY:
00442
00443 command_msg.parking=0;
00444 command_msg.velocity = -2;
00445 command_msg.steering_wheel=0;
00446
00447
00448 current_state=INIT;
00449 break;
00450
00451 case PARALLEL_PARKING:
00452 command_msg.parking=0;
00453 parallel();
00454 break;
00455
00456 case REVERSING:
00457 command_msg.parking=0;
00458 reverse();
00459 break;
00460
00461 case NEUTRAL_VELOCITY_PARKING:
00462
00463 command_msg.parking=1;
00464
00465
00466 current_state=INIT;
00467 break;
00468
00469 default:
00470 cout<<"This state was not defined: "<<current_state<<endl;
00471 };
00472 }
00473
00474 void Maneuvers::loop()
00475 {
00476
00477 ros::Rate r(100);
00478
00479 do
00480 {
00481
00482 gamepad.dispatch();
00483
00484 stateManager();
00485
00486 command_msg.header.stamp = ros::Time::now();
00487
00488 command_pub.publish(command_msg);
00489
00490
00491 ros::spinOnce();
00492 r.sleep();
00493
00494 }while(ros::ok());
00495 }
00496 }
00497
atlascar_base
Author(s): Jorge Almeida, Sérgio Pinho, Miguel Oliveira, Pedro Salvado, Andre Oliveira and Pedro Pinheiro
autogenerated on Thu Nov 20 2014 11:35:17