/**
* @function addNode
* @brief Add _qnew to tree with parent _parentId
* @return id of node added
*/
int B1RRT::addNode( const Eigen::VectorXd &_qnew, int _parentId )
{
/// Update graph vectors -- what does this mean?
configVector.push_back( _qnew );
parentVector.push_back( _parentId );
uintptr_t id = configVector.size() - 1;
kd_insert( kdTree, _qnew.data(), (void*) id ); //&idVector[id]; /// WTH? -- ahq
activeNode = id;
/// Add node to ranking
Eigen::VectorXd entry(3);
entry << id, HeuristicCost( id, targetPose ), 0;
pushRanking( entry );
return id;
}
/**
* This is a straight forward C++ implementation of Wikipedia's A* pseudo code.
* As proof of this the pseudo code is embedded in the source, just above the
* C++ statements.
*
* See http://en.wikipedia.org/wiki/A*#Pseudocode for the complete pseudo code
*
* But also see the discussion page: it explains why the algorithm is only
* correct if the cost function is monotone.This algorithm does not give the
* correct solution in our case. The routes and hence costs differ whether we
* go from (0,0) to (10,10) or vice versa:
*
* 0-0 -> 3-2 -> 4-5 -> 4-8 -> 6-9 -> 10-10, cost = 15
*
* 10-10 -> 10-7 -> 7-5 -> 6-3 -> 3-2 -> 0-0, cost = 14
*
* Use this piece of code as inspiration only.
*
*/
std::vector<Vertex> WikiAStar( Vertex start, Vertex goal)
{
ClosedSet closedSet; // The set of nodes already evaluated.
OpenSet openSet; // The set of tentative nodes to be evaluated, initially containing the start node
VertexMap predecessorMap; // The map of navigated nodes.
start.actualCost = 0.0; // Cost from start along best known path.
start.heuristicCost = start.actualCost + HeuristicCost(start, goal); // Estimated total cost from start to goal through y.
openSet.insert(start);
//while openset is not empty
while(!openSet.empty())
{
// current := the node in openset having the lowest f_score[] value
Vertex current = *openSet.begin();
// if current = goal
if(current == goal)
{
// return reconstruct_path(came_from, goal)
return ConstructPath(predecessorMap,current);
}
// remove current from openset
openSet.erase(current);
// add current to closedset
closedSet.insert(current);
//for each neighbour in neighbour_nodes(current)
std::vector< Vertex > neighbours = GetNeighbours(current);
for(Vertex neighbour : neighbours)
{
// if neighbor in closedset continue (with next neighbour)
if(closedSet.find(neighbour) != closedSet.end())
{
continue;
}
// tentative_g_score := g_score[current] + dist_between(current,neighbour)
double calculatedActualNeighbourCost = current.actualCost + ActualCost(current,neighbour);
// if neighbour not in openset or tentative_g_score < g_score[neighbour]
if(openSet.find(neighbour) == openSet.end() || calculatedActualNeighbourCost < neighbour.actualCost)
{
// Here we deviate from the Wikipedia article, because of the map semantics:
// we cannot change the object's key values once it in the map so we first
// set the vales and then put it into the map.
// g_score[neighbor] := tentative_g_score
neighbour.actualCost = calculatedActualNeighbourCost;
// f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)
neighbour.heuristicCost = neighbour.actualCost + HeuristicCost(neighbour, goal);
// came_from[neighbor] := current
std::pair<VertexMap::iterator,bool> result = predecessorMap.insert(std::make_pair(neighbour,current));
// The following if-statement is not part of the pseudo code but a partial fix for a semantic difference
// in the map of the pseudo code and the c++ std::map
if(result.second==false)
(*result.first).second = current;
// if neighbor not in openset
if(openSet.find(neighbour) == openSet.end())
{
// add neighbor to openset
openSet.insert(neighbour);
}
}
}
}
//return failure
return std::vector<Vertex>();
}
