robowflex::OMPL::OMPLInterfacePlanner Class Reference

A planner that directly uses MoveIt!'s OMPL planning interface. More...

#include <ompl_interface.h>

Inheritance diagram for robowflex::OMPL::OMPLInterfacePlanner:

Public Types
using	PrePlanCallback = std::function< void(const ompl_interface::ModelBasedPlanningContextPtr &context, const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request)>
	Type for the callback function to be called right before planning takes place, when the planning context is available. More...
Public Types inherited from robowflex::Planner
using	ProgressProperty = std::function< std::string()>
	A function that returns the value of a planner property over the course of a run. More...

Public Member Functions
	OMPLInterfacePlanner (const RobotPtr &robot, const std::string &name="")
	Constructor. More...
	OMPLInterfacePlanner (OMPLInterfacePlanner const &)=delete
void	operator= (OMPLInterfacePlanner const &)=delete
bool	initialize (const std::string &config_file="", const OMPL::Settings settings=Settings())
	Initialize planning pipeline. More...
planning_interface::MotionPlanResponse	plan (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) override
	Plan a motion given a request and a scene. Uses the planning pipeline's generatePlan() method, which goes through planning adapters. More...
ompl_interface::ModelBasedPlanningContextPtr	getPlanningContext (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) const
	Returns the planning context used for this motion planning request. More...
ompl::geometric::SimpleSetupPtr	getLastSimpleSetup () const
	Get the last OMPL simple setup used in planning. More...
void	refreshContext (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request, bool force=false) const
	Refreshes the internal planning context. More...
std::map< std::string, Planner::ProgressProperty >	getProgressProperties (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) const override
	Retrieve the planner progress property map for this planner given a specific request. More...
std::vector< std::string >	getPlannerConfigs () const override
	Return all planner configurations offered by this planner. Any of the configurations returned can be set as the planner for a motion planning query sent to plan(). More...
void	preRun (const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request) override
	This function is called before benchmarking. More...
ompl_interface::OMPLInterface &	getInterface () const
	Access the OMPLInterface directly, to customize the planning process. More...
void	setPrePlanCallback (const PrePlanCallback &prePlanCallback)
	Set a callback, to be called right before a planning session for last-minute configuration or external bookkeeping. More...
Public Member Functions inherited from robowflex::Planner
	Planner (const RobotPtr &robot, const std::string &name="")
	Constructor. Takes in a robot description and an optional namespace name. If name is specified, planner parameters are namespaced under the namespace of robot. More...
	Planner (Planner const &)=delete
void	operator= (Planner const &)=delete
const RobotPtr	getRobot () const
	Return the robot for this planner. More...
const std::string &	getName () const
	Get the name of the planner. More...

Private Attributes
std::unique_ptr< ompl_interface::OMPLInterface >	interface_ {nullptr}
	Planning interface. More...
std::vector< std::string >	configs_
	Planning configurations. More...
bool	hybridize_
	Whether or not planner should hybridize solutions. More...
bool	interpolate_
	Whether or not planner should interpolate solutions. More...
ID::Key	last_scene_id_ {ID::getNullKey()}
	ID of last scene. More...
std::string	last_request_hash_
	Hash of last request. More...
ompl_interface::ModelBasedPlanningContextPtr	context_
	Last context. More...
ompl::geometric::SimpleSetupPtr	ss_
PrePlanCallback	pre_plan_callback_
	Callback to be called just before planning. More...

Additional Inherited Members
Protected Attributes inherited from robowflex::Planner
RobotPtr	robot_
	The robot to plan for. More...
IO::Handler	handler_
	The parameter handler for the planner. More...
const std::string	name_
	Namespace for the planner. More...

Detailed Description

A planner that directly uses MoveIt!'s OMPL planning interface.

Definition at line 34 of file ompl_interface.h.

Member Typedef Documentation

PrePlanCallback

using robowflex::OMPL::OMPLInterfacePlanner::PrePlanCallback = std::function<void( const ompl_interface::ModelBasedPlanningContextPtr &context, const SceneConstPtr &scene, const planning_interface::MotionPlanRequest &request)>

Type for the callback function to be called right before planning takes place, when the planning context is available.

Parameters

[in]	context	The planning context, contains SimpleSetup that will be used.
[in]	scene	The planning scene being planned on.
[in]	request	The request to plan a path for.

Definition at line 106 of file ompl_interface.h.

Constructor & Destructor Documentation

OMPLInterfacePlanner() [1/2]

OMPL::OMPLInterfacePlanner::OMPLInterfacePlanner	(	const RobotPtr &	robot,
		const std::string &	name = ""
	)

Constructor.

Parameters

[in]	robot	The robot to plan for.
[in]	name	Optional namespace for planner.

Definition at line 14 of file ompl_interface.cpp.

  : Planner(robot, name)
{
}

OMPLInterfacePlanner() [2/2]

robowflex::OMPL::OMPLInterfacePlanner::OMPLInterfacePlanner ( OMPLInterfacePlanner const &  )

delete

Member Function Documentation

getInterface()

ompl_interface::OMPLInterface & OMPL::OMPLInterfacePlanner::getInterface

(

)

const

Access the OMPLInterface directly, to customize the planning process.

Definition at line 148 of file ompl_interface.cpp.

{
    if (!interface_)
    {
        RBX_WARN("Interface is not initialized before call to OMPLInterfacePlanner::initialize.");
    }
    return *interface_;
}

getLastSimpleSetup()

ompl::geometric::SimpleSetupPtr OMPL::OMPLInterfacePlanner::getLastSimpleSetup

(

)

const

Get the last OMPL simple setup used in planning.

Returns

The last OMPL simple setup used.

Definition at line 138 of file ompl_interface.cpp.

{
    return ss_;
}

getPlannerConfigs()

std::vector< std::string > OMPL::OMPLInterfacePlanner::getPlannerConfigs ( ) const

overridevirtual

Return all planner configurations offered by this planner. Any of the configurations returned can be set as the planner for a motion planning query sent to plan().

Returns

A vector of strings of planner configuration names.

Implements robowflex::Planner.

Definition at line 143 of file ompl_interface.cpp.

{
    return configs_;
}

getPlanningContext()

ompl_interface::ModelBasedPlanningContextPtr OMPL::OMPLInterfacePlanner::getPlanningContext	(	const SceneConstPtr &	scene,
		const planning_interface::MotionPlanRequest &	request
	)		const

Returns the planning context used for this motion planning request.

Parameters

[in]	scene	A planning scene for the same robot_ to compute the plan in.
[in]	request	The motion planning request to solve.

Returns

The motion planning context used by the planner.

Definition at line 47 of file ompl_interface.cpp.

{
    return interface_->getPlanningContext(scene->getSceneConst(), request);
}

getProgressProperties()

std::map< std::string, Planner::ProgressProperty > OMPL::OMPLInterfacePlanner::getProgressProperties ( const SceneConstPtr &  scene, const planning_interface::MotionPlanRequest &  request  ) const

overridevirtual

Retrieve the planner progress property map for this planner given a specific request.

Parameters

[in]	scene	A planning scene for the same robot_ to compute the plan in.
[in]	request	Request to get progress properties for.

Returns

The map of progress properties.

Reimplemented from robowflex::Planner.

Definition at line 76 of file ompl_interface.cpp.

{
    refreshContext(scene, request);
    if (not ss_)
        return {};
 
    const auto &planner = ss_->getPlanner();
 
#if ROBOWFLEX_AT_LEAST_KINETIC
    return planner->getPlannerProgressProperties();
 
    // As in Indigo they are boost::function
#else
    std::map<std::string, Planner::ProgressProperty> ret;
    for (const auto &pair : planner->getPlannerProgressProperties())
    {
        auto function = pair.second;
        ret[pair.first] = [function] { return function(); };
    }
 
    return ret;
#endif
}

initialize()

bool OMPL::OMPLInterfacePlanner::initialize	(	const std::string &	config_file = "",
		const OMPL::Settings	settings = Settings()
	)

Initialize planning pipeline.

Parameters

[in]	config_file	A YAML file containing OMPL planner configurations.
[in]	settings	Settings to set on the parameter server.

Returns

True upon success, false on failure.

Definition at line 19 of file ompl_interface.cpp.

{
    if (!loadOMPLConfig(handler_, config_file, configs_))
        return false;
 
    interface_.reset(new ompl_interface::OMPLInterface(robot_->getModel(), handler_.getHandle()));
 
    settings.setParam(handler_);
 
    interface_->simplifySolutions(settings.simplify_solutions);
    hybridize_ = settings.hybridize_solutions;
    interpolate_ = settings.interpolate_solutions;
 
    auto &pcm = interface_->getPlanningContextManager();
    pcm.setMaximumSolutionSegmentLength(settings.maximum_waypoint_distance);
    pcm.setMinimumWaypointCount(settings.minimum_waypoint_count);
 
    pcm.setMaximumPlanningThreads(settings.max_planning_threads);
    pcm.setMaximumGoalSamples(settings.max_goal_samples);
    pcm.setMaximumStateSamplingAttempts(settings.max_state_sampling_attempts);
    pcm.setMaximumGoalSamplingAttempts(settings.max_goal_sampling_attempts);
 
    if (settings.max_solution_segment_length > std::numeric_limits<double>::epsilon())
        pcm.setMaximumSolutionSegmentLength(settings.max_solution_segment_length);
 
    return true;
}

operator=()

void robowflex::OMPL::OMPLInterfacePlanner::operator= ( OMPLInterfacePlanner const &  )

delete

plan()

planning_interface::MotionPlanResponse OMPL::OMPLInterfacePlanner::plan ( const SceneConstPtr &  scene, const planning_interface::MotionPlanRequest &  request  )

overridevirtual

