Remove added path index from planner adapter function signature (#2285)

moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h

@@ -65,6 +65,11 @@ struct MotionPlanResponse
   moveit_msgs::msg::RobotState start_state;
   std::string planner_id;
 
+  /// 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.
+  std::vector<std::size_t> added_path_index;  // This won't be included into the MotionPlanResponse ROS 2 message!
+
   // \brief Enable checking of query success or failure, for example if(response) ...
   explicit operator bool() const
   {

moveit_core/planning_request_adapter/include/moveit/planning_request_adapter/planning_request_adapter.h

@@ -46,10 +46,6 @@
 /** \brief Generic interface to adapting motion planning requests */
 namespace planning_request_adapter
 {
-namespace
-{
-std::vector<std::size_t> 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
@@ -84,16 +80,11 @@ class PlanningRequestAdapter
    *  @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 */
   [[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<std::size_t>& added_path_index = empty_path_index_vector) const;
+                                  planning_interface::MotionPlanResponse& res) const;
 
   /** \brief Adapt the planning request if needed, call the planner
       function \e planner and update the planning response if
@@ -104,15 +95,11 @@ class PlanningRequestAdapter
    *  @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 */
-  [[nodiscard]] virtual bool
-  adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
-               const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-               std::vector<std::size_t>& added_path_index = empty_path_index_vector) const = 0;
+  [[nodiscard]] virtual bool adaptAndPlan(const PlannerFn& planner,
+                                          const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                          const planning_interface::MotionPlanRequest& req,
+                                          planning_interface::MotionPlanResponse& res) const = 0;
 };
 
 /** \brief Apply a sequence of adapters to a motion plan */
@@ -128,16 +115,11 @@ class PlanningRequestAdapterChain
    *  @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 */
   [[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<std::size_t>& added_path_index = empty_path_index_vector) const;
+                                  planning_interface::MotionPlanResponse& res) const;
 
 private:
   std::vector<PlanningRequestAdapterConstPtr> adapters_;

moveit_core/planning_request_adapter/src/planning_request_adapter.cpp

@@ -64,20 +64,18 @@ bool callPlannerInterfaceSolve(const planning_interface::PlannerManager& planner
 
 bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAdapter::PlannerFn& planner,
                  const planning_scene::PlanningSceneConstPtr& planning_scene,
-                 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                 std::vector<std::size_t>& added_path_index)
+                 const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res)
 {
   try
   {
-    bool result = adapter.adaptAndPlan(planner, planning_scene, req, res, added_path_index);
+    bool result = adapter.adaptAndPlan(planner, planning_scene, req, res);
     RCLCPP_DEBUG_STREAM(LOGGER, adapter.getDescription() << ": " << moveit::core::error_code_to_string(res.error_code));
     return result;
   }
   catch (std::exception& ex)
   {
     RCLCPP_ERROR(LOGGER, "Exception caught executing adapter '%s': %s\nSkipping adapter instead.",
                  adapter.getDescription().c_str(), ex.what());
-    added_path_index.clear();
     return planner(planning_scene, req, res);
   }
 }
@@ -87,15 +85,14 @@ bool callAdapter(const PlanningRequestAdapter& adapter, const PlanningRequestAda
 bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                                           const planning_scene::PlanningSceneConstPtr& planning_scene,
                                           const planning_interface::MotionPlanRequest& req,
-                                          planning_interface::MotionPlanResponse& res,
-                                          std::vector<std::size_t>& added_path_index) const
+                                          planning_interface::MotionPlanResponse& res) const
 {
   return adaptAndPlan(
       [&planner](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
                  planning_interface::MotionPlanResponse& res) {
         return callPlannerInterfaceSolve(*planner, scene, req, res);
       },
-      planning_scene, req, res, added_path_index);
+      planning_scene, req, res);
 }
 
 void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPtr& adapter)
