fix(behavior_path_planner, behavior_static_obstacle_avoidance_module): crash during goal changes (#10205)

* fix(behavior_path_planner, behavior_static_obstacle_avoidance_module): empty path handling

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

* style(pre-commit): autofix

* refactor: use optional

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

* fix: std

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>

---------

Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: shmpwk

planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp

@@ -575,6 +575,8 @@ class PlannerManager
   ModuleUpdateInfo debug_info_;
 
   std::shared_ptr<SceneModuleVisitor> debug_msg_ptr_;
+
+  mutable std::optional<BehaviorModuleOutput> last_valid_reference_path_;
 };
 }  // namespace autoware::behavior_path_planner
 

planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp

@@ -40,7 +40,8 @@ PlannerManager::PlannerManager(rclcpp::Node & node)
     "autoware_behavior_path_planner",
     "autoware::behavior_path_planner::SceneModuleManagerInterface"),
   logger_(node.get_logger().get_child("planner_manager")),
-  clock_(*node.get_clock())
+  clock_(*node.get_clock()),
+  last_valid_reference_path_(std::nullopt)
 {
   current_route_lanelet_ = std::make_shared<std::optional<lanelet::ConstLanelet>>(std::nullopt);
   processing_time_.emplace("total_time", 0.0);
@@ -272,6 +273,23 @@ BehaviorModuleOutput PlannerManager::getReferencePath(
   const std::shared_ptr<PlannerData> & data) const
 {
   const auto reference_path = utils::getReferencePath(current_route_lanelet_->value(), data);
+
+  if (reference_path.path.points.empty()) {
+    RCLCPP_WARN_THROTTLE(
+      logger_, clock_, 5000,
+      "Empty reference path detected. Using last valid reference path if available.");
+
+    if (last_valid_reference_path_.has_value()) {
+      return last_valid_reference_path_.value();
+    }
+
+    RCLCPP_WARN_THROTTLE(
+      logger_, clock_, 5000, "No valid previous reference path available. Creating empty path.");
+    return BehaviorModuleOutput{};
+  }
+
+  last_valid_reference_path_ = reference_path;
+
   publishDebugRootReferencePath(reference_path);
   return reference_path;
 }

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_utils.cpp

@@ -472,6 +472,14 @@ BehaviorModuleOutput getReferencePath(
     *route_handler, current_lanes_with_backward_margin, no_shift_pose, backward_length,
     p.forward_path_length, p);
 
+  if (reference_path.points.empty()) {
+    auto clock{rclcpp::Clock{RCL_ROS_TIME}};
+    RCLCPP_WARN_THROTTLE(
+      rclcpp::get_logger("path_utils"), clock, 5000, "Empty reference path detected.");
+    BehaviorModuleOutput output;
+    return output;
+  }
+
   // clip backward length
   // NOTE: In order to keep backward_path_length at least, resampling interval is added to the
   // backward.

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

@@ -201,6 +201,13 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
   AvoidancePlanningData & data, DebugData & debug)
 {
   autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
+  if (getPreviousModuleOutput().reference_path.points.empty()) {
+    RCLCPP_WARN_THROTTLE(
+      getLogger(), *clock_, 5000, "Previous module reference path is empty. Skip processing.");
+    return;
+  }
+
   // reference pose
   data.reference_pose =
     utils::getUnshiftedEgoPose(getEgoPose(), helper_->getPreviousSplineShiftPath());
@@ -209,6 +216,11 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
   data.current_lanelets = utils::static_obstacle_avoidance::getCurrentLanesFromPath(
     getPreviousModuleOutput().reference_path, planner_data_);
 
+  if (data.current_lanelets.empty()) {
+    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Current lanelets is empty. Skip processing.");
+    return;
+  }
+
   data.extend_lanelets = utils::static_obstacle_avoidance::getExtendLanes(
     data.current_lanelets, getEgoPose(), planner_data_);
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

@@ -1487,6 +1487,12 @@ lanelet::ConstLanelets getExtendLanes(
   const lanelet::ConstLanelets & lanelets, const Pose & ego_pose,
   const std::shared_ptr<const PlannerData> & planner_data)
 {
+  if (lanelets.empty()) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("static_obstacle_avoidance"), "Empty lanelets provided to getExtendLanes");
+    return lanelets;
+  }
+
   lanelet::ConstLanelets extend_lanelets = lanelets;
 
   while (rclcpp::ok()) {