ClearingMobileRobot.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: franchi@diag.uniroma1.it stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------

#ifndef __CLEARING_MOBILE_ROBOT_
#define __CLEARING_MOBILE_ROBOT_


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

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

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


using namespace PlayerCc;
using namespace std;
using namespace boost;
using namespace MipBaselib;
using namespace MipResources;

static void _motionWork ( void* p );

static void _motionClean ( void* p );

enum BehaviorTypes
{
    EXPAND,
    GUARD,
    FOLLOW,
    WANDER,
    NO_BEHAVIOR
};

enum KeepColor
{
    KEEP_UNKNOWN,
    KEEP_ORANGE,
    KEEP_PURPLE
};

class ClearingMobileRobot;

typedef uint IdentityCL;
typedef std::vector<Position > PathCl;
// typedef std::vector<ClearingMobileRobot* > AgentVector;
// typedef std::vector<Position > ViewPoints;

class ClearingAgent;

typedef std::vector<ClearingAgent> AgentVector;



class ClearingAgent{
 Position _pos;
 bool _isLeader;
 IdentityCL _ID;
 BehaviorTypes _behavior;
 
 
 public:
  ClearingAgent(const ClearingAgent &a){
  }
  void operator=(const ClearingAgent& a){
  }
  /*###############*/
  /* Set functions */
  /*###############*/
  void setID(IdentityCL i){
   _ID = i;
  }
  
  void setPosition(Position p){
   _pos = p;
  }
  
  void setIsLeader(){
   _isLeader = true;
  }
  
  void unsetIsLeader(){
   _isLeader = false;
  }
  
  void setBehavior(BehaviorTypes b){
   _behavior = b;
  }
  
  /*###############*/
  /* Get functions */
  /*###############*/
  IdentityCL getID(){
   return _ID;
  }
  
  Position getPosition(){
   return _pos;
  }
  
  bool isLeader(){
   return _isLeader;
  }
  
  BehaviorTypes getBehavior(){
   return _behavior;
  }
  
  /*#################*/
  /* Other functions */
  /*#################*/
  void goTo(Position pos){
  }
  
  int getNumFollowers(){
   return 0;
  }
  
  bool isWaitingForAnyAgent(){
   return false;
  }
  
  void setToFollow(){
  }
  
  Position getLSRPosition(){
   return Position();
  }
  
  Decimal getSensorRange(){
   return 0.0;
  }
  
  Frontier getFrontier(){return Frontier();}
  Cell* getCell(DubInt c){return NULL;}
  Position getCellPosition(DubInt c){return Position();}
  bool isWaitingForAgent(IdentityCL id){return false;}
  void clearWaitingForAgent(){}
  void setFrontier(Frontier& f){}
  void setToWander(){}
  void setToExpand(PathCl &pth, AgentVector& agnt){}
  void setToNoBehavior(){}
  void logData(){}
  void addFollower(ClearingAgent& cla){}
  void addFollower(ClearingMobileRobot* clmr){}
};


class ClearingMobileRobot{
  
 protected :
  
  // protected variables
  MobileRob* _mobileRob;    
  IdentityCL m_iID;        
  
  Position goal;        
  
  Thread _motionThread;     
  
  BehaviorTypes m_eBehavior;
  Frontier m_frontier;
  Frontier m_freeArcs;
  Frontier m_prevFrontier;
  Grid m_LSR;
  Grid m_prevLSR;
  bool m_bFrontierInitialized;
  PathCl m_path;
  ClearingAgent* m_pWaitForAgent;
  AgentVector m_followers;
  
  // Agent action functions
  void Expand( AgentVector neighbors );
  void Guard( AgentVector neighbors );
  void processFrontier( AgentVector neighbors );
  void Wander( AgentVector neighbors );
  
  void recordLSR();
  //bool isCanExpandAlone();
  bool isCellCovered( DubInt iViewPoint, DubInt iCell );
 
 //
 // These next functions are the two main functions for the algorithm
 //
 
 // Pick a viewpoint on current frontier to expand
 Position pickBestViewPoint(int *iNumViewPoints, bool bDebugDisplay = false);
 
 // Update frontier after arriving at new viewpoint, involves clearing
 // some of neighbors frontier arcs and classifying LSR boundary
 void updateFrontier( AgentVector neighbors, bool bDebugDisplay = false );
 
 
 public :
  ClearingMobileRobot( IdentityCL ID, int argc, const char* argv[]);
  
  ClearingMobileRobot( IdentityCL ID, int argc, const char* argv[], MobileRob* rob);
  
  ~ClearingMobileRobot( );
  
  IdentityCL getID(){
   return m_iID;
  }
  
  BehaviorTypes getBehavior(){
   return m_eBehavior;
  }
  
  Grid getLSR(){
   return ((m_bFrontierInitialized) ? m_LSR : m_prevLSR);
  }
  
  LRR getLRR(){
   return ((m_bFrontierInitialized) ? m_LSR._lrr : m_prevLSR._lrr);
  }
  
  Frontier getFrontier(){
   return ((m_bFrontierInitialized) ? m_frontier : m_prevFrontier);
  }
  
  void velCmd(Decimal drive, Decimal turnrate){
   _mobileRob->velCmd(drive,turnrate);
  }
  
  Position getGoal(){
   return goal;
  }
  
  int getNumFrontierSegments(){
   return ((m_bFrontierInitialized) ? m_frontier.size() : m_prevFrontier.size());
  }
  
  int getTotalFrontierCells();
  
  void setFrontier(Frontier frontier, bool bForce = false );
  
  PathCl getPathCl(){
   return m_path;
  }
  
  Pose getPose(){
   return _mobileRob->getPose();;
  }
  
  Position getPosition(){
   return getPose().pos();
  }
  
  Position getViewpoint();
  
  Scan getScan();
  
  ExplorationParams getExplParams();
  
  double getSensorRange(){ 
//         cout << "*****************************************" << endl;
//         cout << (_mobileRob->rangeSensPar()->linRangeMax()) << endl;
   return (double)(_mobileRob->rangeSensPar()->linRangeMax());
  }
  
  double getCoverRange();
  
  double getWidth(){
   return _mobileRob->robPar()->radius();
  }
  
  int getLSRSize(){
   return ((m_bFrontierInitialized) ? m_LSR.columns()*m_LSR.rows() : m_prevLSR.columns()*m_prevLSR.rows());
  }
  
  vector<Cell* > getRaster( DubInt iCell1, DubInt iCell2 );
  
  double getLSRResolution(){
   return ((m_bFrontierInitialized) ? m_LSR.resolution() : m_prevLSR.resolution());
  }
  
  Position getLSRPosition();
  
  LSRBound getLSRBound(){
   return ((m_bFrontierInitialized) ? m_LSR.lsrBound() : m_prevLSR.lsrBound());
  }
  
  Cell* getCell( DubInt indices ){
   return ((m_bFrontierInitialized) ? m_LSR.getCell(indices) : m_prevLSR.getCell(indices));
  }
  
  Cell* getCell( Position pos ){
   return ((m_bFrontierInitialized) ? m_LSR.getCell(pos) : m_prevLSR.getCell(pos));
  }
  
  Cell* getCell( int i ){
   return ((m_bFrontierInitialized) ? m_LSR.getCell(i) : m_prevLSR.getCell(i));
  }
  
  Position getCellPosition( DubInt indices ){
   return getCell(indices)->point();
  }
  
  int getNumFollowers(){
   return m_followers.size();
  }
  
  Position getFollowerPosition();
    
    // State change functions
    void setToExpand( PathCl path, AgentVector neighbors, bool bDoExpand = true );
    void setToGuard( AgentVector neighbors );
    void setToFollow( );
    void setToWander( );
    void setToNoBehavior( );
    
    void goTo( Position pos );
    void addFollower( ClearingAgent* newFollower, bool bMove = true );
    
    // This function is called whenever agent is active
    void doCurrentBehavior( AgentVector neighbors );
    
    bool isLeader();
    bool isReady();
    bool isWaitingForAnyAgent();
    bool isWaitingForAgent(int iID);
    void clearWaitingForAgent();
    bool isTraveling();
    
    void drawBoundary( Graphics2dProxy &gp );
    void drawFrontier( Graphics2dProxy &gp );
    void drawLSR( Graphics2dProxy &gp, bool bOnlyLSR );
    
    bool canDetectTargetPosition( Position targetPos );
};


#endif    //TEST_FRONTIER_PURSUIT_H