feat(behavior_path_planner): update path when object is gone

[ismetatabay]

Signed-off-by: ismet atabay ismet@leodrive.ai

Description

At avoidance module in behavior_path_planner, if the detected objects occur in path, avoidance module shift path points to around the vehicle. Moreover, if the detected objects are gone the shifted path is not updated. The reason of this, avoidance module' current state stays in the "RUNNING" state until the vehicle reach its goal. I added a feature to change this state, and this feature allows the path to be updated by completing the task of the avoidance module if the detected objects are gone.

To control this, I added a parameter with the name enable_update_path_when_object_is_gone and set the default value to false. enable_update_path_when_object_is_gone must be true for this feature to be used.

Related links

closes #2272

Tests performed

Before PR (enable_update_path_when_object_is_gone is set to false):

before_pr.mp4

After PR (enable_update_path_when_object_is_gone is set to true):

Case 1: The vehicle stops.

vehicle_stops.mp4

Case 2: The vehicle moves, avoidance maneuver doesn't starts.

vehicle_runs_wo_avoidance.mp4

Case 3: The vehicle moves, avoidance maneuver starts.

vehicle_runs_w_avoidance.mp4

Notes for reviewers

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

Base: 10.64% // Head: 10.44% // Decreases project coverage by -0.19% ⚠️

Coverage data is based on head (aecab12) compared to base (dd18b3a).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2314      +/-   ##
==========================================
- Coverage   10.64%   10.44%   -0.20%     
==========================================
  Files        1239     1240       +1     
  Lines       82824    85130    +2306     
  Branches    20399    21921    +1522     
==========================================
+ Hits         8815     8894      +79     
- Misses      64319    66265    +1946     
- Partials     9690     9971     +281     

Flag	Coverage Δ		*Carryforward flag
differential	3.17% <0.00%> (?)
total	10.64% <0.00%> (ø)		Carriedforward from 4c1ccc8

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

Impacted Files	Coverage Δ
...lanner/scene_module/avoidance/avoidance_module.hpp	0.00% <ø> (ø)
...r/scene_module/avoidance/avoidance_module_data.hpp	0.00% <ø> (ø)
...or_path_planner/src/behavior_path_planner_node.cpp	0.15% <0.00%> (+0.15%)	⬆️
...er/src/scene_module/avoidance/avoidance_module.cpp	0.00% <0.00%> (ø)
planning/behavior_path_planner/test/input.cpp	60.00% <0.00%> (-7.86%)	⬇️
...ng/behavior_path_planner/test/test_turn_signal.cpp	40.41% <0.00%> (-6.86%)	⬇️
...ner/src/scene_module/avoidance/avoidance_utils.cpp	1.93% <0.00%> (-0.10%)	⬇️
planning/static_centerline_optimizer/src/main.cpp	0.00% <0.00%> (ø)
planning/static_centerline_optimizer/src/utils.cpp	0.00% <0.00%> (ø)
...nning/behavior_path_planner/src/path_utilities.cpp	0.00% <0.00%> (ø)
... and 35 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[satoshi-ota]

@ismetatabay Thanks for your contribution 😄

Could you show the ego behavior in case where the ego is in avoidance maneuver? I think it dangerous that the path suddenly reverts back to its original one when there is huge lateral deviation between the ego and reverted path.

[ismetatabay]

@satoshi-ota Hi,
Thank you for comment, the behavior of the car is as in the video below. What do you suggest in this situation? Maybe we can revert the path if the vehicle is stopped, but we do not change the path while the vehicle is in motion.

2022-11-20.13-55-39.mp4

[satoshi-ota]

@ismetatabay Thanks for your explanation 👍

The following scene is exactly the scenario I was concerned about. In this case, I think it would be better to replan the return path from Ego's position back to the original reference path, because it is an abnormal situation for a system where the vehicle deviates significantly from its path, and I think we have to avoid such a situation as much as possible. What would you think?

As another idea, I think it would be good to make the assumption that the canceling of the avoidance maneuver is limited to only when the ego has not initiated the avoidance and there is not much deviation between the ego and reverted path.

cc @TakaHoribe @zulfaqar-azmi-t4

[ismetatabay]

@satoshi-ota Thanks also for suggesting 2 solutions,
With these solutions we can get a reasonable behavior. In terms of safety, your second suggestion seemed safer to me. An algorithm like this looks good to me:

If the vehicle has not started the avoidance maneuver, the avoidance path can be reverted, if the avoidance maneuver has been started, remain the avoidance path to follow it. The avoidance path follows a very smooth path to return to the old path. So it made sense to me to follow this avoid path to avoid sudden lateral change.

If that's okay I can update the algorithm.

[ismetatabay]

@satoshi-ota hi, I updated the algorithm and the videos in the description according to the cases we talked about earlier 👍 .

[satoshi-ota]

@ismetatabay Thanks a lot 👍 I'll review this, so please wait a few days.

[satoshi-ota]

@ismetatabay I'm working on testing your login in my local environment, and it looks good to me 👍 Finally, could you please add description of avoid canceling (not only related param but also the assumption, flow, etc...) to the documentation?

simplescreenrecorder-2022-11-28_17.46.19.mp4

[ismetatabay]

@satoshi-ota hi, I added "Avoidance Cancelling" section to the documentation. At this section, I explain Avoidance Module behavior at each different cases.

[satoshi-ota]

@ismetatabay Perfect 👍
Thanks for your contribution!

[satoshi-ota]

@ismetatabay Sorry, could you fix this typo? 😅

vehcile -> vehicle

[ismetatabay]

@satoshi-ota Sorry i missed it, i updated it now. 👍

[satoshi-ota]

vehcile -> vehicle

[satoshi-ota]

@ismetatabay Thanks 😄 You can merge this PR 🎉