InterpolatedPath* RRTPlanner::runRRT(MotionInstant start, MotionInstant goal,
const MotionConstraints& motionConstraints,
const Geometry2d::ShapeSet* obstacles) {
InterpolatedPath* path = new InterpolatedPath();
path->setStartTime(RJ::timestamp());
// Initialize two RRT trees
FixedStepTree startTree;
FixedStepTree goalTree;
startTree.init(start.pos, obstacles);
goalTree.init(goal.pos, obstacles);
startTree.step = goalTree.step = .15f;
// Run bi-directional RRT algorithm
Tree* ta = &startTree;
Tree* tb = &goalTree;
for (unsigned int i = 0; i < _maxIterations; ++i) {
Geometry2d::Point r = RandomFieldLocation();
Tree::Point* newPoint = ta->extend(r);
if (newPoint) {
// try to connect the other tree to this point
if (tb->connect(newPoint->pos)) {
// trees connected
// done with global path finding
// the path is from start to goal
// runRRT will handle the rest
break;
}
}
swap(ta, tb);
}
Tree::Point* p0 = startTree.last();
Tree::Point* p1 = goalTree.last();
// sanity check
if (!p0 || !p1 || p0->pos != p1->pos) {
return path;
}
// extract path from RRTs
// add the start tree first...normal order (aka from root to p0)
startTree.addPath(*path, p0);
// add the goal tree in reverse (aka p1 to root)
goalTree.addPath(*path, p1, true);
path = optimize(*path, obstacles, motionConstraints, start.vel, goal.vel);
return path;
}
Point EscapeObstaclesPathPlanner::findNonBlockedGoal(
Point goal, boost::optional<Point> prevGoal, const ShapeSet& obstacles,
int maxItr) {
if (obstacles.hit(goal)) {
FixedStepTree goalTree;
goalTree.init(goal, &obstacles);
goalTree.step = stepSize();
// The starting point is in an obstacle, extend the tree until we find
// an unobstructed point
Point newGoal;
for (int i = 0; i < maxItr; ++i) {
// extend towards a random point
Tree::Point* newPoint = goalTree.extend(RandomFieldLocation());
// if the new point is not blocked, it becomes the new goal
if (newPoint && newPoint->hit.empty()) {
newGoal = newPoint->pos;
break;
}
}
if (!prevGoal || obstacles.hit(*prevGoal)) return newGoal;
// Only use this newly-found point if it's closer to the desired goal by
// at least a certain threshold
float oldDist = (*prevGoal - goal).mag();
float newDist = (newGoal - goal).mag();
if (newDist + *_goalChangeThreshold < oldDist) {
return newGoal;
} else {
return *prevGoal;
}
}
return goal;
}
