C++ PathGeometric类代码示例

bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
{
    if (path.getStateCount() < 3)
        return false;

    if (maxSteps == 0)
        maxSteps = path.getStateCount();

    if (maxEmptySteps == 0)
        maxEmptySteps = path.getStateCount();

    const base::SpaceInformationPtr &si = path.getSpaceInformation();
    std::vector<base::State*> &states = path.getStates();

    // compute pair-wise distances in path (construct only half the matrix)
    std::map<std::pair<const base::State*, const base::State*>, double> distances;
    for (unsigned int i = 0 ; i < states.size() ; ++i)
        for (unsigned int j = i + 2 ; j < states.size() ; ++j)
            distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);

    bool result = false;
    unsigned int nochange = 0;
    for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
    {
        // find closest pair of points
        double minDist = std::numeric_limits<double>::infinity();
        int p1 = -1;
        int p2 = -1;
        for (unsigned int i = 0 ; i < states.size() ; ++i)
            for (unsigned int j = i + 2 ; j < states.size() ; ++j)
            {
                double d = distances[std::make_pair(states[i], states[j])];
                if (d < minDist)
                {
                    minDist = d;
                    p1 = i;
                    p2 = j;
                }
            }

        if (p1 >= 0 && p2 >= 0)
        {
            if (si->checkMotion(states[p1], states[p2]))
            {
                if (freeStates_)
                    for (int i = p1 + 1 ; i < p2 ; ++i)
                        si->freeState(states[i]);
                states.erase(states.begin() + p1 + 1, states.begin() + p2);
                result = true;
                nochange = 0;
            }
            else
                distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
        }
        else
            break;
    }
    return result;
}

void ompl::geometric::PathHybridization::computeHybridPath()
{
    boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
    boost::dijkstra_shortest_paths(g_, root_, boost::predecessor_map(prev));
    if (prev[goal_] != goal_)
    {
        PathGeometric *h = new PathGeometric(si_);
        for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos])
            h->append(stateProperty_[pos]);
        h->reverse();
        hpath_.reset(h);
    }
}

void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc)
{
    if (path.getStateCount() < 3)
        return;

    // try a randomized step of connecting vertices
    bool tryMore = false;
    if (ptc == false)
        tryMore = reduceVertices(path);

    // try to collapse close-by vertices
    if (ptc == false)
        collapseCloseVertices(path);

    // try to reduce verices some more, if there is any point in doing so
    int times = 0;
    while (tryMore && ptc == false && ++times <= 5)
        tryMore = reduceVertices(path);

    // if the space is metric, we can do some additional smoothing
    if(si_->getStateSpace()->isMetricSpace())
    {
        bool tryMore = true;
        unsigned int times = 0;
        do
        {
            bool shortcut = shortcutPath(path);                             // split path segments, not just vertices
            bool better_goal = gsr_ ? findBetterGoal(path, ptc) : false;    // Try to connect the path to a closer goal

            tryMore = shortcut || better_goal;
        } while(ptc == false && tryMore && ++times <= 5);

        // smooth the path with BSpline interpolation
        if(ptc == false)
            smoothBSpline(path, 3, path.length()/100.0);

        // we always run this if the metric-space algorithms were run.  In non-metric spaces this does not work.
        const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
        if (!p.second)
            OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
        else
            if (!p.first)
                OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
    }
}

void ompl::geometric::FMT::traceSolutionPathThroughTree(Motion *goalMotion)
{
    std::vector<Motion*> mpath;
    Motion *solution = goalMotion;

    // Construct the solution path
    while (solution != nullptr)
    {
        mpath.push_back(solution);
        solution = solution->getParent();
    }

    // Set the solution path
    PathGeometric *path = new PathGeometric(si_);
    int mPathSize = mpath.size();
    for (int i = mPathSize - 1 ; i >= 0 ; --i)
        path->append(mpath[i]->getState());
    pdef_->addSolutionPath(base::PathPtr(path), false, -1.0, getName());
}

/* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
{
    if (path.getStateCount() < 3)
        return;

    const base::SpaceInformationPtr &si = path.getSpaceInformation();
    std::vector<base::State*> &states = path.getStates();

    base::State *temp1 = si->allocState();
    base::State *temp2 = si->allocState();

    for (unsigned int s = 0 ; s < maxSteps ; ++s)
    {
        path.subdivide();

        unsigned int i = 2, u = 0, n1 = states.size() - 1;
        while (i < n1)
        {
            if (si->isValid(states[i - 1]))
            {
                si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
                si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
                si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
                if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
                {
                    if (si->distance(states[i], temp1) > minChange)
                    {
                        si->copyState(states[i], temp1);
                        ++u;
                    }
                }
            }

            i += 2;
        }

        if (u == 0)
            break;
    }

    si->freeState(temp1);
    si->freeState(temp2);
}

//.........这里部分代码省略.........

            std::vector<Motion*> nnVec;
            unsigned int k = std::ceil(k_rrg * log((double)(nn_->size() + 1)));
            nn_->nearestK(motion, k, nnVec);
            nn_->add(motion); // if we add the motion before the nearestK call, we will get ourselves...

            IsLessThan isLessThan(this, motion);
            std::sort(nnVec.begin(), nnVec.end(), isLessThan);

            //-------------------------------------------------//
            //  Rewiring Part (i) - find best parent of motion //
            //-------------------------------------------------//
            if (motion->parentApx_ != nnVec.front())
            {
                for (std::size_t i(0); i < nnVec.size(); ++i)
                {
                    Motion *potentialParent = nnVec[i];
                    double dist = distanceFunction(potentialParent, motion);
                    considerEdge(potentialParent, motion, dist);
                }
            }

            //------------------------------------------------------------------//
            //  Rewiring Part (ii)                                              //
            //  check if motion may be a better parent to one of its neighbors  //
            //------------------------------------------------------------------//
            for (std::size_t i(0); i < nnVec.size(); ++i)
            {
                Motion *child = nnVec[i];
                double dist = distanceFunction(motion, child);
                considerEdge(motion, child, dist);
            }

            double dist = 0.0;
            bool sat = goal->isSatisfied(motion->state_, &dist);

            if (sat)
            {
                approxdif = dist;
                solution = motion;
            }
            if (dist < approxdif)
            {
                approxdif = dist;
                approxSol = motion;
            }

            if (solution != nullptr && bestCost_ != solution->costApx_)
            {
                OMPL_INFORM("%s: approximation cost = %g", getName().c_str(),
                    solution->costApx_);
                bestCost_ = solution->costApx_;
            }
        }
    }

    bool solved = false;
    bool approximate = false;

    if (solution == nullptr)
    {
        solution = approxSol;
        approximate = true;
    }

    if (solution != nullptr)
    {
        lastGoalMotion_ = solution;

        /* construct the solution path */
        std::vector<Motion*> mpath;
        while (solution != nullptr)
        {
            mpath.push_back(solution);
            solution = solution->parentApx_;
        }

        /* set the solution path */
        PathGeometric *path = new PathGeometric(si_);
        for (int i = mpath.size() - 1 ; i >= 0 ; --i)
            path->append(mpath[i]->state_);
        // Add the solution path.
        base::PathPtr bpath(path);
        base::PlannerSolution psol(bpath);
        psol.setPlannerName(getName());
        if (approximate)
            psol.setApproximate(approxdif);
        pdef_->addSolutionPath(psol);
        solved = true;
    }

    si_->freeState(xstate);
    if (rmotion->state_)
        si_->freeState(rmotion->state_);
    delete rmotion;

    OMPL_INFORM("%s: Created %u states", getName().c_str(), statesGenerated);

    return base::PlannerStatus(solved, approximate);
}

