ompl/geometric/src/SimpleSetup.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
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 Rice University 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/geometric/SimpleSetup.h"
00038 #include "ompl/tools/config/SelfConfig.h"
00039 
00040 ompl::base::PlannerPtr ompl::geometric::getDefaultPlanner(const base::GoalPtr &goal)
00041 {
00042     return tools::SelfConfig::getDefaultPlanner(goal);
00043 }
00044 
00045 ompl::geometric::SimpleSetup::SimpleSetup(const base::SpaceInformationPtr &si) :
00046     configured_(false), planTime_(0.0), simplifyTime_(0.0), lastStatus_(base::PlannerStatus::UNKNOWN)
00047 {
00048     si_ = si;
00049     pdef_.reset(new base::ProblemDefinition(si_));
00050     psk_.reset(new PathSimplifier(si_));
00051 }
00052 
00053 ompl::geometric::SimpleSetup::SimpleSetup(const base::StateSpacePtr &space) :
00054     configured_(false), planTime_(0.0), simplifyTime_(0.0), lastStatus_(base::PlannerStatus::UNKNOWN)
00055 {
00056     si_.reset(new base::SpaceInformation(space));
00057     pdef_.reset(new base::ProblemDefinition(si_));
00058     psk_.reset(new PathSimplifier(si_));
00059 }
00060 
00061 void ompl::geometric::SimpleSetup::setup()
00062 {
00063     if (!configured_ || !si_->isSetup() || !planner_->isSetup())
00064     {
00065         if (!si_->isSetup())
00066             si_->setup();
00067         if (!planner_)
00068         {
00069             if (pa_)
00070                 planner_ = pa_(si_);
00071             if (!planner_)
00072             {
00073                 OMPL_INFORM("No planner specified. Using default.");
00074                 planner_ = tools::SelfConfig::getDefaultPlanner(getGoal());
00075             }
00076         }
00077         planner_->setProblemDefinition(pdef_);
00078         if (!planner_->isSetup())
00079             planner_->setup();
00080         configured_ = true;
00081     }
00082 }
00083 
00084 void ompl::geometric::SimpleSetup::clear()
00085 {
00086     if (planner_)
00087         planner_->clear();
00088     if (pdef_)
00089         pdef_->clearSolutionPaths();
00090 }
00091 
00092 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner termination condition
00093 ompl::base::PlannerStatus ompl::geometric::SimpleSetup::solve(double time)
00094 {
00095     setup();
00096     lastStatus_ = base::PlannerStatus::UNKNOWN;
00097     time::point start = time::now();
00098     lastStatus_ = planner_->solve(time);
00099     planTime_ = time::seconds(time::now() - start);
00100     if (lastStatus_)
00101         OMPL_INFORM("Solution found in %f seconds", planTime_);
00102     else
00103         OMPL_INFORM("No solution found after %f seconds", planTime_);
00104     return lastStatus_;
00105 }
00106 
00107 ompl::base::PlannerStatus ompl::geometric::SimpleSetup::solve(const base::PlannerTerminationCondition &ptc)
00108 {
00109     setup();
00110     lastStatus_ = base::PlannerStatus::UNKNOWN;
00111     time::point start = time::now();
00112     lastStatus_ = planner_->solve(ptc);
00113     planTime_ = time::seconds(time::now() - start);
00114     if (lastStatus_)
00115         OMPL_INFORM("Solution found in %f seconds", planTime_);
00116     else
00117         OMPL_INFORM("No solution found after %f seconds", planTime_);
00118     return lastStatus_;
00119 }
00120 
00121 void ompl::geometric::SimpleSetup::simplifySolution(const base::PlannerTerminationCondition &ptc)
00122 {
00123     if (pdef_)
00124     {
00125         const base::PathPtr &p = pdef_->getSolutionPath();
00126         if (p)
00127         {
00128             time::point start = time::now();
00129             PathGeometric &path = static_cast<PathGeometric&>(*p);
00130             std::size_t numStates = path.getStateCount();
00131             psk_->simplify(path, ptc);
00132             simplifyTime_ = time::seconds(time::now() - start);
00133             OMPL_INFORM("SimpleSetup: Path simplification took %f seconds and changed from %d to %d states",
00134                         simplifyTime_, numStates, path.getStateCount());
00135             return;
00136         }
00137     }
00138     OMPL_WARN("No solution to simplify");
00139 }
00140 
00141 void ompl::geometric::SimpleSetup::simplifySolution(double duration)
00142 {
00143     if (pdef_)
00144     {
00145         const base::PathPtr &p = pdef_->getSolutionPath();
00146         if (p)
00147         {
00148             time::point start = time::now();
00149             PathGeometric &path = static_cast<PathGeometric&>(*p);
00150             std::size_t numStates = path.getStateCount();
00151             if (duration < std::numeric_limits<double>::epsilon())
00152                 psk_->simplifyMax(static_cast<PathGeometric&>(*p));
00153             else
00154                 psk_->simplify(static_cast<PathGeometric&>(*p), duration);
00155             simplifyTime_ = time::seconds(time::now() - start);
00156             OMPL_INFORM("SimpleSetup: Path simplification took %f seconds and changed from %d to %d states",
00157                         simplifyTime_, numStates, path.getStateCount());
00158             return;
00159         }
00160     }
00161     OMPL_WARN("No solution to simplify");
00162 }
00163 
00164 const std::string ompl::geometric::SimpleSetup::getSolutionPlannerName(void) const
00165 {
00166     if (pdef_)
00167     {
00168         const ompl::base::PathPtr path; // convert to a generic path ptr
00169         ompl::base::PlannerSolution solution(path); // a dummy solution
00170 
00171         // Get our desired solution
00172         pdef_->getSolution(solution);
00173         return solution.plannerName_;
00174     }
00175     throw Exception("No problem definition found");
00176 }
00177 
00178 ompl::geometric::PathGeometric& ompl::geometric::SimpleSetup::getSolutionPath() const
00179 {
00180     if (pdef_)
00181     {
00182         const base::PathPtr &p = pdef_->getSolutionPath();
00183         if (p)
00184             return static_cast<PathGeometric&>(*p);
00185     }
00186     throw Exception("No solution path");
00187 }
00188 
00189 bool ompl::geometric::SimpleSetup::haveExactSolutionPath() const
00190 {
00191     return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
00192 }
00193 
00194 void ompl::geometric::SimpleSetup::getPlannerData(base::PlannerData &pd) const
00195 {
00196     pd.clear();
00197     if (planner_)
00198         planner_->getPlannerData(pd);
00199 }
00200 
00201 void ompl::geometric::SimpleSetup::print(std::ostream &out) const
00202 {
00203     if (si_)
00204     {
00205         si_->printProperties(out);
00206         si_->printSettings(out);
00207     }
00208     if (planner_)
00209     {
00210         planner_->printProperties(out);
00211         planner_->printSettings(out);
00212     }
00213     if (pdef_)
00214         pdef_->print(out);
00215 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines