All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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 bool 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     msg_.debug("Using %u threads", nt);
00062 
00063     bool result = false;
00064     unsigned int np = 0;
00065     const base::GoalPtr &goal = getProblemDefinition()->getGoal();
00066     pp_.clearHybridizationPaths();
00067 
00068     while (time::now() < end)
00069     {
00070         pp_.clearPlanners();
00071         for (unsigned int i = 0 ; i < nt ; ++i)
00072         {
00073             planners_[np]->clear();
00074             pp_.addPlanner(planners_[np]);
00075             np = (np + 1) % planners_.size();
00076         }
00077         if (pp_.solve(std::max(time::seconds(end - time::now()), 0.0), true))
00078         {
00079             result = true;
00080             if (goal->getSolutionPath()->length() <= goal->getMaximumPathLength())
00081             {
00082                 msg_.debug("Terminating early since solution path is shorted than the maximum path length");
00083                 break;
00084             }
00085             if (goal->getSolutionCount() >= maxSol)
00086             {
00087                 msg_.debug("Terminating early since %u solutions were generated", maxSol);
00088                 break;
00089             }
00090         }
00091     }
00092 
00093     // if we have more time, and we have a geometric path, we try to simplify it
00094     if (time::now() < end && result)
00095     {
00096         geometric::PathGeometric *p = dynamic_cast<geometric::PathGeometric*>(goal->getSolutionPath().get());
00097         if (p)
00098         {
00099             geometric::PathSimplifier ps(getProblemDefinition()->getSpaceInformation());
00100             ps.simplify(*p, std::max(time::seconds(end - time::now()), 0.0));
00101         }
00102     }
00103 
00104     return result;
00105 }