//.........这里部分代码省略.........
    {
        TreeData &tree      = startTree ? tStart_ : tGoal_;
        tgi.start = startTree;
        startTree = !startTree;
        TreeData &otherTree = startTree ? tStart_ : tGoal_;

        if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
        {
            const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
            if (st)
            {
                Motion* motion = new Motion(si_);
                si_->copyState(motion->state, st);
                motion->root = motion->state;
                tGoal_->add(motion);
            }

            if (tGoal_->size() == 0)
            {
                logError("Unable to sample any valid states for goal tree");
                break;
            }
        }

        /* sample random state */
        sampler_->sampleUniform(rstate);

        GrowState gs = growTree(tree, tgi, rmotion);

        if (gs != TRAPPED)
        {
            /* remember which motion was just added */
            Motion *addedMotion = tgi.xmotion;

            /* attempt to connect trees */

            /* if reached, it means we used rstate directly, no need top copy again */
            if (gs != REACHED)
                si_->copyState(rstate, tgi.xstate);

            GrowState gsc = ADVANCED;
            tgi.start = startTree;
            while (gsc == ADVANCED)
                gsc = growTree(otherTree, tgi, rmotion);

            Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
            Motion *goalMotion  = startTree ? addedMotion : tgi.xmotion;

            /* if we connected the trees in a valid way (start and goal pair is valid)*/
            if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
            {
                // it must be the case that either the start tree or the goal tree has made some progress
                // so one of the parents is not NULL. We go one step 'back' to avoid having a duplicate state
                // on the solution path
                if (startMotion->parent)
                    startMotion = startMotion->parent;
                else
                    goalMotion = goalMotion->parent;

                connectionPoint_ = std::make_pair<base::State*, base::State*>(startMotion->state, goalMotion->state);

                /* construct the solution path */
                Motion *solution = startMotion;
                std::vector<Motion*> mpath1;
                while (solution != NULL)
                {
                    mpath1.push_back(solution);
                    solution = solution->parent;
                }

                solution = goalMotion;
                std::vector<Motion*> mpath2;
                while (solution != NULL)
                {
                    mpath2.push_back(solution);
                    solution = solution->parent;
                }

                PathGeometric *path = new PathGeometric(si_);
                path->getStates().reserve(mpath1.size() + mpath2.size());
                for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
                    path->append(mpath1[i]->state);
                for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
                    path->append(mpath2[i]->state);

                pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
                solved = true;
                break;
            }
        }
    }

    si_->freeState(tgi.xstate);
    si_->freeState(rstate);
    delete rmotion;

    logInform("Created %u states (%u start + %u goal)", tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());

    return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}

