00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00127 static bool timePassed(const time::point &endTime)
00128 {
00129 return time::now() > endTime;
00130 }
00131
00132
00133 static bool evaluateFlag(const bool *flag)
00134 {
00135 return *flag;
00136 }
00137
00138
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 }