Tools/PotentialFields/AStarSearch.h

Go to the documentation of this file.

/**
* @file AStarSearch.h
* 
* Definition of classes AStarSearch, PotentialfieldAStarNode 
* and PotentialfieldAStarParameterSet
*
* @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
*/

#ifndef A_STAR_SEARCH_H_
#define A_STAR_SEARCH_H_


#include "PfieldDatatypes.h"
#include "Pfield.h"
#include "FieldObject.h"
#ifdef POTENTIALFIELDS_FOR_GT2004_
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/DebugDrawings.h"
#endif //POTENTIALFIELDS_FOR_GT2004_


/**
* @class PotentialfieldAStarParameterSet
*
* A class for all parameters for A* search
*/
class PotentialfieldAStarParameterSet
{
public:
  /** Constructor*/
  PotentialfieldAStarParameterSet() 
  {stabilizationObject = 0; useStabilization = false;}

  /** The minimum branching factor*/
  unsigned int minBranchingFactor;
  /** The maximum branching factor*/
  unsigned int maxBranchingFactor;
  /** The minimum expansion radius*/
  double minExpansionRadius;
  /** The maximum expansion radius*/
  double maxExpansionRadius;
  /** The minimum distance to the goal*/
  double distanceToGoal;
  /** A pointer to the potential field in which the search is performed*/
  Potentialfield* field;
  /** The minimum distance for enlarging expansion radius und reducing branching factor*/
  double endOfNear;
  /** The maximum distance for enlarging expansion radius und reducing branching factor*/
  double endOfFar;
  /** The length of a generated vector*/
  double standardGradientLength;
  /** The first position of the search*/
  PfVec startPosition;
  /** The number of consecutive calls of the A*-search */
  unsigned long numberOfCalls;
  /** Flag: Use stabilization element, if true*/
  bool useStabilization;
  /** The stabilization object*/
  Object* stabilizationObject;
  /** The distance of the stabilization object*/
  double stabilizationDistance;
};


/**
* @class PotentialfieldAStarNode
*
* A class describing a node in an A* search tree
*/
class PotentialfieldAStarNode
{
public:
  /** Constructor */
  PotentialfieldAStarNode()
  { expanded = false; parentNode = 0;}

  /** Copy-Constructor
  * @param node Another node to copy data from
  */
  PotentialfieldAStarNode(const PotentialfieldAStarNode& node)
  {
    pose = node.pose;
    fValue = node.fValue;
    gValue = node.gValue;
    parentNode = node.parentNode;  
    expanded = node.expanded;
    valueAtPos = node.valueAtPos;
    expansionRadius = node.expansionRadius;
  }

  /** Sets the index of the parent node of this node
  * @param parentNode The index of the parent node
  */
  void setParentNode(unsigned int parentNode)
  { this->parentNode = parentNode;}

  /** Returns the index of the parent node
  * @return The parent node
  */
  unsigned int getParentNode() const 
  { return parentNode;}

  /** Sets the function values
  * @param g The costs of the path to this node
  * @param h The heuristic costs from this node to the goal
  */
  void setFunctionValues(double g, double h)
  { gValue = g; fValue = g+h;}

  /** Returns the value of this node
  * @return The value f
  */
  double f() const
  { return fValue;}

  /** Returns the costs of the path to this node
  * @return The path costs g
  */
  double g() const
  { return gValue;}
  
  /** Sets the pose of this node
  * @param pose The pose
  */
  void setPose(const PfPose& pose) 
  { this->pose = pose;}

  /** Returns the pose of this node
  * @return The pose
  */
  PfPose getPose() const
  { return pose;}

  /** Sets the value of the potential field at the position of the node
  * @param parameterSet The parameter set
  */
  void setValueAtPos(const PotentialfieldAStarParameterSet& parameterSet);

  /** Expands this node
  * @param searchTree The search tree of A* search, new nodes will be inserted
  * @param expandedNodes A list of previously expanded nodes
  * @param goal The goal of the search
  * @param parameterSet The parameter set for the search
  * @param ownNodeNum The index of this node (in the seachTree)
  */
  void expand(std::vector<PotentialfieldAStarNode>& searchTree,
              const std::vector<unsigned int>& expandedNodes,
              const PotentialfieldAStarNode& goal,
              const PotentialfieldAStarParameterSet& parameterSet,
              unsigned int ownNodeNum);

  /** Checks if this node has been expanded
  * @return true, if the node has been expanded
  */
  bool hasBeenExpanded() const 
  { return expanded;}

  /** Checks if this node has reached the goal
  * @param goal The goal
  * @param parameterSet The parameter set
  * @return true, if the node has reached the goal
  */
  bool hasReached(const PotentialfieldAStarNode& goal,
                  const PotentialfieldAStarParameterSet& parameterSet) const;

