fix(behavior_path_planner): add distance to returned point in avoidance module #915

shulanbushangshu · 2022-05-17T09:47:32Z

Description

To solve #833
We can calculate the maximum longitudinal distance in the direction of the lane according to the part of the obstacle in the lane. Append the calculated maximum longitudinal distance to the longitudinal starting distance of the return point.
The schematic of the change is shown below：

Tests performed

1.Run planning_simulator.
2.place ego position and goal
3. Set a obstacle near lanes
4. result -- the path will maintain the calculated offset for the horizontal distance where obstacle is present

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 · 2022-05-17T11:21:03Z

Codecov Report

Merging #915 (ea651a6) into main (a72a2f2) will decrease coverage by 0.11%.
The diff coverage is 0.00%.

@@           Coverage Diff            @@
##            main    #915      +/-   ##
========================================
- Coverage   9.55%   9.43%   -0.12%     
========================================
  Files        935     935              
  Lines      57795   58663     +868     
  Branches   10430   10609     +179     
========================================
+ Hits        5522    5537      +15     
- Misses     47734   48558     +824     
- Partials    4539    4568      +29

Flag	Coverage Δ		*Carryforward flag
differential	`2.71% <0.00%> (?)`
total	`9.55% <0.00%> (ø)`		Carriedforward from e8c776f

