AnytimePathShortening.cpp

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ryan Luna */

#include "ompl/geometric/planners/AnytimePathShortening.h"
#include "ompl/geometric/PathHybridization.h"
#include "ompl/geometric/PathSimplifier.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/base/objectives/PathLengthOptimizationObjective.h"

#include <thread>

ompl::geometric::AnytimePathShortening::AnytimePathShortening(const ompl::base::SpaceInformationPtr &si)
  : ompl::base::Planner(si, "APS")
  , defaultNumPlanners_(std::max(1u, std::thread::hardware_concurrency()))
{
    specs_.approximateSolutions = true;
    specs_.multithreaded = true;
    specs_.optimizingPaths = true;

    Planner::declareParam<bool>("shortcut", this, &AnytimePathShortening::setShortcut,
                                &AnytimePathShortening::isShortcutting, "0,1");
    Planner::declareParam<bool>("hybridize", this, &AnytimePathShortening::setHybridize,
                                &AnytimePathShortening::isHybridizing, "0,1");
    Planner::declareParam<unsigned int>("max_hybrid_paths", this, &AnytimePathShortening::setMaxHybridizationPath,
                                        &AnytimePathShortening::maxHybridizationPaths, "0:1:50");
    Planner::declareParam<unsigned int>("num_planners", this, &AnytimePathShortening::setDefaultNumPlanners,
                                        &AnytimePathShortening::getDefaultNumPlanners, "0:64");

    addPlannerProgressProperty("best cost REAL", [this]
                               {
                                   return getBestCost();
                               });
}

ompl::geometric::AnytimePathShortening::~AnytimePathShortening() = default;

void ompl::geometric::AnytimePathShortening::addPlanner(base::PlannerPtr &planner)
{
    if (planner && planner->getSpaceInformation().get() != si_.get())
    {
        OMPL_ERROR("NOT adding planner %s: SpaceInformation instances are different", planner->getName().c_str());
        return;
    }

    // Ensure all planners are unique instances
    for (auto &i : planners_)
    {
        if (planner.get() == i.get())
        {
            OMPL_ERROR("NOT adding planner %s: Planner instances MUST be unique", planner->getName().c_str());
            return;
        }
    }

    planners_.push_back(planner);
}

void ompl::geometric::AnytimePathShortening::setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef)
{
    ompl::base::Planner::setProblemDefinition(pdef);
    for (auto &planner : planners_)
        planner->setProblemDefinition(pdef);
}

ompl::base::PlannerStatus
ompl::geometric::AnytimePathShortening::solve(const ompl::base::PlannerTerminationCondition &ptc)
{
    base::Goal *goal = pdef_->getGoal().get();
    std::vector<std::thread *> threads(planners_.size());
    geometric::PathHybridization phybrid(si_);
    base::Path *bestSln = nullptr;

    base::OptimizationObjectivePtr opt = pdef_->getOptimizationObjective();
    if (!opt)
    {
        OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
                    "planning time.",
                    getName().c_str());
        opt = std::make_shared<base::PathLengthOptimizationObjective>(si_);
        pdef_->setOptimizationObjective(opt);
    }
    else
    {
        if (dynamic_cast<base::PathLengthOptimizationObjective *>(opt.get()) == nullptr)
            OMPL_WARN("The optimization objective is not set for path length.  The specified optimization criteria may "
                      "not be optimized over.");
    }

    // Disable output from the motion planners, except for errors
    msg::LogLevel currentLogLevel = msg::getLogLevel();
    msg::setLogLevel(std::max(msg::LOG_ERROR, currentLogLevel));
    while (!ptc())
    {
        // We have found a solution that is good enough
        if ((bestSln != nullptr) && opt->isSatisfied(base::Cost(bestSln->length())))
            break;

        // Clear any previous planning data for the set of planners
        clear();

        // Spawn a thread for each planner.  This will shortcut the best path after solving.
        for (size_t i = 0; i < threads.size(); ++i)
            threads[i] = new std::thread([this, i, &ptc]
                                         {
                                             return threadSolve(planners_[i].get(), ptc);
                                         });

        // Join each thread, and then delete it
        for (auto &thread : threads)
        {
            thread->join();
            delete thread;
        }

        // Hybridize the set of paths computed.  Add the new hybrid path to the mix.
        if (hybridize_ && !ptc())
        {
            const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
            for (size_t j = 0; j < paths.size() && j < maxHybridPaths_ && !ptc(); ++j)
                phybrid.recordPath(paths[j].path_, false);

            phybrid.computeHybridPath();
            const base::PathPtr &hsol = phybrid.getHybridPath();
            if (hsol)
            {
                auto *pg = static_cast<geometric::PathGeometric *>(hsol.get());
                double difference = 0.0;
                bool approximate = !goal->isSatisfied(pg->getStates().back(), &difference);
                pdef_->addSolutionPath(hsol, approximate, difference);
            }
        }

        // keep track of the best solution found so far
        if (pdef_->getSolutionCount() > 0)
            bestSln = pdef_->getSolutionPath().get();
    }
    msg::setLogLevel(currentLogLevel);

    if (bestSln != nullptr)
    {
        if (goal->isSatisfied(static_cast<geometric::PathGeometric *>(bestSln)->getStates().back()))
            return base::PlannerStatus::EXACT_SOLUTION;
        return base::PlannerStatus::APPROXIMATE_SOLUTION;
    }
    return base::PlannerStatus::UNKNOWN;
}

