Add documentation and cleanups for PlanningRequestAdapter and PlanningRequestAdapterChain classes (#2142)

sjahr · web-flow · commit 226f81c6ee6e · 2023-05-12T07:44:00.000Z
* Cleanups

* Add documentation and more cleanups

* Revert size_t change
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
@@ -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,
@@ -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
@@ -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,
@@ -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
