PursuitGraph.hpp

Go to the documentation of this file.

#ifndef LGL_PURSUIT_GRAPH_HPP
#define LGL_PURSUIT_GRAPH_HPP

#include <lgl/VisibilityMap.hpp>
#include <lgl/PursuitGNode.hpp>
#include <lgl/PursuitNavigator.hpp>
#include <lgl/CellID.hpp>

#include <vector>

#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>

#ifndef DEFAULT_PURSUIT_TEMPORAL_TOLERANCE
#define DEFAULT_PURSUIT_TEMPORAL_TOLERANCE 15
#endif

#ifndef DEFAULT_RISK_TERM_MAX_VALUE 
#define DEFAULT_RISK_TERM_MAX_VALUE 15
#endif

namespace jafar {
  namespace lgl {

    struct tr1Node 
    {
      RISK_LEVEL b ;    // risk or not
      Cell_ID r;      // robot position
      Cell_ID t;      // target position
      unsigned int rt;  // risk term

      bool operator< (const tr1Node &rhs) const
      {
        if ( b!=rhs.b ) return ( b  < rhs.b ) ;
        if ( r!=rhs.r ) return ( r  < rhs.r ) ;
        if ( t!=rhs.t ) return ( t  < rhs.t ) ;
        if ( rt!=rhs.rt ) return ( rt < rhs.rt) ;
        
        return (false); // equals
      }

      bool operator== (const tr1Node &rhs) const
      {
        if ( b==rhs.b && r==rhs.r && t==rhs.t && rt==rhs.rt ) return true ;
        return false;
      }
    };

    struct tr2Node 
    {
      RISK_LEVEL b ;                      // risk or not
      Cell_ID r;                        // robot position
      unsigned int rt;                    // risk term
      std::vector < boost::shared_ptr< PursuitGNode> > p;   // parent nodes 

      bool operator< (const tr2Node &rhs) const
      {
        if ( b!=rhs.b   ) return ( b  < rhs.b   ) ;
        if ( r!=rhs.r   ) return ( r  < rhs.r   ) ;
        if ( rt!=rhs.rt ) return ( rt < rhs.rt  ) ;
        if ( p!=rhs.p   ) return ( p  < rhs.p   ) ;
        
        return (false); // equals
      }

      bool operator== (const tr2Node &rhs) const
      {
        if ( b==rhs.b && r==rhs.r && rt==rhs.rt && p==rhs.p ) return true ;
        return false;
      }
    };
    
    
    class PursuitGraph {
    
      private :

        // Provide GeoData information and navigation services
        PursuitNavigator* navigator ;

        // VisibilityMap : use for visibility information
        VisibilityMap *visibilityMap; 

        // ysize is the Y-axis size of the map 
        // (use by Cell_ID)
        unsigned int ysize ;
        unsigned int xsize ;

        // Root of the graph (tree)
        boost::shared_ptr<PursuitGNode> root;

        // risk, danger, and front (leaves) nodes
        std::vector< boost::weak_ptr<PursuitGNode> > risk;
        std::vector< boost::weak_ptr<PursuitGNode> > danger;
        std::vector< boost::weak_ptr<PursuitGNode> > front;

        // set of (currently used) Nodes 
        std::set< boost::shared_ptr<PursuitGNode> > setOfNodes;

        /* all nodes (=list) organized as a multi-map container
         * using the ID of the robot position as key
         */
        std::multimap< Cell_ID, boost::weak_ptr<PursuitGNode> > nodeMap;

        // current number of stages (levels) in the graph (tree)
        // This stands for the temporal horizon of the pursuit graph.
        unsigned int horizon ;
        
        // temporal reference
        unsigned long int time ;
        
        // temporal tolerance when the robot cannot see the target
        // (ie how long he is authorized to stay in this situation)
        unsigned int  pursuitTemporalTolerance;

        // the maximal risk term value
        // (beyond this limit, the risk become a danger)
        unsigned int riskTermMaxValue ;

        // estimation of the time needed to compute a new stage (or to complete of stage)
        double d_est ;

      public : 

