Skip to content

Commit

Permalink
move default planner selection from SimpleSetup to tools/SelfConfig.h
Browse files Browse the repository at this point in the history
  • Loading branch information
@mamoll
mamoll committed Sep 13, 2014
1 parent 7258458 commit 78f5b68
Show file tree
Hide file tree
Showing 6 changed files with 57 additions and 59 deletions.
2 changes: 0 additions & 2 deletions src/ompl/control/SimpleSetup.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -307,8 +307,6 @@ namespace ompl
base::ParamSet params_;
};

/** \brief Given a goal specification, decide on a planner for that goal */
base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal);
}

}
Expand Down
20 changes: 2 additions & 18 deletions src/ompl/control/src/SimpleSetup.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,23 +35,7 @@
/* Author: Ioan Sucan */

#include "ompl/control/SimpleSetup.h"
#include "ompl/control/planners/rrt/RRT.h"
#include "ompl/control/planners/kpiece/KPIECE1.h"

ompl::base::PlannerPtr ompl::control::getDefaultPlanner(const base::GoalPtr &goal)
{
base::PlannerPtr planner;
if (!goal)
throw Exception("Unable to allocate default planner for unspecified goal definition");

SpaceInformationPtr si = boost::static_pointer_cast<SpaceInformation, base::SpaceInformation>(goal->getSpaceInformation());
if (si->getStateSpace()->hasDefaultProjection())
planner = base::PlannerPtr(new KPIECE1(si));
else
planner = base::PlannerPtr(new RRT(si));

return planner;
}
#include "ompl/tools/config/SelfConfig.h"

ompl::control::SimpleSetup::SimpleSetup(const SpaceInformationPtr &si) :
configured_(false), planTime_(0.0), last_status_(base::PlannerStatus::UNKNOWN)
Expand Down Expand Up @@ -82,7 +66,7 @@ void ompl::control::SimpleSetup::setup()
if (!planner_)
{
OMPL_INFORM("No planner specified. Using default.");
planner_ = getDefaultPlanner(getGoal());
planner_ = tools::SelfConfig::getDefaultPlanner(getGoal());
}
}
planner_->setProblemDefinition(pdef_);
Expand Down
2 changes: 0 additions & 2 deletions src/ompl/geometric/SimpleSetup.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -327,8 +327,6 @@ namespace ompl
base::ParamSet params_;
};

/** \brief Given a goal specification, decide on a planner for that goal */
base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal);
}

}
Expand Down
39 changes: 2 additions & 37 deletions src/ompl/geometric/src/SimpleSetup.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,42 +35,7 @@
/* Author: Ioan Sucan */

#include "ompl/geometric/SimpleSetup.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/geometric/planners/rrt/RRTConnect.h"
#include "ompl/geometric/planners/rrt/RRT.h"
#include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
#include "ompl/geometric/planners/kpiece/KPIECE1.h"

ompl::base::PlannerPtr ompl::geometric::getDefaultPlanner(const base::GoalPtr &goal)
{
base::PlannerPtr planner;
if (!goal)
throw Exception("Unable to allocate default planner for unspecified goal definition");

// if we can sample the goal region, use a bi-directional planner
if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
{
// if we have a default projection
if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
planner = base::PlannerPtr(new LBKPIECE1(goal->getSpaceInformation()));
else
planner = base::PlannerPtr(new RRTConnect(goal->getSpaceInformation()));
}
// other use a single-tree planner
else
{
// if we have a default projection
if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
planner = base::PlannerPtr(new KPIECE1(goal->getSpaceInformation()));
else
planner = base::PlannerPtr(new RRT(goal->getSpaceInformation()));
}

if (!planner)
throw Exception("Unable to allocate default planner");

return planner;
}
#include "ompl/tools/config/SelfConfig.h"