  /** Computes the distance to another node
  * @param other The other node
  * @return The distance
  */
  double distToOtherNode(const PotentialfieldAStarNode& other)
  { return (pose.pos - other.getPose().pos).length();}

protected:
  /** The pose of the node*/
  PfPose pose;
  /** The value of the node*/
  double fValue;
  /** The path costs to the node*/
  double gValue;
  /** The index of the parent node*/
  unsigned int parentNode;  
  /** Flag: true, if the node has been expanded*/
  bool expanded;
  /** The potential field value at the position of the node*/
  double valueAtPos;
  /** The expansion radius of the node*/
  double expansionRadius;

  /** Returns the expansion radius of the node
  * @return The expansion radius
  */
  double getExpansionRadius() const
  { return expansionRadius;}

  /** Computes the path costs to another node
  * @param node The other node
  * @param parameterSet The parameter set
  * @return The path costs
  */
  double computeCostsTo(const PotentialfieldAStarNode& node,
                        const PotentialfieldAStarParameterSet& parameterSet) const;

  /** Computes the heuristic costs between two positions
  * @param p1 The first position
  * @param p2 The second position
  * @param currentBranchingFactor The current branching factor
  * @return The heuristic costs
  */
  double computeHeuristicBetween(const PfPose& p1, const PfPose& p2,
                                 unsigned int currentBranchingFactor) const;

  /** Computes current parameters for expansion
  * @param currentExpansionRadius The current expansion radius
  * @param currentBranchingFactor The current branching factor
  * @param parameterSet The parameter set
  */
  void computeCurrentParameters(double& currentExpansionRadius, 
                                unsigned int& currentBranchingFactor,
                                const PotentialfieldAStarParameterSet& parameterSet) const;

  /** Checks if a new node is too close to another previously expanded node
  * @param searchTree The search tree
  * @param expandedNodes A list containing all previously expanded nodes
  * @param pose The pose of the new node
  * @return true, if the pose is too close
  */
  bool tooCloseToAnotherNode(std::vector<PotentialfieldAStarNode>& searchTree,
                             const std::vector<unsigned int>& expandedNodes,
                             const PfPose& pose) const;
};