        PursuitGraph(   PursuitNavigator &_navigator, 
                VisibilityMap & _visibilityMap, 
                unsigned int _pursuitTemporalTolerance = DEFAULT_PURSUIT_TEMPORAL_TOLERANCE, 
                unsigned int _riskTermMaxValue = DEFAULT_RISK_TERM_MAX_VALUE
        ) :
          navigator(&_navigator),
          visibilityMap(&_visibilityMap),
          horizon(0),
          time(0),
          pursuitTemporalTolerance(_pursuitTemporalTolerance),
          riskTermMaxValue (_riskTermMaxValue)
        {
          ysize = visibilityMap->getysize();
          xsize = visibilityMap->getxsize();

          //TODO check for navigator and visibilityMap compatibility (eg: same dimensions)
        }
      
        ~PursuitGraph() {}

        void computeAll ( Cell_ID cellR, Cell_ID cellT, const unsigned int _horizon = 10  ); 
        void computeAll ( int xR, int yR, int xT, int yT, const unsigned int _horizon = 10  ); 

        void computeAllBefore ( Cell_ID cellR, Cell_ID cellT, const double tMax = 5000.0  ); 
        void computeAllBefore ( int xR, int yR, int xT, int yT, const double tMax = 5000.0  ); 

        /* Update the PursuitGraph according to a new given position of the target
         * 
         * return true if a solution is found in the graph, false otherwise.
         *
         * @newtargetPos is the new given position of the target
         * @newrobotPos is the new position of the robot, according to the new position of the target and to the graph
         */
        bool updatePG( Cell_ID newtargetPos, Cell_ID &newrobotPos);
        bool updatePG( int xT, int yT, int &xR, int &yR );

        /* compute new stage until each member of the front has a time equal to root->time + horizon */
        void completeToHorizon();
        void completeToHorizonBefore( const double &tMax ); // tMax in ms


        /* get functions */
        unsigned int gethorizon() const ;

        const std::vector< boost::weak_ptr<PursuitGNode> > & getrisk() const ;
        const std::vector< boost::weak_ptr<PursuitGNode> > & getdanger() const ;
        const std::vector< boost::weak_ptr<PursuitGNode> > & getfront() const ;

        const std::set< boost::shared_ptr<PursuitGNode> > & getsetOfNodes() const ;

        const std::multimap< Cell_ID, boost::weak_ptr<PursuitGNode> > & getnodeMap() const ;

        const boost::shared_ptr<PursuitGNode> & getroot() const ;

        PursuitNavigator* getnavigator();

        unsigned int getysize() const;
        unsigned int getxsize() const;

        unsigned long int gettime() const;

        void disp() ;
        void disp_risk() ;
        void disp_danger() ;

      private :

        void computeAll ( const boost::shared_ptr <PursuitGNode> &_root, const unsigned int _horizon = 10 ); 

        void computeAllBefore ( const boost::shared_ptr <PursuitGNode> &_root, const double tMax = 5.0 ); 
        

        void setRoot() ;

        void computeNewStage(); // TRHOW EXCEPTION

        void clean() ;

        //void reorient() ;
        void reorient() ;

        /* Make new Nodes by linking robot positions and target positions
         */
        void makeNewNodes(  std::vector< boost::shared_ptr < PursuitGNode > > &newNodes, 
                  const unsigned int &riskTerm,
                  const Cell_ID &robotPos, 
                  const std::set< Cell_ID > &robotAccess, 
                  const std::set< Cell_ID > &targetAccess
                  ); // THROW EXCEPTION

        /* develop the node of the front with a time equal to the argument
         * similar to computeNewStage for the content
         */
        void completeStage( unsigned long int stage) ;

        /* risk management */
        void manageRisk(  const Cell_ID &robotPos, 
                  const boost::shared_ptr<PursuitGNode> &risk, 
                  std::vector< boost::shared_ptr< PursuitGNode > > &solution 
                  );  

        /* for a given set of positions, return another set of positions, if it exits, of positions from which ALL the first ones are visible
         */
        void findIdealPositions( const std::set<Cell_ID> &targetPos, std::set<Cell_ID> &positions);

        /* for a given set of positions, return another set of positions, if it exits, of positions from which at least ONE of the first ones are visible
         */
        void findAnyPositions( const std::set<Cell_ID> &targetPos, std::set<Cell_ID> &positions);

        /* for a given position of the robot and a given position of the target
         * compute a risk level 
         * (use for the root risk level computation)
         */
        RISK_LEVEL computeRiskLevel(const Cell_ID &robotPos, const Cell_ID &targetPos);

    };

  }
}

#endif /* LGL_PURSUIT_GRAPH_HPP */