All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/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 
00040 void ompl::tools::OptimizePlan::addPlanner(const base::PlannerPtr &planner)
00041 {
00042     if (planner && planner->getSpaceInformation().get() != getProblemDefinition()->getSpaceInformation().get())
00043         throw Exception("Planner instance does not match space information");
00044     planners_.push_back(planner);
00045 }
00046 
00047 void ompl::tools::OptimizePlan::addPlannerAllocator(const base::PlannerAllocator &pa)
00048 {
00049     planners_.push_back(pa(getProblemDefinition()->getSpaceInformation()));
00050 }
00051 
00052 void ompl::tools::OptimizePlan::clearPlanners(void)
00053 {
00054     planners_.clear();
00055 }
00056 
00057 ompl::base::PlannerStatus ompl::tools::OptimizePlan::solve(double solveTime, unsigned int maxSol, unsigned int nthreads)
00058 {
00059     time::point end = time::now() + time::seconds(solveTime);
00060     unsigned int nt = std::min(nthreads, (unsigned int)planners_.size());
00061     logDebug("Using %u threads", nt);
00062 
00063     base::PlannerStatus result;
00064     unsigned int np = 0;
00065     const base::ProblemDefinitionPtr &pdef = getProblemDefinition();
00066     const base::GoalPtr &goal = pdef->getGoal();
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->getSolutionPath()->length() <= goal->getMaximumPathLength())
00085             {
00086                 logDebug("Terminating early since solution path is shorted than the maximum path length");
00087                 break;
00088             }
00089             if (pdef->getSolutionCount() >= maxSol)
00090             {
00091                 logDebug("Terminating early since %u solutions were generated", maxSol);
00092                 break;
00093             }
00094         }
00095     }
00096 
00097     // if we have more time, and we have a geometric path, we try to simplify it
00098     if (time::now() < end && result)
00099     {
00100         geometric::PathGeometric *p = dynamic_cast<geometric::PathGeometric*>(pdef->getSolutionPath().get());
00101         if (p)
00102         {
00103             geometric::PathSimplifier ps(getProblemDefinition()->getSpaceInformation());
00104             ps.simplify(*p, std::max(time::seconds(end - time::now()), 0.0));
00105         }
00106     }
00107 
00108     return result;
00109 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines