In order to implement a new motion planner the following rules need to be followed:
#include "ompl/base/Planner.h" // often useful headers: #include "ompl/util/RandomNumbers.h" #include "ompl/tools/config/SelfConfig.h" namespace ompl { class myNewPlanner : public base::Planner { public: myNewPlanner(const base::SpaceInformationPtr &si) : base::Planner(si, "the planner's name") { // the specifications of this planner (ompl::base::PlannerSpecs) specs_.approximateSolutions = ...; specs_.recognizedGoal = ...; ... } virtual ~myNewPlanner(void) { // free any allocated memory } virtual bool solve(const base::PlannerTerminationCondition &ptc) { // make sure the planner is configured correctly; this makes sure there // are input states and a goal is specified checkValidity(); // get problem definition from pdef_ base::Goal *goal = pdef_->getGoal().get(); // get input states with pis_ while (const base::State *st = pis_.nextStart()) { // use start states // usually, copies of st should be made } // if needed, sample states from the goal region (and wait until a state is sampled) const base::State *st = pis_.nextGoal(ptc); // or sample a new goal state only if available: const base::State *st = pis_.nextGoal(); // call routines from si_ as needed // periodically check if ptc() returns true. // if it does, terminate planning. // use msg_ to output messages. // compute a valid motion plan and set it in the goal pdef_->getGoal()->setSolutionPath(...); return pdef_->getGoal()->isAchieved(); } virtual void clear(void) { Planner::clear(); // clear the data structures } // optionally, if additional setup/configuration is needed, the setup() method can be implemented virtual void setup(void) { Planner::setup(); // perhaps attempt some auto-configuration SelfConfig sc(si_, getName()); sc.configure... } virtual void getPlannerData(base::PlannerData &data) const { // fill data with the states and edges that were created // in the exploration data structure // perhaps also fill control::PlannerData } }; }
Testing a newly added planner:
// create a planning context such that planner->solve() can be called and expected to find a solution base::PlannerPtr planner; // create a testing scenario for the planner PlannerTest pt(planner); pt.test();