ompl/control/planners/ltl/src/LTLProblemDefinition.cpp
00001 #include "ompl/control/PathControl.h" 00002 #include "ompl/control/planners/ltl/LTLProblemDefinition.h" 00003 #include "ompl/control/planners/ltl/LTLSpaceInformation.h" 00004 #include "ompl/base/ProblemDefinition.h" 00005 00006 namespace ob = ompl::base; 00007 namespace oc = ompl::control; 00008 00009 oc::LTLProblemDefinition::LTLProblemDefinition(const LTLSpaceInformationPtr& ltlsi) 00010 : ob::ProblemDefinition(ltlsi), ltlsi_(ltlsi) 00011 { 00012 createGoal(); 00013 } 00014 00015 void oc::LTLProblemDefinition::addLowerStartState(const ob::State* s) 00016 { 00017 ob::ScopedState<ob::CompoundStateSpace> fullStart(si_); 00018 ltlsi_->getFullState(s, fullStart.get()); 00019 addStartState(fullStart); 00020 } 00021 00022 ob::PathPtr oc::LTLProblemDefinition::getLowerSolutionPath(void) const 00023 { 00024 PathControl* fullPath = static_cast<PathControl*>(getSolutionPath().get()); 00025 ob::PathPtr lowPathPtr(new PathControl(ltlsi_->getLowSpace())); 00026 PathControl* lowPath = static_cast<PathControl*>(lowPathPtr.get()); 00027 00028 if (fullPath->getStateCount() > 0) 00029 { 00030 for(size_t i = 0; i < fullPath->getStateCount()-1; ++i) 00031 lowPath->append(ltlsi_->getLowLevelState(fullPath->getState(i)), 00032 fullPath->getControl(i), 00033 fullPath->getControlDuration(i)); 00034 00035 // The last state does not have a control 00036 lowPath->append(ltlsi_->getLowLevelState(fullPath->getState(fullPath->getStateCount()-1))); 00037 } 00038 00039 return lowPathPtr; 00040 } 00041 00042 void oc::LTLProblemDefinition::createGoal(void) 00043 { 00044 class LTLGoal : public base::Goal 00045 { 00046 public: 00047 LTLGoal(const LTLSpaceInformationPtr& ltlsi) 00048 : ob::Goal(ltlsi), ltlsi_(ltlsi), prod_(ltlsi->getProductGraph()) {} 00049 virtual ~LTLGoal(void) {} 00050 virtual bool isSatisfied(const ob::State* s) const 00051 { 00052 return prod_->isSolution(ltlsi_->getProdGraphState(s)); 00053 } 00054 protected: 00055 const LTLSpaceInformationPtr ltlsi_; 00056 const ProductGraphPtr prod_; 00057 }; 00058 00059 // Some compilers have trouble with LTLGoal being hidden in this function, 00060 // and so we explicitly cast it to its base type. 00061 setGoal(ob::GoalPtr(static_cast<ob::Goal*>(new LTLGoal(ltlsi_)))); 00062 }