feat(obstacle_stop_planner): add hold stop margin distance

[satoshi-ota]

Signed-off-by: satoshi-ota satoshi.ota928@gmail.com

Description

Now, this ego always try to stop at the stop position by re-starting (re-approach) even if the ego is stopped near the planned stop position in the module. On the other hand, if the stopping performance of the vehicle is poor (e.g. it cannot stop for one meter) and once it starts traveling, there is a possibility that the vehicle may over the stop position, in which case there may be a issue for the vehicle to continue stopping on the spot, if the distance between the ego and stop position is closer than threshold.

So, in this PR, I fixed following points.

add new param hold_stop_margin_distance (default: 0.0 [m])
if the following condisions are satisfied, the ego keep stopping at current position.
condition 1. the ego has stopped
condition 2. the distance between the ego and stop point is shorter than hold_stop_margin_distance

test perform (hold_stop_margin_distance = 2.0 [m])

simplescreenrecorder-2022-07-26_17.58.32.mp4

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

Merging #1434 (2233b26) into main (4504ae1) will decrease coverage by 0.05%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main    #1434      +/-   ##
==========================================
- Coverage   10.13%   10.07%   -0.06%     
==========================================
  Files        1118     1118              
  Lines       77393    77780     +387     
  Branches    17989    17989              
==========================================
  Hits         7840     7840              
- Misses      62284    62671     +387     
  Partials     7269     7269              

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	10.11% <0.00%> (ø)		Carriedforward from a8d4d5a

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

Impacted Files	Coverage Δ
...ner/include/obstacle_stop_planner/debug_marker.hpp	0.00% <ø> (ø)
planning/obstacle_stop_planner/src/node.cpp	0.00% <0.00%> (ø)

---

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 4504ae1...2233b26. Read the comment docs.

[h-ohta]

@satoshi-ota How Can I check the distance between ego and obstacle? Is some information published?

[satoshi-ota]

@satoshi-ota How Can I check the distance between ego and obstacle? Is some information published?

@h-ohta Sorry for late reply. I checked the distance by using rviz measure or tempolary embedded debug code.

Unfortunatly, there is no infomation about the distance between the ego and collision obstacle in debug message now. But this module has and outputs debug values array, so I can add the distance infomation and output as debug message. I'll add it.

[satoshi-ota]

@h-ohta I added the infomation about the distance between the ego and obstacle as a debug message. Please check that in plotjuggler.

• topic name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop/debug_values
• data: data.4 is the infomation about the distance between the ego and collision obstacle

[satoshi-ota]

@TakaHoribe I confirmed that there is no critical degradation in this PR by evaluator.
There is maybe bugs in this PR... 🤔

This PR: https://evaluation.tier4.jp/evaluation/reports/49ea6a0f-b177-50c8-95ed-1ba2a72ef8a5?project_id=prd_jt (682/768)
Baseline: https://evaluation.tier4.jp/evaluation/reports/ddd34bcc-0fb6-5ea8-8844-0a4961745f5f?project_id=prd_jt (700/768)

[satoshi-ota]

@TakaHoribe @h-ohta I fixed some bugs and re-run test. Test result is similar to baseline.

This PR: https://evaluation.tier4.jp/evaluation/reports/d9b53322-3336-5f2a-8e91-428cc2d14e59?project_id=prd_jt (702/768)
Baseline: https://evaluation.tier4.jp/evaluation/reports/ddd34bcc-0fb6-5ea8-8844-0a4961745f5f?project_id=prd_jt (700/768)

[TakaHoribe]

LGTM