Skip to content

Commit

Permalink
Add documentation and cleanups for PlanningRequestAdapter and Plannin…
Browse files Browse the repository at this point in the history 
…gRequestAdapterChain classes (moveit#2142)

* Cleanups

* Add documentation and more cleanups

* Revert size_t change
  • Loading branch information
@sjahr
sjahr authored May 12, 2023
1 parent f807733 commit 226f81c
Show file tree
Hide file tree
Showing 2 changed files with 103 additions and 56 deletions.
83 changes: 64 additions & 19 deletions ...anning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,36 +48,61 @@ namespace planning_request_adapter
{
MOVEIT_CLASS_FORWARD(PlanningRequestAdapter); // Defines PlanningRequestAdapterPtr, ConstPtr, WeakPtr... etc

/** @brief Concept in MoveIt which can be used to modify the planning problem and resulting trajectory (pre-processing
* and/or post-processing) for a motion planner. PlanningRequestAdapter enable using multiple motion planning and
* trajectory generation algorithms in sequence to produce robust motion plans.
*/
class PlanningRequestAdapter
{
public:
/// \brief Functional interface to call a planning function
using PlannerFn =
std::function<bool(const planning_scene::PlanningSceneConstPtr&, const planning_interface::MotionPlanRequest&,
planning_interface::MotionPlanResponse&)>;

PlanningRequestAdapter()
{
}

virtual ~PlanningRequestAdapter()
{
}
virtual ~PlanningRequestAdapter() = default;

/** \brief Initialize parameters using the passed Node and parameter namespace.
* @param node Node instance used by the adapter
* @param parameter_namespace Parameter namespace for adapter
If no initialization is needed, simply implement as empty */
virtual void initialize(const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) = 0;

/// Get a short string that identifies the planning request adapter
/** \brief Get a description of this adapter
* @return Returns a short string that identifies the planning request adapter
*/
virtual std::string getDescription() const
{
return "";
}

/** \brief Adapt the planning request if needed, call the planner
function \e planner and update the planning response if
needed.
* @param planner Pointer to the planner used to solve the passed problem
* @param planning_scene Representation of the environment for the planning
* @param req Motion planning request with a set of constraints
* @param res Reference to which the generated motion plan response is written to
* @return True if response got generated correctly */
bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const;

/** \brief Adapt the planning request if needed, call the planner
function \e planner and update the planning response if
needed. If the response is changed, the index values of the
states added without planning are added to \e
added_path_index
* @param planner Pointer to the planner used to solve the passed problem
* @param planning_scene Representation of the environment for the planning
* @param req Motion planning request with a set of constraints
* @param res Reference to which the generated motion plan response is written to
* @param added_path_index Sometimes planning request adapters may add states on the solution path (e.g.,
add the current state of the robot as prefix, when the robot started to plan only from near that state, as the
current state itself appears to touch obstacles). This is helpful because the added states should not be considered
invalid in all situations.
* @return True if response got generated correctly */
bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
Expand All @@ -87,7 +112,16 @@ class PlanningRequestAdapter
function \e planner and update the planning response if
needed. If the response is changed, the index values of the
states added without planning are added to \e
added_path_index */
added_path_index
* @param planner Pointer to the planner used to solve the passed problem
* @param planning_scene Representation of the environment for the planning
* @param req Motion planning request with a set of constraints
* @param res Reference to which the generated motion plan response is written to
* @param added_path_index Sometimes planning request adapters may add states on the solution path (e.g.,
add the current state of the robot as prefix, when the robot started to plan only from near that state, as the
current state itself appears to touch obstacles). This is helpful because the added states should not be
considered invalid in all situations.
* @return True if response got generated correctly */
virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res,
Expand Down Expand Up @@ -115,24 +149,35 @@ class PlanningRequestAdapter
}
};

/// Apply a sequence of adapters to a motion plan
/** \brief Apply a sequence of adapters to a motion plan */
class PlanningRequestAdapterChain
{
public:
PlanningRequestAdapterChain()
{
}

void addAdapter(const PlanningRequestAdapterConstPtr& adapter)
{
adapters_.push_back(adapter);
}

/** \brief Add new adapter to the back of the chain
* @param adapter Adapter to be added */
void addAdapter(const PlanningRequestAdapterConstPtr& adapter);

/** \brief Iterate through the chain and call all adapters and planners in the correct order
* @param planner Pointer to the planner used to solve the passed problem
* @param planning_scene Representation of the environment for the planning
* @param req Motion planning request with a set of constraints
* @param res Reference to which the generated motion plan response is written to
* @return True if response got generated correctly */
bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const;

/** \brief Iterate through the chain and call all adapters and planners in the correct order
* @param planner Pointer to the planner used to solve the passed problem
* @param planning_scene Representation of the environment for the planning
* @param req Motion planning request with a set of constraints
* @param res Reference to which the generated motion plan response is written to
* @param added_path_index Sometimes planning request adapters may add states on the solution path (e.g.,
add the current state of the robot as prefix, when the robot started to plan only from near that state, as the
current state itself appears to touch obstacles). This is helpful because the added states should not be
considered invalid in all situations.
* @return True if response got generated correctly */
bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
Expand Down
76 changes: 39 additions & 37 deletions moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,17 +103,22 @@ bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManag
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const
{
std::vector<std::size_t> dummy;
return adaptAndPlan(planner, planning_scene, req, res, dummy);
std::vector<std::size_t> empty_added_path_index;
return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index);
}

void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPtr& adapter)
{
adapters_.push_back(adapter);
}

bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const
{
std::vector<std::size_t> dummy;
return adaptAndPlan(planner, planning_scene, req, res, dummy);
std::vector<std::size_t> empty_added_path_index;
return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index);
}

bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
Expand All @@ -128,48 +133,45 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner
added_path_index.clear();
return callPlannerInterfaceSolve(*planner, planning_scene, req, res);
}
else
// the index values added by each adapter
std::vector<std::vector<std::size_t>> added_path_index_each(adapters_.size());

// if there are adapters, construct a function for each, in order,
// so that in the end we have a nested sequence of functions that calls all adapters
// and eventually the planner in the correct order.
PlanningRequestAdapter::PlannerFn fn = [&planner = *planner](const planning_scene::PlanningSceneConstPtr& scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) {
return callPlannerInterfaceSolve(planner, scene, req, res);
};

for (int i = adapters_.size() - 1; i >= 0; --i)
{
// the index values added by each adapter
std::vector<std::vector<std::size_t> > added_path_index_each(adapters_.size());

// if there are adapters, construct a function for each, in order,
// so that in the end we have a nested sequence of functions that calls all adapters
// and eventually the planner in the correct order.
PlanningRequestAdapter::PlannerFn fn = [&planner = *planner](const planning_scene::PlanningSceneConstPtr& scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) {
return callPlannerInterfaceSolve(planner, scene, req, res);
fn = [&adapter = *adapters_[i], fn, &added_path_index = added_path_index_each[i]](
const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) {
return callAdapter(adapter, fn, scene, req, res, added_path_index);
};
}

for (int i = adapters_.size() - 1; i >= 0; --i)
{
fn = [&adapter = *adapters_[i], fn, &added_path_index = added_path_index_each[i]](
const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) {
return callAdapter(adapter, fn, scene, req, res, added_path_index);
};
}

bool result = fn(planning_scene, req, res);
added_path_index.clear();
bool result = fn(planning_scene, req, res);
added_path_index.clear();

// merge the index values from each adapter
for (std::vector<std::size_t>& added_states_by_each_adapter : added_path_index_each)
// merge the index values from each adapter
for (std::vector<std::size_t>& added_states_by_each_adapter : added_path_index_each)
{
for (std::size_t& added_index : added_states_by_each_adapter)
{
for (std::size_t& added_index : added_states_by_each_adapter)
for (std::size_t& index_in_path : added_path_index)
{
for (std::size_t& index_in_path : added_path_index)
{
if (added_index <= index_in_path)
index_in_path++;
}
added_path_index.push_back(added_index);
if (added_index <= index_in_path)
index_in_path++;
}
added_path_index.push_back(added_index);
}
std::sort(added_path_index.begin(), added_path_index.end());
return result;
}
std::sort(added_path_index.begin(), added_path_index.end());
return result;
}

} // end of namespace planning_request_adapter

0 comments on commit 226f81c

Please sign in to comment.