feat(obstacle_cruise_planner): improve stop and cruise behavior for cut-in & out

[brkay54]

Description

• To increase the response capacity of the obstacle_cruise_planner within the scope of Dense-Urban ODD, we want it to be sensitive to dynamic objects approaching from outside Ego's path and stop if there is a collision risk, as mentioned in the issue.

• Avoiding unnecessary stopping of Ego in the presence of transient objects that do not pose a risk of collision. To solve these problems and provide a more human-like autonomous driving experience, we needed to improve the stop behavior of the obstacle_cruise_planner.

• We want to improve the cruising behavior for the outside obstacle. If another vehicle in the side lanes is approaching Ego's trajectory, cruise planning starts too late.

Improvement on filtering:

In obstacle_cruise_planner, all the PredictedObjects were filtered if their lateral distance exceeded some small distance limitations. To achieve a behavior determination for PredictedObjects who are away from trajectory but approaching with some velocity, I needed to change the filtering algorithm.

With the current implementation, if the predicted object is far away but its perpendicular velocity to the trajectory is high and if it is inside the trajectory within the time parameter max_lateral_time_margin, it will be considered as a target obstacle and we can use it for the behavior determination.

Improvement on stop behavior:

To handle both transient objects and objects that come to the trajectory that may cause a collision, I divided the stop behavior into two: inside and outside.

For the objects that are inside the trajectory (precise_lat_dist > max_lat_margin_for_stop), we are checking the collision point. If there is a collision point, we are looking object's position when the Ego reaches the collision point. If there is no intersection with the future position, we don't set stop, otherwise set stop.

Improvement on cruising:

To be able to select the vehicles in side lanes who intend to drive through our trajectory, we added a time margin that calculates the time to reach the trajectory. By using this, we can select the vehicles earlier as cruise obstacle.

Related links

Parent Issue:

• [Dense-Urban-ODD] Enhance Cruise Planner's Responsiveness to Other Road Users #7864

How was this PR tested?

PR tested by using PSim, however, I am currently working to create some scenarios for a detailed test of this feature. I will upload them into AWF Project asap.

Transient Obstacle Tests:

ocr-transient-obstacle.mp4

Possible Collision Obstacles:

ocr-possible-collision.mp4

Cruising for Vehicles come from side lane:

sidebyside-test.mp4

Notes for reviewers

Please test this PR with the parameters below:

• feat(obstacle_cruise_planner): improve stop and cruise behavior for cut-in & out autoware_launch#1142

Interface changes

None.

ROS Parameter Changes

Additions and removals

Change type	Parameter Name	Type	Default Value	Description
Modified	common.min_behavior_stop_margin	double	6.0	Updated minimum stop margin for behavior-based stopping.
Modified	behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop	double	1.0	Updated velocity threshold for switching from cruise to stop.
Modified	behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise	double	1.5	Updated velocity threshold for switching from stop to cruise.
Modified	behavior_determination.crossing_obstacle.obstacle_velocity_threshold	double	1.0	Updated velocity threshold for determining crossing obstacles for cruise or stop.
Modified	behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold	double	0.523599	Updated yaw angle threshold for crossing obstacles.
Added	behavior_determination.stop.outside_obstacle.max_lateral_time_margin	double	4.0	Maximum time margin for lateral collision checking between obstacles and ego.
Added	behavior_determination.stop.outside_obstacle.num_of_predicted_paths	int	3	Number of highest confidence predicted paths to check collision between obstacle and ego.
Modified	behavior_determination.stop.crossing_obstacle.collision_time_margin	double	2.0	Updated time threshold for collision between obstacle and ego.
Added	behavior_determination.cruise.outside_obstacle.max_lateral_time_margin	double	5.0	Maximum time margin for lateral collision checking between obstacles and trajectory band with ego's width.
Added	behavior_determination.cruise.outside_obstacle.num_of_predicted_paths	int	3	Number of highest confidence predicted paths to check collision between obstacle and ego.
Modified	behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold	double	1.5	Updated velocity threshold for obstacles outside the trajectory to cruise or stop.
Modified	behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold	double	0.2	Updated time threshold to decide cut-in obstacle for cruise or stop.
Modified	behavior_determination.cruise.outside_obstacle.max_prediction_time_for_collision_check	double	10.0	Updated prediction time for checking collision between obstacle and ego.
Added	behavior_determination.stop.min_velocity_to_reach_collision_point	double	2.0	minimum velocity margin to calculate time to reach collision

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[yuki-takagi-66]

Thank you for the PR. I've been waiting for this change.
As I commented out, I feel we have to refactor createStopObstacleForPredictedObject().
Could you check it?

[brkay54]

Some scenario tests:

Possible Collision (UC-NTR-003-0001):

possible_collision.mp4

Pedestrian Jay Walk (UC-VRU-001-0002):

