feat(behavior_path_planner): add processOnEntry in start_planner (aut…

…owarefoundation#4953)

* add process on entry in start_planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* delete description

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -82,12 +82,12 @@ struct PullOverStatus
   size_t current_path_idx{0};
   bool require_increment_{true};  // if false, keep current path idx.
   std::shared_ptr<PathWithLaneId> prev_stop_path{nullptr};
-  lanelet::ConstLanelets current_lanes{};
-  lanelet::ConstLanelets pull_over_lanes{};
-  std::vector<DrivableLanes> lanes{};  // current + pull_over
-  bool has_decided_path{false};
-  bool is_safe_static_objects{false};   // current path is safe against static objects
-  bool is_safe_dynamic_objects{false};  // current path is safe against dynamic objects
+  lanelet::ConstLanelets current_lanes{};    // TODO(someone): explain
+  lanelet::ConstLanelets pull_over_lanes{};  // TODO(someone): explain
+  std::vector<DrivableLanes> lanes{};        // current + pull_over
+  bool has_decided_path{false};  // if true, the path is decided and safe against static objects
+  bool is_safe_static_objects{false};   // current path is safe against *static* objects
+  bool is_safe_dynamic_objects{false};  // current path is safe against *dynamic* objects
   bool prev_is_safe{false};
   bool has_decided_velocity{false};
   bool has_requested_approval{false};
@@ -308,6 +308,7 @@ class GoalPlannerModule : public SceneModuleInterface
   std::pair<double, double> calcDistanceToPathChange() const;
 
   // safety check
+  void initializeSafetyCheckParameters();
   SafetyCheckParams createSafetyCheckParams() const;
   void updateSafetyCheckTargetObjectsData(
     const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -98,6 +98,7 @@ class StartPlannerModule : public SceneModuleInterface
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
   CandidateOutput planCandidate() const override;
+  void processOnEntry() override;
   void processOnExit() override;
 
   void setParameters(const std::shared_ptr<StartPlannerParameters> & parameters)
@@ -123,6 +124,9 @@ class StartPlannerModule : public SceneModuleInterface
   bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; }
 
 private:
+
+  void initializeSafetyCheckParameters();
+
   std::shared_ptr<StartPlannerParameters> parameters_;
   mutable std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params_;
   mutable std::shared_ptr<ObjectsFilteringParams> objects_filtering_params_;

planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
+#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp"
 #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp"
 #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp"
 
@@ -85,6 +86,11 @@ void updatePathProperty(
   std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
   const std::pair<double, double> & pairs_terminal_velocity_and_accel);
 
+void updateSafetyCheckTargetObjectsData(
+  StartGoalPlannerData & data, const PredictedObjects & filtered_objects,
+  const TargetObjectsOnLane & target_objects_on_lane,
+  const std::vector<PoseWithVelocityStamped> & ego_predicted_path);
+
 std::pair<double, double> getPairsTerminalVelocityAndAccel(
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel,
   const size_t current_path_idx);

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -265,6 +265,15 @@ void GoalPlannerModule::initializeOccupancyGridMap()
   occupancy_grid_map_->setParam(occupancy_grid_map_param);
 }
 
+void GoalPlannerModule::initializeSafetyCheckParameters()
+{
+  utils::start_goal_planner_common::updateEgoPredictedPathParams(
+    ego_predicted_path_params_, parameters_);
+  utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_);
+  utils::start_goal_planner_common::updateObjectsFilteringParams(
+    objects_filtering_params_, parameters_);
+}
+
 void GoalPlannerModule::processOnEntry()
 {
   // Initialize occupancy grid map
@@ -273,6 +282,10 @@ void GoalPlannerModule::processOnEntry()
     parameters_->use_occupancy_grid_for_path_collision_check) {
     initializeOccupancyGridMap();
   }
+  // Initialize safety checker
+  if (parameters_->safety_check_params.enable_safety_check) {
+    initializeSafetyCheckParameters();
+  }
 }
 
 void GoalPlannerModule::processOnExit()
@@ -362,11 +375,6 @@ bool GoalPlannerModule::isExecutionReady() const
   }
 
   if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) {
-    utils::start_goal_planner_common::updateEgoPredictedPathParams(
-      ego_predicted_path_params_, parameters_);
-    utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_);
-    utils::start_goal_planner_common::updateObjectsFilteringParams(
-      objects_filtering_params_, parameters_);
     if (!isSafePath()) {
       RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects");
       return false;
@@ -703,7 +711,6 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output)
   if (status_.prev_is_safe || status_.prev_stop_path == nullptr) {
     // safe -> not_safe or no prev_stop_path: generate new stop_path
     output.path = std::make_shared<PathWithLaneId>(generateStopPath());
-    output.reference_path = getPreviousModuleOutput().reference_path;
     status_.prev_stop_path = output.path;
     // set stop path as pull over path
     mutex_.lock();
@@ -718,10 +725,10 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output)
   } else {
     // not_safe -> not_safe: use previous stop path
     output.path = status_.prev_stop_path;
-    output.reference_path = getPreviousModuleOutput().reference_path;
     RCLCPP_WARN_THROTTLE(
       getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path");
   }
+  output.reference_path = getPreviousModuleOutput().reference_path;
 }
 
 void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
@@ -1103,6 +1110,13 @@ bool GoalPlannerModule::incrementPathIndex()
 
 PathWithLaneId GoalPlannerModule::getCurrentPath() const
 {
+  if (status_.pull_over_path == nullptr) {
+    return PathWithLaneId{};
+  }
+
+  if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) {
+    return PathWithLaneId{};
+  }
   return status_.pull_over_path->partial_paths.at(status_.current_path_idx);
 }
 
@@ -1522,7 +1536,8 @@ bool GoalPlannerModule::isSafePath() const
   const double hysteresis_factor =
     status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate;
 
-  updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path);
+  utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData(
+    goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
 
   for (const auto & object : target_objects_on_lane.on_current_lane) {
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -133,6 +133,14 @@ BehaviorModuleOutput StartPlannerModule::run()
   return plan();
 }
 
+void StartPlannerModule::processOnEntry()
+{
+  // Initialize safety checker
+  if (parameters_->safety_check_params.enable_safety_check) {
+    initializeSafetyCheckParameters();
+  }
+}
+
 void StartPlannerModule::processOnExit()
 {
   resetPathCandidate();
@@ -183,11 +191,6 @@ bool StartPlannerModule::isExecutionReady() const
   }
 
   if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) {
-    utils::start_goal_planner_common::updateEgoPredictedPathParams(
-      ego_predicted_path_params_, parameters_);
-    utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_);
-    utils::start_goal_planner_common::updateObjectsFilteringParams(
-      objects_filtering_params_, parameters_);
     if (!isSafePath()) {
       RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects");
       return false;
@@ -335,6 +338,15 @@ CandidateOutput StartPlannerModule::planCandidate() const
   return CandidateOutput{};
 }
 
+void StartPlannerModule::initializeSafetyCheckParameters()
+{
+  utils::start_goal_planner_common::updateEgoPredictedPathParams(
+    ego_predicted_path_params_, parameters_);
+  utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_);
+  utils::start_goal_planner_common::updateObjectsFilteringParams(
+    objects_filtering_params_, parameters_);
+}
+
 PathWithLaneId StartPlannerModule::getFullPath() const
 {
   // combine partial pull out path
@@ -1059,7 +1071,8 @@ bool StartPlannerModule::isSafePath() const
   const double hysteresis_factor =
     status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate;
 
-  updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path);
+  utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData(
+    start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
 
   for (const auto & object : target_objects_on_lane.on_current_lane) {
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(

planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp

@@ -142,6 +142,16 @@ void updatePathProperty(
   ego_predicted_path_params->acceleration = acceleration;
 }
 
+void updateSafetyCheckTargetObjectsData(
+  StartGoalPlannerData & data, const PredictedObjects & filtered_objects,
+  const TargetObjectsOnLane & target_objects_on_lane,
+  const std::vector<PoseWithVelocityStamped> & ego_predicted_path)
+{
+  data.filtered_objects = filtered_objects;
+  data.target_objects_on_lane = target_objects_on_lane;
+  data.ego_predicted_path = ego_predicted_path;
+}
+
 std::pair<double, double> getPairsTerminalVelocityAndAccel(
   const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel,
   const size_t current_path_idx)