ompl::base::PlannerStatus ompl::geometric::LazyRRT::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    base::Goal                 *goal   = pdef_->getGoal().get();
    base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);

    while (const base::State *st = pis_.nextStart())
    {
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, st);
        motion->valid = true;
        nn_->add(motion);
    }

    if (nn_->size() == 0)
    {
        OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
        return base::PlannerStatus::INVALID_START;
    }

    if (!sampler_)
        sampler_ = si_->allocStateSampler();

    OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());

    Motion *solution = NULL;
    double  distsol  = -1.0;
    Motion *rmotion  = new Motion(si_);
    base::State *rstate = rmotion->state;
    base::State *xstate = si_->allocState();

    bool solutionFound = false;

    while (ptc == false && !solutionFound)
    {
        /* sample random state (with goal biasing) */
        if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
            goal_s->sampleGoal(rstate);
        else
            sampler_->sampleUniform(rstate);

        /* find closest state in the tree */
        Motion *nmotion = nn_->nearest(rmotion);
        assert(nmotion != rmotion);
        base::State *dstate = rstate;

        /* find state to add */
        double d = si_->distance(nmotion->state, rstate);
        if (d > maxDistance_)
        {
            si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
            dstate = xstate;
        }

        /* create a motion */
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, dstate);
        motion->parent = nmotion;
        nmotion->children.push_back(motion);
        nn_->add(motion);

        double dist = 0.0;
        if (goal->isSatisfied(motion->state, &dist))
        {
            distsol = dist;
            solution = motion;
            solutionFound = true;
            lastGoalMotion_ = solution;

            // Check that the solution is valid:
            // construct the solution path
            std::vector<Motion*> mpath;
            while (solution != NULL)
            {
                mpath.push_back(solution);
                solution = solution->parent;
            }

            // check each segment along the path for validity
            for (int i = mpath.size() - 1 ; i >= 0 && solutionFound; --i)
                if (!mpath[i]->valid)
                {
                    if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
                        mpath[i]->valid = true;
                    else
                    {
                        removeMotion(mpath[i]);
                        solutionFound = false;
                        lastGoalMotion_ = NULL;
                    }
                }

            if (solutionFound)
            {
                // set the solution path
                PathGeometric *path = new PathGeometric(si_);
                for (int i = mpath.size() - 1 ; i >= 0 ; --i)
                    path->append(mpath[i]->state);

                pdef_->addSolutionPath(base::PathPtr(path), false, distsol, getName());
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                            nbh[i]->incCost = nbhIncCost;
                            nbh[i]->cost = nbhNewCost;
                            nbh[i]->parent->children.push_back(nbh[i]);

                            // Update the costs of the node's children
                            updateChildCosts(nbh[i]);

                            checkForSolution = true;
                        }
                    }
                }
            }

            // Add the new motion to the goalMotion_ list, if it satisfies the goal
            double distanceFromGoal;
            if (goal->isSatisfied(motion->state, &distanceFromGoal))
            {
                goalMotions_.push_back(motion);
                checkForSolution = true;
            }

            // Checking for solution or iterative improvement
            if (checkForSolution)
            {
                for (size_t i = 0; i < goalMotions_.size(); ++i)
                {
                    if (opt_->isCostBetterThan(goalMotions_[i]->cost, bestCost))
                    {
                        bestCost = goalMotions_[i]->cost;
                        bestCost_ = bestCost;
                    }

                    sufficientlyShort = opt_->isSatisfied(goalMotions_[i]->cost);
                    if (sufficientlyShort)
                    {
                        solution = goalMotions_[i];
                        break;
                    }
                    else if (!solution ||
                             opt_->isCostBetterThan(goalMotions_[i]->cost,solution->cost))
                        solution = goalMotions_[i];
                }
            }

            // Checking for approximate solution (closest state found to the goal)
            if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
            {
                approximation = motion;
                approximatedist = distanceFromGoal;
            }
        }

        // terminate if a sufficient solution is found
        if (solution && sufficientlyShort)
            break;
    }

    bool approximate = (solution == 0);
    bool addedSolution = false;
    if (approximate)
        solution = approximation;
    else
        lastGoalMotion_ = solution;

    if (solution != 0)
    {
        // construct the solution path
        std::vector<Motion*> mpath;
        while (solution != 0)
        {
            mpath.push_back(solution);
            solution = solution->parent;
        }

        // set the solution path
        PathGeometric *geoPath = new PathGeometric(si_);
        for (int i = mpath.size() - 1 ; i >= 0 ; --i)
            geoPath->append(mpath[i]->state);

        base::PathPtr path(geoPath);
        // Add the solution path, whether it is approximate (not reaching the goal), and the
        // distance from the end of the path to the goal (-1 if satisfying the goal).
        base::PlannerSolution psol(path, approximate, approximate ? approximatedist : -1.0, getName());
        // Does the solution satisfy the optimization objective?
        psol.optimized_ = sufficientlyShort;

        pdef_->addSolutionPath (psol);

        addedSolution = true;
    }

    si_->freeState(xstate);
    if (rmotion->state)
        si_->freeState(rmotion->state);
    delete rmotion;

    OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree.", getName().c_str(), statesGenerated, rewireTest, goalMotions_.size());

    return base::PlannerStatus(addedSolution, approximate);
}