ped_reaction.mp4

Transient Object (UC-NTR-003-0002):

transient_obj.mp4

Stop with high negative acceleration:

sudden_deccelarate_obj.mp4

[codecov]

Codecov Report

Attention: Patch coverage is 6.66667% with 210 lines in your changes missing coverage. Please review.

Project coverage is 26.64%. Comparing base (c2438fd) to head (30f22cf).
Report is 3 commits behind head on main.

Files with missing lines	Patch %	Lines
...ning/autoware_obstacle_cruise_planner/src/node.cpp	7.10%	196 Missing ⚠️
...ware_obstacle_cruise_planner/src/polygon_utils.cpp	0.00%	12 Missing ⚠️
...utoware/obstacle_cruise_planner/common_structs.hpp	0.00%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #8072      +/-   ##
==========================================
- Coverage   26.70%   26.64%   -0.06%     
==========================================
  Files        1296     1297       +1     
  Lines       95675    95841     +166     
  Branches    39083    39142      +59     
==========================================
- Hits        25547    25540       -7     
- Misses      67488    67644     +156     
- Partials     2640     2657      +17     

Flag	Coverage Δ		*Carryforward flag
differential	12.79% <6.66%> (?)
total	26.69% <ø> (-0.01%)	⬇️	Carriedforward from d4d40cd

*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.

[yuki-takagi-66]

@brkay54
It is a great feature, so many matters need to be considered, and the feature still needs to be refined.
It behaves undesirably in some situations. Could you check the followings?

1. A stop wall is inserted against the object at the lane change destination.
   

2. A stop decision is made even though the ego can pass the collision point before the pedestrian (the pedestrian will stop).
   

3.No stop wall against the in front object

[brkay54]

@yuki-takagi-66 -san, thank you for your review, and sorry for being late. Actually, I was planning to create two PRs: improvement for stop obstacles and improvement for cruise obstacles. However, without cruise improvements, we made unnecessary stops in some cases (Like the problem 1 you mentioned).

Therefore I merged both improvements and updated this PR. Could you please take another look at the PR description?

I solved almost all the problems you mentioned and retested scenarios:

1. A stop wall is inserted against the object at the lane change destination.

After improvements in cruise planning, it was solved.

Video: https://youtu.be/64EdnFO_U-Q

2. A stop decision is made even though the ego can pass the collision point before the pedestrian (the pedestrian will stop).

For this problem, I tested all the cases. There are five cases and 4 of them were passed. Only the one case whose Ego velocity 30 kph failed. I investigated the cause and I saw that stop planning for that case made sense to me. I checked the collision time margin:

time_to_reach_collision_point: 3.00236, time_to_start_cross: 3, time_to_end_cross: 6.2

If the planner does not set a stop, ego and obstacle will crash.

Video: https://youtu.be/aCxZ_bDvbHc

3.No stop wall against the in front object

It is solved: https://youtu.be/WxU56BZn6Fs

Thanks for your efforts again. And, could you review the updated PR again? Also, for Autoware Evaluator could you use the parameters I want to merge with this PR?

• feat(obstacle_cruise_planner): improve stop and cruise behavior for cut-in & out autoware_launch#1142

[yuki-takagi-66]

@brkay54
Thank you for your cooperation, I'll take a look.

[yuki-takagi-66]

@brkay54
Please let me know when your changes are complete.

[yuki-takagi-66]

Overall, LGTM. Almost all scenarios in tier4 has passed!!
I've add some comments and could you check the comments?

[yuki-takagi-66]

I know this problem, and I think we should care this problem more carefully.
Therefore, could you separate this change from this PR?

[brkay54]

Maybe it would be better to create an issue about this problem. I don't know well how to produce the predicted paths when using scenario simulator but I can investigate. What do you think?

[yuki-takagi-66]

Can I ask you to create the issue and solve this problem?
I saw this problem also when using the Psim with use_sim_time:=true

If this problem does not blocking any of your work, I feel we have option to ignore this problem.

[brkay54]

Maybe it would be better to create an issue about this problem. I don't know well how to produce the predicted paths when using scenario simulator but I can investigate. What do you think?

[yuki-takagi-66]

Thank you for the great job.
I have no concerns and all tier4 internal tests have passe.

[brkay54]

@yuki-takagi-66 -san thank you so much for your effort and suggestions 🙏🏻 . Also, could you approve the PR in Autoware Launch?

Also, because I made some critical changes, I would like to be a maintainer of this package. If you and colleagues from the TierIV side don't mind, can I add myself as a maintainer?

[yuki-takagi-66]

@brkay54

Also, because I made some critical changes, I would like to be a maintainer of this package. If you and colleagues from the TierIV side don't mind, can I add myself as a maintainer?

Thank you for your contribution. TierIV would greatly appreciate it if you could become a maintainer.