feat(start_planner): keep distance back static objects from start pose

[kyoichi-sugahara]

Description

Following launcher PR should be merged first
autowarefoundation/autoware_launch#815

keep enough distance between back static object and start pose.
This distance is necessary for avoiding stack which is caused by other module's stop point insertion.

NOTE: margin against front objects can be smaller than margin against back objects, because start_planner expect the other module like avoidance to change the trajectory.

Tests performed

• PASS TIERIV INTERNAL 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.

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 6 lines in your changes are missing coverage. Please review.

Project coverage is 14.86%. Comparing base (9530c8a) to head (e27daf8).
Report is 282 commits behind head on main.

❗ Current head e27daf8 differs from pull request most recent head 29571ff. Consider uploading reports for the commit 29571ff to get more accurate results

Files	Patch %	Lines
...behavior_path_start_planner_module/src/manager.cpp	0.00%	3 Missing ⚠️
..._start_planner_module/src/start_planner_module.cpp	0.00%	3 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6149      +/-   ##
==========================================
- Coverage   14.99%   14.86%   -0.13%     
==========================================
  Files        1839     1845       +6     
  Lines      127213   126613     -600     
  Branches    38132    37871     -261     
==========================================
- Hits        19071    18822     -249     
+ Misses      86808    86634     -174     
+ Partials    21334    21157     -177     

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	14.86% <ø> (-0.13%)	⬇️	Carriedforward from e2bd68f

*This pull request uses carry forward flags. Click here to find out more.

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

[kyoichi-sugahara]

@kosuke55
confirmed no degression with evaluator's result.
Could you review the PR?

[kosuke55]

nits:
this could be heavy.
I think there are better ways to make it faster, such as extracting the closest object, the closest point, etc.

[kosuke55]

pseudo code

backward_objects = extraceBackwardObjects(lanelets, ego_pose, objectse); // extract objects whose arc length is less than ego's one
closest_backward_object = getClosestObject(backward_objects); // get object whose arc length is max
closet_object_corner_point = getClosestCornerPoint(closest_backward_object); // get cornaner whose arc length is max
most_backward_ego_corner_point = getMostBackwardEgoCornerPoint(ego_pose, vehicle_footprint); // get corner whose arc length is min
distance = most_backward_ego_corner_point - closet_object_corner_point; // get arc length between two points

[kyoichi-sugahara]

Thanks!
I will consider improving the efficiency

[kyoichi-sugahara]

In my current implementation, distance a is calculated but distance b should be calculated, correct?
If so, the method of extracting objects for back distance against back object should be changed. In this sketch, only object [2] should be extracted.

And for caring lateral collision implemented with the following implementetion. objects[0,1,2] should be extracted 🤔

      (utils::checkCollisionBetweenFootprintAndObjects(
        local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes,
        parameters_->objects_collision_check_margins.back())))

[kosuke55]

I will consider improving the efficiency

thanks! but it's ok if it takes time to improve because the check target objects may be not so many

In my current implementation, distance a is calculated but distance b should be calculated, correct?

no, I think distance a should be calculated,

And for caring lateral collision implemented with the following implementetion. objects[0,1,2] should be extracted 🤔

I agree

[kyoichi-sugahara]

how about creating polygon along the start_pose_candidates_path?

autoware.universe/planning/behavior_path_start_planner_module/src/start_planner_module.cpp

Line 885 in 1121f3b

const PathWithLaneId start_pose_candidates_path = calcBackwardPathFromStartPose();

And extracts closest object overlaps with the polygon

a: objects_collision_check_margins.back()
b: back_objects_collision_check_margin

It's heavier but checking lateral distance of object pose is not considering the size of the object?

[kosuke55]

looks good!
the attention area is along the start_pose_candidates_path not just nagative x-coords direction, right?

[kosuke55]

which pattern did you mean?

[kosuke55]

and, better to check just side to ego?

[kosuke55]

(sorry for my mistake, this pseudo code did not consider footprint)

#6149 (comment)

    const auto obj_arc_coods = getArcCoordinates(lanelets, object.pose.position);
    if(std::abs(ego_arc_coords.distance -obj_arc_coods.distance) > lateral_threshold){
      continue;
    }

not just check object.pose.position but need to check 4 footprint points by like std::any

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.

[xmfcx]

@kyoichi-sugahara @kosuke55 could you rebase and work on this PR?