planners/sandros/PL_Sandros.h

Go to the documentation of this file.

//Sandros Planner Implemetation header file,
//Start at April 20
//By Huang Yifeng 

#ifndef PL_Sandros_h
#define PL_Sandros_h 1

#pragma warning ( disable : 4250 )

//#include "Planners\PL_Boolean_Output.h"
//#include "Planners\PL_OpenGL.h"
#include "Planners\PL_GraphBase.h"

// Additional LEDA Classes
#include <LEDA/node_map.h>
#include <LEDA/node_pq.h>
#include <LEDA/node_set.h>
#include <LEDA/queue.h>

#define NODEINU true
#define NODEINV false
#define LEDA_CHECKING_OFF
//-----------------------------------------------------------------------------
//
// PL_Sandros
//
// this class represents a SANDROS style planner
//
//-----------------------------------------------------------------------------

enum NodeInUVorPsPt{
        NodeInU,
        NodeInV,
        NodeInPs,
        NodeInPt,
        Nothing
};

class PL_Sandros
: 
//      public PL_Boolean_Output, //## Inherits: <unnamed>%376A93290334
//  virtual public PL_OpenGL  //## Inherits: <unnamed>%3980C6E702E4
//      Change the base class here
        public PL_GraphBase
{
public:

        PL_Sandros();
        //## Destructor (generated)
        virtual ~PL_Sandros();

        //Plan - this function is called when the user presses the plan button
        virtual bool Plan ();

        //Save - this function is called when the MPK tries to save the planner's state
        virtual bool Save (const char *filename) const;
        //Load - this function loads up a file that was previously saved with "Save"
        virtual bool Load (const char *filename);

        //DrawExplicit - this function contains the GL calls to draw the planner 
        virtual bool DrawExplicit () const;

        virtual void SetStartConfig( const Configuration& config );
        virtual void SetGoalConfig ( const Configuration& config );

        virtual void SetCollisionDetector (CD_BasicStyle* collisionDetector);

        virtual void ClearGraph();
        
        int GetRefinedNode (int i);
        void SetValue(int m_Grid,int m_Neighbor,int m_Tolerance);

protected:
        //statue indicator
        SuccessResultType plan_success;         // Indicates if the planner has been successful in finding a path
        BOOL m_bNoOtherSequence;                        //if true then go to the node refinement
        BOOL m_bResolutionLimit;                        //if true, means that all the nodes in Q have been refined
        
        //Date structer used in Sandros
        node_pq<int>*   m_quenQ;        // pointer to open priority queue, store the nodes that need to be refined
        

        node_map<node>  m_nodePs;
        queue<node>* m_quenQ1;          //addtional queue to store nodes to be refined

        node_map<node>  m_nodePt;
        node_map<int>   m_refine_level;         //
        node_map<bool>  m_nodeInPsIsland;

        node_map<Configuration> m_nodeInUPs;

        node_map<NodeInUVorPsPt>        m_nodeUVPsPt;
        edge_map<bool> m_bEdgeHiden;

        edge_array<double>      m_c;

        //local copy of sandros environment
        int Tolerance;
        int seedofa;
        int dof;
        int current_res;
        double ThreshHold;
        double stride;
        int nextNo;
        Configuration NodeRefinedc;
        node NodeRefined;

protected:
        //imeplemtation
        void ConstructGraph();
        bool SearchForSequence(node_list& L);
        double DistanceSandros(const node& n1,const node& n2);
        void InitailizeGraph();
        void DealwithSequence(const node_list& L);
        edge FindEdgeBetweenTwoNodes(const node& n1,const node& n2);
        void SaveSandrosPath(const node_list& L);
        node GenerateNewNode(int res,const node n1);
        bool IsInterferingSan(const Configuration& n1,const node& n2,Configuration& p);         //here use a linear path use as local planner
        bool IsNeighber(const node& n3,const node& n2);
        node GenerateNewNodeGrid(int res,const node n1,int no);
        node FindNextNodeToRefined();
        bool IsNeighberSan(const Configuration& c1,const node& n2);

};

#endif

---