*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% <ø> (ø)`
...er/src/scene_module/avoidance/avoidance_module.cpp	`0.00% <0.00%> (ø)`
...ner/src/scene_module/occlusion_spot/grid_utils.cpp	`7.07% <0.00%> (-2.72%)`	⬇️
...ocity_planner/src/scene_module/stop_line/debug.cpp	`0.00% <0.00%> (ø)`
...ocity_planner/src/scene_module/stop_line/scene.cpp	`0.00% <0.00%> (ø)`
...ity_planner/src/scene_module/stop_line/manager.cpp	`0.00% <0.00%> (ø)`
...lanner/src/scene_module/occlusion_spot/manager.cpp	`0.00% <0.00%> (ø)`
...ene_module/occlusion_spot/scene_occlusion_spot.cpp	`0.00% <0.00%> (ø)`
... and 3 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 a72a2f2...ea651a6. Read the comment docs.

Signed-off-by: jack.song <jack.song@autocore.ai>

shulanbushangshu · 2022-05-20T09:05:30Z

@zulfaqar-azmi-t4 @TakaHoribe ,can you review this pr ? I add distance by considering the length of obstacle and the lane to the returned point in the avoidance module of behavior_path_planner.

Signed-off-by: jack.song <jack.song@autocore.ai>

taikitanaka3 · 2022-05-28T03:32:55Z

@shulanbushangshu
Can you add the desctiption of this change with figure at PR description or issue, and please update avoidace-design.md according to agreed chage?

taikitanaka3 · 2022-05-31T07:32:50Z

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

+    const auto dx = check_point.x() - object_pose.position.x;
+    const auto dy = check_point.y() - object_pose.position.y;
+    const Eigen::Vector3d diff_vec{dx, dy, 0};
+    const Eigen::Vector3d cross_vec = base_unit_vec.cross(diff_vec);


@shulanbushangshu
Thank you for description can you also add how long is "distance_add_to_returnpoint" ?

and I suggest these changes (please give me opinion if I'm wrong.)

I think this name( "distance_add_to_returnpoint") should be something like longitudinal_collision_margin

longitudinal margin should be added to "NOT closer footprint point" because it's a bug which should be fixed.

@taikitanaka3
Yes,you are right.In the changes,the "distance_add_to_returnpoint" is calculated according to the check points (including the intersections of the obstacle and the lane boundary, and the contour point of the obstacle within the lane).
The "distance_add_to_returnpoint" can be as longitudinal_collision_margin.
The schematic of the "distance_add_to_returnpoint" is shown below：

In the code,I use the check points to calculate the maximum distance in the "object_closest_pose" point direction.The maximum distance is the "distance_add_to_returnpoint"

@shulanbushangshu
can you add better name for check point or add description to readme?

@zulfaqar-azmi-t4
can you share figures with @shulanbushangshu used in readme?

@taikitanaka3
Yes, this is the drawio file.

feature_behavior_path_planner.zip

zulfaqar-azmi-t4 · 2022-05-31T14:07:52Z

Test is conducted based on approval by default.
It seems no issue for now.

zulfaqar-azmi-t4 · 2022-05-31T14:26:13Z

@shulanbushangshu
Since the length of the bus is a little longer, i think we should also consider the following case and properly define the suitable behavior. Referring to your schematics, I think for this case, point B and C should be adjust accordingly.

end_of_line.mp4

taikitanaka3 · 2022-06-01T15:37:35Z

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

-    ap_return.start_longitudinal = o.longitudinal;
-    ap_return.end_longitudinal =
-      o.longitudinal + std::min(nominal_return_distance, return_remaining_distance);
+    ap_return.start_longitudinal = o.longitudinal + o.distance_add_to_returnpoint;


@shulanbushangshu
why do you add longitudinal distance to start _longitudinal?

taikitanaka3 · 2022-06-01T15:37:35Z

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

-    ap_return.start_longitudinal = o.longitudinal;
-    ap_return.end_longitudinal =
-      o.longitudinal + std::min(nominal_return_distance, return_remaining_distance);
+    ap_return.start_longitudinal = o.longitudinal + o.distance_add_to_returnpoint;


@shulanbushangshu
why do you add longitudinal margin to start _longitudinal?

@taikitanaka3
I want to separate the longitudinal end of the starting point from the longitudinal start of the return point so that "BC" can maintain the maximum offset.
.

@shulanbushangshu
My apologies, I might misunderstood your intention. Assuming I take this image as the example, it essentially mean you're shifting the start_longitudinal pose from point B to point C.

Is it possible if you can provide some sketch (simple is also fine) regarding maintaining the maximum offset for "BC so that we can better understand the situation?

@zulfaqar-azmi-t4 @taikitanaka3
Thank you for your review.Let me briefly describe the process of issue#833:
In the process of obstacle avoidance simulation, I found that the default lateral safety margin was a little large. When the intersection between obstacle and lane was very small, there would be an offset of about 1.5m, as shown in the figure below.

Therefore, I changed the default lateral safety margin and conducted simulation on this basis.There are two variables :''lateral_collision_margin" and "lateral_collision_safety_buffer".I set "lateral_collision_safety_buffer" to 0.0.
In the following simulation, issue#833 occurs, as shown below:

Of course, if the lateral safety distance margin is enlarged, there may be no collision. But I think it is possible to reduce the risk of parameter adjustment by calculating the compensation amount of a longitudinal distance.According to the above situation, I calculated the compensation amount of the longitudinal distance according to the obstacles and lanes.I want to separate the longitudinal end of the starting point from the longitudinal start of the return point so that "BC" can maintain the maximum offset.
The distance is calculated as follows:

In this example,the 'distance_add_to_returnpoint'='d2'+'d4'.
Calculation principle: calculate the minimum distance and maximum distance from the contour point and intersection point to the reference point. The maximum distance minus the minimum distance is used as the compensation distance.
Please consider whether my idea is necessary and correct.

@shulanbushangshu
About this image, the result indeed seems weird. it is possible for you to reproduce them with the debug marker publish_debug_marker=true in the avoidance.param.yaml, add the marker to rviz via planning > lane_driving > scenario_planning > behavior_planning >
behavior_path_planner > debug > marker > MarkerArray and share the image with us? And if it's okay, avoidance.param.yaml file as well.

@shulanbushangshu
I've tested your code but it looks like something wrong, I set lateral collision safety margin to zero as you tested

In your algorithm I think you also need to add margin for start longitudinal too according to collision of foot print and lanelet.
I added relatively light way to make collision margin.
#1041
And please be careful for computation cost for many object case

@zulfaqar-azmi-t4 @taikitanaka3
Thank you for reviewing.
This is the debug marker:

@taikitanaka3 ,you are right.I forgot to change the vertical distance of the starting point.I will fix it.
The two issues((#833,#934)) I raised are aimed at different problems.
The first is to solve the problem of early return (there will be problems if there are long obstacles).
The second is to solve the offset distance (there may be problems in calculating the offset distance in some cases) We also consider the points on the contour of the obstacle and the distance to the boundary. I think it is necessary to modify the calculation method of lateral offset distance.
To sum up, the first issue can be solved by longitudinal distance compensation; The second issue can be solved by lateral distance compensation.
Of course, the above two problems will lead to the risk of collision. In this PR, only the first issue is solved.

@shulanbushangshu
Good, but at first I want to merge fix PR for considering vehicle size in frenet-coordinate is it ok?
your PR looks like new feature that we haven't tested yet.(for the reason you change first avoid point and second return point according to the collision of lanelet and collision edge case is not fully defined or tested yet)
#1041 (comment)

@taikitanaka3.No problem

… with crosswalk (autowarefoundation#915) fix(behavior_velocity_crosswalk_module): stop at stop line associated with crosswalk (autowarefoundation#5231) don't consider margin when stop line is found Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

…params (autowarefoundation#915) * chore: separate param files Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: fix launch Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: rearrange param Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * style(pre-commit): autofix * refactor: rearrange param file Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: move densification_params Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * style(pre-commit): autofix * fix(centerpoint): align param namespace with pointpainting Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: param Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: remove build_only from yaml Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> --------- Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

shulanbushangshu closed this May 18, 2022

shulanbushangshu reopened this May 19, 2022

add distance to returned point(#833)

cfaffdc

Signed-off-by: jack.song <jack.song@autocore.ai>

shulanbushangshu requested review from tkimura4, zulfaqar-azmi-t4, takayuki5168 and BonoloAWF May 19, 2022 07:12

add distance to returned point(#833)

9efb010

Signed-off-by: jack.song <jack.song@autocore.ai>

takayuki5168 requested review from rej55 and TakaHoribe and removed request for takayuki5168 May 19, 2022 09:30

shulanbushangshu requested a review from taikitanaka3 May 23, 2022 08:12

shulanbushangshu and others added 3 commits May 25, 2022 10:17

add distance to returned point in avoidance module(#833)

e8c776f

Signed-off-by: jack.song <jack.song@autocore.ai>

add distance to returned point

77c4cfb

Signed-off-by: jack.song <jack.song@autocore.ai>

ci(pre-commit): autofix

ea651a6

taikitanaka3 reviewed May 31, 2022

View reviewed changes

taikitanaka3 reviewed Jun 1, 2022

View reviewed changes

taikitanaka3 mentioned this pull request Jun 6, 2022

Collision path when the part of obstacle crosses the centerline #934

Closed

3 tasks

shulanbushangshu closed this Jun 9, 2022

shulanbushangshu reopened this Jun 14, 2022

shulanbushangshu closed this Jun 14, 2022

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(behavior_path_planner): add distance to returned point in avoidance module #915

fix(behavior_path_planner): add distance to returned point in avoidance module #915

shulanbushangshu commented May 17, 2022 •

edited

Loading

codecov bot commented May 17, 2022 •

edited

Loading

shulanbushangshu commented May 20, 2022

taikitanaka3 commented May 28, 2022

taikitanaka3 May 31, 2022 •

edited

Loading

shulanbushangshu May 31, 2022

taikitanaka3 Jun 1, 2022

zulfaqar-azmi-t4 Jun 2, 2022

zulfaqar-azmi-t4 commented May 31, 2022

zulfaqar-azmi-t4 commented May 31, 2022 •

edited

Loading

taikitanaka3 Jun 1, 2022

taikitanaka3 Jun 1, 2022 •

edited

Loading

shulanbushangshu Jun 2, 2022

zulfaqar-azmi-t4 Jun 2, 2022

shulanbushangshu Jun 2, 2022

zulfaqar-azmi-t4 Jun 3, 2022 •

edited

Loading

taikitanaka3 Jun 5, 2022 •

edited

Loading

taikitanaka3 Jun 6, 2022 •

edited

Loading

shulanbushangshu Jun 6, 2022

taikitanaka3 Jun 6, 2022

shulanbushangshu Jun 6, 2022

fix(behavior_path_planner): add distance to returned point in avoidance module #915

fix(behavior_path_planner): add distance to returned point in avoidance module #915

Conversation

shulanbushangshu commented May 17, 2022 • edited Loading

Description

Related links

Tests performed

Notes for reviewers

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

codecov bot commented May 17, 2022 • edited Loading

Codecov Report

shulanbushangshu commented May 20, 2022

taikitanaka3 commented May 28, 2022

taikitanaka3 May 31, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

zulfaqar-azmi-t4 commented May 31, 2022

zulfaqar-azmi-t4 commented May 31, 2022 • edited Loading

Choose a reason for hiding this comment

taikitanaka3 Jun 1, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

zulfaqar-azmi-t4 Jun 3, 2022 • edited Loading

Choose a reason for hiding this comment

taikitanaka3 Jun 5, 2022 • edited Loading

Choose a reason for hiding this comment

taikitanaka3 Jun 6, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

shulanbushangshu commented May 17, 2022 •

edited

Loading

codecov bot commented May 17, 2022 •

edited

Loading

taikitanaka3 May 31, 2022 •

edited

Loading

zulfaqar-azmi-t4 commented May 31, 2022 •

edited

Loading

taikitanaka3 Jun 1, 2022 •

edited

Loading

zulfaqar-azmi-t4 Jun 3, 2022 •

edited

Loading

taikitanaka3 Jun 5, 2022 •

edited

Loading

taikitanaka3 Jun 6, 2022 •

edited

Loading