void ompl::geometric::AnytimePathShortening::threadSolve(base::Planner *planner,
                                                         const base::PlannerTerminationCondition &ptc)
{
    // compute a motion plan
    base::PlannerStatus status = planner->solve(ptc);

    // Shortcut the best solution found so far
    if (shortcut_ && status == base::PlannerStatus::EXACT_SOLUTION)
    {
        geometric::PathGeometric *sln = static_cast<geometric::PathGeometric *>(pdef_->getSolutionPath().get());
        auto pathCopy(std::make_shared<geometric::PathGeometric>(*sln));
        geometric::PathSimplifier ps(pdef_->getSpaceInformation());
        if (ps.shortcutPath(*pathCopy))
        {
            double difference = 0.0;
            bool approximate = !pdef_->getGoal()->isSatisfied(pathCopy->getStates().back(), &difference);
            pdef_->addSolutionPath(pathCopy, approximate, difference);
        }
    }
}

void ompl::geometric::AnytimePathShortening::clear()
{
    Planner::clear();
    for (auto &planner : planners_)
        planner->clear();
}

void ompl::geometric::AnytimePathShortening::getPlannerData(ompl::base::PlannerData &data) const
{
    if (planners_.empty())
        return;

    OMPL_WARN("Returning planner data for planner #0");
    getPlannerData(data, 0);
}

void ompl::geometric::AnytimePathShortening::getPlannerData(ompl::base::PlannerData &data, unsigned int idx) const
{
    if (planners_.size() < idx)
        return;
    planners_[idx]->getPlannerData(data);
}

void ompl::geometric::AnytimePathShortening::setup()
{
    Planner::setup();

    if (planners_.empty())
    {
        planners_.reserve(defaultNumPlanners_);
        for (unsigned int i = 0; i < defaultNumPlanners_; ++i)
        {
            planners_.push_back(tools::SelfConfig::getDefaultPlanner(pdef_->getGoal()));
            planners_.back()->setProblemDefinition(pdef_);
        }
        OMPL_INFORM("%s: No planners specified; using %u instances of %s", getName().c_str(), planners_.size(),
                    planners_[0]->getName().c_str());
    }

    for (auto &planner : planners_)
        planner->setup();
}

void ompl::geometric::AnytimePathShortening::checkValidity()
{
    for (auto &planner : planners_)
        planner->checkValidity();
}

unsigned int ompl::geometric::AnytimePathShortening::getNumPlanners() const
{
    return planners_.size();
}

ompl::base::PlannerPtr ompl::geometric::AnytimePathShortening::getPlanner(unsigned int idx) const
{
    assert(idx < planners_.size());
    return planners_[idx];
}

bool ompl::geometric::AnytimePathShortening::isShortcutting() const
{
    return shortcut_;
}

void ompl::geometric::AnytimePathShortening::setShortcut(bool shortcut)
{
    shortcut_ = shortcut;
}

bool ompl::geometric::AnytimePathShortening::isHybridizing() const
{
    return hybridize_;
}

void ompl::geometric::AnytimePathShortening::setHybridize(bool hybridize)
{
    hybridize_ = hybridize;
}

unsigned int ompl::geometric::AnytimePathShortening::maxHybridizationPaths() const
{
    return maxHybridPaths_;
}

void ompl::geometric::AnytimePathShortening::setMaxHybridizationPath(unsigned int maxPathCount)
{
    maxHybridPaths_ = maxPathCount;
}

void ompl::geometric::AnytimePathShortening::setDefaultNumPlanners(unsigned int numPlanners)
{
    defaultNumPlanners_ = numPlanners;
}

unsigned int ompl::geometric::AnytimePathShortening::getDefaultNumPlanners() const
{
    return defaultNumPlanners_;
}

std::string ompl::geometric::AnytimePathShortening::getBestCost() const
{
    base::Cost bestCost(std::numeric_limits<double>::quiet_NaN());
    if (pdef_ && pdef_->getSolutionCount() > 0)
        bestCost = base::Cost(pdef_->getSolutionPath()->length());
    return std::to_string(bestCost.value());
}