void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
{
    RNG rng;

    std::vector<Motion*> solution;
    base::State *xstate = si_->allocState();
    bool      startTree = rng.uniformBool();

    while (!sol->found && ptc == false)
    {
        bool retry = true;
        while (retry && !sol->found && ptc == false)
        {
            removeList_.lock.lock();
            if (!removeList_.motions.empty())
            {
                if (loopLock_.try_lock())
                {
                    retry = false;
                    std::map<Motion*, bool> seen;
                    for (unsigned int i = 0 ; i < removeList_.motions.size() ; ++i)
                        if (seen.find(removeList_.motions[i].motion) == seen.end())
                            removeMotion(*removeList_.motions[i].tree, removeList_.motions[i].motion, seen);
                    removeList_.motions.clear();
                    loopLock_.unlock();
                }
            }
            else
                retry = false;
            removeList_.lock.unlock();
        }

        if (sol->found || ptc)
            break;

        loopLockCounter_.lock();
        if (loopCounter_ == 0)
            loopLock_.lock();
        loopCounter_++;
        loopLockCounter_.unlock();


        TreeData &tree      = startTree ? tStart_ : tGoal_;
        startTree = !startTree;
        TreeData &otherTree = startTree ? tStart_ : tGoal_;

        Motion *existing = selectMotion(rng, tree);
        if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
            continue;

        /* create a motion */
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, xstate);
        motion->parent = existing;
        motion->root = existing->root;

        existing->lock.lock();
        existing->children.push_back(motion);
        existing->lock.unlock();

        addMotion(tree, motion);

        if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
        {
            sol->lock.lock();
            if (!sol->found)
            {
                sol->found = true;
                PathGeometric *path = new PathGeometric(si_);
                for (unsigned int i = 0 ; i < solution.size() ; ++i)
                    path->append(solution[i]->state);
                pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
            }
            sol->lock.unlock();
        }


        loopLockCounter_.lock();
        loopCounter_--;
        if (loopCounter_ == 0)
            loopLock_.unlock();
        loopLockCounter_.unlock();
    }

    si_->freeState(xstate);
}

//.........这里部分代码省略.........
                            distance_inf(opt_->infiniteCost()).
                            distance_zero(opt_->identityCost()).
                            visitor(AStarGoalVisitor<Vertex>(goal)));
    }
    catch (AStarFoundGoal&)
    {
    }
    if (prev[goal] == goal)
        throw Exception(name_, "Could not find solution path");

    // First, get the solution states without copying them, and check them for validity.
    // We do all the node validity checks for the vertices, as this may remove a larger
    // part of the graph (compared to removing an edge).
    std::vector<const base::State*> states(1, stateProperty_[goal]);
    std::set<Vertex> milestonesToRemove;
    for (Vertex pos = prev[goal]; prev[pos] != pos; pos = prev[pos])
    {
        const base::State *st = stateProperty_[pos];
        unsigned int &vd = vertexValidityProperty_[pos];
        if ((vd & VALIDITY_TRUE) == 0)
            if (si_->isValid(st))
                vd |= VALIDITY_TRUE;
        if ((vd & VALIDITY_TRUE) == 0)
            milestonesToRemove.insert(pos);
        if (milestonesToRemove.empty())
            states.push_back(st);
    }

    // We remove *all* invalid vertices. This is not entirely as described in the original LazyPRM
    // paper, as the paper suggest removing the first vertex only, and then recomputing the
    // shortest path. Howeve, the paper says the focus is on efficient vertex & edge removal,
    // rather than collision checking, so this modification is in the spirit of the paper.
    if (!milestonesToRemove.empty())
    {
        unsigned long int comp = vertexComponentProperty_[start];
        // Remember the current neighbors.
        std::set<Vertex> neighbors;
        for (std::set<Vertex>::iterator it = milestonesToRemove.begin() ; it != milestonesToRemove.end() ; ++it)
        {
            boost::graph_traits<Graph>::adjacency_iterator nbh, last;
            for (boost::tie(nbh, last) = boost::adjacent_vertices(*it, g_) ; nbh != last ; ++nbh)
                if (milestonesToRemove.find(*nbh) == milestonesToRemove.end())
                    neighbors.insert(*nbh);
            // Remove vertex from nearest neighbors data structure.
            nn_->remove(*it);
            // Free vertex state.
            si_->freeState(stateProperty_[*it]);
            // Remove all edges.
            boost::clear_vertex(*it, g_);
            // Remove the vertex.
            boost::remove_vertex(*it, g_);
        }
        // Update the connected component ID for neighbors.
        for (std::set<Vertex>::iterator it = neighbors.begin() ; it != neighbors.end() ; ++it)
        {
            if (comp == vertexComponentProperty_[*it])
            {
                unsigned long int newComponent = componentCount_++;
                componentSize_[newComponent] = 0;
                markComponent(*it, newComponent);
            }
        }
        return base::PathPtr();
    }

    // start is checked for validity already
    states.push_back(stateProperty_[start]);

    // Check the edges too, if the vertices were valid. Remove the first invalid edge only.
    std::vector<const base::State*>::const_iterator prevState = states.begin(), state = prevState + 1;
    Vertex prevVertex = goal, pos = prev[goal];
    do
    {
        Edge e = boost::lookup_edge(pos, prevVertex, g_).first;
        unsigned int &evd = edgeValidityProperty_[e];
        if ((evd & VALIDITY_TRUE) == 0)
        {
            if (si_->checkMotion(*state, *prevState))
                evd |= VALIDITY_TRUE;
        }
        if ((evd & VALIDITY_TRUE) == 0)
        {
            boost::remove_edge(e, g_);
            unsigned long int newComponent = componentCount_++;
            componentSize_[newComponent] = 0;
            markComponent(pos, newComponent);
            return base::PathPtr();
        }
        prevState = state;
        ++state;
        prevVertex = pos;
        pos = prev[pos];
    }
    while (prevVertex != pos);

    PathGeometric *p = new PathGeometric(si_);
    for (std::vector<const base::State*>::const_reverse_iterator st = states.rbegin(); st != states.rend(); ++st)
        p->append(*st);
    return base::PathPtr(p);
}

