Skip to content

Commit

Permalink
Add OMPL planner 'AnytimePathShortening' (moveit#1686)
Browse files Browse the repository at this point in the history 
* Add OMPL planner 'AnytimePathShortening'

* clean up registration of planner allocators

* add parameter to geometric::AnytimePathShortening method to set planner instances and their parameters via config file

* clang-format

* make anytime path planning backwards compatible
  • Loading branch information
@mamoll @henningkayser
mamoll authored and henningkayser committed Oct 31, 2019
1 parent d8ad630 commit 9af7a5e
Show file tree
Hide file tree
Showing 4 changed files with 45 additions and 75 deletions.
100 changes: 26 additions & 74 deletions moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <set>
#include <utility>

#include <ompl/geometric/planners/AnytimePathShortening.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/rrt/pRRT.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
Expand Down Expand Up @@ -114,7 +115,6 @@ static ompl::base::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si,
if (!new_name.empty())
planner->setName(new_name);
planner->params().setParams(spec.config_, true);
planner->setup();
return planner;
}
} // namespace
Expand All @@ -134,79 +134,31 @@ ompl_interface::PlanningContextManager::plannerSelector(const std::string& plann

void ompl_interface::PlanningContextManager::registerDefaultPlanners()
{
registerPlannerAllocator( //
"geometric::RRT", //
std::bind(&allocatePlanner<og::RRT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::RRTConnect", //
std::bind(&allocatePlanner<og::RRTConnect>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::LazyRRT", //
std::bind(&allocatePlanner<og::LazyRRT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::TRRT", //
std::bind(&allocatePlanner<og::TRRT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::EST", //
std::bind(&allocatePlanner<og::EST>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::SBL", //
std::bind(&allocatePlanner<og::SBL>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::KPIECE", //
std::bind(&allocatePlanner<og::KPIECE1>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::BKPIECE", //
std::bind(&allocatePlanner<og::BKPIECE1>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::LBKPIECE", //
std::bind(&allocatePlanner<og::LBKPIECE1>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::RRTstar", //
std::bind(&allocatePlanner<og::RRTstar>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::PRM", //
std::bind(&allocatePlanner<og::PRM>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::PRMstar", //
std::bind(&allocatePlanner<og::PRMstar>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::FMT", //
std::bind(&allocatePlanner<og::FMT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::BFMT", //
std::bind(&allocatePlanner<og::BFMT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::PDST", //
std::bind(&allocatePlanner<og::PDST>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::STRIDE", //
std::bind(&allocatePlanner<og::STRIDE>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::BiTRRT", //
std::bind(&allocatePlanner<og::BiTRRT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::LBTRRT", //
std::bind(&allocatePlanner<og::LBTRRT>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::BiEST", //
std::bind(&allocatePlanner<og::BiEST>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::ProjEST", //
std::bind(&allocatePlanner<og::ProjEST>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::LazyPRM", //
std::bind(&allocatePlanner<og::LazyPRM>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::LazyPRMstar", //
std::bind(&allocatePlanner<og::LazyPRMstar>, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3));
registerPlannerAllocator( //
"geometric::SPARS", //
std::bind(&allocatePlanner<og::SPARS>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator( //
"geometric::SPARStwo", //
std::bind(&allocatePlanner<og::SPARStwo>, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
registerPlannerAllocator("geometric::AnytimePathShortening", allocatePlanner<og::AnytimePathShortening>);
registerPlannerAllocator("geometric::BFMT", allocatePlanner<og::BFMT>);
registerPlannerAllocator("geometric::BiEST", allocatePlanner<og::BiEST>);
registerPlannerAllocator("geometric::BiTRRT", allocatePlanner<og::BiTRRT>);
registerPlannerAllocator("geometric::BKPIECE", allocatePlanner<og::BKPIECE1>);
registerPlannerAllocator("geometric::EST", allocatePlanner<og::EST>);
registerPlannerAllocator("geometric::FMT", allocatePlanner<og::FMT>);
registerPlannerAllocator("geometric::KPIECE", allocatePlanner<og::KPIECE1>);
registerPlannerAllocator("geometric::LazyPRM", allocatePlanner<og::LazyPRM>);
registerPlannerAllocator("geometric::LazyPRMstar", allocatePlanner<og::LazyPRMstar>);
registerPlannerAllocator("geometric::LazyRRT", allocatePlanner<og::LazyRRT>);
registerPlannerAllocator("geometric::LBKPIECE", allocatePlanner<og::LBKPIECE1>);
registerPlannerAllocator("geometric::LBTRRT", allocatePlanner<og::LBTRRT>);
registerPlannerAllocator("geometric::PDST", allocatePlanner<og::PDST>);
registerPlannerAllocator("geometric::PRM", allocatePlanner<og::PRM>);
registerPlannerAllocator("geometric::PRMstar", allocatePlanner<og::PRMstar>);
registerPlannerAllocator("geometric::ProjEST", allocatePlanner<og::ProjEST>);
registerPlannerAllocator("geometric::RRT", allocatePlanner<og::RRT>);
registerPlannerAllocator("geometric::RRTConnect", allocatePlanner<og::RRTConnect>);
registerPlannerAllocator("geometric::RRTstar", allocatePlanner<og::RRTstar>);
registerPlannerAllocator("geometric::SBL", allocatePlanner<og::SBL>);
registerPlannerAllocator("geometric::SPARS", allocatePlanner<og::SPARS>);
registerPlannerAllocator("geometric::SPARStwo", allocatePlanner<og::SPARStwo>);
registerPlannerAllocator("geometric::STRIDE", allocatePlanner<og::STRIDE>);
registerPlannerAllocator("geometric::TRRT", allocatePlanner<og::TRRT>);
}

void ompl_interface::PlanningContextManager::registerDefaultStateSpaces()
Expand Down
3 changes: 2 additions & 1 deletion moveit_setup_assistant/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,8 @@ find_package(catkin REQUIRED COMPONENTS
urdf
srdfdom
)
include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
find_package(ompl REQUIRED)
include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${OMPL_INCLUDE_DIRS})

# Qt Stuff
if(rviz_QT_VERSION VERSION_LESS "5")
Expand Down
1 change: 1 addition & 0 deletions moveit_setup_assistant/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
<build_depend>libogre-dev</build_depend>
<build_depend>qtbase5-dev</build_depend>
<build_depend>libqt5-opengl-dev</build_depend>
<build_depend>ompl</build_depend>

<depend>moveit_core</depend>
<depend>moveit_ros_planning</depend>
Expand Down
16 changes: 16 additions & 0 deletions moveit_setup_assistant/src/tools/moveit_config_data.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,9 @@
#include <ros/console.h>
#include <ros/package.h> // for getting file path for loading images

// OMPL version
#include <ompl/config.h>

namespace moveit_setup_assistant
{
// File system
Expand Down Expand Up @@ -546,6 +549,19 @@ std::vector<OMPLPlannerDescription> MoveItConfigData::getOMPLPlanners()
{
std::vector<OMPLPlannerDescription> planner_des;

OMPLPlannerDescription aps("AnytimePathShortening", "geometric");
aps.addParameter("shortcut", "true", "Attempt to shortcut all new solution paths");
aps.addParameter("hybridize", "true", "Compute hybrid solution trajectories");
aps.addParameter("max_hybrid_paths", "24", "Number of hybrid paths generated per iteration");
aps.addParameter("num_planners", "4", "The number of default planners to use for planning");
#if OMPL_VERSION_VALUE >= 1005000
// This parameter was added in OMPL 1.5.0
aps.addParameter("planners", "", "A comma-separated list of planner types (e.g., \"PRM,EST,RRTConnect\""
"Optionally, planner parameters can be passed to change the default:"
"\"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]\"");
#endif
planner_des.push_back(aps);

OMPLPlannerDescription sbl("SBL", "geometric");
sbl.addParameter("range", "0.0", "Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()");
planner_des.push_back(sbl);
Expand Down

0 comments on commit 9af7a5e

Please sign in to comment.