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());
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_;
const LTLSpaceInformation * ltlsi_
Handle to the control::SpaceInformation object.
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.
SpaceInformationPtr si_
The space information for which planning is done.
A boost shared pointer wrapper for ompl::base::Path.