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 9569edc8e13..7aaa6ac113f 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 @@ -46,6 +46,10 @@ /** \brief Generic interface to adapting motion planning requests */ namespace planning_request_adapter { +namespace +{ +std::vector EMPTY_PATH_INDEX_VECTOR = {}; +} 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 @@ -60,8 +64,6 @@ class PlanningRequestAdapter std::function; - 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 @@ -71,23 +73,7 @@ class PlanningRequestAdapter /** \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; + [[nodiscard]] virtual std::string getDescription() const = 0; /** \brief Adapt the planning request if needed, call the planner function \e planner and update the planning response if @@ -103,10 +89,11 @@ class PlanningRequestAdapter 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, - std::vector& added_path_index) const; + [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index = EMPTY_PATH_INDEX_VECTOR) const; /** \brief Adapt the planning request if needed, call the planner function \e planner and update the planning response if @@ -122,10 +109,10 @@ class PlanningRequestAdapter 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, - std::vector& added_path_index) const = 0; + [[nodiscard]] virtual bool + adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, + std::vector& added_path_index = EMPTY_PATH_INDEX_VECTOR) const = 0; protected: /** \brief Helper param for getting a parameter using a namespace **/ @@ -157,17 +144,6 @@ class PlanningRequestAdapterChain * @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 @@ -178,10 +154,11 @@ class PlanningRequestAdapterChain 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, - std::vector& added_path_index) const; + [[nodiscard]] bool adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index = EMPTY_PATH_INDEX_VECTOR) const; private: std::vector adapters_; 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 5144964e038..0a7a7bddfc5 100644 --- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp +++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp @@ -98,29 +98,11 @@ bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManag planning_scene, req, res, added_path_index); } -bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const -{ - 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, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res) const -{ - 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, const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req,