@@ -106,17 +103,13 @@ void PlanningRequestAdapterChain::addAdapter(const PlanningRequestAdapterConstPt
 bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                const planning_interface::MotionPlanRequest& req,
-                                               planning_interface::MotionPlanResponse& res,
-                                               std::vector<std::size_t>& added_path_index) const
+                                               planning_interface::MotionPlanResponse& res) const
 {
   // if there are no adapters, run the planner directly
   if (adapters_.empty())
   {
-    added_path_index.clear();
     return callPlannerInterfaceSolve(*planner, planning_scene, req, res);
   }
-  // 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
@@ -129,30 +122,14 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner
 
   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);
-    };
+    fn = [&adapter = *adapters_[i],
+          fn](const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
+              planning_interface::MotionPlanResponse& res) { return callAdapter(adapter, fn, scene, req, res); };
   }
 
   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)
-  {
-    for (std::size_t& added_index : added_states_by_each_adapter)
-    {
-      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);
-    }
-  }
-  std::sort(added_path_index.begin(), added_path_index.end());
+  std::sort(res.added_path_index.begin(), res.added_path_index.end());
   return result;
 }
 

moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp

@@ -53,7 +53,7 @@
 
 namespace chomp
 {
-static rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_planner");
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_planner");
 
 class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter
 {
@@ -173,8 +173,8 @@ class OptimizerAdapter : public planning_request_adapter::PlanningRequestAdapter
   }
 
   bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& /*added_path_index*/) const override
+                    const planning_interface::MotionPlanRequest& req,
+                    planning_interface::MotionPlanResponse& res) const override
   {
     RCLCPP_DEBUG(LOGGER, "CHOMP: adaptAndPlan ...");
 

moveit_planners/stomp/src/stomp_moveit_smoothing_adapter.cpp

@@ -71,8 +71,8 @@ class StompSmoothingAdapter : public planning_request_adapter::PlanningRequestAd
   }
 
   bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& ps,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& /*added_path_index*/) const override
+                    const planning_interface::MotionPlanRequest& req,
+                    planning_interface::MotionPlanResponse& res) const override
   {
     // Following call to planner() calls the motion planner defined for the pipeline and stores the trajectory inside
     // the MotionPlanResponse res variable which is then passed to STOMP for optimization

moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h

@@ -136,18 +136,6 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
                     const planning_interface::MotionPlanRequest& req,
                     planning_interface::MotionPlanResponse& res) const;
 
-  /** \brief Call the motion planner plugin and the sequence of planning request adapters (if any).
-      \param planning_scene The planning scene where motion planning is to be done
-      \param req The request for motion planning
-      \param res The motion planning response
-      \param adapter_added_state_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. */
-  bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& adapter_added_state_index) const;
-
   /** \brief Request termination, if a generatePlan() function is currently computing plans */
   void terminate() const;
 

moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp

@@ -151,7 +151,7 @@ void planning_pipeline::PlanningPipeline::configure()
   // load the planner request adapters
   if (!adapter_plugin_names_.empty())
   {
-    std::vector<planning_request_adapter::PlanningRequestAdapterConstPtr> ads;
+    std::vector<planning_request_adapter::PlanningRequestAdapterConstPtr> planning_request_adapter_vector;
     try
     {
       adapter_plugin_loader_ =
@@ -168,31 +168,32 @@ void planning_pipeline::PlanningPipeline::configure()
     {
       for (const std::string& adapter_plugin_name : adapter_plugin_names_)
       {
-        planning_request_adapter::PlanningRequestAdapterPtr ad;
+        planning_request_adapter::PlanningRequestAdapterPtr planning_request_adapter;
         try
         {
-          ad = adapter_plugin_loader_->createUniqueInstance(adapter_plugin_name);
+          planning_request_adapter = adapter_plugin_loader_->createUniqueInstance(adapter_plugin_name);
         }
         catch (pluginlib::PluginlibException& ex)
         {
           RCLCPP_FATAL(LOGGER, "Exception while loading planning adapter plugin '%s': %s", adapter_plugin_name.c_str(),
                        ex.what());
           throw;
         }
-        if (ad)
+        if (planning_request_adapter)
         {
-          ad->initialize(node_, parameter_namespace_);
-          ads.push_back(std::move(ad));
+          planning_request_adapter->initialize(node_, parameter_namespace_);
+          planning_request_adapter_vector.push_back(std::move(planning_request_adapter));
         }
       }
     }
-    if (!ads.empty())
+    if (!planning_request_adapter_vector.empty())
     {
       adapter_chain_ = std::make_unique<planning_request_adapter::PlanningRequestAdapterChain>();
-      for (planning_request_adapter::PlanningRequestAdapterConstPtr& ad : ads)
+      for (planning_request_adapter::PlanningRequestAdapterConstPtr& planning_request_adapter :
+           planning_request_adapter_vector)
       {
-        RCLCPP_INFO(LOGGER, "Using planning request adapter '%s'", ad->getDescription().c_str());
-        adapter_chain_->addAdapter(ad);
+        RCLCPP_INFO(LOGGER, "Using planning request adapter '%s'", planning_request_adapter->getDescription().c_str());
+        adapter_chain_->addAdapter(planning_request_adapter);
       }
     }
   }
@@ -247,15 +248,6 @@ void planning_pipeline::PlanningPipeline::checkSolutionPaths(bool flag)
 bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                        const planning_interface::MotionPlanRequest& req,
                                                        planning_interface::MotionPlanResponse& res) const
