37 #include "ompl/extensions/opende/OpenDESimpleSetup.h"
38 #include "ompl/util/Exception.h"
39 #include <boost/thread.hpp>
43 if (!dynamic_cast<OpenDEControlSpace*>(space.get()))
44 throw Exception(
"OpenDE Control Space needed for OpenDE Simple Setup");
60 void ompl::control::OpenDESimpleSetup::useEnvParams(
void)
62 si_->setPropagationStepSize(getStateSpace()->as<OpenDEStateSpace>()->getEnvironment()->stepSize_);
63 si_->setMinMaxControlDuration(getStateSpace()->as<OpenDEStateSpace>()->getEnvironment()->minControlSteps_,
64 getStateSpace()->as<OpenDEStateSpace>()->getEnvironment()->maxControlSteps_);
87 if (!si_->getStateValidityChecker())
89 logInform(
"Using default state validity checker for OpenDE");
92 if (pdef_->getStartStateCount() == 0)
94 logInform(
"Using the initial state of OpenDE as the starting state for the planner");
95 pdef_->addStartState(getCurrentState());
102 if (haveSolutionPath())
103 playPath(pdef_->getSolutionPath(), timeFactor);
109 if (dynamic_cast<PathControl*>(path.get()))
112 if (!dynamic_cast<geometric::PathGeometric*>(path.get()))
121 timeFactor * si_->getPropagationStepSize() * (double)(pg.
getStateCount() - 1));
126 boost::this_thread::sleep(d);
134 Control *c = si_->allocControl();
150 si_->propagate(s0, control, steps, s1);
153 p->
getControls().push_back(si_->cloneControl(control));
160 Control *c = si_->allocControl();