base::StateSpacePtr space(new base::SE2StateSpace()); // set bounds ....
base::StateSpacePtr space(new base::SE2StateSpace()); // set bounds for state space control::ControlSpacePtr cspace(new control::RealVectorControlSpace(space)); // set bounds for cspace
// define this class: class myStateValidityCheckerClass : public base::StateValidityChecker { public: myStateValidityCheckerClass(const base::SpaceInformationPtr &si) : base::StateValidityChecker(si) { } virtual bool isValid(const base::State *state) const { return ...; } }; // or this function: bool myStateValidityCheckerFunction(const base::State *state) { return ...; } base::SpaceInformationPtr si(space); // either this call: si->setStateValidityChecker(base::StateValidityCheckerPtr(new myStateValidityCheckerClass(si))); // or this call: si->setStateValidityChecker(boost::bind(&myStateValidityCheckerFunction, _1)); si->setStateValidityCheckingResolution(0.03); // 3% si->setup();
base::SpaceInformationPtr si(...); base::ProblemDefinitionPtr pdef(new base::ProblemDefinition(si)); base::ScopedState start; // fill start state base::ScopedState goal; // fill goal state pdef->setStartAndGoalStates(start, goal);
using namespace ompl; base::SpaceInformationPtr si(...); base::ProblemDefinition pdef(si); // set start states & goal region for the problem definition base::PlannerPtr planner(new geometric::SBL(si)); planner->setProblemDefinition(pdef); planner->solve(1.0); if (pdef->getGoal()->getSolutionPath()) { // do something with the solution } planner->clear();