37 #include "ompl/base/ProblemDefinition.h"
38 #include "ompl/base/goals/GoalState.h"
39 #include "ompl/base/goals/GoalStates.h"
40 #include "ompl/control/SpaceInformation.h"
41 #include "ompl/control/PathControl.h"
45 #include <boost/thread/mutex.hpp>
53 class ProblemDefinition::PlannerSolutionSet
57 PlannerSolutionSet(
void)
61 void add(
const PlannerSolution &s)
63 boost::mutex::scoped_lock slock(lock_);
64 int index = solutions_.size();
65 solutions_.push_back(s);
66 solutions_.back().index_ = index;
67 std::sort(solutions_.begin(), solutions_.end());
72 boost::mutex::scoped_lock slock(lock_);
78 boost::mutex::scoped_lock slock(lock_);
79 std::vector<PlannerSolution> copy = solutions_;
83 bool isApproximate(
void)
85 boost::mutex::scoped_lock slock(lock_);
87 if (!solutions_.empty())
88 result = solutions_[0].approximate_;
92 double getDifference(
void)
94 boost::mutex::scoped_lock slock(lock_);
96 if (!solutions_.empty())
97 diff = solutions_[0].difference_;
101 PathPtr getTopSolution(
void)
103 boost::mutex::scoped_lock slock(lock_);
105 if (!solutions_.empty())
106 copy = solutions_[0].path_;
112 boost::mutex::scoped_lock slock(lock_);
113 std::size_t result = solutions_.size();
119 std::vector<PlannerSolution> solutions_;
133 addStartState(start);
134 setGoalState(goal, threshold);
148 for (
unsigned int i = 0 ; i < startStates_.size() ; ++i)
149 if (si_->equalStates(state, startStates_[i]))
162 bool b = si_->satisfiesBounds(state);
166 v = si_->isValid(state);
168 OMPL_DEBUG(
"%s state is not valid", start ?
"Start" :
"Goal");
171 OMPL_DEBUG(
"%s state is not within space bounds", start ?
"Start" :
"Goal");
175 std::stringstream ss;
176 si_->printState(state, ss);
177 ss <<
" within distance " << dist;
178 OMPL_DEBUG(
"Attempting to fix %s state %s", start ?
"start" :
"goal", ss.str().c_str());
180 State *temp = si_->allocState();
181 if (si_->searchValidNearby(temp, state, dist, attempts))
183 si_->copyState(state, temp);
187 OMPL_WARN(
"Unable to fix %s state", start ?
"start" :
"goal");
188 si_->freeState(temp);
199 for (
unsigned int i = 0 ; i < startStates_.size() ; ++i)
200 if (!fixInvalidInputState(startStates_[i], distStart,
true, attempts))
207 if (!fixInvalidInputState(const_cast<State*>(goal->
getState()), distGoal,
false, attempts))
216 if (!fixInvalidInputState(const_cast<State*>(goals->
getState(i)), distGoal,
false, attempts))
226 for (
unsigned int i = 0 ; i < startStates_.size() ; ++i)
227 states.push_back(startStates_[i]);
236 states.push_back (goals->
getState(i));
244 unsigned int startIndex;
245 if (isTrivial(&startIndex, NULL))
248 pc->
append(startStates_[startIndex]);
250 sic->nullControl(null);
251 pc->
append(startStates_[startIndex], null, 0.0);
252 sic->freeControl(null);
258 State *result1 = sic->allocState();
259 State *result2 = sic->allocState();
260 sic->nullControl(nc);
262 for (
unsigned int k = 0 ; k < startStates_.size() && !path ; ++k)
264 const State *start = startStates_[k];
265 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
267 sic->copyState(result1, start);
268 for (
unsigned int i = 0 ; i < sic->getMaxControlDuration() && !path ; ++i)
269 if (sic->propagateWhileValid(result1, nc, 1, result2))
271 if (goal_->isSatisfied(result2))
275 pc->
append(result2, nc, (i + 1) * sic->getPropagationStepSize());
279 std::swap(result1, result2);
283 sic->freeState(result1);
284 sic->freeState(result2);
285 sic->freeControl(nc);
290 std::vector<const State*> states;
293 if (si_->isValid(goal->
getState()) && si_->satisfiesBounds(goal->
getState()))
298 if (si_->isValid(goals->
getState(i)) && si_->satisfiesBounds(goals->
getState(i)))
299 states.push_back(goals->
getState(i));
303 unsigned int startIndex;
304 if (isTrivial(&startIndex))
312 for (
unsigned int i = 0 ; i < startStates_.size() && !path ; ++i)
314 const State *start = startStates_[i];
315 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
317 for (
unsigned int j = 0 ; j < states.size() && !path ; ++j)
318 if (si_->checkMotion(start, states[j]))
340 for (
unsigned int i = 0 ; i < startStates_.size() ; ++i)
342 const State *start = startStates_[i];
343 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
346 if (goal_->isSatisfied(start, &dist))
366 return solutions_->getSolutionCount() > 0;
371 return solutions_->getSolutionCount();
376 return solutions_->getTopSolution();
382 OMPL_WARN(
"Adding approximate solution");
388 return solutions_->isApproximate();
393 return solutions_->getDifference();
398 return solutions_->getSolutions();
408 out <<
"Start states:" << std::endl;
409 for (
unsigned int i = 0 ; i < startStates_.size() ; ++i)
410 si_->printState(startStates_[i], out);
414 out <<
"Goal = NULL" << std::endl;
415 out <<
"There are " << solutions_->getSolutionCount() <<
" solutions" << std::endl;
421 return nonExistenceProof_;
426 nonExistenceProof_.reset();
431 return nonExistenceProof_;
436 nonExistenceProof_ = nonExistenceProof;