1 #include "ompl/control/PathControl.h"
2 #include "ompl/control/planners/ltl/LTLProblemDefinition.h"
3 #include "ompl/control/planners/ltl/LTLSpaceInformation.h"
4 #include "ompl/base/ProblemDefinition.h"
9 oc::LTLProblemDefinition::LTLProblemDefinition(
const LTLSpaceInformationPtr& ltlsi)
10 :
ob::ProblemDefinition(ltlsi), ltlsi_(ltlsi)
15 void oc::LTLProblemDefinition::addLowerStartState(
const ob::State* s)
18 ltlsi_->getFullState(s, fullStart.get());
19 addStartState(fullStart);
22 ob::PathPtr oc::LTLProblemDefinition::getLowerSolutionPath(
void)
const
24 PathControl* fullPath =
static_cast<PathControl*
>(getSolutionPath().get());
25 ob::PathPtr lowPathPtr(
new PathControl(ltlsi_->getLowSpace()));
26 PathControl* lowPath =
static_cast<PathControl*
>(lowPathPtr.get());
28 if (fullPath->getStateCount() > 0)
30 for(
size_t i = 0; i < fullPath->getStateCount()-1; ++i)
31 lowPath->append(ltlsi_->getLowLevelState(fullPath->getState(i)),
32 fullPath->getControl(i),
33 fullPath->getControlDuration(i));
36 lowPath->append(ltlsi_->getLowLevelState(fullPath->getState(fullPath->getStateCount()-1)));
42 void oc::LTLProblemDefinition::createGoal(
void)
44 class LTLGoal :
public base::Goal
47 LTLGoal(
const LTLSpaceInformationPtr& ltlsi)
48 :
ob::Goal(ltlsi), ltlsi_(ltlsi), prod_(ltlsi->getProductGraph()) {}
49 virtual ~LTLGoal(
void) {}
50 virtual bool isSatisfied(
const ob::State* s)
const
52 return prod_->isSolution(ltlsi_->getProdGraphState(s));
55 const LTLSpaceInformationPtr ltlsi_;
56 const ProductGraphPtr prod_;
61 setGoal(
ob::GoalPtr(static_cast<ob::Goal*>(
new LTLGoal(ltlsi_))));
Definition of a scoped state.
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition of an abstract state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
A boost shared pointer wrapper for ompl::base::Goal.
A boost shared pointer wrapper for ompl::base::Path.