ompl/base/src/Planner.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/base/Planner.h"
00038 #include "ompl/util/Exception.h"
00039 #include "ompl/base/goals/GoalSampleableRegion.h"
00040 #include <sstream>
00041 #include <boost/thread.hpp>
00042 
00043 ompl::base::Planner::Planner(const SpaceInformationPtr &si, const std::string &name) :
00044     si_(si), pis_(this), name_(name), setup_(false)
00045 {
00046     if (!si_)
00047         throw Exception(name_, "Invalid space information instance for planner");
00048 }
00049 
00050 const ompl::base::PlannerSpecs& ompl::base::Planner::getSpecs() const
00051 {
00052     return specs_;
00053 }
00054 
00055 const std::string& ompl::base::Planner::getName() const
00056 {
00057     return name_;
00058 }
00059 
00060 void ompl::base::Planner::setName(const std::string &name)
00061 {
00062     name_ = name;
00063 }
00064 
00065 const ompl::base::SpaceInformationPtr&  ompl::base::Planner::getSpaceInformation() const
00066 {
00067     return si_;
00068 }
00069 
00070 const ompl::base::ProblemDefinitionPtr& ompl::base::Planner::getProblemDefinition() const
00071 {
00072     return pdef_;
00073 }
00074 
00075 void ompl::base::Planner::setProblemDefinition(const ProblemDefinitionPtr &pdef)
00076 {
00077     pdef_ = pdef;
00078     pis_.update();
00079 }
00080 
00081 const ompl::base::PlannerInputStates& ompl::base::Planner::getPlannerInputStates() const
00082 {
00083     return pis_;
00084 }
00085 
00086 void ompl::base::Planner::setup()
00087 {
00088     if (!si_->isSetup())
00089     {
00090         OMPL_INFORM("%s: Space information setup was not yet called. Calling now.", getName().c_str());
00091         si_->setup();
00092     }
00093 
00094     if (setup_)
00095         OMPL_WARN("%s: Planner setup called multiple times", getName().c_str());
00096     else
00097         setup_ = true;
00098 }
00099 
00100 void ompl::base::Planner::checkValidity()
00101 {
00102     if (!isSetup())
00103         setup();
00104     pis_.checkValidity();
00105 }
00106 
00107 bool ompl::base::Planner::isSetup() const
00108 {
00109     return setup_;
00110 }
00111 
00112 void ompl::base::Planner::clear()
00113 {
00114     pis_.clear();
00115     pis_.update();
00116 }
00117 
00118 void ompl::base::Planner::getPlannerData(PlannerData &data) const
00119 {
00120     for (PlannerProgressProperties::const_iterator it = plannerProgressProperties_.begin() ; it != plannerProgressProperties_.end() ; ++it)
00121         data.properties[it->first] = it->second();
00122 }
00123 
00124 ompl::base::PlannerStatus ompl::base::Planner::solve(const PlannerTerminationConditionFn &ptc, double checkInterval)
00125 {
00126     return solve(PlannerTerminationCondition(ptc, checkInterval));
00127 }
00128 
00129 ompl::base::PlannerStatus ompl::base::Planner::solve(double solveTime)
00130 {
00131     if (solveTime < 1.0)
00132         return solve(timedPlannerTerminationCondition(solveTime));
00133     else
00134         return solve(timedPlannerTerminationCondition(solveTime, std::min(solveTime / 100.0, 0.1)));
00135 }
00136 
00137 void ompl::base::Planner::printProperties(std::ostream &out) const
00138 {
00139     out << "Planner " + getName() + " specs:" << std::endl;
00140     out << "Multithreaded:                 " << (getSpecs().multithreaded ? "Yes" : "No") << std::endl;
00141     out << "Reports approximate solutions: " << (getSpecs().approximateSolutions ? "Yes" : "No") << std::endl;
00142     out << "Can optimize solutions:        " << (getSpecs().optimizingPaths ? "Yes" : "No") << std::endl;
00143     out << "Aware of the following parameters:";
00144     std::vector<std::string> params;
00145     params_.getParamNames(params);
00146     for (unsigned int i = 0 ; i < params.size() ; ++i)
00147         out << " " << params[i];
00148     out << std::endl;
00149 }
00150 
00151 void ompl::base::Planner::printSettings(std::ostream &out) const
00152 {
00153     out << "Declared parameters for planner " << getName() << ":" << std::endl;
00154     params_.print(out);
00155 }
00156 
00157 void ompl::base::PlannerInputStates::clear()
00158 {
00159     if (tempState_)
00160     {
00161         si_->freeState(tempState_);
00162         tempState_ = NULL;
00163     }
00164     addedStartStates_ = 0;
00165     sampledGoalsCount_ = 0;
00166     pdef_ = NULL;
00167     si_ = NULL;
00168 }
00169 
00170 void ompl::base::PlannerInputStates::restart()
00171 {
00172     addedStartStates_ = 0;
00173     sampledGoalsCount_ = 0;
00174 }
00175 
00176 bool ompl::base::PlannerInputStates::update()
00177 {
00178     if (!planner_)
00179         throw Exception("No planner set for PlannerInputStates");
00180     return use(planner_->getProblemDefinition());
00181 }
00182 
00183 void ompl::base::PlannerInputStates::checkValidity() const
00184 {
00185     std::string error;
00186 
00187     if (!pdef_)
00188         error = "Problem definition not specified";
00189     else
00190     {
00191         if (pdef_->getStartStateCount() <= 0)
00192             error = "No start states specified";
00193         else
00194             if (!pdef_->getGoal())
00195                 error = "No goal specified";
00196     }
00197 
00198     if (!error.empty())
00199     {
00200         if (planner_)
00201             throw Exception(planner_->getName(), error);
00202         else
00203             throw Exception(error);
00204     }
00205 }
00206 
00207 bool ompl::base::PlannerInputStates::use(const ProblemDefinitionPtr &pdef)
00208 {
00209     if (pdef)
00210         return use(pdef.get());
00211     else
00212     {
00213         clear();
00214         return true;
00215     }
00216 }
00217 
00218 bool ompl::base::PlannerInputStates::use(const ProblemDefinition *pdef)
00219 {
00220     if (pdef_ != pdef)
00221     {
00222         clear();
00223         pdef_ = pdef;
00224         si_ = pdef->getSpaceInformation().get();
00225         return true;
00226     }
00227     return false;
00228 }
00229 
00230 const ompl::base::State* ompl::base::PlannerInputStates::nextStart()
00231 {
00232     if (pdef_ == NULL || si_ == NULL)
00233     {
00234         std::string error = "Missing space information or problem definition";
00235         if (planner_)
00236             throw Exception(planner_->getName(), error);
00237         else
00238             throw Exception(error);
00239     }
00240 
00241     while (addedStartStates_ < pdef_->getStartStateCount())
00242     {
00243         const base::State *st = pdef_->getStartState(addedStartStates_);
00244         addedStartStates_++;
00245         bool bounds = si_->satisfiesBounds(st);
00246         bool valid = bounds ? si_->isValid(st) : false;
00247         if (bounds && valid)
00248             return st;
00249         else
00250         {
00251             OMPL_WARN("%s: Skipping invalid start state (invalid %s)",
00252                       planner_ ? planner_->getName().c_str() : "PlannerInputStates",
00253                       bounds ? "state": "bounds");
00254             std::stringstream ss;
00255             si_->printState(st, ss);
00256             OMPL_DEBUG("%s: Discarded start state %s",
00257                        planner_ ? planner_->getName().c_str() : "PlannerInputStates",
00258                        ss.str().c_str());
00259         }
00260     }
00261     return NULL;
00262 }
00263 
00264 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal()
00265 {
00266     // This initialization is safe since we are in a non-const function anyway.
00267     static PlannerTerminationCondition ptc = plannerAlwaysTerminatingCondition();
00268     return nextGoal(ptc);
00269 }
00270 
00271 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(const PlannerTerminationCondition &ptc)
00272 {
00273     if (pdef_ == NULL || si_ == NULL)
00274     {
00275         std::string error = "Missing space information or problem definition";
00276         if (planner_)
00277             throw Exception(planner_->getName(), error);
00278         else
00279             throw Exception(error);
00280     }
00281 
00282     const GoalSampleableRegion *goal = pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : NULL;
00283 
00284     if (goal)
00285     {
00286         time::point start_wait;
00287         bool first = true;
00288         bool attempt = true;
00289         while (attempt)
00290         {
00291             attempt = false;
00292 
00293             if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
00294             {
00295                 if (tempState_ == NULL)
00296                     tempState_ = si_->allocState();
00297                 do
00298                 {
00299                     goal->sampleGoal(tempState_);
00300                     sampledGoalsCount_++;
00301                     bool bounds = si_->satisfiesBounds(tempState_);
00302                     bool valid = bounds ? si_->isValid(tempState_) : false;
00303                     if (bounds && valid)
00304                     {
00305                         if (!first) // if we waited, show how long
00306                         {
00307                             OMPL_DEBUG("%s: Waited %lf seconds for the first goal sample.",
00308                                        planner_ ? planner_->getName().c_str() : "PlannerInputStates",
00309                                        time::seconds(time::now() - start_wait));
00310                         }
00311                         return tempState_;
00312                     }
00313                     else
00314                     {
00315                         OMPL_WARN("%s: Skipping invalid goal state (invalid %s)",
00316                                   planner_ ? planner_->getName().c_str() : "PlannerInputStates",
00317                                   bounds ? "state": "bounds");
00318                         std::stringstream ss;
00319                         si_->printState(tempState_, ss);
00320                         OMPL_DEBUG("%s: Discarded goal state %s",
00321                                    planner_ ? planner_->getName().c_str() : "PlannerInputStates",
00322                                    ss.str().c_str());
00323                     }
00324                 }
00325                 while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
00326             }
00327             if (goal->couldSample() && !ptc)
00328             {
00329                 if (first)
00330                 {
00331                     first = false;
00332                     start_wait = time::now();
00333                     OMPL_DEBUG("%s: Waiting for goal region samples ...",
00334                                planner_ ? planner_->getName().c_str() : "PlannerInputStates");
00335                 }
00336                 boost::this_thread::sleep(time::seconds(0.01));
00337                 attempt = !ptc;
00338             }
00339         }
00340     }
00341 
00342     return NULL;
00343 }
00344 
00345 bool ompl::base::PlannerInputStates::haveMoreStartStates() const
00346 {
00347     if (pdef_)
00348         return addedStartStates_ < pdef_->getStartStateCount();
00349     return false;
00350 }
00351 
00352 bool ompl::base::PlannerInputStates::haveMoreGoalStates() const
00353 {
00354     if (pdef_ && pdef_->getGoal())
00355         if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION))
00356             return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount();
00357     return false;
00358 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines