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
00027 #ifndef EVADER_H
00028 #define EVADER_H
00029
00030 #include <libplayerc++/playerc++.h>
00031 #include <vector>
00032 #include <list>
00033 #include <unistd.h>
00034 #include <math.h>
00035 #include <cstdlib>
00036 #include <assert.h>
00037
00038
00039 #include <R2.h>
00040 #include <Grid.h>
00041 #include <Scan.h>
00042
00043 #include "args.h"
00044 #include "utils.h"
00045
00046
00047 using namespace PlayerCc;
00048 using namespace std;
00049 using namespace boost;
00050
00051 class Evader;
00052
00053 typedef uint Identity;
00054 typedef std::vector<Position > EvaderPath;
00055 typedef std::vector<Evader* > EvaderVector;
00056 typedef std::vector<Position > AgentPositions;
00057
00058 enum EvasionTypes
00059 {
00060 RANDOM_WALK,
00061 SMART_WALK,
00062 RUNAWAY,
00063 NO_EVASION
00064 };
00065
00066 class Evader{
00067
00068 protected :
00069 Identity m_iID;
00070 PlayerClient* m_pClient;
00071 Position2dProxy* m_pPosProxy;
00072 LaserProxy* m_pLaserProxy;
00073
00074 EvasionTypes m_eEvasion;
00075
00076 Grid m_LSR;
00077 EvaderPath m_path;
00078
00079 void recordLSR();
00080
00081 public :
00082 Evader( Identity ID, PlayerClient* client, Position2dProxy* posProxy, LaserProxy* laserProxy );
00083 ~Evader( );
00084
00085 void clientRead();
00086 Identity getID(){ return m_iID; }
00087 EvasionTypes getEvasion(){ return m_eEvasion; }
00088
00089 Pose getPose(){ return Pose(m_pPosProxy->GetXPos(), m_pPosProxy->GetYPos(), m_pPosProxy->GetYaw()); }
00090 Position getPosition(){ return getPose().pos(); }
00091 double getWidth(){ return m_pPosProxy->GetSize().sw; }
00092 Scan getScan();
00093 Position getLSRPosition();
00094 Position getCellPosition( DubInt indices );
00095
00096 void randomWalk();
00097 void smartWalk( AgentPositions agentPosVec );
00098 void runaway( AgentPositions agentPosVec );
00099
00100 void setToNoEvasion();
00101 void setToRandomWalk();
00102 void setToSmartWalk();
00103 void setToRunaway();
00104
00105 void goTo( Position pos );
00106 void stop(){ m_pPosProxy->SetMotorEnable(false); }
00107
00108 void doEvasion( AgentPositions agentPosVec );
00109
00110 void drawLSR( Graphics2dProxy &gp, bool bOnlyLSR );
00111 };
00112
00113 Evader::Evader( Identity ID, PlayerClient* client, Position2dProxy* posProxy, LaserProxy* laserProxy )
00114 {
00115 m_iID = ID;
00116 m_pClient = client;
00117 m_pPosProxy = posProxy;
00118 m_pLaserProxy = laserProxy;
00119
00120 m_eEvasion = NO_EVASION;
00121 m_LSR = Grid();
00122 m_path.clear();
00123
00124
00125 m_pPosProxy->SetMotorEnable(true);
00126 m_pPosProxy->RequestGeom();
00127
00128 while( m_pLaserProxy->GetMaxRange() <= 0.0 || m_pLaserProxy->GetCount() <= 0 )
00129 {
00130 if( gDebug > 9 ) cout<< " waiting for real data ..." << endl;
00131 clientRead();
00132 }
00133 }
00134
00135 void Evader::clientRead()
00136 {
00137 m_pClient->Read();
00138 }
00139
00140 void Evader::recordLSR()
00141 {
00142
00143 double maxRange = m_pLaserProxy->GetMaxRange();
00144 ExplorationParams params = ExplorationParams( maxRange, (m_pLaserProxy->GetMaxAngle() - m_pLaserProxy->GetMinAngle()), m_pLaserProxy->GetCount(), m_pLaserProxy->GetScanRes());
00145
00146 m_LSR = Grid( params, getScan(), getPose(), 3*maxRange, 3*maxRange );
00147
00148 m_LSR.defineBoard( gMinArcSize, gMinRadialArcSize );
00149 }
00150
00151 Scan Evader::getScan( )
00152 {
00153 clientRead();
00154
00155 int iCount = m_pLaserProxy->GetCount();
00156
00157 vector<Ray> rays;
00158 vector<Ray> rays2;
00159
00160 for( int i = 0; i < iCount; i++ )
00161 {
00162
00163 rays.push_back( Ray( m_pLaserProxy->GetRange(i), m_pLaserProxy->GetBearing(i), true ) );
00164 }
00165
00166 return Scan( rays, Time() );
00167 }
00168
00169 Position Evader::getCellPosition( DubInt indices )
00170 {
00171 Cell* pCell = m_LSR.getCell(indices);
00172 assert( pCell != NULL );
00173 return pCell->point();
00174 }
00175
00176 void Evader::randomWalk()
00177 {
00178 if( gDebug > 5 ) cout<< " Evader " << getID() << ": Doing random walk" << endl;
00179
00180 clientRead();
00181
00182 recordLSR();
00183 m_path.clear();
00184
00185 int iRandom = pickIndexInRange(0, m_LSR._lrr.size());
00186
00187 Position randomPos = getPosition() + getCellPosition(m_LSR._lrr[iRandom]);
00188
00189 m_path.push_back(randomPos);
00190
00191 goTo( randomPos );
00192
00193 if( gDebug > 5 ) drawRedX( randomPos, 0.05 );
00194 }
00195
00196 void Evader::smartWalk( AgentPositions agentPosVec )
00197 {
00198 if( gDebug > 5 ) cout<< " Evader " << getID() << ": Doing smart walk" << endl;
00199
00200 if( gDebug > 9 )
00201 {
00202 cout<< "Avoiding agents at : ";
00203 for( int j = 0; j < agentPosVec.size(); j++ )
00204 {
00205 cout<< agentPosVec[j].print() << ", " ;
00206 }
00207 cout << endl;
00208 }
00209
00210 clientRead();
00211
00212 Position curPos = getPosition();
00213
00214 if( m_path.size() > 0 && curPos.dist(m_path.back()) > 2*m_pPosProxy->GetSize().sw )
00215 {
00216 bool bSafe = true;
00217 for( int j = 0; j < agentPosVec.size(); j++ )
00218 {
00219 if( m_path.back().dist( agentPosVec[j] ) < 1.5*m_pLaserProxy->GetMaxRange() )
00220 {
00221 bSafe = false;
00222 break;
00223 }
00224 }
00225
00226 if( bSafe ) return;
00227 }
00228
00229 recordLSR();
00230 m_path.clear();
00231
00232 if( gDebug > 9 )
00233 {
00234 gGP->Clear();
00235 drawLSR( *gGP, false );
00236 }
00237
00238 LRR lrr = m_LSR._lrr;
00239 double bestMinDist = 0.0;
00240
00241
00242 for( int i = 0; i < 9; i++ )
00243 {
00244 int iRandom = pickIndexInRange(0, lrr.size());
00245
00246 Position randomPos = getPosition() + getCellPosition(lrr[iRandom]);
00247
00248 if( gDebug > 9 ) drawX( gColorGray, randomPos, 0.05 );
00249
00250 double minDist = 100.0;
00251 for( int j = 0; j < agentPosVec.size(); j++ )
00252 {
00253 if( gDebug > 9 ) drawX( gColorRed, agentPosVec[j], 0.05 );
00254
00255 double dist = randomPos.dist( agentPosVec[j] );
00256
00257 if( dist < minDist )
00258 {
00259 minDist = dist;
00260 }
00261 }
00262
00263 if( minDist > bestMinDist )
00264 {
00265 bestMinDist = minDist;
00266 m_path.clear();
00267 m_path.push_back(randomPos);
00268 }
00269
00270 if( bestMinDist > (1.2*m_pLaserProxy->GetMaxRange()) )
00271 {
00272 break;
00273 }
00274 }
00275
00276 goTo( m_path.back() );
00277
00278 if( gDebug > 1 ) drawRedX( m_path.back(), 0.05 );
00279
00280 if( gDebug > 9 ) cout << "Best min dist: " << bestMinDist << endl;
00281
00282 if( gDebug > 9 )
00283 {
00284 cout<< "Waiting for go ahead..." << endl;
00285 char input[10];
00286 cin>> input;
00287 }
00288 }
00289
00290 void Evader::runaway(AgentPositions agentPosVec)
00291 {
00292 if( gDebug > 5 ) cout<< " Evader " << getID() << ": doing runaway" << endl;
00293
00294 clientRead();
00295
00296 recordLSR();
00297 m_path.clear();
00298
00299 LRR lrr = m_LSR._lrr;
00300 Position cellPosition;
00301 double minDist;
00302 Position bestPosition = getPosition();
00303 double bestMinDist = 0.0;
00304
00305 for( int i = 0; i < lrr.size(); i++ )
00306 {
00307 cellPosition = getCellPosition(lrr[i]);
00308 minDist = 100000.0;
00309
00310 for( int j = 0; j < agentPosVec.size(); j++ )
00311 {
00312 minDist = std::min( minDist, (double)cellPosition.dist(agentPosVec[j]) );
00313
00314 if( minDist > bestMinDist )
00315 {
00316 bestPosition = getPosition() + cellPosition;
00317 bestMinDist = minDist;
00318 }
00319 }
00320 }
00321
00322
00323 m_path.push_back(bestPosition);
00324
00325 goTo(bestPosition);
00326
00327 if( gDebug > 5 ) drawRedX( bestPosition, 0.05 );
00328 }
00329
00330 void Evader::setToNoEvasion( )
00331 {
00332 if( gDebug > 5 ) cout<< " Evader " << getID() << ": set to NO_EVASION" << endl;
00333
00334 m_eEvasion = NO_EVASION;
00335
00336 m_LSR = Grid();
00337 m_path.clear();
00338
00339
00340 goTo( getPosition() );
00341 stop();
00342
00343 }
00344
00345 void Evader::setToRandomWalk( )
00346 {
00347 if( gDebug > 5 ) cout<< " Evader " << getID() << ": set to RANDOM_WALK" << endl;
00348
00349 m_eEvasion = RANDOM_WALK;
00350
00351 m_LSR = Grid();
00352 m_path.clear();
00353
00354
00355 m_pPosProxy->SetMotorEnable(true);
00356 }
00357
00358 void Evader::setToSmartWalk( )
00359 {
00360 if( gDebug > 5 ) cout<< " Evader " << getID() << ": set to SMART_WALK" << endl;
00361
00362 m_eEvasion = SMART_WALK;
00363
00364 m_LSR = Grid();
00365 m_path.clear();
00366
00367
00368 m_pPosProxy->SetMotorEnable(true);
00369 }
00370
00371 void Evader::setToRunaway( )
00372 {
00373 if( gDebug > 5 ) cout<< " Evader " << getID() << ": set to RUNAWAY" << endl;
00374
00375 m_eEvasion = RUNAWAY;
00376
00377 m_LSR = Grid();
00378 m_path.clear();
00379
00380
00381 m_pPosProxy->SetMotorEnable(true);
00382 }
00383
00384 void Evader::goTo( Position pos )
00385 {
00386 m_pPosProxy->GoTo((double)(pos.x()),(double)(pos.y()),(double)(pos.bearing().dCast2Pi()));
00387 }
00388
00389 void Evader::doEvasion( AgentPositions agentPosVec )
00390 {
00391 if( gDebug > 8 ) cout<< " " << getID() << ": doing current behavior" << endl;
00392
00393 clientRead();
00394
00395 switch (m_eEvasion) {
00396 case RANDOM_WALK:
00397 randomWalk();
00398 break;
00399 case SMART_WALK:
00400 smartWalk(agentPosVec);
00401 break;
00402 case RUNAWAY:
00403 runaway(agentPosVec);
00404 break;
00405 case NO_EVASION:
00406 break;
00407 default:
00408 cerr<< "ERROR: No behavior category" << endl;
00409 assert(false);
00410 }
00411 }
00412
00413
00414 void Evader::drawLSR( Graphics2dProxy &gp, bool bOnlyLSR )
00415 {
00416 Position curPos = getPosition();
00417
00418
00419 int LSRsize = m_LSR.columns()*m_LSR.rows();
00420 player_color_t color;
00421
00422 for( int i = 0; i < LSRsize; i++ )
00423 {
00424 Position LSRpoint = m_LSR.getCell(i)->point();
00425
00426 if( !bOnlyLSR && m_LSR.getCell(i)->isFrontier() )
00427 {
00428 color.alpha = 0;
00429 color.red = 0;
00430 color.green = 0;
00431 color.blue = 255;
00432
00433 drawSquare( color, curPos + LSRpoint, 0.01, true );
00434 }
00435 else if( !bOnlyLSR && m_LSR.getCell(i)->isObstacle() )
00436 {
00437 color.alpha = 0;
00438 color.red = 255;
00439 color.green = 0;
00440 color.blue = 0;
00441
00442 drawSquare( color, curPos + LSRpoint, 0.01, true );
00443 }
00444 else if( !bOnlyLSR && m_LSR.getCell(i)->isLRR() )
00445 {
00446 color.alpha = 0;
00447 color.red = 0;
00448 color.green = 180;
00449 color.blue = 0;
00450
00451 drawSquare( color, curPos + LSRpoint, 0.01, true );
00452 }
00453 else if( m_LSR.getCell(i)->isLSR() )
00454 {
00455 color.alpha = 0;
00456 color.red = 0;
00457 color.green = 255;
00458 color.blue = 0;
00459
00460 drawSquare( color, curPos + LSRpoint, 0.01, true );
00461 }
00462 }
00463
00464 }
00465
00466
00467 #endif //EVADER_H