ompl/tools/multiplan/src/OptimizePlan.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include "ompl/tools/multiplan/OptimizePlan.h"
00038 #include "ompl/geometric/PathSimplifier.h"
00039 #include "ompl/base/OptimizationObjective.h"
00040 
00041 void ompl::tools::OptimizePlan::addPlanner(const base::PlannerPtr &planner)
00042 {
00043     if (planner && planner->getSpaceInformation().get() != getProblemDefinition()->getSpaceInformation().get())
00044         throw Exception("Planner instance does not match space information");
00045     planners_.push_back(planner);
00046 }
00047 
00048 void ompl::tools::OptimizePlan::addPlannerAllocator(const base::PlannerAllocator &pa)
00049 {
00050     planners_.push_back(pa(getProblemDefinition()->getSpaceInformation()));
00051 }
00052 
00053 void ompl::tools::OptimizePlan::clearPlanners()
00054 {
00055     planners_.clear();
00056 }
00057 
00058 ompl::base::PlannerStatus ompl::tools::OptimizePlan::solve(double solveTime, unsigned int maxSol, unsigned int nthreads)
00059 {
00060     time::point end = time::now() + time::seconds(solveTime);
00061     unsigned int nt = std::min(nthreads, (unsigned int)planners_.size());
00062     OMPL_DEBUG("Using %u threads", nt);
00063 
00064     base::PlannerStatus result;
00065     unsigned int np = 0;
00066     const base::ProblemDefinitionPtr &pdef = getProblemDefinition();
00067     pp_.clearHybridizationPaths();
00068 
00069     while (time::now() < end)
00070     {
00071         pp_.clearPlanners();
00072         for (unsigned int i = 0 ; i < nt ; ++i)
00073         {
00074             planners_[np]->clear();
00075             pp_.addPlanner(planners_[np]);
00076             np = (np + 1) % planners_.size();
00077         }
00078         base::PlannerStatus localResult = pp_.solve(std::max(time::seconds(end - time::now()), 0.0), true);
00079         if (localResult)
00080         {
00081             if (result != base::PlannerStatus::EXACT_SOLUTION)
00082                 result = localResult;
00083 
00084             if (!pdef->hasOptimizationObjective())
00085             {
00086                 OMPL_DEBUG("Terminating early since there is no optimization objective specified");
00087                 break;
00088             }
00089 
00090             base::Cost obj_cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective());
00091 
00092             if (pdef->getOptimizationObjective()->isSatisfied(obj_cost))
00093             {
00094                 OMPL_DEBUG("Terminating early since solution path satisfies the optimization objective");
00095                 break;
00096             }
00097             if (pdef->getSolutionCount() >= maxSol)
00098             {
00099                 OMPL_DEBUG("Terminating early since %u solutions were generated", maxSol);
00100                 break;
00101             }
00102         }
00103     }
00104 
00105     // if we have more time, and we have a geometric path, we try to simplify it
00106     if (time::now() < end && result)
00107     {
00108         geometric::PathGeometric *p = dynamic_cast<geometric::PathGeometric*>(pdef->getSolutionPath().get());
00109         if (p)
00110         {
00111             geometric::PathSimplifier ps(getProblemDefinition()->getSpaceInformation());
00112             ps.simplify(*p, std::max(time::seconds(end - time::now()), 0.0));
00113         }
00114     }
00115 
00116     return result;
00117 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines