public void generateRRTObstaclesWithPath(int numNodes, OccupancyGrid occupancy, int pathSearchDepth)
{
// Create a new tree which we will populate with nodes, with the root of the tree
// set to the initial configuration
tree = new Tree(initialConfiguration);
// Loop until the graph is the requested size
for (int i = 0; i < numNodes; i++) {
bool clearConf = false;
StateConfig randConf = null;
StateConfig newConf = null;
TreeNode<StateConfig> closestNode = null;
// Generate a random configuration within the configuration space until one is
// found which does not fall in any occupied cell in the occupancy grid
while (!clearConf){
randConf = getRandomConfiguration(limits);
Tuple<float,float> pos = randConf.getPosition();
// If the random configuration is occupied, generate a new one.
if(occupancy.isOccupied(pos.first, pos.second))
continue;
// Find the closest node on the current graph to the point that was generated
closestNode = tree.findClosest(randConf, Tree.SearchMethod.BFS);
StateConfig closestConf = closestNode.content;
// Generate a new configuration by moving along the hyper-edge connecting
// the closest configuration and the randomly generated one.
newConf = closestConf + (randConf - closestConf) * increment;
Tuple<float,float> newPos = newConf.getPosition();
if (occupancy.isOccupied(newPos.first, newPos.second))
continue;
if (!occupancy.obstacleFreeStraightPath(closestConf.getPosition(), newConf.getPosition(), pathSearchDepth))
continue;
clearConf = true;
}
// Add the new configuration as a child of the closest node.
TreeNode<StateConfig> newNode = tree.addNode(newConf, closestNode);
// If we are within range of the goal with the new node, stop expanding the tree.
if (checkGoalDist(newNode))
break;
}
}
/*
* Generates an RRT with the specified number of nodes. The goalBias
* parameter defines how biased the RRT should be towards choosing the
* goal as the configuration to go towards. It should be a value between
* 0 and 1.
*/
public void generateRRTBasic(int numNodes)
{
// Create a new tree which we will populate with nodes, with the
// initial configuration as the root of the tree.
tree = new Tree(initialConfiguration);
// Loop until the tree has the requested number of nodes
for (int i = 0; i < numNodes; i++) {
// Generate a random configuration within the configuration space
StateConfig randConf = getRandomConfiguration(limits);
// Find the closest node on the current graph to the point that was generated
TreeNode<StateConfig> closestNode = tree.findClosest(randConf, Tree.SearchMethod.BFS);
StateConfig closestConf = closestNode.content;
// Generate a new configuration by moving along the hyper-edge connecting
// the closest configuration and the randomly generated one.
StateConfig newConf = closestConf + (randConf - closestConf) * increment;
// Add the new configuration as a child of the closest node.
TreeNode<StateConfig> newNode = tree.addNode(newConf, closestNode);
// If we are within range of the goal with the new node, stop expanding the tree.
if (checkGoalDist(newNode))
break;
}
}