diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 31ee9060580c8e6272b46b12b696f36b7766b8ee..6b8ccafd4762fedd9e0dfaee8044c5f68a0a646d 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -321,7 +321,13 @@ NavfnPlanner::getPlanFromPotential( planner_->setStart(map_goal); - planner_->calcPath(costmap_->getSizeInCellsX() * 4); + int path_len = planner_->calcPath(costmap_->getSizeInCellsX() * 4); + if (path_len == 0) { + RCLCPP_DEBUG(node_->get_logger(), "No path found\n"); + return false; + } + + RCLCPP_DEBUG(node_->get_logger(), "Path found, %d steps\n", path_len); // extract the plan float * x = planner_->getPathX();