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