Пример #1
0
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;
}