ompl::geometric::SimpleSetup::SimpleSetup(const base::SpaceInformationPtr &si) :
configured_(false), planTime_(0.0), simplifyTime_(0.0), lastStatus_(base::PlannerStatus::UNKNOWN)
Expand Down Expand Up @@ -103,7 +68,7 @@ void ompl::geometric::SimpleSetup::setup()
if (!planner_)
{
OMPL_INFORM("No planner specified. Using default.");
planner_ = getDefaultPlanner(getGoal());
planner_ = tools::SelfConfig::getDefaultPlanner(getGoal());
}
}
planner_->setProblemDefinition(pdef_);
Expand Down
5 changes: 5 additions & 0 deletions src/ompl/tools/config/SelfConfig.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@
#define OMPL_TOOLS_SELF_CONFIG_

#include "ompl/config.h"
#include "ompl/base/Goal.h"
#include "ompl/base/Planner.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
#include "ompl/datastructures/NearestNeighborsGNAT.h"
Expand Down Expand Up @@ -96,6 +98,9 @@ namespace ompl
return new NearestNeighborsSqrtApprox<_T>();
}

/** \brief Given a goal specification, decide on a planner for that goal */
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal);

private:

/// @cond IGNORE
Expand Down
48 changes: 48 additions & 0 deletions src/ompl/tools/config/src/SelfConfig.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,12 @@

#include "ompl/tools/config/SelfConfig.h"
#include "ompl/tools/config/MagicConstants.h"
#include "ompl/geometric/planners/rrt/RRTConnect.h"
#include "ompl/geometric/planners/rrt/RRT.h"
#include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
#include "ompl/geometric/planners/kpiece/KPIECE1.h"
#include "ompl/control/planners/rrt/RRT.h"
#include "ompl/control/planners/kpiece/KPIECE1.h"
#include "ompl/util/Console.h"
#include <boost/thread/mutex.hpp>
#include <boost/shared_ptr.hpp>
Expand Down Expand Up @@ -240,3 +246,45 @@ void ompl::tools::SelfConfig::print(std::ostream &out) const
boost::mutex::scoped_lock iLock(impl_->lock_);
impl_->print(out);
}

ompl::base::PlannerPtr ompl::tools::SelfConfig::getDefaultPlanner(const base::GoalPtr &goal)
{
base::PlannerPtr planner;
if (!goal)
throw Exception("Unable to allocate default planner for unspecified goal definition");

base::SpaceInformationPtr si(goal->getSpaceInformation());
control::SpaceInformationPtr siC(boost::dynamic_pointer_cast<control::SpaceInformation, base::SpaceInformation>(si));
if (siC) // kinodynamic planning
{
// if we have a default projection
if (siC->getStateSpace()->hasDefaultProjection())
planner = base::PlannerPtr(new control::KPIECE1(siC));
// otherwise use a single-tree planner
else
planner = base::PlannerPtr(new control::RRT(siC));
}
// if we can sample the goal region, use a bi-directional planner
else if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
{
// if we have a default projection
if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
planner = base::PlannerPtr(new geometric::LBKPIECE1(goal->getSpaceInformation()));
else
planner = base::PlannerPtr(new geometric::RRTConnect(goal->getSpaceInformation()));
}
// otherwise use a single-tree planner
else
{
// if we have a default projection
if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
planner = base::PlannerPtr(new geometric::KPIECE1(goal->getSpaceInformation()));
else
planner = base::PlannerPtr(new geometric::RRT(goal->getSpaceInformation()));
}

if (!planner)
throw Exception("Unable to allocate default planner");

return planner;
}

5 comments on commit 78f5b68

@davetcoleman
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This breaks moveit_planners_ompl... fix on the way!

@davetcoleman
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually, could we leave getDefaultPlanners in SimpleSetup in the short run, marked as Deprecated, and have it call the new location one? Otherwise its going to be tricky with using the latest OMPL with MoveIt!. Proper tick-tocking would be nice.

@mamoll
Copy link
Member Author

@mamoll mamoll commented on 78f5b68 Sep 19, 2014

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'll put the functions back and mark them as deprecated. Can you file an issue downstream?

@davetcoleman
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done moveit/moveit_planners#54

@mamoll
Copy link
Member Author

@mamoll mamoll commented on 78f5b68 Sep 19, 2014

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Restored deprecated functions in 0ce70f9

Please sign in to comment.