Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/PotentialFields/AStarSearch.h

Go to the documentation of this file.

    00001 /**
    00002 * @file AStarSearch.h
    00003 *
    00004 * Definition of classes AStarSearch, PotentialfieldAStarNode
    00005 * and PotentialfieldAStarParameterSet
    00006 *
    00007 * @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
    00008 */
    00009
    00010 #ifndef A_STAR_SEARCH_H_
    00011 #define A_STAR_SEARCH_H_
    00012
    00013
    00014 #include "PfieldDatatypes.h"
    00015 #include "Pfield.h"
    00016 #include "FieldObject.h"
    00017 #ifdef POTENTIALFIELDS_FOR_GT2004_
    00018 #include "Tools/Debugging/Debugging.h"
    00019 #include "Tools/Debugging/DebugDrawings.h"
    00020 #endif //POTENTIALFIELDS_FOR_GT2004_
    00021
    00022
    00023 /**
    00024 * @class PotentialfieldAStarParameterSet
    00025 *
    00026 * A class for all parameters for A* search
    00027 */
    00028 class PotentialfieldAStarParameterSet
    00029 {
    00030 public:
    00031   /** Constructor*/
    00032   PotentialfieldAStarParameterSet()
    00033   {stabilizationObject = 0; useStabilization = false;}
    00034
    00035   /** The minimum branching factor*/
    00036   unsigned int minBranchingFactor;
    00037   /** The maximum branching factor*/
    00038   unsigned int maxBranchingFactor;
    00039   /** The minimum expansion radius*/
    00040   double minExpansionRadius;
    00041   /** The maximum expansion radius*/
    00042   double maxExpansionRadius;
    00043   /** The minimum distance to the goal*/
    00044   double distanceToGoal;
    00045   /** A pointer to the potential field in which the search is performed*/
    00046   Potentialfield* field;
    00047   /** The minimum distance for enlarging expansion radius und reducing branching factor*/
    00048   double endOfNear;
    00049   /** The maximum distance for enlarging expansion radius und reducing branching factor*/
    00050   double endOfFar;
    00051   /** The length of a generated vector*/
    00052   double standardGradientLength;
    00053   /** The first position of the search*/
    00054   PfVec startPosition;
    00055   /** The number of consecutive calls of the A*-search */
    00056   unsigned long numberOfCalls;
    00057   /** Flag: Use stabilization element, if true*/
    00058   bool useStabilization;
    00059   /** The stabilization object*/
    00060   PotentialFieldsObject* stabilizationObject;
    00061   /** The distance of the stabilization object*/
    00062   double stabilizationDistance;
    00063 };
    00064
    00065
    00066 /**
    00067 * @class PotentialfieldAStarNode
    00068 *
    00069 * A class describing a node in an A* search tree
    00070 */
    00071 class PotentialfieldAStarNode
    00072 {
    00073 public:
    00074   /** Constructor */
    00075   PotentialfieldAStarNode()
    00076   { expanded = false; parentNode = 0;}
    00077
    00078   /** Copy-Constructor
    00079   * @param node Another node to copy data from
    00080   */
    00081   PotentialfieldAStarNode(const PotentialfieldAStarNode& node)
    00082   {
    00083     pose = node.pose;
    00084     fValue = node.fValue;
    00085     gValue = node.gValue;
    00086     parentNode = node.parentNode;
    00087     expanded = node.expanded;
    00088     valueAtPos = node.valueAtPos;
    00089     expansionRadius = node.expansionRadius;
    00090   }
    00091
    00092   /** Sets the index of the parent node of this node
    00093   * @param parentNode The index of the parent node
    00094   */
    00095   void setParentNode(unsigned int parentNode)
    00096   { this->parentNode = parentNode;}
    00097
    00098   /** Returns the index of the parent node
    00099   * @return The parent node
    00100   */
    00101   unsigned int getParentNode() const
    00102   { return parentNode;}
    00103
    00104   /** Sets the function values
    00105   * @param g The costs of the path to this node
    00106   * @param h The heuristic costs from this node to the goal
    00107   */
    00108   void setFunctionValues(double g, double h)
    00109   { gValue = g; fValue = g+h;}
    00110
    00111   /** Returns the value of this node
    00112   * @return The value f
    00113   */
    00114   double f() const
    00115   { return fValue;}
    00116
    00117   /** Returns the costs of the path to this node
    00118   * @return The path costs g
    00119   */
    00120   double g() const
    00121   { return gValue;}
    00122
    00123   /** Sets the pose of this node
    00124   * @param pose The pose
    00125   */
    00126   void setPose(const PfPose& pose)
    00127   { this->pose = pose;}
    00128
    00129   /** Returns the pose of this node
    00130   * @return The pose
    00131   */
    00132   PfPose getPose() const
    00133   { return pose;}
    00134
    00135   /** Sets the value of the potential field at the position of the node
    00136   * @param parameterSet The parameter set
    00137   */
    00138   void setValueAtPos(const PotentialfieldAStarParameterSet& parameterSet);
    00139
    00140   /** Expands this node
    00141   * @param searchTree The search tree of A* search, new nodes will be inserted
    00142   * @param expandedNodes A list of previously expanded nodes
    00143   * @param goal The goal of the search
    00144   * @param parameterSet The parameter set for the search
    00145   * @param ownNodeNum The index of this node (in the seachTree)
    00146   */
    00147   void expand(std::vector<PotentialfieldAStarNode>& searchTree,
    00148               const std::vector<unsigned int>& expandedNodes,
    00149               const PotentialfieldAStarNode& goal,
    00150               const PotentialfieldAStarParameterSet& parameterSet,
    00151               unsigned int ownNodeNum);
    00152
    00153   /** Checks if this node has been expanded
    00154   * @return true, if the node has been expanded
    00155   */
    00156   bool hasBeenExpanded() const
    00157   { return expanded;}
    00158
    00159   /** Checks if this node has reached the goal
    00160   * @param goal The goal
    00161   * @param parameterSet The parameter set
    00162   * @return true, if the node has reached the goal
    00163   */
    00164   bool hasReached(const PotentialfieldAStarNode& goal,
    00165                   const PotentialfieldAStarParameterSet& parameterSet) const;
    00166
    00167   /** Computes the distance to another node
    00168   * @param other The other node
    00169   * @return The distance
    00170   */
    00171   double distToOtherNode(const PotentialfieldAStarNode& other)
    00172   { return (pose.pos - other.getPose().pos).length();}
    00173
    00174 protected:
    00175   /** The pose of the node*/
    00176   PfPose pose;
    00177   /** The value of the node*/
    00178   double fValue;
    00179   /** The path costs to the node*/
    00180   double gValue;
    00181   /** The index of the parent node*/
    00182   unsigned int parentNode;
    00183   /** Flag: true, if the node has been expanded*/
    00184   bool expanded;
    00185   /** The potential field value at the position of the node*/
    00186   double valueAtPos;
    00187   /** The expansion radius of the node*/
    00188   double expansionRadius;
    00189
    00190   /** Returns the expansion radius of the node
    00191   * @return The expansion radius
    00192   */
    00193   double getExpansionRadius() const
    00194   { return expansionRadius;}
    00195
    00196   /** Computes the path costs to another node
    00197   * @param node The other node
    00198   * @param parameterSet The parameter set
    00199   * @return The path costs
    00200   */
    00201   double computeCostsTo(const PotentialfieldAStarNode& node,
    00202                         const PotentialfieldAStarParameterSet& parameterSet) const;
    00203
    00204   /** Computes the heuristic costs between two positions
    00205   * @param p1 The first position
    00206   * @param p2 The second position
    00207   * @param currentBranchingFactor The current branching factor
    00208   * @return The heuristic costs
    00209   */
    00210   double computeHeuristicBetween(const PfPose& p1, const PfPose& p2,
    00211                                  unsigned int currentBranchingFactor) const;
    00212
    00213   /** Computes current parameters for expansion
    00214   * @param currentExpansionRadius The current expansion radius
    00215   * @param currentBranchingFactor The current branching factor
    00216   * @param parameterSet The parameter set
    00217   */
    00218   void computeCurrentParameters(double& currentExpansionRadius,
    00219                                 unsigned int& currentBranchingFactor,
    00220                                 const PotentialfieldAStarParameterSet& parameterSet) const;
    00221
    00222   /** Checks if a new node is too close to another previously expanded node
    00223   * @param searchTree The search tree
    00224   * @param expandedNodes A list containing all previously expanded nodes
    00225   * @param pose The pose of the new node
    00226   * @return true, if the pose is too close
    00227   */
    00228   bool tooCloseToAnotherNode(std::vector<PotentialfieldAStarNode>& searchTree,
    00229                              const std::vector<unsigned int>& expandedNodes,
    00230                              const PfPose& pose) const;
    00231 };
    00232
    00233
    00234 /**
    00235 * @class AStarSearch
    00236 *
    00237 * An abstract implementation of the A* search algorithm.
    00238 * Works with any appropriate type N of nodes, type P of parameter sets
    00239 * and type U of unit
    00240 */
    00241 template<class N, class P, class U> class AStarSearch
    00242 {
    00243 public:
    00244   /** Constructor
    00245   * @param minCacheSize The number of nodes to allocate memory for
    00246   * @param maxTreeSize The maximum number of nodes to expand
    00247   * @param parameterSet A parameter set to check the maximum number of nodes to expand
    00248   */
    00249   AStarSearch(long minCacheSize, long maxTreeSize, P parameterSet)
    00250   {
    00251     unsigned int elementsToReserve;
    00252     if(maxTreeSize != -1)
    00253     {
    00254       elementsToReserve = (maxTreeSize+parameterSet.maxBranchingFactor);
    00255       this->maxTreeSize = maxTreeSize;
    00256     }
    00257     else
    00258     {
    00259       elementsToReserve = minCacheSize;
    00260       this->maxTreeSize = -1;
    00261     }
    00262     searchTree.reserve(elementsToReserve);
    00263     expandedNodes.reserve(elementsToReserve);
    00264   }
    00265
    00266
    00267   /** The main search function
    00268   * @param start The starting node
    00269   * @param goal The goal node
    00270   * @param parameterSet The current parameter set
    00271   * @param pathLength Returns the length of the path to the goal
    00272   * @return The next node to go to
    00273   */
    00274   N search(const N& start, const N& goal, const P& parameterSet, U& pathLength)
    00275   {
    00276     this->parameterSet = parameterSet;
    00277     if(start.hasReached(goal, parameterSet))
    00278     {
    00279       return start;
    00280     }
    00281     searchTree.clear();
    00282     expandedNodes.clear();
    00283     searchTree.push_back(start);
    00284     searchTree[0].setParentNode(0);
    00285     unsigned int indexOfBestNode;
    00286     unsigned int nextNodeToExpand;
    00287     while(true)
    00288     {
    00289       nextNodeToExpand = findIndexOfNextNode();
    00290       if(searchTree.size() >= (unsigned int)maxTreeSize)
    00291       {
    00292         indexOfBestNode = nextNodeToExpand;
    00293         break;
    00294       }
    00295       else
    00296       {
    00297         unsigned int sizeBeforeExpand(searchTree.size());
    00298         searchTree[nextNodeToExpand].expand(searchTree, expandedNodes,
    00299                                             goal, parameterSet, nextNodeToExpand);
    00300         expandedNodes.push_back(nextNodeToExpand);
    00301         unsigned int sizeAfterExpand(searchTree.size());
    00302         int result(testNewNodesAgainstGoal(sizeBeforeExpand, sizeAfterExpand-1,goal));
    00303         if(result != -1)
    00304         {
    00305           indexOfBestNode = result;
    00306           break;
    00307         }
    00308       }
    00309     }
    00310     N result = backtraceNode(indexOfBestNode, pathLength);
    00311 #ifdef POTENTIALFIELDS_FOR_GT2004_
    00312     //OUTPUT(idText,text, searchTree.size()<<" nodes, "<<expandedNodes.size()<<" expanded nodes");
    00313     COMPLEX_DRAWING(behavior_aStarSearch,draw(indexOfBestNode));
    00314     DEBUG_DRAWING_FINISHED(behavior_aStarSearch);
    00315 #endif //POTENTIALFIELDS_FOR_GT2004_
    00316     return result;
    00317   }
    00318
    00319 private:
    00320   /** A container for all nodes*/
    00321   std::vector<N> searchTree;
    00322   /** Indizes of all expanded nodes*/
    00323   std::vector<unsigned int> expandedNodes;
    00324   /** The maximum number of nodes to expand*/
    00325   long maxTreeSize;
    00326   /** The parameter set*/
    00327   P parameterSet;
    00328
    00329   /**
    00330   * Finds the next node to be expanded
    00331   * @return The index of the node
    00332   */
    00333   unsigned int findIndexOfNextNode()
    00334   {
    00335     unsigned int expandableNode(0);
    00336     double f;
    00337     while(searchTree[expandableNode].hasBeenExpanded())
    00338     {
    00339       ++expandableNode;
    00340     }
    00341     f = searchTree[expandableNode].f();
    00342     for(unsigned int i=(expandableNode+1); i<searchTree.size(); i++)
    00343     {
    00344       if((!searchTree[i].hasBeenExpanded()) && (searchTree[i].f() < f))
    00345       {
    00346         f = searchTree[i].f();
    00347         expandableNode = i;
    00348       }
    00349     }
    00350     return expandableNode;
    00351   }
    00352
    00353
    00354   /** Tests if any node has reached the goal
    00355   * @param firstNode The first node to test
    00356   * @param lastNode The last node to test
    00357   * @param goal The goal to test against
    00358   * @return -1, if no node has reached the goal, the number of the node otherwise
    00359   */
    00360   int testNewNodesAgainstGoal(unsigned int firstNode, unsigned int lastNode,
    00361                               const N& goal)
    00362   {
    00363     for(unsigned int i = firstNode; i<= lastNode; i++)
    00364     {
    00365       if(searchTree[i].hasReached(goal, parameterSet))
    00366       {
    00367         return (int)i;
    00368       }
    00369     }
    00370     return -1;
    00371   }
    00372
    00373
    00374   /** Backtraces the best node found to the first node after the start node
    00375   * @param indexOfBestNode The index of the best node found
    00376   * @param pathLength Returns the length of the backtraced path
    00377   * @return The root node of the path to the node
    00378   */
    00379   N backtraceNode(unsigned int indexOfBestNode, U& pathLength)
    00380   {
    00381     unsigned int currentNode(indexOfBestNode);
    00382     pathLength = searchTree[currentNode].distToOtherNode(searchTree[searchTree[currentNode].getParentNode()]);
    00383     while(searchTree[currentNode].getParentNode() != 0)
    00384     {
    00385       currentNode = searchTree[currentNode].getParentNode();
    00386       pathLength += searchTree[currentNode].distToOtherNode(searchTree[searchTree[currentNode].getParentNode()]);
    00387     }
    00388     return searchTree[currentNode];
    00389   }
    00390
    00391 #ifdef POTENTIALFIELDS_FOR_GT2004_
    00392   /** Draw all expanded paths and the selected path on the RobotControl field view
    00393   * @param indexOfBestNode The index of the last expanded node
    00394   */
    00395   void draw(unsigned int indexOfBestNode)
    00396   {
    00397     PfPose pose, parentPose;
    00398     for(unsigned int i=0; i<searchTree.size(); i++)
    00399     {
    00400       pose = searchTree[i].getPose();
    00401       parentPose = searchTree[searchTree[i].getParentNode()].getPose();
    00402       LINE(behavior_aStarSearch, pose.pos.x, pose.pos.y, parentPose.pos.x, parentPose.pos.y,
    00403         1, Drawings::ps_solid, Drawings::red);
    00404     }
    00405     unsigned int currentNode(indexOfBestNode);
    00406     do
    00407     {
    00408       pose = searchTree[currentNode].getPose();
    00409       currentNode = searchTree[currentNode].getParentNode();
    00410       parentPose = searchTree[currentNode].getPose();
    00411       LINE(behavior_aStarSearch, pose.pos.x, pose.pos.y, parentPose.pos.x, parentPose.pos.y,
    00412         1, Drawings::ps_solid, Drawings::yellow);
    00413     } while(currentNode != 0);
    00414     if(parameterSet.useStabilization)
    00415     {
    00416       PfPose stabPose(parameterSet.stabilizationObject->getPose());
    00417       LARGE_DOT(behavior_aStarSearch, stabPose.pos.x, stabPose.pos.y,
    00418                 Drawings::red, Drawings::red);
    00419     }
    00420   }
    00421 #endif //POTENTIALFIELDS_FOR_GT2004_
    00422 };
    00423
    00424
    00425 #endif  //A_STAR_SEARCH_H_