/**
* @class AStarSearch
*
* An abstract implementation of the A* search algorithm.
* Works with any appropriate type N of nodes, type P of parameter sets
* and type U of unit
*/
template<class N, class P, class U> class AStarSearch
{
public:
  /** Constructor 
  * @param minCacheSize The number of nodes to allocate memory for
  * @param maxTreeSize The maximum number of nodes to expand
  * @param parameterSet A parameter set to check the maximum number of nodes to expand
  */
  AStarSearch(long minCacheSize, long maxTreeSize, P parameterSet)
  {
    unsigned int elementsToReserve;
    if(maxTreeSize != -1)
    {
      elementsToReserve = (maxTreeSize+parameterSet.maxBranchingFactor);
      this->maxTreeSize = maxTreeSize;
    }
    else
    {
      elementsToReserve = minCacheSize;
      this->maxTreeSize = -1;
    }
    searchTree.reserve(elementsToReserve);
    expandedNodes.reserve(elementsToReserve);
  }


  /** The main search function
  * @param start The starting node
  * @param goal The goal node
  * @param parameterSet The current parameter set
  * @param pathLength Returns the length of the path to the goal
  * @return The next node to go to
  */
  N search(const N& start, const N& goal, const P& parameterSet, U& pathLength)
  {
    this->parameterSet = parameterSet;
    if(start.hasReached(goal, parameterSet))
    {
      return start;
    }
    searchTree.clear();
    expandedNodes.clear();
    searchTree.push_back(start);
    searchTree[0].setParentNode(0);
    unsigned int indexOfBestNode;
    unsigned int nextNodeToExpand;
    while(true)
    {
      nextNodeToExpand = findIndexOfNextNode();
      if(searchTree.size() >= (unsigned int)maxTreeSize)
      {
        indexOfBestNode = nextNodeToExpand;
        break;
      }
      else
      {
        unsigned int sizeBeforeExpand(searchTree.size());
        searchTree[nextNodeToExpand].expand(searchTree, expandedNodes, 
                                            goal, parameterSet, nextNodeToExpand);
        expandedNodes.push_back(nextNodeToExpand);
        unsigned int sizeAfterExpand(searchTree.size());
        int result(testNewNodesAgainstGoal(sizeBeforeExpand, sizeAfterExpand-1,goal));
        if(result != -1)
        {
          indexOfBestNode = result;
          break;
        }
      }
    }
    N result = backtraceNode(indexOfBestNode, pathLength);
#ifdef POTENTIALFIELDS_FOR_GT2004_
    //OUTPUT(idText,text, searchTree.size()<<" nodes, "<<expandedNodes.size()<<" expanded nodes");
    COMPLEX_DRAWING(behavior_aStarSearch,draw(indexOfBestNode));
    DEBUG_DRAWING_FINISHED(behavior_aStarSearch); 
#endif //POTENTIALFIELDS_FOR_GT2004_
    return result;
  }
  
private:
  /** A container for all nodes*/
  std::vector<N> searchTree;
  /** Indizes of all expanded nodes*/
  std::vector<unsigned int> expandedNodes;
  /** The maximum number of nodes to expand*/
  long maxTreeSize;
  /** The parameter set*/
  P parameterSet;

  /** 
  * Finds the next node to be expanded
  * @return The index of the node
  */
  unsigned int findIndexOfNextNode()
  {
    unsigned int expandableNode(0);
    double f;
    while(searchTree[expandableNode].hasBeenExpanded())
    {
      ++expandableNode;
    }
    f = searchTree[expandableNode].f();
    for(unsigned int i=(expandableNode+1); i<searchTree.size(); i++)
    {
      if((!searchTree[i].hasBeenExpanded()) && (searchTree[i].f() < f))
      {
        f = searchTree[i].f();
        expandableNode = i;
      }
    }
    return expandableNode;
  }


  /** Tests if any node has reached the goal
  * @param firstNode The first node to test
  * @param lastNode The last node to test
  * @param goal The goal to test against
  * @return -1, if no node has reached the goal, the number of the node otherwise
  */
  int testNewNodesAgainstGoal(unsigned int firstNode, unsigned int lastNode,
                              const N& goal)
  {
    for(unsigned int i = firstNode; i<= lastNode; i++)
    {
      if(searchTree[i].hasReached(goal, parameterSet))
      {
        return (int)i;
      }
    }
    return -1;
  }


  /** Backtraces the best node found to the first node after the start node
  * @param indexOfBestNode The index of the best node found
  * @param pathLength Returns the length of the backtraced path
  * @return The root node of the path to the node
  */
  N backtraceNode(unsigned int indexOfBestNode, U& pathLength)
  {
    unsigned int currentNode(indexOfBestNode);
    pathLength = searchTree[currentNode].distToOtherNode(searchTree[searchTree[currentNode].getParentNode()]);
    while(searchTree[currentNode].getParentNode() != 0)
    {
      currentNode = searchTree[currentNode].getParentNode();
      pathLength += searchTree[currentNode].distToOtherNode(searchTree[searchTree[currentNode].getParentNode()]);
    }
    return searchTree[currentNode];
  }

#ifdef POTENTIALFIELDS_FOR_GT2004_
  /** Draw all expanded paths and the selected path on the RobotControl field view
  * @param indexOfBestNode The index of the last expanded node
  */
  void draw(unsigned int indexOfBestNode)
  {
    PfPose pose, parentPose;
    for(unsigned int i=0; i<searchTree.size(); i++)
    {
      pose = searchTree[i].getPose();
      parentPose = searchTree[searchTree[i].getParentNode()].getPose();
      LINE(behavior_aStarSearch, pose.pos.x, pose.pos.y, parentPose.pos.x, parentPose.pos.y, 
        1, Drawings::ps_solid, Drawings::red);
    }
    unsigned int currentNode(indexOfBestNode);
    do
    {
      pose = searchTree[currentNode].getPose();
      currentNode = searchTree[currentNode].getParentNode();
      parentPose = searchTree[currentNode].getPose();
      LINE(behavior_aStarSearch, pose.pos.x, pose.pos.y, parentPose.pos.x, parentPose.pos.y, 
        1, Drawings::ps_solid, Drawings::yellow);
    } while(currentNode != 0);
    if(parameterSet.useStabilization)
    {
      PfPose stabPose(parameterSet.stabilizationObject->getPose());
      LARGE_DOT(behavior_aStarSearch, stabPose.pos.x, stabPose.pos.y, 
                Drawings::red, Drawings::red);
    }
  }
#endif //POTENTIALFIELDS_FOR_GT2004_
};


#endif  //A_STAR_SEARCH_H_



/*
* $Log: AStarSearch.h,v $
* Revision 1.2  2004/06/17 16:20:30  tim
* improved ;-)
*
* Revision 1.1.1.1  2004/05/22 17:37:25  cvsadm
* created new repository GT2004_WM
*
* Revision 1.2  2004/01/28 08:27:16  dueffert
* doxygen bugs fixed
*
* Revision 1.1  2004/01/20 15:42:20  tim
* Added potential fields implementation
*
*/

---