Plan a motion given a request and a scene. Uses the planning pipeline's generatePlan() method, which goes through planning adapters.

Parameters

[in]	scene	A planning scene for the same robot_ to compute the plan in.
[in]	request	The motion planning request to solve.

Returns

The motion planning response generated by the planner.

Implements robowflex::Planner.

Definition at line 59 of file ompl_interface.cpp.

{
    planning_interface::MotionPlanResponse response;
    response.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
 
    refreshContext(scene, request);
    if (not ss_)
        return response;
 
    if (pre_plan_callback_)
        pre_plan_callback_(context_, scene, request);
 
    context_->solve(response);
    return response;
}

preRun()

void OMPL::OMPLInterfacePlanner::preRun ( const SceneConstPtr &  scene, const planning_interface::MotionPlanRequest &  request  )

overridevirtual

This function is called before benchmarking.

Parameters

[in]	scene	Scene to plan for.
[in]	request	Planning request.

Reimplemented from robowflex::Planner.

Definition at line 53 of file ompl_interface.cpp.

{
    refreshContext(scene, request, true);
}

refreshContext()

void OMPL::OMPLInterfacePlanner::refreshContext	(	const SceneConstPtr &	scene,
		const planning_interface::MotionPlanRequest &	request,
		bool	force = false
	)		const

Refreshes the internal planning context.

Parameters

[in]	scene	A planning scene for the same robot_ to compute the plan in.
[in]	request	The motion planning request to solve.
[in]	force	If true, forces a refresh of the context.

Definition at line 101 of file ompl_interface.cpp.

{
    const auto &scene_id = scene->getKey();
    const auto &request_hash = IO::getMessageMD5(request);
 
    bool same_scene = compareIDs(scene_id, last_scene_id_);
    bool same_request = request_hash == last_request_hash_;
 
    if (not force and ss_ and same_scene and same_request)
    {
        RBX_INFO("Reusing Cached Context!");
        return;
    }
 
    context_ = getPlanningContext(scene, request);
    if (not context_)
    {
        RBX_ERROR("Context was not set!");
        ss_ = nullptr;
        return;
    }
 
#if ROBOWFLEX_AT_LEAST_MELODIC
    context_->setInterpolation(interpolate_);
    context_->setHybridize(hybridize_);
#endif
 
    ss_ = context_->getOMPLSimpleSetup();
 
    last_scene_id_ = scene_id;
    last_request_hash_ = request_hash;
 
    RBX_INFO("Refreshed Context!");
}

setPrePlanCallback()

void OMPL::OMPLInterfacePlanner::setPrePlanCallback

(

const PrePlanCallback &

prePlanCallback

)

Set a callback, to be called right before a planning session for last-minute configuration or external bookkeeping.

refreshContext() will already have been called, so the SimpleSetup obtained by getLastSimpleSetup() will be the one used for planning.

Definition at line 157 of file ompl_interface.cpp.

{
    pre_plan_callback_ = prePlanCallback;
}

Member Data Documentation

configs_

std::vector<std::string> robowflex::OMPL::OMPLInterfacePlanner::configs_

private

Planning configurations.

Definition at line 120 of file ompl_interface.h.

context_

ompl_interface::ModelBasedPlanningContextPtr robowflex::OMPL::OMPLInterfacePlanner::context_

mutableprivate

Last context.

Definition at line 127 of file ompl_interface.h.

hybridize_

bool robowflex::OMPL::OMPLInterfacePlanner::hybridize_

private

Whether or not planner should hybridize solutions.

Definition at line 121 of file ompl_interface.h.

interface_

std::unique_ptr<ompl_interface::OMPLInterface> robowflex::OMPL::OMPLInterfacePlanner::interface_ {nullptr}

private

Planning interface.

Definition at line 119 of file ompl_interface.h.

interpolate_

bool robowflex::OMPL::OMPLInterfacePlanner::interpolate_

private

Whether or not planner should interpolate solutions.

Definition at line 122 of file ompl_interface.h.

last_request_hash_

std::string robowflex::OMPL::OMPLInterfacePlanner::last_request_hash_

mutableprivate

Hash of last request.

Definition at line 125 of file ompl_interface.h.

last_scene_id_

ID::Key robowflex::OMPL::OMPLInterfacePlanner::last_scene_id_ {ID::getNullKey()}

mutableprivate

ID of last scene.

Definition at line 124 of file ompl_interface.h.

pre_plan_callback_

PrePlanCallback robowflex::OMPL::OMPLInterfacePlanner::pre_plan_callback_

private

Callback to be called just before planning.

Definition at line 131 of file ompl_interface.h.

ss_

ompl::geometric::SimpleSetupPtr robowflex::OMPL::OMPLInterfacePlanner::ss_

mutableprivate

Last OMPL simple setup used for planning.

Definition at line 128 of file ompl_interface.h.

---

The documentation for this class was generated from the following files:

• ompl_interface.h
• ompl_interface.cpp