refactor(start_planner): refactor debug and safety check logic

[kyoichi-sugahara]

Description

This PR should be merged first!!!

This commit refactors the start_planner module in several ways:

• The verbose parameter is removed from the YAML configuration file and replaced with a print_debug_info parameter under a new debug section.
• A new function, initVariables(), is introduced to initialize member variables, which is called during processOnEntry() and processOnExit().
• The function isBackwardDrivingComplete() is renamed to hasFinishedBackwardDriving() to better reflect its purpose. It also includes additional debug prints.
• A new function, checkCollisionWithDynamicObjects(), is introduced to encapsulate the logic for checking collisions with dynamic objects.
• The function isExecutionReady() is modified to use the new checkCollisionWithDynamicObjects() function.
• The function resetStatus() is simplified by directly assigning an empty PullOutStatus object to status_.
• The function updatePullOutStatus() now uses the new hasFinishedBackwardDriving() function.
• The logging of pull out status is now controlled by the new print_debug_info parameter rather than the removed verbose parameter.

These changes improve the clarity of the code and make it easier to control debug output.

Tests performed

• PASS TIERIV INTERNAL SCENARIOS
  • release scenarios
  • future scenarios

Effects on system behavior

Not applicable.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[kosuke55]

is it ok to change the order of processing?

[kyoichi-sugahara]

If there's no need for hasFinishedBackwardDriving() to be executed first, then this change is ok

[kosuke55]

the following function returns true when a collision is detected.
but checkCollisionWithDynamicObjects is the opposite.

    if (utils::checkCollisionBetweenFootprintAndObjects(
          local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes,
          parameters_->collision_check_margin)) {

[kyoichi-sugahara]

changed to hasCollisionWithDynamicObjects()

[kosuke55]

      path_safety_check:
        # EgoPredictedPath
          acceleration: 1.0
          max_velocity: 1.0

this parameters is updated in processing, so is not parameters.

void updatePathProperty(
  std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
  const std::pair<double, double> & pairs_terminal_velocity_and_accel)
{
  // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is
  // necessary to ensure a reasonable path length.
  // TODO(Sugahara): define them as parameter
  const double min_accel_for_ego_predicted_path = 1.0;
  const double acceleration =
    std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path);

  ego_predicted_path_params->max_velocity = pairs_terminal_velocity_and_accel.first;
  ego_predicted_path_params->acceleration = acceleration;
}

[kosuke55]

The nested bool calculation is complicated because we need to see 5 bool values at same

status_.found_pull_out_path && (!isWaitingApproval() || !(parameters_->safety_check_params.enable_safety_check && status_.driving_forward) || isSafePath()

[kosuke55]

maybe it is easier to understand if the condition match with the comments.

  const bool is_safe =
    !(status_.found_pull_out_path || (isWaitingApproval() && !checkCollisionWithDynamicObjects());

[kyoichi-sugahara]

changed as follows

  // Evaluate safety. The situation is not safe if any of the following conditions are met:
  // 1. pull out path has not been found
  // 2. waiting for approval and there is a collision with dynamic objects
  if (!status_.found_pull_out_path) {
    is_safe = false;
  }

  if (requiresDynamicObjectsCollisionDetection()) {
    is_safe = !hasCollisionWithDynamicObjects();
  }

[kosuke55]

why not just is_safe = isSafePath(); ? 🙏

[kosuke55]

hasCollisionWithDynamicObjects is currently just !isSafePath(); but will be updated as TODO in other PR?

bool StartPlannerModule::hasCollisionWithDynamicObjects() const
{
  // TODO(Sugahara): update path, params for predicted path and so on in this function to avoid
  // mutable
  return !isSafePath();
}

[kyoichi-sugahara]

asCollisionWithDynamicObjects is currently just !isSafePath(); but will be updated as TODO in other PR?

Yes I mean that.
want to separate object filetering part and create predicted path part from isSafe() function

[kyoichi-sugahara]

@kosuke55
I believe I've addressed all the changes, could you please confirm if everything is okay?

[kosuke55]

LGTM! thanks!

[codecov]

Codecov Report

Attention: 1408 lines in your changes are missing coverage. Please review.

Comparison is base (765a596) 15.32% compared to head (51bb115) 12.68%.
Report is 154 commits behind head on main.

Files	Patch %	Lines
...anner/src/utils/avoidance/shift_line_generator.cpp	13.19%	435 Missing and 65 partials ⚠️
...ehavior_path_planner/src/utils/avoidance/utils.cpp	3.75%	195 Missing and 10 partials ⚠️
.../scene_module/goal_planner/goal_planner_module.cpp	0.58%	168 Missing and 2 partials ⚠️
...er/src/scene_module/avoidance/avoidance_module.cpp	10.48%	111 Missing and 17 partials ⚠️
...cene_module/start_planner/start_planner_module.cpp	23.52%	68 Missing and 10 partials ⚠️
...ule/dynamic_avoidance/dynamic_avoidance_module.cpp	1.56%	63 Missing ⚠️
..._path_planner/src/marker_utils/avoidance/debug.cpp	0.00%	61 Missing ⚠️
...th_planner/src/scene_module/lane_change/normal.cpp	0.00%	28 Missing ⚠️
...ath_planner/src/scene_module/avoidance/manager.cpp	24.24%	0 Missing and 25 partials ⚠️
.../scene_module/goal_planner/goal_planner_module.hpp	9.52%	15 Missing and 4 partials ⚠️
... and 27 more

Additional details and impacted files

@@             Coverage Diff             @@
##             main    #5747       +/-   ##
===========================================
- Coverage   15.32%   12.68%    -2.65%     
===========================================
  Files        1721      109     -1612     
  Lines      118559    15200   -103359     
  Branches    37995     8462    -29533     
===========================================
- Hits        18169     1928    -16241     
+ Misses      79657    10304    -69353     
+ Partials    20733     2968    -17765     

Flag	Coverage Δ
differential	12.68% <10.54%> (?)
total	?

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.