Fix bugs

moveit · Oct 12, 2023 · b53a5f6 · b53a5f6
1 parent 3b0f51f
commit b53a5f6
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 10 deletions.
diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h
@@ -69,12 +69,13 @@ namespace
  */
 template <class TAdapter>
 [[nodiscard]] bool callAdapterChain(const std::vector<TAdapter>& adapter_vector,
-                                           const planning_scene::PlanningSceneConstPtr& planning_scene,
-                                           planning_interface::MotionPlanRequest& req,
-                                           planning_interface::MotionPlanResponse& res, const rclcpp::Logger& logger)
+                                    const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                    planning_interface::MotionPlanRequest& req,
+                                    planning_interface::MotionPlanResponse& res, const rclcpp::Logger& logger)
 {
   for (const auto& adapter : adapter_vector)
   {
+    assert(adapter);
     RCLCPP_INFO(logger, "Calling PlanningAdapter '%s'", adapter->getDescription().c_str());
     // If adapter does not succeed, break chain and return false
     if (!adapter->adapt(planning_scene, req, res))
@@ -97,9 +98,9 @@ template <class TAdapter>
  */
 template <class TPluginClass>
 void loadPluginVector(const std::shared_ptr<rclcpp::Node>& node,
-                             const std::unique_ptr<pluginlib::ClassLoader<TPluginClass>>& plugin_loader,
-                             std::vector<std::shared_ptr<const TPluginClass>>& plugin_vector,
-                             const std::vector<std::string>& plugin_names, const std::string& parameter_namespace)
+                      const std::unique_ptr<pluginlib::ClassLoader<TPluginClass>>& plugin_loader,
+                      std::vector<std::shared_ptr<const TPluginClass>>& plugin_vector,
+                      const std::vector<std::string>& plugin_names, const std::string& parameter_namespace)
 {
   // Try loading a plugin for each plugin name
   for (const std::string& plugin_name : plugin_names)
@@ -127,7 +128,6 @@ void loadPluginVector(const std::shared_ptr<rclcpp::Node>& node,
     {
       RCLCPP_WARN(node->get_logger(), "Failed to load adapter '%s'", plugin_name.c_str());
     }
-    plugin_vector.push_back(std::move(adapter));
   }
 };
 }  // namespace

diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
@@ -211,11 +211,9 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
   planning_interface::MotionPlanRequest mutable_request = req;
   try
   {
-    RCLCPP_INFO(LOGGER, "Let's plan");
     // Call plan request adapter chain
     if (!planning_request_adapter_vector_.empty())
     {
-      RCLCPP_INFO(LOGGER, "Call plan request adapter chain");
       solved = callAdapterChain<planning_interface::PlanningRequestAdapterConstPtr>(
           planning_request_adapter_vector_, planning_scene, mutable_request, res, node_->get_logger());
     }
@@ -235,7 +233,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
     // Call plan response adapter chain
     if (solved)
     {
-      RCLCPP_INFO(LOGGER, "Call plan response adapter chain");
       if (!planning_request_adapter_vector_.empty())
       {
         solved = callAdapterChain<planning_interface::PlanningResponseAdapterConstPtr>(