From 226f81c6ee6ec30826d562450eb8b6d5f1af0084 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 12 May 2023 09:44:00 +0200 Subject: [PATCH] Add documentation and cleanups for PlanningRequestAdapter and PlanningRequestAdapterChain classes (#2142) * Cleanups * Add documentation and more cleanups * Revert size_t change --- .../planning_request_adapter.h | 83 ++++++++++++++----- .../src/planning_request_adapter.cpp | 76 ++++++++--------- 2 files changed, 103 insertions(+), 56 deletions(-) diff --git a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h index 624370d98a..9569edc8e1 100644 --- a/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h +++ b/moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h @@ -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; - 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, @@ -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, @@ -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, diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp index da8245cac3..5144964e03 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -103,8 +103,13 @@ bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManag const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const { - std::vector dummy; - return adaptAndPlan(planner, planning_scene, req, res, dummy); + std::vector 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, @@ -112,8 +117,8 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res) const { - std::vector dummy; - return adaptAndPlan(planner, planning_scene, req, res, dummy); + std::vector empty_added_path_index; + return adaptAndPlan(planner, planning_scene, req, res, empty_added_path_index); } bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, @@ -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> 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 > 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& added_states_by_each_adapter : added_path_index_each) + // merge the index values from each adapter + for (std::vector& 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