EGraphTemp.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------


#ifndef EVADER_H
#define EVADER_H

#include <libplayerc++/playerc++.h>
#include <vector>
#include <list>
#include <unistd.h>
#include <math.h>
#include <cstdlib>
#include <assert.h>

// MIP
#include <R2.h>
#include <Grid.h>
#include <Scan.h>

#include "args.h"
#include "utils.h"
//#include "testFrontierPursuit.h"

using namespace PlayerCc;
using namespace std;
using namespace boost;

class Evader;

typedef uint Identity;
typedef std::vector<Position > EvaderPath;
typedef std::vector<Evader* > EvaderVector;
typedef std::vector<Position > AgentPositions;

enum EvasionTypes
{
    RANDOM_WALK,
    SMART_WALK,
    RUNAWAY,
    NO_EVASION
};

class Evader{
    
protected :
    Identity m_iID;
    PlayerClient* m_pClient;
    Position2dProxy* m_pPosProxy;
    LaserProxy* m_pLaserProxy;
    
    EvasionTypes m_eEvasion;
    
    Grid m_LSR;
    EvaderPath m_path;
    
    void recordLSR();
    
public :
    Evader( Identity ID, PlayerClient* client, Position2dProxy* posProxy, LaserProxy* laserProxy );
    ~Evader( );
    
    void clientRead();
    Identity getID(){ return m_iID; }
    EvasionTypes getEvasion(){ return m_eEvasion; }

    Pose getPose(){ return Pose(m_pPosProxy->GetXPos(), m_pPosProxy->GetYPos(), m_pPosProxy->GetYaw()); }
    Position getPosition(){ return getPose().pos(); }
    double getWidth(){ return m_pPosProxy->GetSize().sw; }
    Scan getScan();
    Position getLSRPosition();
    Position getCellPosition( DubInt indices );
    
    void randomWalk();
    void smartWalk( AgentPositions agentPosVec );
    void runaway( AgentPositions agentPosVec );
    
    void setToNoEvasion();
    void setToRandomWalk();
    void setToSmartWalk();
    void setToRunaway();
    
    void goTo( Position pos );
    void stop(){ m_pPosProxy->SetMotorEnable(false); }
    
    void doEvasion( AgentPositions agentPosVec );
    
    void drawLSR( Graphics2dProxy &gp, bool bOnlyLSR );
};

Evader::Evader( Identity ID, PlayerClient* client, Position2dProxy* posProxy, LaserProxy* laserProxy )
{
    m_iID = ID;
    m_pClient = client;
    m_pPosProxy = posProxy;
    m_pLaserProxy = laserProxy;
    
    m_eEvasion = NO_EVASION;
    m_LSR = Grid();
    m_path.clear();
    
    // Set up position proxy
    m_pPosProxy->SetMotorEnable(true);
    m_pPosProxy->RequestGeom();
    
    while( m_pLaserProxy->GetMaxRange() <= 0.0 || m_pLaserProxy->GetCount() <= 0 )
    {
        if( gDebug > 9 ) cout<< "  waiting for real data ..." << endl;
        clientRead();
    }
}

void Evader::clientRead()
{
    m_pClient->Read();
}

void Evader::recordLSR()
{
    // TODO: what is good size of final grid?
    double maxRange = m_pLaserProxy->GetMaxRange();
    ExplorationParams params = ExplorationParams( maxRange, (m_pLaserProxy->GetMaxAngle() - m_pLaserProxy->GetMinAngle()), m_pLaserProxy->GetCount(), m_pLaserProxy->GetScanRes());
    
    m_LSR = Grid( params, getScan(), /*Pose(getPosition(), 0)*/ getPose(), 3*maxRange, 3*maxRange );
    
    m_LSR.defineBoard( gMinArcSize, gMinRadialArcSize );
}

Scan Evader::getScan( )
{
    clientRead();
    
    int iCount = m_pLaserProxy->GetCount();
    
    vector<Ray> rays;
    vector<Ray> rays2;
    
    for( int i = 0; i < iCount; i++ )
    {
        // TODO: any easy entry for bValid?
        rays.push_back( Ray( m_pLaserProxy->GetRange(i), m_pLaserProxy->GetBearing(i), /*bValid*/ true ) );
    }
    
    return Scan( rays, Time() );
}

