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 #include <utility>
00043 
00044 ompl::base::Planner::Planner(const SpaceInformationPtr &si, const std::string &name) :
00045     si_(si), pis_(this), name_(name), type_(PLAN_UNKNOWN), setup_(false), msg_(name)
00046 {
00047     if (!si_)
00048         throw Exception(name_, "Invalid space information instance for planner");
00049 }
00050 
00051 ompl::base::PlannerType ompl::base::Planner::getType(void) const
00052 {
00053     return type_;
00054 }
00055 
00056 const std::string& ompl::base::Planner::getName(void) const
00057 {
00058     return name_;
00059 }
00060 
00061 void ompl::base::Planner::setName(const std::string &name)
00062 {
00063     name_ = 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 namespace ompl
00125 {
00126     // return true if a certain point in time has passed
00127     static bool timePassed(const time::point &endTime)
00128     {
00129         return time::now() > endTime;
00130     }
00131 
00132     // return if an externally passed flag is true
00133     static bool evaluateFlag(const bool *flag)
00134     {
00135         return *flag;
00136     }
00137 
00138     // periodically evaluate a termination condition and store the result at an indicated location
00139     static void periodicConditionEvaluator(const base::PlannerTerminationCondition &ptc, double checkInterval, bool *flag)
00140     {
00141         time::duration s = time::seconds(checkInterval);
00142         do
00143         {
00144             bool shouldTerminate = ptc();
00145             if (shouldTerminate)
00146                 *flag = true;
00147             if (*flag == false)
00148                 boost::this_thread::sleep(s);
00149         } while (*flag == false);
00150     }
00151 
00152     static bool alwaysTrue(void)
00153     {
00154         return true;
00155     }
00156 }
00157 
00158 bool ompl::base::Planner::solve(const PlannerTerminationCondition &ptc, double checkInterval)
00159 {
00160     bool flag = false;
00161     boost::thread condEvaluator(boost::bind(&periodicConditionEvaluator, ptc, checkInterval, &flag));
00162     bool result = solve(boost::bind(&evaluateFlag, &flag));
00163     flag = true;
00164     condEvaluator.interrupt();
00165     condEvaluator.join();
00166     return result;
00167 }
00168 
00169 bool ompl::base::Planner::solve(double solveTime)
00170 {
00171     if (solveTime < 1.0)
00172         return solve(boost::bind(&timePassed, time::now() + time::seconds(solveTime)));
00173     else
00174         return solve(boost::bind(&timePassed, time::now() + time::seconds(solveTime)), std::min(solveTime / 100.0, 0.1));
00175 }
00176 
00177 void ompl::base::PlannerInputStates::clear(void)
00178 {
00179     if (tempState_)
00180     {
00181         si_->freeState(tempState_);
00182         tempState_ = NULL;
00183     }
00184     addedStartStates_ = 0;
00185     sampledGoalsCount_ = 0;
00186     pdef_ = NULL;
00187     si_ = NULL;
00188 }
00189 
00190 bool ompl::base::PlannerInputStates::update(void)
00191 {
00192     if (!planner_)
00193         throw Exception("No planner set for PlannerInputStates");
00194     return use(planner_->getSpaceInformation(), planner_->getProblemDefinition());
00195 }
00196 
00197 void ompl::base::PlannerInputStates::checkValidity(void) const
00198 {
00199     std::string error;
00200 
00201     if (!pdef_)
00202         error = "Problem definition not specified";
00203     else
00204     {
00205         if (pdef_->getStartStateCount() <= 0)
00206             error = "No start states specified";
00207         else
00208             if (!pdef_->getGoal())
00209                 error = "No goal specified";
00210     }
00211 
00212     if (!error.empty())
00213     {
00214         if (planner_)
00215             throw Exception(planner_->getName(), error);
00216         else
00217             throw Exception(error);
00218     }
00219 }
00220 
00221 bool ompl::base::PlannerInputStates::use(const SpaceInformationPtr &si, const ProblemDefinitionPtr &pdef)
00222 {
00223     if (si && pdef)
00224         return use(si.get(), pdef.get());
00225     else
00226     {
00227         clear();
00228         return true;
00229     }
00230 }
00231 
00232 bool ompl::base::PlannerInputStates::use(const SpaceInformation *si, const ProblemDefinition *pdef)
00233 {
00234     if (pdef_ != pdef || si_ != si)
00235     {
00236         clear();
00237         pdef_ = pdef;
00238         si_ = si;
00239         return true;
00240     }
00241     return false;
00242 }
00243 
00244 const ompl::base::State* ompl::base::PlannerInputStates::nextStart(void)
00245 {
00246     if (pdef_ == NULL || si_ == NULL)
00247     {
00248         std::string error = "Missing space information or problem definition";
00249         if (planner_)
00250             throw Exception(planner_->getName(), error);
00251         else
00252             throw Exception(error);
00253     }
00254 
00255     while (addedStartStates_ < pdef_->getStartStateCount())
00256     {
00257         const base::State *st = pdef_->getStartState(addedStartStates_);
00258         addedStartStates_++;
00259         bool bounds = si_->satisfiesBounds(st);
00260         bool valid = bounds ? si_->isValid(st) : false;
00261         if (bounds && valid)
00262             return st;
00263         else
00264         {
00265             msg::Interface msg(planner_ ? planner_->getName() : "");
00266             msg.warn("Skipping invalid start state (invalid %s)", bounds ? "state": "bounds");
00267         }
00268     }
00269     return NULL;
00270 }
00271 
00272 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(void)
00273 {
00274     static PlannerTerminationCondition ptc = boost::bind(&alwaysTrue);
00275     return nextGoal(ptc);
00276 }
00277 
00278 const ompl::base::State* ompl::base::PlannerInputStates::nextGoal(const PlannerTerminationCondition &ptc)
00279 {
00280     if (pdef_ == NULL || si_ == NULL)
00281     {
00282         std::string error = "Missing space information or problem definition";
00283         if (planner_)
00284             throw Exception(planner_->getName(), error);
00285         else
00286             throw Exception(error);
00287     }
00288 
00289     const GoalSampleableRegion *goal = dynamic_cast<const GoalSampleableRegion*>(pdef_->getGoal().get());
00290 
00291     if (goal)
00292     {
00293         const GoalLazySamples *gls = dynamic_cast<const GoalLazySamples*>(goal);
00294         bool first = true;
00295         bool attempt = true;
00296         while (attempt)
00297         {
00298             attempt = false;
00299 
00300             if (sampledGoalsCount_ < goal->maxSampleCount())
00301             {
00302                 if (tempState_ == NULL)
00303                     tempState_ = si_->allocState();
00304 
00305                 do
00306                 {
00307                     goal->sampleGoal(tempState_);
00308                     sampledGoalsCount_++;
00309                     bool bounds = si_->satisfiesBounds(tempState_);
00310                     bool valid = bounds ? si_->isValid(tempState_) : false;
00311                     if (bounds && valid)
00312                         return tempState_;
00313                     else
00314                     {
00315                         msg::Interface msg(planner_ ? planner_->getName() : "");
00316                         msg.warn("Skipping invalid goal state (invalid %s)", bounds ? "state": "bounds");
00317                     }
00318                 }
00319                 while (sampledGoalsCount_ < goal->maxSampleCount() && !ptc());
00320             }
00321 
00322             if (gls && goal->canSample() && !ptc())
00323             {
00324                 if (first)
00325                 {
00326                     first = false;
00327                     msg::Interface msg(planner_ ? planner_->getName() : "");
00328                     msg.debug("Waiting for goal region samples ...");
00329                 }
00330                 boost::this_thread::sleep(time::seconds(0.01));
00331                 attempt = !ptc();
00332             }
00333         }
00334     }
00335     return NULL;
00336 }
00337 
00338 bool ompl::base::PlannerInputStates::haveMoreStartStates(void) const
00339 {
00340     if (pdef_)
00341         return addedStartStates_ < pdef_->getStartStateCount();
00342     return false;
00343 }
00344 
00345 bool ompl::base::PlannerInputStates::haveMoreGoalStates(void) const
00346 {
00347     if (pdef_ && pdef_->getGoal())
00348     {
00349         const GoalSampleableRegion *goal = dynamic_cast<const GoalSampleableRegion*>(pdef_->getGoal().get());
00350         if (goal)
00351             return sampledGoalsCount_ < goal->maxSampleCount();
00352     }
00353     return false;
00354 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator