fix(goal_planner): fix sudden stop and simpify process

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -240,35 +240,31 @@
 
   void reset()
   {
-    stop_path = nullptr;
-    stop_path_after_approval = nullptr;
     found_path = false;
     safety_status = SafetyStatus{};
     has_decided_path = false;
   }
 
-  std::shared_ptr<PathWithLaneId> stop_path{nullptr};
-  std::shared_ptr<PathWithLaneId> stop_path_after_approval{nullptr};
   bool found_path{false};
   SafetyStatus safety_status{};
   bool has_decided_path{false};
 };
 
 // store stop_pose_ pointer with reason string
 struct PoseWithString
 {
   std::optional<Pose> * pose;
   std::string string;
 
   explicit PoseWithString(std::optional<Pose> * shared_pose) : pose(shared_pose), string("") {}
 
   void set(const Pose & new_pose, const std::string & new_string)
   {
     *pose = new_pose;
     string = new_string;
   }
 
   void set(const std::string & new_string) { string = new_string; }
 
   void clear()
   {
@@ -387,7 +383,7 @@
   ThreadSafeData thread_safe_data_;
 
   std::unique_ptr<LastApprovalData> last_approval_data_{nullptr};
-  PreviousPullOverData prev_data_{nullptr};
+  PreviousPullOverData prev_data_{};
 
   // approximate distance from the start point to the end point of pull_over.
   // this is used as an assumed value to decelerate, etc., before generating the actual path.
@@ -428,7 +424,7 @@
   void decelerateBeforeSearchStart(
     const Pose & search_start_offset_pose, PathWithLaneId & path) const;
   PathWithLaneId generateStopPath() const;
-  PathWithLaneId generateFeasibleStopPath() const;
+  PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const;
 
   void keepStoppedWithCurrentPath(PathWithLaneId & path) const;
   double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;
@@ -477,19 +473,8 @@
 
   // output setter
   void setOutput(BehaviorModuleOutput & output) const;
-  void setStopPath(BehaviorModuleOutput & output) const;
-  void updatePreviousData(const BehaviorModuleOutput & output);
+  void updatePreviousData();
 
-  /**
-   * @brief Sets a stop path in the current path based on safety conditions and previous paths.
-   *
-   * This function sets a stop path in the current path. Depending on whether the previous safety
-   * judgement against dynamic objects were safe or if a previous stop path existed, it either
-   * generates a new stop path or uses the previous stop path.
-   *
-   * @param output BehaviorModuleOutput
-   */
-  void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const;
   void setModifiedGoal(BehaviorModuleOutput & output) const;
   void setTurnSignalInfo(BehaviorModuleOutput & output) const;
 

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -61,8 +61,8 @@
 : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{parameters},
   vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
   thread_safe_data_{mutex_, clock_},
   debug_stop_pose_with_info_{&stop_pose_}
 {
   LaneDepartureChecker lane_departure_checker{};
   lane_departure_checker.setVehicleInfo(vehicle_info_);
@@ -276,7 +276,7 @@
 
 void GoalPlannerModule::onFreespaceParkingTimer()
 {
   if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }
 
@@ -806,7 +806,9 @@
 
   if (!thread_safe_data_.foundPullOverPath()) {
     // situation : not safe against static objects use stop_path
-    setStopPath(output);
+    output.path = generateStopPath();
+    RCLCPP_WARN_THROTTLE(
+      getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path");
     setDrivableAreaInfo(output);
     return;
   }
@@ -816,7 +818,11 @@
     // situation : not safe against dynamic objects after approval
     // insert stop point in current path if ego is able to stop with acceleration and jerk
     // constraints
-    setStopPathFromCurrentPath(output);
+    output.path =
+      generateFeasibleStopPath(thread_safe_data_.get_pull_over_path()->getCurrentPath());
+    RCLCPP_WARN_THROTTLE(
+      getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path");
+    debug_stop_pose_with_info_.set(std::string("feasible stop after approval"));
   } else {
     // situation : (safe against static and dynamic objects) or (safe against static objects and
     // before approval) don't stop
@@ -835,59 +841,7 @@
     setTurnSignalInfo(output);
   }
 }
 
-void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const
-{
-  if (prev_data_.found_path || !prev_data_.stop_path) {
-    // safe -> not_safe or no prev_stop_path: generate new stop_path
-    output.path = generateStopPath();
-    RCLCPP_WARN_THROTTLE(
-      getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path");
-  } else {
-    // not_safe -> not_safe: use previous stop path
-    output.path = *prev_data_.stop_path;
-    RCLCPP_WARN_THROTTLE(
-      getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path");
-    // stop_pose_ is removed in manager every loop, so need to set every loop.
-    const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path);
-    if (stop_pose_opt) {
-      debug_stop_pose_with_info_.set(stop_pose_opt.value(), std::string("keep previous stop pose"));
-    }
-  }
-}
-
-void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const
-{
-  // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path
-  if (prev_data_.safety_status.is_safe || !prev_data_.stop_path_after_approval) {
-    auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
-    const auto stop_path =
-      behavior_path_planner::utils::parking_departure::generateFeasibleStopPath(
-        current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop,
-        parameters_->maximum_jerk_for_stop);
-    if (stop_path) {
-      output.path = *stop_path;
-      debug_stop_pose_with_info_.set(std::string("feasible stop after approval"));
-      RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path");
-    } else {
-      output.path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
-      RCLCPP_WARN_THROTTLE(
-        getLogger(), *clock_, 5000,
-        "Collision detected, no feasible stop path found, cannot stop.");
-    }
-  } else {
-    // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path
-    output.path = *prev_data_.stop_path_after_approval;
-    // stop_pose_ is removed in manager every loop, so need to set every loop.
-    const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path);
-    if (stop_pose_opt) {
-      debug_stop_pose_with_info_.set(
-        stop_pose_opt.value(), std::string("keep feasible stop after approval"));
-    }
-    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path");
-  }
-}
-
 void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
 {
   if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) {
@@ -1027,7 +981,7 @@
     return output;
   }
 
   setDebugData();
 
   return output;
 }
@@ -1096,7 +1050,7 @@
   path_candidate_ =
     std::make_shared<PathWithLaneId>(thread_safe_data_.get_pull_over_path()->getFullPath());
 
-  updatePreviousData(output);
+  updatePreviousData();
 
   return output;
 }
@@ -1123,12 +1077,8 @@
   setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath());
 }
 
-void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output)
+void GoalPlannerModule::updatePreviousData()
 {
-  if (prev_data_.found_path || !prev_data_.stop_path) {
-    prev_data_.stop_path = std::make_shared<PathWithLaneId>(output.path);
-  }
-
   // for the next loop setOutput().
   // this is used to determine whether to generate a new stop path or keep the current stop path.
   prev_data_.found_path = thread_safe_data_.foundPullOverPath();
@@ -1151,18 +1101,7 @@
   } else {
     prev_data_.safety_status.safe_start_time = std::nullopt;
   }
   prev_data_.safety_status.is_safe = is_safe;
-
-  // If safety check is enabled, save current path as stop_path_after_approval
-  // This path is set only once after approval.
-  if (!isActivated() || (!is_safe && prev_data_.stop_path_after_approval)) {
-    return;
-  }
-  auto stop_path = std::make_shared<PathWithLaneId>(output.path);
-  for (auto & point : stop_path->points) {
-    point.point.longitudinal_velocity_mps = 0.0;
-  }
-  prev_data_.stop_path_after_approval = stop_path;
 }
 
 BehaviorModuleOutput GoalPlannerModule::planWaitingApproval()
@@ -1254,46 +1193,46 @@
   //     difference between the outer and inner sides)
   // 4. feasible stop
   const auto stop_pose_with_info =
     std::invoke([&]() -> std::optional<std::pair<Pose, std::string>> {
       if (thread_safe_data_.foundPullOverPath()) {
         return std::make_pair(
           thread_safe_data_.get_pull_over_path()->start_pose, "stop at selected start pose");
       }
       if (thread_safe_data_.get_closest_start_pose()) {
         return std::make_pair(
           thread_safe_data_.get_closest_start_pose().value(), "stop at closest start pose");
       }
       if (!decel_pose) {
         return std::nullopt;
       }
       return std::make_pair(decel_pose.value(), "stop at search start pose");
     });
   if (!stop_pose_with_info) {
-    const auto feasible_stop_path = generateFeasibleStopPath();
+    const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path);
     // override stop pose info debug string
     debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose"));
     return feasible_stop_path;
   }
   const Pose stop_pose = stop_pose_with_info->first;
 
   // if stop pose is closer than min_stop_distance, stop as soon as possible
   const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, stop_pose);
   const auto min_stop_distance = calcFeasibleDecelDistance(
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0);
   const double eps_vel = 0.01;
   const bool is_stopped = std::abs(current_vel) < eps_vel;
   const double buffer = is_stopped ? stop_distance_buffer_ : 0.0;
   if (min_stop_distance && ego_to_stop_distance + buffer < *min_stop_distance) {
-    const auto feasible_stop_path = generateFeasibleStopPath();
+    const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path);
     debug_stop_pose_with_info_.set(
       std::string("feasible stop: stop pose is closer than min_stop_distance"));
     return feasible_stop_path;
   }
 
   // slow down for turn signal, insert stop point to stop_pose
   auto stop_path = extended_prev_path;
   decelerateForTurnSignal(stop_pose, stop_path);
   debug_stop_pose_with_info_.set(stop_pose, stop_pose_with_info->second);
 
   // slow down before the search area.
   if (decel_pose) {
@@ -1316,22 +1255,22 @@
   return stop_path;
 }
 
-PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const
+PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId & path) const
 {
   // calc minimum stop distance under maximum deceleration
   const auto min_stop_distance = calcFeasibleDecelDistance(
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0);
   if (!min_stop_distance) {
-    return getPreviousModuleOutput().path;
+    return path;
   }
 
   // set stop point
-  auto stop_path = getPreviousModuleOutput().path;
+  auto stop_path = path;
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const auto stop_idx =
     motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points);
   if (stop_idx) {
     debug_stop_pose_with_info_.set(stop_path.points.at(*stop_idx).point.pose, "feasible stop");
   }
 
   return stop_path;
@@ -1514,10 +1453,10 @@
     parameters_->forward_goal_search_length);
 
   const auto expanded_pull_over_lanes =
     left_side_parking_ ? lanelet::utils::getExpandedLanelets(
                            pull_over_lanes, parameters_->detection_bound_offset, 0.0)
                        : lanelet::utils::getExpandedLanelets(
                            pull_over_lanes, 0.0, parameters_->detection_bound_offset);
 
   const auto [pull_over_lane_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
@@ -2114,18 +2053,18 @@
   }
 
   // Visualize stop pose info
   if (debug_stop_pose_with_info_.pose->has_value()) {
     visualization_msgs::msg::MarkerArray stop_pose_marker_array{};
     const auto color = createMarkerColor(1.0, 1.0, 1.0, 0.99);
     auto marker = createDefaultMarker(
       header.frame_id, header.stamp, "stop_pose_info", 0,
       visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 0.5), color);
     marker.pose = debug_stop_pose_with_info_.pose->value();
     marker.text = debug_stop_pose_with_info_.string;
     stop_pose_marker_array.markers.push_back(marker);
     add(stop_pose_marker_array);
     add(createPoseMarkerArray(marker.pose, "stop_pose", 1.0, 1.0, 1.0, 0.9));
   }
 }
 
 void GoalPlannerModule::printParkingPositionError() const