planners/prm/PL_PRM.h

Go to the documentation of this file.

//=========================================================================
//
//  File:  PL_PRM.h
//
//  Created by Ian Gipson, Modified by Shane Schneider.
//
//              Full and Lazy PRM planner.  Uses PL_GraphBase as parent class
//              the basic graph structure and access functions.  This planner
//              will modify the graph and implement the PRM planners.

#ifndef PL_PRM_h
#define PL_PRM_h 1

#pragma warning( disable : 4250 )

// Parent Classes
#include "Planners\PL_GraphBase.h"

// Additional LEDA Classes
#include <LEDA/node_map.h>
#include <LEDA/edge_map.h> 
#include <LEDA/node_list.h>
#include <LEDA/node_pq.h>
#include <LEDA/list.h>




//-------------- Forward Declarations -----------
enum PRM_StateMachineType;

//## Class: PL_PRM%3921BC600296
//## Category: Planners%36FB140B003C
//## Persistence: Transient
//## Cardinality/Multiplicity: n

class PL_PRM : 
        public PL_GraphBase  //## Inherits: Basic graph planner
{

  public:
    //## Destructor (generated)
      virtual ~PL_PRM();

          // Default Constructor
          PL_PRM();                     

          virtual bool DrawExplicit () const;

      virtual bool Plan ();

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

          // Set the Collision Detector for the graph.
          virtual void SetCollisionDetector (CD_BasicStyle* collisionDetector);

          virtual void *GetParameters();
          virtual bool SetParameters(const void *param);
          virtual bool ValidateParameters();

    // Additional Public Declarations
          void SetRadiusTol ( double tol );
          double GetRadiusTol ();
          
          void SetPRMMode ( BOOL& isLazy, BOOL& validNodes, BOOL& validEdges );
          void GetPRMMode ( BOOL& isLazy, BOOL& validNodes, BOOL& validEdges ) const;

          void SetUseMidPts ( BOOL use_them ) { useMidPts = use_them; };
          BOOL GetUseMidPts () const { return useMidPts; };

          PRM_StateMachineType GetLastPlanningState () const { return lastPlanningState; }; 

          void SetInitQuant ( int quant ) { initQuant = quant; };
          int  GetInitQuant () const { return initQuant; };

          void SetEnhanceQuant ( int quant ) { enhanceQuant = quant; };
          int  GetEnhanceQuant () const { return enhanceQuant; };

          void SetSeedRatio ( double ratio ) { seedRatio = ratio; };
          double GetSeedRatio () const { return seedRatio; };

          // Add Node Routine.
          BOOL AddNode();

          //copies the settings from a different planner
          void CopySettings( PlannerBase* original );

  protected:
        // Additional Implementation Declarations
          virtual void ConnectNode( const node& newnode );  // Connects the given node with edges.
          virtual void ConnectEdgesFull( const node& n1, const double& radius_squared );  // Connects the given node with edges, Full PRM style.
          virtual void ConnectEdgesLazy( const node& n1, const double& radius_squared );  // Connects the given node with edges, Lazy PRM style.

          // State Machine Functions
          SuccessResultType BuildInitRoadMap();
          SuccessResultType FindPath();
          SuccessResultType VerifyPath();
          SuccessResultType EnhanceRoadMap();

          // SaveContents
          //    Saves the graph and other relevent PRM parameters to the given ostream pointer
          virtual bool SaveContents( std::ofstream& outfile ) const;

          // LoadContents
          //    Load the graph and other relevent PRM parameters to the given ostream pointer
          virtual bool LoadContents( std::ifstream& infile );

          // Tree Graph Functions
          void SetGraphMode ( const bool& treeGraph );  // Sets the graph mode for either connect tree or fully connected graph
          bool NodeInConnectionList( const node& n1, const list<int>& connectIDs); // checks if node has a connectionID that is in the given list.




    // Additional Protected Declarations
          double diagonal_squared;      // records the current squared weighted diagonal.
          double radiusTol;                     // used for calculating the radius of neighbours to connect to.

          BOOL lazyMode;                        // Indicates if planner is currently in LazyPRM mode.
          BOOL validNodesOnly;          // Indicates if only valid nodes are to be added
          BOOL validEdgesOnly;          // Indicates if only valid edges are to be added
          BOOL useMidPts;                       // Indicates if midpoints of deleted (or proposed invalid) edges are to be used as seeds.

          PRM_StateMachineType lastPlanningState;       // Records what was the last state of the planning state machine.

          int initQuant;        // initial number of nodes to add when building roadmap.
          int enhanceQuant;     // number of nodes to add to roadmap when enhancing.
          double seedRatio; // Ratio of normally distributed nodes about seeds to uniformly distributed nodes.

          list<Configuration> config_seeds;   // List of configurations to be used as seeds during enhancement.

     
  protected:
    // Additional Private Declarations
        
          // Additional Graph statitics:
          node_map<BOOL> nodeChecked;   // Indicates if node has been checked for collisions
          edge_map<BOOL> edgeChecked;   // Indicates if edge has been checked for collisions

          node_map<BOOL> uniformlyAdded;// Indicates if the node was added uniformly added or based on a seed.

          // Tree Connected Graphs
          node_map<int>* connectIDp;    // Records the connection ID for tree connected nodes.
          int baseConnectID;                    // Records the next available connect graph component ID.


          // Find Path Variables.
          //    These are saved as part of the class because we want to resume from an interupted
          //    search.  These variables will then have to be available outside the scope of the FindPath
          //    Function so they can be access on subsequent iterations.  NOTE:  These variables
          //    should only be modified by FindPath only!
          node_map<node>   pred;   // stores the edge pointers to the prev node for any given path of nodes
          node_map<double> dist;   // records the distance of the current calculated path
          node_pq<double>* openp;       // pointer to the open list used to store possible nodes for the path
                                                                // This needs to be a properly maintained pointer to the priority queue as
                                                                // a priority queue can only be established for a properly defined graph.
          
  private: //## implementation
    // Additional Implementation Declarations
          
          // A* Cost Function
          inline double Astar_f( const node& n, const double& currcost ) const;
          

};


// Class PL_PRM 


// PRM Specific ENUM Types
enum PRM_StateMachineType
{
        PRM_DONE = 0,
        PRM_START = 1,
        PRM_BUILD_INIT_ROADMAP,
        PRM_FIND_PATH,
        PRM_VERIFY_PATH,
        PRM_TRANSLATE_PATH,
        PRM_ENHANCE_ROADMAP,
        PRM_TIMER_EXPIRED = -1,
        PRM_FAILURE = -2
};


#endif

---