//.........这里部分代码省略.........

        // if we have not sampled too many goals already
        if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
        {
            const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
            if (st)
            {
                Motion* motion = new Motion(si_);
                si_->copyState(motion->state, st);
                motion->root = motion->state;
                projectionEvaluator_->computeCoordinates(motion->state, xcoord);
                dGoal_.addMotion(motion, xcoord);
            }
            if (dGoal_.getMotionCount() == 0)
            {
                msg_.error("Unable to sample any valid states for goal tree");
                break;
            }
        }

        Discretization<Motion>::Cell *ecell    = NULL;
        Motion                       *existing = NULL;
        disc.selectMotion(existing, ecell);
        assert(existing);
        if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
        {
            std::pair<base::State*, double> fail(xstate, 0.0);
            bool keep = si_->checkMotion(existing->state, xstate, fail);
            if (!keep && fail.second > minValidPathFraction_)
                keep = true;

            if (keep)
            {
                /* create a motion */
                Motion *motion = new Motion(si_);
                si_->copyState(motion->state, xstate);
                motion->root = existing->root;
                motion->parent = existing;

                projectionEvaluator_->computeCoordinates(motion->state, xcoord);
                disc.addMotion(motion, xcoord);

                Discretization<Motion>::Cell* cellC = otherDisc.getGrid().getCell(xcoord);

                if (cellC && !cellC->data->motions.empty())
                {
                    Motion* connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)];

                    if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root) &&
                        si_->checkMotion(motion->state, connectOther->state))
                    {
                        /* extract the motions and put them in solution vector */

                        std::vector<Motion*> mpath1;
                        while (motion != NULL)
                        {
                            mpath1.push_back(motion);
                            motion = motion->parent;
                        }

                        std::vector<Motion*> mpath2;
                        while (connectOther != NULL)
                        {
                            mpath2.push_back(connectOther);
                            connectOther = connectOther->parent;
                        }

                        if (startTree)
                            mpath1.swap(mpath2);

                        PathGeometric *path = new PathGeometric(si_);
                        path->getStates().reserve(mpath1.size() + mpath2.size());
                        for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
                            path->append(mpath1[i]->state);
                        for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
                            path->append(mpath2[i]->state);

                        goal->addSolutionPath(base::PathPtr(path), false, 0.0);
                        solved = true;
                        break;
                    }
                }
            }
            else
              ecell->data->score *= failedExpansionScoreFactor_;
        }
        else
            ecell->data->score *= failedExpansionScoreFactor_;
        disc.updateCell(ecell);
    }

    si_->freeState(xstate);

    msg_.inform("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
                dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
                dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
                dGoal_.getCellCount(), dGoal_.getGrid().countExternal());

    return solved;
}

