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