Position Evader::getCellPosition( DubInt indices )
{
    Cell* pCell = m_LSR.getCell(indices);
    assert( pCell != NULL );
    return pCell->point();
}

void Evader::randomWalk()
{
    if( gDebug > 5 ) cout<< "  Evader " << getID() << ":  Doing random walk" << endl;
    
    clientRead();
    
    recordLSR();
    m_path.clear();
    
    int iRandom = pickIndexInRange(0, m_LSR._lrr.size());
    
    Position randomPos = getPosition() + getCellPosition(m_LSR._lrr[iRandom]);
    
    m_path.push_back(randomPos);
    
    goTo( randomPos );
    
    if( gDebug > 5 ) drawRedX( randomPos, 0.05 );
}

void Evader::smartWalk( AgentPositions agentPosVec )
{
    if( gDebug > 5 ) cout<< "  Evader " << getID() << ":  Doing smart walk" << endl;
    
    if( gDebug > 9 )
    {
        cout<< "Avoiding agents at : ";
        for( int j = 0; j < agentPosVec.size(); j++ )
        {
            cout<< agentPosVec[j].print() << ", " ;
        }
        cout << endl;
    }
    
    clientRead();
    
    Position curPos = getPosition();
    
    if( m_path.size() > 0 && curPos.dist(m_path.back()) > 2*m_pPosProxy->GetSize().sw )
    {
        bool bSafe = true;
        for( int j = 0; j < agentPosVec.size(); j++ )
        {
            if( m_path.back().dist( agentPosVec[j] ) < 1.5*m_pLaserProxy->GetMaxRange() )
            {
                bSafe = false;
                break;
            }
        }
        
        if( bSafe ) return;
    }
    
    recordLSR();
    m_path.clear();
    
    if( gDebug > 9 ) 
    {
        gGP->Clear();
        drawLSR( *gGP, false );
    }
    
    LRR lrr = m_LSR._lrr;
    double bestMinDist = 0.0;
    
    
    for( int i = 0; i < 9; i++ )
    {
        int iRandom = pickIndexInRange(0, lrr.size());
    
        Position randomPos = getPosition() + getCellPosition(lrr[iRandom]);
        
        if( gDebug > 9 ) drawX( gColorGray, randomPos, 0.05 );
        
        double minDist = 100.0;
        for( int j = 0; j < agentPosVec.size(); j++ )
        {
            if( gDebug > 9 ) drawX( gColorRed, agentPosVec[j], 0.05 );
            
            double dist = randomPos.dist( agentPosVec[j] );
            
            if( dist < minDist )
            {
                minDist = dist;
            }
        }
        
        if( minDist > bestMinDist )
        {
            bestMinDist = minDist;
            m_path.clear();
            m_path.push_back(randomPos);
        }
        
        if( bestMinDist > (1.2*m_pLaserProxy->GetMaxRange()) )
        {
            break;
        }
    }
    
    goTo( m_path.back() );
    
    if( gDebug > 1 ) drawRedX( m_path.back(), 0.05 );
    
    if( gDebug > 9 ) cout << "Best min dist: " << bestMinDist << endl;
    
    if( gDebug > 9 )
    {
        cout<< "Waiting for go ahead..." << endl;
        char input[10];
        cin>> input;
    }
}

void Evader::runaway(AgentPositions agentPosVec)
{
    if( gDebug > 5 ) cout<< "  Evader " << getID() << ":  doing runaway" << endl;
    
    clientRead();
    
    recordLSR();
    m_path.clear();
    
    LRR lrr = m_LSR._lrr;
    Position cellPosition;
    double minDist;
    Position bestPosition = getPosition();
    double bestMinDist = 0.0;
    
    for( int i = 0; i < lrr.size(); i++ )
    {
        cellPosition = getCellPosition(lrr[i]);
        minDist = 100000.0;
        
        for( int j = 0; j < agentPosVec.size(); j++ )
        {
            minDist = std::min( minDist, (double)cellPosition.dist(agentPosVec[j]) );
            
            if( minDist > bestMinDist )
            {
                bestPosition = getPosition() + cellPosition;
                bestMinDist = minDist;
            }
        }
    }
    
    
    m_path.push_back(bestPosition);
    
    goTo(bestPosition);
    
    if( gDebug > 5 ) drawRedX( bestPosition, 0.05 );
}

