37 #include "ompl/control/PathControl.h" 38 #include "ompl/control/planners/ltl/LTLProblemDefinition.h" 39 #include "ompl/control/planners/ltl/LTLSpaceInformation.h" 40 #include "ompl/base/ProblemDefinition.h" 45 oc::LTLProblemDefinition::LTLProblemDefinition(
const LTLSpaceInformationPtr <lsi)
46 :
ob::ProblemDefinition(ltlsi), ltlsi_(ltlsi)
51 void oc::LTLProblemDefinition::addLowerStartState(
const ob::State *s)
54 ltlsi_->getFullState(s, fullStart.get());
55 addStartState(fullStart);
58 ob::PathPtr oc::LTLProblemDefinition::getLowerSolutionPath()
const 60 PathControl *fullPath =
static_cast<PathControl *
>(getSolutionPath().get());
61 auto lowPath(std::make_shared<PathControl>(ltlsi_->getLowSpace()));
63 if (fullPath->getStateCount() > 0)
65 for (
size_t i = 0; i < fullPath->getStateCount() - 1; ++i)
66 lowPath->append(ltlsi_->getLowLevelState(fullPath->getState(i)), fullPath->getControl(i),
67 fullPath->getControlDuration(i));
70 lowPath->append(ltlsi_->getLowLevelState(fullPath->getState(fullPath->getStateCount() - 1)));
76 void oc::LTLProblemDefinition::createGoal()
78 class LTLGoal :
public base::Goal
81 LTLGoal(
const LTLSpaceInformationPtr <lsi) :
ob::Goal(ltlsi), ltlsi_(ltlsi), prod_(ltlsi->getProductGraph())
84 ~LTLGoal()
override =
default;
85 bool isSatisfied(
const ob::State *s)
const override 87 return prod_->isSolution(ltlsi_->getProdGraphState(s));
91 const LTLSpaceInformationPtr ltlsi_;
92 const ProductGraphPtr prod_;
97 setGoal(std::make_shared<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 shared pointer wrapper for ompl::base::Path.