fix(behavior_path_planner): fix ap point for long obeject

[taikitanaka3]

Signed-off-by: tanaka3 ttatcoder@outlook.jp

Description

because current implementation consider avoidance by only one shift point it's possible to collide with long obstacle such as bus, truck or trailer.
So I added longitudinal length of vehicle for avoidance return point in frenet-frame

before

maybe OK for car

NG for bus

after
OK for car

OK for bus

parallel case

Related links

#833

Tests performed

by psim
Please set lateral_collision_safety_margin to zero when testing

Notes for reviewers

This PR won't fix maybe too much margin case.

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.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

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.
• The PR is ready for merge.

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

[codecov]

Codecov Report

Merging #1041 (a54c92a) into main (4e18fe0) will decrease coverage by 0.20%.
The diff coverage is 0.00%.

@@           Coverage Diff            @@
##            main   #1041      +/-   ##
========================================
- Coverage   9.33%   9.12%   -0.21%     
========================================
  Files        988     981       -7     
  Lines      66698   61975    -4723     
  Branches   11587   11193     -394     
========================================
- Hits        6223    5655     -568     
+ Misses     55302   51400    -3902     
+ Partials    5173    4920     -253     

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	9.23% <0.00%> (-0.08%)	⬇️	Carriedforward from 287f4b9

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

Impacted Files	Coverage Δ
...r/scene_module/avoidance/avoidance_module_data.hpp	0.00% <ø> (ø)
...er/src/scene_module/avoidance/avoidance_module.cpp	0.00% <0.00%> (ø)
...ner/src/scene_module/avoidance/avoidance_utils.cpp	0.00% <0.00%> (ø)
system/default_ad_api/src/interface.cpp	0.00% <0.00%> (-71.43%)	⬇️
...ng_simulator/vehicle_model/sim_model_interface.hpp	0.00% <0.00%> (-66.67%)	⬇️
...d_api_msgs/srv/interface_version__type_support.cpp	0.00% <0.00%> (-54.55%)	⬇️
.../_autoware_ad_api_msgs_s.ep.rosidl_typesupport_c.c	0.00% <0.00%> (-51.81%)	⬇️
...auto_common/include/helper_functions/type_name.hpp	66.66% <0.00%> (-33.34%)	⬇️
...l/dds_fastrtps/interface_version__type_support.cpp	0.00% <0.00%> (-32.72%)	⬇️
...s/srv/detail/interface_version__type_support_c.cpp	0.00% <0.00%> (-31.00%)	⬇️
... and 625 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 4e18fe0...a54c92a. Read the comment docs.

[taikitanaka3]

@TakaHoribe @shulanbushangshu
After discussion with @zulfaqar-azmi-t4 -san
adding longitudinal collision margin like this method is good for stability of ego path to avoid chattering for long obstacle such as Bus or Truck or Trailer.
To Add more stable start/end point margin will be feature task(because of computation cost and no safety assurance )

So this PR only handle for bug fix and this PR is ready to marge

[shulanbushangshu]

@taikitanaka3.I have read the changed code carefully.
The following code：
ap_return.start_longitudinal = o.longitudinal + o.length;
ap_return.end_longitudinal =
o.longitudinal + std::min(nominal_return_distance, return_remaining_distance);

Whether "ap_return.end_longitudinal" needs to add o.length.This keeps the return distance unchanged

[TakaHoribe]

You can remote TODO message in calcAvoidanceTargetObjects by this fix.

[taikitanaka3]

[zulfaqar-azmi-t4]

@taikitanaka3
Sorry, I can just reproduced them in Kashiwanoha map.
safety_margin in set to 0 in this case

Before

It seems that Jerk might plays some role in causing this to happen.

After

[taikitanaka3]

@zulfaqar-azmi-t4
#1041 (comment) do you mean case after is better ?

[kurogane1031]

@zulfaqar-azmi-t4 #1041 (comment) do you mean case after is better ?

Sorry, for the lack of explanation.
I think the solution might be good, but there might be some bug in the current shift length computation (which I think related to case after). I think it will still cause issue even if we apply your solution. I am currently working on the fix.

[TakaHoribe]

LGTM.
@zulfaqar-azmi-t4 If you still have any suggestions.

[zulfaqar-azmi-t4]

@TakaHoribe @taikitanaka3
LGTM

Just to note, as informed here, the shift length issue will still occurs and #1055 supposed to solve them.

But this PR is nice since it will be helpful for the feature as illustrated in the sketch.

[taikitanaka3]

@zulfaqar-azmi-t4 im going to test with your pr and if it goes well i will merge this.

[taikitanaka3]

@zulfaqar-azmi-t4
with your fix PR the shift lateral distance is same as vehicle closest point with this param Nice

      lateral_collision_margin: 0.0                  # [m]
      lateral_collision_safety_buffer: 0.0        # [m]

[taikitanaka3]

nominal case
lateral_collision_safety_margin : 1.0