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 "ompl/base/GoalLazySamples.h"
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), msg_(name)
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     msg_.setPrefix(name_);
00064 }
00065 
00066 const ompl::base::SpaceInformationPtr&  ompl::base::Planner::getSpaceInformation(void) const
00067 {
00068     return si_;
00069 }
00070 
00071 const ompl::base::ProblemDefinitionPtr& ompl::base::Planner::getProblemDefinition(void) const
00072 {
00073     return pdef_;
00074 }
00075 
00076 void ompl::base::Planner::setProblemDefinition(const ProblemDefinitionPtr &pdef)
00077 {
00078     pdef_ = pdef;
00079     pis_.update();
00080 }
00081 
00082 const ompl::base::PlannerInputStates& ompl::base::Planner::getPlannerInputStates(void) const
00083 {
00084     return pis_;
00085 }
00086 
00087 void ompl::base::Planner::setup(void)
00088 {
00089     if (!si_->isSetup())
00090     {
00091         msg_.inform("Space information setup was not yet called. Calling now.");
00092         si_->setup();
00093     }
00094 
00095     if (setup_)
00096         msg_.warn("Planner setup called multiple times");
00097     else
00098         setup_ = true;
00099 }
00100 
00101 void ompl::base::Planner::checkValidity(void)
00102 {
00103     if (!isSetup())
00104         setup();
00105     pis_.checkValidity();
00106 }
00107 
00108 bool ompl::base::Planner::isSetup(void) const
00109 {
00110     return setup_;
00111 }
00112 
00113 void ompl::base::Planner::clear(void)
00114 {
00115     pis_.clear();
00116     pis_.update();
00117 }
00118 
00119 void ompl::base::Planner::getPlannerData(PlannerData &data) const
00120 {
00121     data.si = si_;
00122 }
00123 
00124 bool ompl::base::Planner::solve(const PlannerTerminationConditionFn &ptc, double checkInterval)
00125 {
00126     return solve(PlannerThreadedTerminationCondition(ptc, checkInterval));
00127 }
00128 
00130 namespace ompl
00131 {
00132     // return true if a certain point in time has passed
00133     static bool timePassed(const time::point &endTime)
00134     {
00135         return time::now() > endTime;
00136     }
00137 }
00139 
00140 bool ompl::base::Planner::solve(double solveTime)
00141 {
00142     if (solveTime < 1.0)
00143         return solve(PlannerTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(solveTime))));
00144     else
00145         return solve(PlannerThreadedTerminationCondition(boost::bind(&timePassed, time::now() + time::seconds(solveTime)), std::min(solveTime / 100.0, 0.1)));
00146 }
00147 
00148 void ompl::base::PlannerInputStates::clear(void)
00149 {
00150     if (tempState_)
00151     {
00152         si_->freeState(tempState_);
00153         tempState_ = NULL;
00154     }
00155     addedStartStates_ = 0;
00156     sampledGoalsCount_ = 0;
00157     pdef_ = NULL;
00158     si_ = NULL;
00159 }
00160 
00161 void ompl::base::PlannerInputStates::restart(void)
00162 {
00163     addedStartStates_ = 0;
00164     sampledGoalsCount_ = 0;
00165 }
00166 
00167 bool ompl::base::PlannerInputStates::update(void)
00168 {
00169     if (!planner_)
00170         throw Exception("No planner set for PlannerInputStates");
00171     return use(planner_->getSpaceInformation(), planner_->getProblemDefinition());
00172 }
00173 
00174 void ompl::base::PlannerInputStates::checkValidity(void) const
00175 {
00176     std::string error;
00177 
00178     if (!pdef_)
00179         error = "Problem definition not specified";
00180     else
00181     {
00182         if (pdef_->getStartStateCount() <= 0)
00183             error = "No start states specified";
00184         else
00185             if (!pdef_->getGoal())
00186                 error = "No goal specified";
00187     }
00188 
00189     if (!error.empty())
00190     {
00191         if (planner_)
00192             throw Exception(planner_->getName(), error);
00193         else
00194             throw Exception(error);
00195     }
00196 }
00197 
00198 bool ompl::base::PlannerInputStates::use(const SpaceInformationPtr &si, const ProblemDefinitionPtr &pdef)
00199 {
00200     if (si && pdef)
00201         return use(si.get(), pdef.get());
00202     else
00203     {
00204         clear();
00205         return true;
00206     }
00207 }
00208 
00209 bool ompl::base::PlannerInputStates::use(const SpaceInformation *si, const ProblemDefinition *pdef)
00210 {
00211     if (pdef_ != pdef || si_ != si)
00212     {
00213         clear();
00214         pdef_ = pdef;
00215         si_ = si;
00216         return true;
00217     }
00218     return false;
00219 }
00220 
00221 const ompl::base::State* ompl::base::PlannerInputStates::nextStart(void)
00222 {
00223     if (pdef_ == NULL || si_ == NULL)
00224     {
00225         std::string error = "Missing space information or problem definition";
00226         if (planner_)
00227             throw Exception(planner_->getName(), error);
00228         else
00229             throw Exception(error);
00230     }
00231 
00232     while (addedStartStates_ < pdef_->getStartStateCount())
00233     {
00234         const base::State *st = pdef_->getStartState(addedStartStates_);
00235         addedStartStates_++;
00236         bool bounds = si_->satisfiesBounds(st);
00237         bool valid = bounds ? si_->isValid(st) : false;
00238         if (bounds && valid)
00239             return st;
00240         else
00241         {
00242             msg::Interface msg(planner_ ? planner_->getName() : "");
00243             msg.warn("Skipping invalid start state (invalid %s)", bounds ? "state": "bounds");
00244         }
00245     }
00246     return NULL;
00247 }
00248 
00249 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(void)
00250 {
00251     static PlannerAlwaysTerminatingCondition ptc;
00252     return nextGoal(ptc);
00253 }
00254 
00255 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(const PlannerTerminationCondition &ptc)
00256 {
00257     if (pdef_ == NULL || si_ == NULL)
00258     {
00259         std::string error = "Missing space information or problem definition";
00260         if (planner_)
00261             throw Exception(planner_->getName(), error);
00262         else
00263             throw Exception(error);
00264     }
00265 
00266     const GoalSampleableRegion *goal = dynamic_cast<const GoalSampleableRegion*>(pdef_->getGoal().get());
00267 
00268     if (goal)
00269     {
00270         const GoalLazySamples *gls = dynamic_cast<const GoalLazySamples*>(goal);
00271         bool first = true;
00272         bool attempt = true;
00273         while (attempt)
00274         {
00275             attempt = false;
00276 
00277             if (sampledGoalsCount_ < goal->maxSampleCount())
00278             {
00279                 if (tempState_ == NULL)
00280                     tempState_ = si_->allocState();
00281 
00282                 do
00283                 {
00284                     goal->sampleGoal(tempState_);
00285                     sampledGoalsCount_++;
00286                     bool bounds = si_->satisfiesBounds(tempState_);
00287                     bool valid = bounds ? si_->isValid(tempState_) : false;
00288                     if (bounds && valid)
00289                         return tempState_;
00290                     else
00291                     {
00292                         msg::Interface msg(planner_ ? planner_->getName() : "");
00293                         msg.warn("Skipping invalid goal state (invalid %s)", bounds ? "state": "bounds");
00294                     }
00295                 }
00296                 while (sampledGoalsCount_ < goal->maxSampleCount() && !ptc());
00297             }
00298 
00299             if (gls && goal->canSample() && !ptc())
00300             {
00301                 if (first)
00302                 {
00303                     first = false;
00304                     msg::Interface msg(planner_ ? planner_->getName() : "");
00305                     msg.debug("Waiting for goal region samples ...");
00306                 }
00307                 boost::this_thread::sleep(time::seconds(0.01));
00308                 attempt = !ptc();
00309             }
00310         }
00311     }
00312     return NULL;
00313 }
00314 
00315 bool ompl::base::PlannerInputStates::haveMoreStartStates(void) const
00316 {
00317     if (pdef_)
00318         return addedStartStates_ < pdef_->getStartStateCount();
00319     return false;
00320 }
00321 
00322 bool ompl::base::PlannerInputStates::haveMoreGoalStates(void) const
00323 {
00324     if (pdef_ && pdef_->getGoal())
00325     {
00326         const GoalSampleableRegion *goal = dynamic_cast<const GoalSampleableRegion*>(pdef_->getGoal().get());
00327         if (goal)
00328             return sampledGoalsCount_ < goal->maxSampleCount();
00329     }
00330     return false;
00331 }