feat(behavior_path_planner): add occupancy_grid_based_collision_detector #1259

kosuke55 · 2022-07-06T10:30:32Z

Description

Add occupancy_grid_map used for processes(like collision check) in behavior_path_planner
This is the part of #873

This class is almost same to https://github.com/autowarefoundation/autoware.universe/blob/main/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp

usecase
collision check
https://user-images.githubusercontent.com/39142679/177531965-ecf93ffb-26c6-491c-8506-936797fa4a97.mp4

Tests performed

check pull_over module #873 can work.

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.

tkimura4 · 2022-07-06T15:50:47Z

planning/behavior_path_planner/src/occupancy_grid_map/occupancy_grid_map.cpp

@@ -0,0 +1,205 @@
+// Copyright 2021 Tier IV, Inc. All rights reserved.


Tier IV -> TIER IV

tkimura4 · 2022-07-06T15:50:53Z

...ehavior_path_planner/include/behavior_path_planner/occupancy_grid_map/occupancy_grid_map.hpp

@@ -0,0 +1,147 @@
+// Copyright 2021 Tier IV, Inc. All rights reserved.


Tier IV -> TIER IV

...ehavior_path_planner/include/behavior_path_planner/occupancy_grid_map/occupancy_grid_map.hpp

tkimura4 · 2022-07-07T00:44:14Z

With #873 ,
I confirmed that the pull_over module correctly performed collision detection using the occupation grid map

2022-07-07-09-37-48.mp4

e

taikitanaka3 · 2022-07-07T00:52:42Z

planning/behavior_path_planner/src/occupancy_grid_map/occupancy_grid_map.cpp

+  return pose_local;
+}
+
+geometry_msgs::msg::Pose global2local(


please consider using free space planner's package

autoware.universe/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp

Line 35 in 1bab84e

int discretizeAngle(const double theta, const int theta_size)

or make library later to reduce maintenance cost.

Yes, I want to make it common since it is almost the same.
Currently, It can't include free_space_planner in behavior_path, so it is like a duplicate. I would like to leave it as it is this time...

Then Please add comment TODO that this is duplicated and will be replaced to common package

yukkysaito · 2022-07-07T04:03:30Z

this occ grid is only for pull over?

kosuke55 · 2022-07-07T04:04:56Z

@yukkysaito
yes, now it is only used in pull_over. but maybe it will be used also in pull_out.

yukkysaito · 2022-07-07T04:17:12Z

@kosuke55 I wanna discuss the directory structure and name about occ grid.
Can you talk face to face? (the result will be shared here)

codecov · 2022-07-07T10:21:54Z

Codecov Report

Merging #1259 (98efbff) into main (dccd042) will decrease coverage by 0.07%.
The diff coverage is 0.00%.

@@           Coverage Diff            @@
##            main   #1259      +/-   ##
========================================
- Coverage   9.72%   9.64%   -0.08%     
========================================
  Files       1073    1078       +5     
  Lines      73996   74581     +585     
  Branches   16304   16624     +320     
========================================
  Hits        7195    7195              
- Misses     60214   60793     +579     
- Partials    6587    6593       +6

Flag	Coverage Δ		*Carryforward flag
differential	`0.87% <0.00%> (?)`
total	`9.70% <0.00%> (ø)`		Carriedforward from dccd042

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

Impacted Files	Coverage Δ
.../include/behavior_path_planner/debug_utilities.hpp	`0.00% <ø> (ø)`
...lanner/scene_module/avoidance/avoidance_module.hpp	`0.00% <ø> (ø)`
...r/scene_module/avoidance/avoidance_module_data.hpp	`0.00% <ø> (ø)`
...nner/scene_module/lane_change/lane_change_path.hpp	`0.00% <ø> (ø)`
...th_planner/scene_module/pull_out/pull_out_path.hpp	`0.00% <ø> (ø)`
..._planner/scene_module/pull_over/pull_over_path.hpp	`0.00% <ø> (ø)`
...nner/scene_module/side_shift/side_shift_module.hpp	`0.00% <ø> (ø)`
.../utils/occupancy_grid_based_collision_detector.hpp	`0.00% <0.00%> (ø)`
...r_path_planner/scene_module/utils/path_shifter.hpp	`0.00% <ø> (ø)`
...path_planner/src/scene_module/lane_change/util.cpp	`0.00% <ø> (ø)`
... and 11 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 1a1540b...98efbff. Read the comment docs.

kosuke55 · 2022-07-07T10:54:32Z

@yukkysaito
moved path_shifter and occupancy_grid_based_collision_checker to scene_module/utils in 3c3ea26
I put *cpp files in utils directly.

yukkysaito

LGTM

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

…tor (#1259) * feat(behavior_path_planner): add occupancy_grid_based_collision_detector Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * modify copyright Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * move occ and path_shifter to utils Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix humble werror Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix precomit Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

…tor (tier4#1259) * feat(behavior_path_planner): add occupancy_grid_based_collision_detector Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * modify copyright Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * move occ and path_shifter to utils Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix humble werror Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix precomit Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

…tor (autowarefoundation#1259) * feat(behavior_path_planner): add occupancy_grid_based_collision_detector Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * modify copyright Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * move occ and path_shifter to utils Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix humble werror Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix precomit Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

…tor (tier4#1259) * feat(behavior_path_planner): add occupancy_grid_based_collision_detector Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * modify copyright Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * move occ and path_shifter to utils Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix humble werror Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix precomit Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

…et prev_state_stop when using time to red signal (autowarefoundation#1259) * fix(behavior_velocity_traffic_light): stop when the signal is timed out Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> * fix(behavior_velocity_traffic_light): set prev_state_stop when using time to red signal Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> --------- Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

kosuke55 marked this pull request as ready for review July 6, 2022 10:30

kosuke55 changed the title ~~(behavior_path_planner): add occupancy_grid_map~~ feat(behavior_path_planner): add occupancy_grid_map Jul 6, 2022

kosuke55 force-pushed the feature/behavior_path_occ_grid branch from 59d4173 to 5db0d63 Compare July 6, 2022 10:33

kosuke55 requested review from tkimura4 and yukkysaito July 6, 2022 10:38

kosuke55 mentioned this pull request Jul 6, 2022

feat(behavior_path_planner): update pull_over module #873

Merged

7 tasks

kosuke55 requested a review from TakaHoribe July 6, 2022 10:46

tkimura4 reviewed Jul 6, 2022

View reviewed changes

taikitanaka3 reviewed Jul 7, 2022

View reviewed changes

...ehavior_path_planner/include/behavior_path_planner/occupancy_grid_map/occupancy_grid_map.hpp Outdated Show resolved Hide resolved

tkimura4 self-requested a review July 7, 2022 00:42

taikitanaka3 reviewed Jul 7, 2022

View reviewed changes

kosuke55 force-pushed the feature/behavior_path_occ_grid branch from ff212ea to 5957d4d Compare July 7, 2022 10:04

kosuke55 force-pushed the feature/behavior_path_occ_grid branch from c758bd2 to 319e157 Compare July 7, 2022 10:52

kosuke55 force-pushed the feature/behavior_path_occ_grid branch from 319e157 to 3c3ea26 Compare July 7, 2022 11:23

yukkysaito approved these changes Jul 7, 2022

View reviewed changes

kosuke55 force-pushed the feature/behavior_path_occ_grid branch from 4035c10 to 2912b92 Compare July 7, 2022 15:26

kosuke55 changed the title ~~feat(behavior_path_planner): add occupancy_grid_map~~ feat(behavior_path_planner): add occupancy_grid_based_collision_detector Jul 7, 2022

kosuke55 added 5 commits July 8, 2022 00:31

feat(behavior_path_planner): add occupancy_grid_based_collision_detector

a91198c

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

modify copyright

22ce1a6

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

move occ and path_shifter to utils

342801c

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

fix humble werror

b69db67

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

fix precomit

98efbff

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

kosuke55 force-pushed the feature/behavior_path_occ_grid branch from 2912b92 to 98efbff Compare July 7, 2022 15:33

kosuke55 merged commit bbe9420 into autowarefoundation:main Jul 7, 2022

kosuke55 deleted the feature/behavior_path_occ_grid branch July 7, 2022 15:57

tier4-autoware-public-bot bot mentioned this pull request Jul 8, 2022

chore: sync upstream tier4/autoware.universe#81

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(behavior_path_planner): add occupancy_grid_based_collision_detector #1259

feat(behavior_path_planner): add occupancy_grid_based_collision_detector #1259

kosuke55 commented Jul 6, 2022 •

edited

Loading

tkimura4 Jul 6, 2022

tkimura4 Jul 6, 2022

tkimura4 commented Jul 7, 2022

taikitanaka3 Jul 7, 2022

kosuke55 Jul 7, 2022

taikitanaka3 Jul 7, 2022

yukkysaito commented Jul 7, 2022

kosuke55 commented Jul 7, 2022 •

edited

Loading

yukkysaito commented Jul 7, 2022

codecov bot commented Jul 7, 2022 •

edited

Loading

kosuke55 commented Jul 7, 2022 •

edited

Loading

yukkysaito left a comment

		@@ -0,0 +1,205 @@
		// Copyright 2021 Tier IV, Inc. All rights reserved.

		@@ -0,0 +1,147 @@
		// Copyright 2021 Tier IV, Inc. All rights reserved.

feat(behavior_path_planner): add occupancy_grid_based_collision_detector #1259

feat(behavior_path_planner): add occupancy_grid_based_collision_detector #1259

Conversation

kosuke55 commented Jul 6, 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

tkimura4 Jul 6, 2022

Choose a reason for hiding this comment

tkimura4 Jul 6, 2022

Choose a reason for hiding this comment

tkimura4 commented Jul 7, 2022

taikitanaka3 Jul 7, 2022

Choose a reason for hiding this comment

kosuke55 Jul 7, 2022

Choose a reason for hiding this comment

taikitanaka3 Jul 7, 2022

Choose a reason for hiding this comment

yukkysaito commented Jul 7, 2022

kosuke55 commented Jul 7, 2022 • edited Loading

yukkysaito commented Jul 7, 2022

codecov bot commented Jul 7, 2022 • edited Loading

Codecov Report

kosuke55 commented Jul 7, 2022 • edited Loading

yukkysaito left a comment

Choose a reason for hiding this comment

kosuke55 commented Jul 6, 2022 •

edited

Loading

kosuke55 commented Jul 7, 2022 •

edited

Loading

codecov bot commented Jul 7, 2022 •

edited

Loading

kosuke55 commented Jul 7, 2022 •

edited

Loading