PursuitNavigator.hpp

Go to the documentation of this file.

#ifndef LGL_PURSUIT_NAVIGATOR_HPP
#define LGL_PURSUIT_NAVIGATOR_HPP

#include <lgl/GeoData.hpp>
#include <lgl/CellID.hpp>

#include <string>
#include <set>
#include <vector>
#include <list>
#include <queue>

#include <boost/shared_ptr.hpp>

namespace jafar {
  namespace lgl {
    
    /* Structure used for navigation computation, especialy A-Star */
    class pNavPos 
    {

    public :
      int x ;       // x Position
      int y ;       // y Position

      double g ;      // cost from start to the current position
      double h ;      // heuristic : estimated cost from the current position to the goal

      unsigned int t ;  // time

      std::list< boost::shared_ptr< pNavPos> > prev; // previous positions (cf A-Star)

      pNavPos(int _x, int _y, double _g, double _h, unsigned int _t) :
        x (_x),
        y (_y),
        g (_g),
        h (_h),
        t (_t)
      { }

      pNavPos(int _x, int _y, double _g, double _h, unsigned int _t, boost::shared_ptr< pNavPos > &_prev) :
        x (_x),
        y (_y),
        g (_g),
        h (_h),
        t (_t)
      {  prev.push_back(_prev) ; }

      bool operator< (const pNavPos &rhs) const
      {
        // NB : this is NOT the inverse function of >
        return ( g < rhs.g );
      }

      bool operator> (const pNavPos &rhs) const
      {
        // NB : this is NOT the inverse function of <
        return ( (g+h) > (rhs.g + rhs.h));
      }

    };

    /* comparators using shared_ptr (used for priority_queue) */ 
    template <typename Pointer> struct ptr_less
    { 
      bool operator()( const Pointer & lhs, const Pointer & rhs)
      {
        return (*lhs < *rhs) ;
      }     
    };
    template <typename Pointer> struct ptr_greater
    { 
      bool operator()( const Pointer & lhs, const Pointer & rhs)
      {
        return (*lhs > *rhs) ;
      }     
    };
    
    struct ptr_cmp
    { 
      bool operator()( const boost::shared_ptr< pNavPos > & lhs, const boost::shared_ptr< pNavPos > & rhs)
      {
        return ( (lhs->x < rhs->x) || (lhs->x == rhs->x && lhs->y < rhs->y ) );
      }     
    };



    class PursuitNavigator {
    
      private :
        
        // Geodata information
        // used for accessibility information
        // robGeoData -> robot  accessibility model
        // tarGeoData -> target accessibility model
        GeoData* robGeoData ;
        GeoData* tarGeoData ;

        // ysize is the Y-axis size of the map 
        // (use by Cell_ID)
        unsigned int ysize ;
        unsigned int xsize ;
        
      public : 
    
        PursuitNavigator( const std::string &robotAccessMapPath, const std::string &targetAccessMapPath) 
        {
          robGeoData = new GeoData (robotAccessMapPath  );
          tarGeoData = new GeoData (targetAccessMapPath );

          ysize = 2 * robGeoData->getysize() + 1 ;      // (2*N+1) vertices for N cells
          xsize = 2 * robGeoData->getxsize() + 1 ;      // (2*N+1) vertices for N cells 
          //TODO Check compatibility between the two GeoData 
        }

        ~PursuitNavigator() 
        {
          if ( robGeoData != NULL )
            delete robGeoData ;

          if ( tarGeoData != NULL )
            delete tarGeoData ;
        }

        /* Find the accessible cells for the robot
         * Here : 3x3 models (9-connexity)
         */
        void findRobotAccessibility( const Cell_ID &currentRobotPos, std::set<Cell_ID> &robotAccess);

        /* Find the accessible cells for the robot
         * Here : 3x3 models (9-connexity)
         */
        void findTargetAccessibility( const std::set<Cell_ID> &currentTargetPos, std::set<Cell_ID> &targetAcess );

        /* tool function : compute distance */
        double distCellID ( Cell_ID a, Cell_ID b) ;

        /* tool function : compute distance */
        double distPos ( pNavPos A, pNavPos B ) ;
        double distPos ( int xA, int yA, int xB, int yB) ;

        /* Find a way from the Cell_ID Start to the Cell_ID Goal 
         * Return the path as a Cell_ID vector.
         *
         * Use the algorithm A-Star to compute the path.
         * Return true if a Path has been found, false otherwise.
         *
         */
        bool navAStar ( Cell_ID start, Cell_ID goal, std::vector<Cell_ID> &Path);
        bool navAStar ( int xStart, int yStart, int xGoal, int yGoal, std::vector<Cell_ID> &Path);

        /* Find a way from the Cell_ID Start to the closest cell of the set Goals 
         * Return the path as a Cell_ID vector.
         *
         * Use the algorithm Dijkstra to compute the path.
         * Return true if a Path has been found, false otherwise.
         *
         */
        bool nav2Set ( Cell_ID start, const std::set< Cell_ID > &goals, std::vector<Cell_ID> &Path);
        bool nav2Set ( int xStart, int yStart, const std::set<Cell_ID> &goals, std::vector<Cell_ID> &Path);

        bool nav2Set ( Cell_ID start, const std::set< Cell_ID > &goals, std::vector<Cell_ID> &Path, const unsigned int &pursuitTemporalTolerance);

        /* heuristic for A-Star algorithm 
         * Here heuristic = distance
         */
        double heuristic(int xA, int yA, int xB, int yB) ;
        
        /* develop the nodes in the A-Star algorithm (use heuristics)
         */
        void addNeighbours(   boost::shared_ptr< pNavPos> &currPos, 
                    const int &xGoal, const int &yGoal, 
                    std::set< boost::shared_ptr<pNavPos>, ptr_cmp > &visitedPos, 
                    std::list<  boost::shared_ptr<pNavPos> >  &queue);

        /* develop the nodes in the Djisktra algorithm (do NOT use any heuristic)
         */
        void addNeighbours_classic(   boost::shared_ptr< pNavPos> &currPos, 
                        std::set< boost::shared_ptr<pNavPos>, ptr_cmp > &visitedPos, 
                        std::list<  boost::shared_ptr<pNavPos> >  &queue);

        /* 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);

        /* get functions */
        GeoData* getrobGeoData() ;
        GeoData* gettarGeoData() ;

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

    };

  }
}

#endif /* LGL_PURSUIT_NAVIGATOR_HPP */