-{
-  std::vector<std::size_t> dummy;
-  return generatePlan(planning_scene, req, res, dummy);
-}
-
-bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
-                                                       const planning_interface::MotionPlanRequest& req,
-                                                       planning_interface::MotionPlanResponse& res,
-                                                       std::vector<std::size_t>& adapter_added_state_index) const
 {
   // Set planning pipeline active
   active_ = true;
@@ -265,7 +257,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
   {
     received_request_publisher_->publish(req);
   }
-  adapter_added_state_index.clear();
 
   if (!planner_instance_)
   {
@@ -281,11 +272,11 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
   {
     if (adapter_chain_)
     {
-      solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index);
-      if (!adapter_added_state_index.empty())
+      solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res);
+      if (!res.added_path_index.empty())
       {
         std::stringstream ss;
-        for (std::size_t added_index : adapter_added_state_index)
+        for (std::size_t added_index : res.added_path_index)
           ss << added_index << ' ';
         RCLCPP_INFO(LOGGER, "Planning adapters have added states at index positions: [ %s]", ss.str().c_str());
       }
@@ -327,7 +318,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
         for (std::size_t i = 0; i < index.size() && !problem; ++i)
         {
           bool found = false;
-          for (std::size_t added_index : adapter_added_state_index)
+          for (std::size_t added_index : res.added_path_index)
           {
             if (index[i] == added_index)
             {

moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp

@@ -62,8 +62,8 @@ class AddRuckigTrajectorySmoothing : public planning_request_adapter::PlanningRe
   }
 
   bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& /*added_path_index*/) const override
+                    const planning_interface::MotionPlanRequest& req,
+                    planning_interface::MotionPlanResponse& res) const override
   {
     bool result = planner(planning_scene, req, res);
     if (result && res.trajectory)

moveit_ros/planning/planning_request_adapter_plugins/src/add_time_optimal_parameterization.cpp

@@ -62,8 +62,8 @@ class AddTimeOptimalParameterization : public planning_request_adapter::Planning
   }
 
   bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& /*added_path_index*/) const override
+                    const planning_interface::MotionPlanRequest& req,
+                    planning_interface::MotionPlanResponse& res) const override
   {
     bool result = planner(planning_scene, req, res);
     if (result && res.trajectory)

moveit_ros/planning/planning_request_adapter_plugins/src/empty.cpp

@@ -51,8 +51,8 @@ class Empty : public planning_request_adapter::PlanningRequestAdapter
   }
 
   bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
-                    const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                    std::vector<std::size_t>& /*added_path_index*/) const override
+                    const planning_interface::MotionPlanRequest& req,
+                    planning_interface::MotionPlanResponse& res) const override
   {
     return planner(planning_scene, req, res);
   }