void Evader::setToNoEvasion( )
{
    if( gDebug > 5 ) cout<< "  Evader " << getID() << ":  set to NO_EVASION" << endl;
    
    m_eEvasion = NO_EVASION;
    
    m_LSR = Grid();
    m_path.clear();
    
    // Stop any current motion
    goTo( getPosition() );
    stop();
    
}

void Evader::setToRandomWalk( )
{
    if( gDebug > 5 ) cout<< "  Evader " << getID() << ":  set to RANDOM_WALK" << endl;
    
    m_eEvasion = RANDOM_WALK;
    
    m_LSR = Grid();
    m_path.clear();
    
    // Enable motors
    m_pPosProxy->SetMotorEnable(true);
}

void Evader::setToSmartWalk( )
{
    if( gDebug > 5 ) cout<< "  Evader " << getID() << ":  set to SMART_WALK" << endl;
    
    m_eEvasion = SMART_WALK;
    
    m_LSR = Grid();
    m_path.clear();
    
    // Enable motors
    m_pPosProxy->SetMotorEnable(true);
}

void Evader::setToRunaway( )
{
    if( gDebug > 5 ) cout<< "  Evader " << getID() << ":  set to RUNAWAY" << endl;
    
    m_eEvasion = RUNAWAY;
    
    m_LSR = Grid();
    m_path.clear();
    
    // Enable motors
    m_pPosProxy->SetMotorEnable(true);
}

void Evader::goTo( Position pos )
{
    m_pPosProxy->GoTo((double)(pos.x()),(double)(pos.y()),(double)(pos.bearing().dCast2Pi()));
}

void Evader::doEvasion( AgentPositions agentPosVec )
{
    if( gDebug > 8 ) cout<< "  " << getID() << ":  doing current behavior" << endl;
    
    clientRead();
    
    switch (m_eEvasion) {
        case RANDOM_WALK:
            randomWalk();
            break;
        case SMART_WALK:
            smartWalk(agentPosVec);
            break;
        case RUNAWAY:
            runaway(agentPosVec);
            break;
        case NO_EVASION:
            break;
        default:
            cerr<< "ERROR: No behavior category" << endl;
            assert(false);
    }
}


void Evader::drawLSR( Graphics2dProxy &gp, bool bOnlyLSR )
{
    Position curPos = getPosition();
    //player_point_2d_t point;
    
    int LSRsize = m_LSR.columns()*m_LSR.rows();
    player_color_t color;
    
    for( int i = 0; i < LSRsize; i++ )
    {
        Position LSRpoint = m_LSR.getCell(i)->point();
        
        if( !bOnlyLSR && m_LSR.getCell(i)->isFrontier() )
        {
            color.alpha = 0;
            color.red = 0;
            color.green = 0;
            color.blue = 255;

            drawSquare( color, curPos + LSRpoint, 0.01, true );
        }
        else if( !bOnlyLSR && m_LSR.getCell(i)->isObstacle() )
        {
            color.alpha = 0;
            color.red = 255;
            color.green = 0;
            color.blue = 0;

            drawSquare( color, curPos + LSRpoint, 0.01, true );
        }
        else if( !bOnlyLSR && m_LSR.getCell(i)->isLRR() )
        {
            color.alpha = 0;
            color.red = 0;
            color.green = 180;
            color.blue = 0;

            drawSquare( color, curPos + LSRpoint, 0.01, true );
        }
        else if( m_LSR.getCell(i)->isLSR() )
        {            
            color.alpha = 0;
            color.red = 0;
            color.green = 255;
            color.blue = 0;

            drawSquare( color, curPos + LSRpoint, 0.01, true );
        }
    }
    
}


#endif    //EVADER_H