Merge branch 'main' into fix/check_servo_frames_before_using

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

@@ -194,7 +194,7 @@ class PlannerManager
   virtual bool canServiceRequest(const MotionPlanRequest& req) const = 0;
 
   /// \brief Specify the settings to be used for specific algorithms
-  virtual void setPlannerConfigurations(const PlannerConfigurationMap& pcs) = 0;
+  virtual void setPlannerConfigurations(const PlannerConfigurationMap& pcs);
 
   /// \brief Get the settings for a specific algorithm
   const PlannerConfigurationMap& getPlannerConfigurations() const

moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp

@@ -55,17 +55,10 @@ class CHOMPPlannerManager : public planning_interface::PlannerManager
   bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node,
                   const std::string& /* unused */) override
   {
-    planning_interface::PlannerConfigurationMap pconfig;
     for (const std::string& group : model->getJointModelGroupNames())
     {
       planning_contexts_[group] = std::make_shared<CHOMPPlanningContext>("chomp_planning_context", group, model, node);
-      const planning_interface::PlannerConfigurationSettings planner_config_settings{
-        group, group, std::map<std::string, std::string>()
-      };
-      pconfig[planner_config_settings.name] = planner_config_settings;
     }
-
-    setPlannerConfigurations(pconfig);
     return true;
   }
 
@@ -120,11 +113,6 @@ class CHOMPPlannerManager : public planning_interface::PlannerManager
     algs[0] = "CHOMP";
   }
 
-  void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pcs) override
-  {
-    config_settings_ = pcs;
-  }
-
 protected:
   std::map<std::string, CHOMPPlanningContextPtr> planning_contexts_;
 };

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.h

@@ -117,12 +117,6 @@ class CommandPlanner : public planning_interface::PlannerManager
    */
   void registerContextLoader(const pilz_industrial_motion_planner::PlanningContextLoaderPtr& planning_context_loader);
 
-  /**
-   * @brief Specify the settings to be used for an algorithms
-   * @param pcs Map of planner configurations
-   */
-  void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pcs) override;
-
 private:
   /// Plugin loader
   std::unique_ptr<pluginlib::ClassLoader<PlanningContextLoader>> planner_context_loader;

moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp

@@ -103,19 +103,6 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c
     registerContextLoader(loader_pointer);
   }
 
-  // Specify for which joint model groups this planner is usable
-  planning_interface::PlannerConfigurationMap pconfig;
-
-  for (const auto& group : model_->getJointModelGroupNames())
-  {
-    const planning_interface::PlannerConfigurationSettings planner_config_settings{
-      group, group, std::map<std::string, std::string>()
-    };
-    pconfig[planner_config_settings.name] = planner_config_settings;
-  }
-
-  setPlannerConfigurations(pconfig);
-
   return true;
 }
 
@@ -188,11 +175,6 @@ void CommandPlanner::registerContextLoader(
   }
 }
 
-void CommandPlanner::setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pcs)
-{
-  config_settings_ = pcs;
-}
-
 }  // namespace pilz_industrial_motion_planner
 
 PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::CommandPlanner, planning_interface::PlannerManager)

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h

@@ -152,10 +152,6 @@ class MoveItCpp
   /** \brief Get all loaded planning pipeline instances mapped to their reference names */
   const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& getPlanningPipelines() const;
 
-  /** \brief Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning group
-   */
-  std::set<std::string> getPlanningPipelineNames(const std::string& group_name = "") const;
-
   /** \brief Get the stored instance of the planning scene monitor */
   const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const;
   planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst();
@@ -183,7 +179,6 @@ class MoveItCpp
 
   // Planning
   std::map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
-  std::map<std::string, std::set<std::string>> groups_pipelines_map_;
   std::map<std::string, std::set<std::string>> groups_algorithms_map_;
 
   // Execution

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h

@@ -216,7 +216,6 @@ class PlanningComponent
   const moveit::core::JointModelGroup* joint_model_group_;
 
   // Planning
-  std::set<std::string> planning_pipeline_names_;
   // The start state used in the planning motion request
   moveit::core::RobotStatePtr considered_start_state_;
   std::vector<moveit_msgs::msg::Constraints> current_goal_constraints_;

moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp

@@ -157,23 +157,6 @@ bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options)
     return false;
   }
 
-  // Retrieve group/pipeline mapping for faster lookup
-  std::vector<std::string> group_names = robot_model_->getJointModelGroupNames();
-  for (const auto& pipeline_entry : planning_pipelines_)
-  {
-    for (const auto& group_name : group_names)
-    {
-      const auto& pipeline = pipeline_entry.second;
-      for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations())
-      {
-        if (planner_configuration.second.group == group_name)
-        {
-          groups_pipelines_map_[group_name].insert(pipeline_entry.first);
-        }
-      }
-    }
-  }
-
   return true;
 }
 
@@ -213,18 +196,6 @@ const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& MoveItCpp::
   return planning_pipelines_;
 }
 
-std::set<std::string> MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const
-{
-  if (group_name.empty() || groups_pipelines_map_.count(group_name) == 0)
-  {
-    RCLCPP_ERROR(LOGGER, "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.",
-                 group_name.c_str());
-    return {};  // empty
-  }
-
-  return groups_pipelines_map_.at(group_name);
-}
-
 const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const
 {
   return planning_scene_monitor_;

moveit_ros/planning/moveit_cpp/src/planning_component.cpp

@@ -57,7 +57,6 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const MoveIt
     RCLCPP_FATAL_STREAM(LOGGER, error);
     throw std::runtime_error(error);
   }
-  planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name);
   plan_request_parameters_.load(node_);
   RCLCPP_DEBUG_STREAM(
       LOGGER, "Default plan request parameters loaded with --"
@@ -79,7 +78,6 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const rclcpp
     RCLCPP_FATAL_STREAM(LOGGER, error);
     throw std::runtime_error(error);
   }
-  planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name);
 }
 
 const std::vector<std::string> PlanningComponent::getNamedTargetStates()
@@ -178,7 +176,9 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest
 
   // Run planning attempt
   ::planning_interface::MotionPlanResponse res;
-  if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end())
+  const auto& pipelines = moveit_cpp_->getPlanningPipelines();
+  auto it = pipelines.find(parameters.planning_pipeline);
+  if (it == pipelines.end())
   {
     RCLCPP_ERROR(LOGGER, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str());
     plan_solution.error_code_ = moveit::core::MoveItErrorCode::FAILURE;
@@ -188,8 +188,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan(const PlanRequest
     }
     return plan_solution;
   }
-  const planning_pipeline::PlanningPipelinePtr pipeline =
-      moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline);
+  const planning_pipeline::PlanningPipelinePtr pipeline = it->second;
   pipeline->generatePlan(planning_scene, req, res);
 
   plan_solution.error_code_ = res.error_code_;