37 #include <ompl/control/SpaceInformation.h>
38 #include <ompl/base/goals/GoalState.h>
39 #include <ompl/base/spaces/SE2StateSpace.h>
40 #include <ompl/control/spaces/RealVectorControlSpace.h>
41 #include <ompl/control/planners/kpiece/KPIECE1.h>
42 #include <ompl/control/planners/rrt/RRT.h>
43 #include <ompl/control/planners/est/EST.h>
44 #include <ompl/control/planners/syclop/SyclopRRT.h>
45 #include <ompl/control/planners/syclop/SyclopEST.h>
46 #include <ompl/control/SimpleSetup.h>
47 #include <ompl/config.h>
50 namespace ob = ompl::base;
51 namespace oc = ompl::control;
70 sampler->sampleUniform(s);
72 rng_.uniformReal(regionBounds.low[0], regionBounds.high[0]));
74 rng_.uniformReal(regionBounds.low[1], regionBounds.high[1]));
106 const oc::RealVectorControlSpace::ControlType *rctrl = control->
as<oc::RealVectorControlSpace::ControlType>();
109 (*pos)[0] + (*rctrl)[0] * duration * cos(rot->value);
112 (*pos)[1] + (*rctrl)[0] * duration * sin(rot->value);
115 rot->value + (*rctrl)[1];
136 cbounds.setLow(-0.3);
137 cbounds.setHigh(0.3);
164 pdef->setStartAndGoalStates(start, goal, 0.1);
175 planner->setProblemDefinition(pdef);
185 pdef->print(std::cout);
195 std::cout <<
"Found solution:" << std::endl;
198 path->print(std::cout);
201 std::cout <<
"No solution found" << std::endl;
205 void planWithSimpleSetup(
void)
222 cbounds.setLow(-0.3);
223 cbounds.setHigh(0.3);
231 ss.setStatePropagator(boost::bind(&propagate, _1, _2, _3, _4));
234 ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
250 ss.setStartAndGoalStates(start, goal, 0.05);
257 std::cout <<
"Found solution:" << std::endl;
260 ss.getSolutionPath().asGeometric().print(std::cout);
263 std::cout <<
"No solution found" << std::endl;
266 int main(
int,
char **)
268 std::cout <<
"OMPL version: " << OMPL_VERSION << std::endl;
272 std::cout << std::endl << std::endl;
274 planWithSimpleSetup();