feat(behavior_path_planner): add error handling for null output (#4352)

* feat: add curvature consideration in shift start

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

---------

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Author: TakaHoribe

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -449,6 +449,8 @@ class PlannerManager
     const std::vector<SceneModulePtr> & request_modules, const std::shared_ptr<PlannerData> & data,
     const BehaviorModuleOutput & previous_module_output);
 
+  std::string getNames(const std::vector<SceneModulePtr> & modules) const;
+
   boost::optional<lanelet::ConstLanelet> root_lanelet_{boost::none};
 
   std::vector<SceneModuleManagerPtr> manager_ptrs_;

planning/behavior_path_planner/src/planner_manager.cpp

@@ -178,6 +178,14 @@ void PlannerManager::generateCombinedDrivableArea(
 std::vector<SceneModulePtr> PlannerManager::getRequestModules(
   const BehaviorModuleOutput & previous_module_output) const
 {
+  if (!previous_module_output.path) {
+    RCLCPP_ERROR_STREAM(
+      logger_, "Current module output is null. Skip candidate module check."
+                 << "\n      - Approved  module list: " << getNames(approved_module_ptrs_)
+                 << "\n      - Candidate module list: " << getNames(candidate_module_ptrs_));
+    return {};
+  }
+
   std::vector<SceneModulePtr> request_modules{};
 
   /**
@@ -742,4 +750,13 @@ std::shared_ptr<SceneModuleVisitor> PlannerManager::getDebugMsg()
   return debug_msg_ptr_;
 }
 
+std::string PlannerManager::getNames(const std::vector<SceneModulePtr> & modules) const
+{
+  std::stringstream ss;
+  for (const auto & m : modules) {
+    ss << "[" << m->name() << "], ";
+  }
+  return ss.str();
+}
+
 }  // namespace behavior_path_planner