LTLProblemDefinition.cpp
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"
5 
6 namespace ob = ompl::base;
7 namespace oc = ompl::control;
8 
9 oc::LTLProblemDefinition::LTLProblemDefinition(const LTLSpaceInformationPtr& ltlsi)
10  : ob::ProblemDefinition(ltlsi), ltlsi_(ltlsi)
11 {
12  createGoal();
13 }
14 
15 void oc::LTLProblemDefinition::addLowerStartState(const ob::State* s)
16 {
18  ltlsi_->getFullState(s, fullStart.get());
19  addStartState(fullStart);
20 }
21 
22 ob::PathPtr oc::LTLProblemDefinition::getLowerSolutionPath(void) const
23 {
24  PathControl* fullPath = static_cast<PathControl*>(getSolutionPath().get());
25  ob::PathPtr lowPathPtr(new PathControl(ltlsi_->getLowSpace()));
26  PathControl* lowPath = static_cast<PathControl*>(lowPathPtr.get());
27 
28  if (fullPath->getStateCount() > 0)
29  {
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));
34 
35  // The last state does not have a control
36  lowPath->append(ltlsi_->getLowLevelState(fullPath->getState(fullPath->getStateCount()-1)));
37  }
38 
39  return lowPathPtr;
40 }
41 
42 void oc::LTLProblemDefinition::createGoal(void)
43 {
44  class LTLGoal : public base::Goal
45  {
46  public:
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
51  {
52  return prod_->isSolution(ltlsi_->getProdGraphState(s));
53  }
54  protected:
55  const LTLSpaceInformationPtr ltlsi_;
56  const ProductGraphPtr prod_;
57  };
58 
59  // Some compilers have trouble with LTLGoal being hidden in this function,
60  // and so we explicitly cast it to its base type.
61  setGoal(ob::GoalPtr(static_cast<ob::Goal*>(new LTLGoal(ltlsi_))));
62 }
Definition of a scoped state.
Definition: ScopedState.h:56
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
Definition of an abstract state.
Definition: State.h:50
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
A boost shared pointer wrapper for ompl::base::Goal.
A boost shared pointer wrapper for ompl::base::Path.