Fix small bug

moveit · Nov 17, 2023 · 2e8c166 · 2e8c166
1 parent c39cee9
commit 2e8c166
Showing 1 changed file with 27 additions and 21 deletions.
diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
@@ -288,28 +288,34 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr&
     // Call planners
     for (const auto& planner : planner_vector_)
     {
-      if (res.error_code)
+      // Update reference trajectory with latest solution (if available)
+      if (res.trajectory)
       {
-        // Update reference trajectory with latest solution (if available)
-        if (res.trajectory)
-        {
-          mutable_request.trajectory_constraints.constraints = getTrajectoryConstraints(res.trajectory);
-        }
-        planning_interface::PlanningContextPtr context =
-            planner->getPlanningContext(planning_scene, mutable_request, res.error_code);
-        if (context)
-        {
-          RCLCPP_INFO(node_->get_logger(), "Calling Planner '%s'", planner->getDescription().c_str());
-          context->solve(res);
-          publishPipelineState(mutable_request, res, planner->getDescription());
-        }
-        else
-        {
-          RCLCPP_ERROR(node_->get_logger(),
-                       "Failed to create PlanningContext for planner '%s'. Aborting planning pipeline.",
-                       planner->getDescription().c_str());
-          res.error_code = moveit::core::MoveItErrorCode::PLANNING_FAILED;
-        }
+        mutable_request.trajectory_constraints.constraints = getTrajectoryConstraints(res.trajectory);
+      }
+
+      // Try creating a planning context
+      planning_interface::PlanningContextPtr context =
+          planner->getPlanningContext(planning_scene, mutable_request, res.error_code);
+      if (!context)
+      {
+        RCLCPP_ERROR(node_->get_logger(),
+                     "Failed to create PlanningContext for planner '%s'. Aborting planning pipeline.",
+                     planner->getDescription().c_str());
+        res.error_code = moveit::core::MoveItErrorCode::PLANNING_FAILED;
+        break;
+      }
+
+      // Run planner
+      RCLCPP_INFO(node_->get_logger(), "Calling Planner '%s'", planner->getDescription().c_str());
+      context->solve(res);
+      publishPipelineState(mutable_request, res, planner->getDescription());
+
+      // If planner does not succeed, break chain and return false
+      if (!res.error_code)
+      {
+        RCLCPP_ERROR(node_->get_logger(), "Planner '%s' failed", planner->getDescription());
+        break;
       }
     }