//.........这里部分代码省略.........
        {
            const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
            if (st)
            {
                Motion* motion = new Motion(si_);
                si_->copyState(motion->state, st);
                motion->root = motion->state;
                motion->valid = true;
                projectionEvaluator_->computeCoordinates(motion->state, xcoord);
                dGoal_.addMotion(motion, xcoord);
            }
            if (dGoal_.getMotionCount() == 0)
            {
                OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
                break;
            }
        }

        Discretization<Motion>::Cell *ecell    = NULL;
        Motion                       *existing = NULL;
        disc.selectMotion(existing, ecell);
        assert(existing);
        sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);

        /* create a motion */
        Motion* motion = new Motion(si_);
        si_->copyState(motion->state, xstate);
        motion->parent = existing;
        motion->root = existing->root;
        existing->children.push_back(motion);
        projectionEvaluator_->computeCoordinates(motion->state, xcoord);
        disc.addMotion(motion, xcoord);

        /* attempt to connect trees */
        Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
        if (ocell && !ocell->data->motions.empty())
        {
            Motion* connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];

            if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root))
            {
                Motion* connect = new Motion(si_);
                si_->copyState(connect->state, connectOther->state);
                connect->parent = motion;
                connect->root = motion->root;
                motion->children.push_back(connect);
                projectionEvaluator_->computeCoordinates(connect->state, xcoord);
                disc.addMotion(connect, xcoord);

                if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
                {
                    if (startTree)
                        connectionPoint_ = std::make_pair(connectOther->state, motion->state);
                    else
                        connectionPoint_ = std::make_pair(motion->state, connectOther->state);

                    /* extract the motions and put them in solution vector */

                    std::vector<Motion*> mpath1;
                    while (motion != NULL)
                    {
                        mpath1.push_back(motion);
                        motion = motion->parent;
                    }

                    std::vector<Motion*> mpath2;
                    while (connectOther != NULL)
                    {
                        mpath2.push_back(connectOther);
                        connectOther = connectOther->parent;
                    }

                    if (startTree)
                        mpath1.swap(mpath2);

                    PathGeometric *path = new PathGeometric(si_);
                    path->getStates().reserve(mpath1.size() + mpath2.size());
                    for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
                        path->append(mpath1[i]->state);
                    for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
                        path->append(mpath2[i]->state);

                    pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
                    solved = true;
                    break;
                }
            }
        }
    }

    si_->freeState(xstate);

    OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
                getName().c_str(),
                dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
                dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
                dGoal_.getCellCount(), dGoal_.getGrid().countExternal());

    return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}

bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, const base::PlannerTerminationCondition &ptc,
                                                     unsigned int samplingAttempts, double rangeRatio, double snapToVertex)
{
    if (path.getStateCount() < 2)
        return false;

    if (!gsr_)
    {
        OMPL_WARN("%s: No goal sampleable object to sample a better goal from.", "PathSimplifier::findBetterGoal");
        return false;
    }

    unsigned int maxGoals = std::min((unsigned)10, gsr_->maxSampleCount()); // the number of goals we will sample
    unsigned int failedTries = 0;
    bool betterGoal = false;

    const base::StateSpacePtr& ss = si_->getStateSpace();
    std::vector<base::State*> &states = path.getStates();

    // dists[i] contains the cumulative length of the path up to and including state i
    std::vector<double> dists(states.size(), 0.0);
    for (unsigned int i = 1 ; i < dists.size() ; ++i)
        dists[i] = dists[i-1] + si_->distance(states[i-1], states[i]);

    // Sampled states closer than 'threshold' distance to any existing state in the path
    // are snapped to the close state
    double threshold = dists.back() * snapToVertex;
    // The range (distance) of a single connection that will be attempted
    double rd = rangeRatio * dists.back();

    base::State* temp = si_->allocState();
    base::State* tempGoal = si_->allocState();

    while(!ptc && failedTries++ < maxGoals && !betterGoal)
    {
        gsr_->sampleGoal(tempGoal);

        // Goal state is not compatible with the start state
        if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
            continue;

        unsigned int numSamples = 0;
        while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
        {
            // sample a state within rangeRatio
            double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0), dists.back());    // Sample a random point within rd of the end of the path

            std::vector<double>::iterator end = std::lower_bound(dists.begin(), dists.end(), t);
            std::vector<double>::iterator start = end;
            while(start != dists.begin() && *start >= t)
                start -= 1;

            unsigned int startIndex = start - dists.begin();
            unsigned int endIndex = end - dists.begin();

            // Snap the random point to the nearest vertex, if within the threshold
            if (t - (*start) < threshold) // snap to the starting waypoint
                endIndex = startIndex;
            if ((*end) - t < threshold)  // snap to the ending waypoint
                startIndex = endIndex;

            // Compute the state value and the accumulated cost up to that state
            double costToCome = dists[startIndex];
            base::State* state;
            if (startIndex == endIndex)
            {
                state = states[startIndex];
            }
            else
            {
                double tSeg = (t - (*start)) / (*end - *start);
                ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
                state = temp;

                costToCome += si_->distance(states[startIndex], state);
            }

            double costToGo = si_->distance(state, tempGoal);
            double candidateCost = costToCome + costToGo;

            // Make sure we improve before attempting validation
            if (dists.back() - candidateCost > std::numeric_limits<float>::epsilon() && si_->checkMotion(state, tempGoal))
            {
                // insert the new states
                if (startIndex == endIndex)
                {
                    // new intermediate state
                    si_->copyState(states[startIndex], state);
                    // new goal state
                    si_->copyState(states[startIndex+1], tempGoal);

                    if (freeStates_)
                    {
                        for(size_t i = startIndex + 2; i < states.size(); ++i)
                            si_->freeState(states[i]);
                    }
                    states.erase(states.begin() + startIndex + 2, states.end());
                }
                else
                {
//.........这里部分代码省略.........

bool ompl::geometric::LazyRRT::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    base::Goal                 *goal   = pdef_->getGoal().get();
    base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);

    while (const base::State *st = pis_.nextStart())
    {
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, st);
        motion->valid = true;
        nn_->add(motion);
    }

    if (nn_->size() == 0)
    {
        msg_.error("There are no valid initial states!");
        return false;
    }

    if (!sampler_)
        sampler_ = si_->allocStateSampler();

    msg_.inform("Starting with %u states", nn_->size());

    Motion *solution = NULL;
    double  distsol  = -1.0;
    Motion *rmotion  = new Motion(si_);
    base::State *rstate = rmotion->state;
    base::State *xstate = si_->allocState();

 RETRY:

    while (ptc() == false)
    {
        /* sample random state (with goal biasing) */
        if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
            goal_s->sampleGoal(rstate);
        else
            sampler_->sampleUniform(rstate);

        /* find closest state in the tree */
        Motion *nmotion = nn_->nearest(rmotion);
        assert(nmotion != rmotion);
        base::State *dstate = rstate;

        /* find state to add */
        double d = si_->distance(nmotion->state, rstate);
        if (d > maxDistance_)
        {
            si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
            dstate = xstate;
        }

        /* create a motion */
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, dstate);
        motion->parent = nmotion;
        nmotion->children.push_back(motion);
        nn_->add(motion);

        double dist = 0.0;
        if (goal->isSatisfied(motion->state, &dist))
        {
            distsol = dist;
            solution = motion;
            break;
        }
    }

    bool solved = false;
    if (solution != NULL)
    {
        /* construct the solution path */
        std::vector<Motion*> mpath;
        while (solution != NULL)
        {
            mpath.push_back(solution);
            solution = solution->parent;
        }

        /* check the path */
        for (int i = mpath.size() - 1 ; i >= 0 ; --i)
            if (!mpath[i]->valid)
            {
                if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
                    mpath[i]->valid = true;
                else
                {
                    removeMotion(mpath[i]);
                    goto RETRY;
                }
            }

        /* set the solution path */
        PathGeometric *path = new PathGeometric(si_);
        for (int i = mpath.size() - 1 ; i >= 0 ; --i)
            path->append(mpath[i]->state);

        goal->addSolutionPath(base::PathPtr(path), false, distsol);
//.........这里部分代码省略.........