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; }