From 2a13d3032a2c0d21bd7b287d1aead96684423434 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 30 Jan 2023 09:40:29 +0900 Subject: [PATCH 01/72] fix(lanelet2_map_loader): delete unused parameters (#2761) * fix(lanelet2_map_loader): delete unused parameters * Update lanelet2_map_loader.launch.xml --- map/map_loader/launch/lanelet2_map_loader.launch.xml | 4 ---- 1 file changed, 4 deletions(-) diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml index ed9c691e86e30..08457362b23bb 100644 --- a/map/map_loader/launch/lanelet2_map_loader.launch.xml +++ b/map/map_loader/launch/lanelet2_map_loader.launch.xml @@ -3,7 +3,6 @@ - @@ -11,10 +10,7 @@ - - - From 18cd828875733c28208ba9160859de1e48aea270 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 30 Jan 2023 01:51:50 +0100 Subject: [PATCH 02/72] build(signal_processing): add build dependency (#2736) --- common/signal_processing/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/common/signal_processing/package.xml b/common/signal_processing/package.xml index 9496e87fc8240..df75114decfb0 100644 --- a/common/signal_processing/package.xml +++ b/common/signal_processing/package.xml @@ -12,6 +12,7 @@ Ali BOYALI autoware_cmake + libboost-dev ament_cmake_auto geometry_msgs rclcpp From 05da0bed5181f72a95604adb7d17d2824bd9ba67 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 30 Jan 2023 11:41:26 +0900 Subject: [PATCH 03/72] ci(pre-commit): update isort version to avoid CI failure (#2777) ci(pre-commit): update isort version to avoid pre-commit failure Signed-off-by: Tomohito Ando --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b69ba032fe35c..a42f76f318aa9 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -56,7 +56,7 @@ repos: args: [-w, -s, -i=4] - repo: https://github.com/pycqa/isort - rev: 5.10.1 + rev: 5.12.0 hooks: - id: isort From 140a92b585a61d2429c64eaf205bf874e1597484 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Mon, 30 Jan 2023 05:48:21 +0000 Subject: [PATCH 04/72] chore: sync files (#2778) * chore: sync files Signed-off-by: GitHub * style(pre-commit): autofix --------- Signed-off-by: GitHub Co-authored-by: kenji-miyake Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .github/workflows/deploy-docs.yaml | 1 + .github/workflows/pre-commit.yaml | 2 +- .pre-commit-config-optional.yaml | 2 +- .pre-commit-config.yaml | 22 +++++++++++----------- common/tier4_debug_tools/package.xml | 2 +- 5 files changed, 15 insertions(+), 14 deletions(-) diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index 270d04e6403d2..5f2d09d414806 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -4,6 +4,7 @@ on: push: branches: - main + - galactic paths: - mkdocs.yaml - "**/*.md" diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index 7fd1cc90fe97d..bda722c87ef09 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -5,7 +5,7 @@ on: jobs: pre-commit: - if: ${{ github.event.repository.private }} + if: ${{ github.event.repository.private }} # Use pre-commit.ci for public repositories runs-on: ubuntu-latest steps: - name: Generate token diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index a805f1201c414..e0019e10d5210 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/tcort/markdown-link-check - rev: v3.10.0 + rev: v3.10.3 hooks: - id: markdown-link-check args: [--config=.markdown-link-check.json] diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a42f76f318aa9..c54e4ce2f8462 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,10 +1,10 @@ ci: - autofix_commit_msg: "ci(pre-commit): autofix" + autofix_commit_msg: "style(pre-commit): autofix" autoupdate_commit_msg: "ci(pre-commit): autoupdate" repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.3.0 + rev: v4.4.0 hooks: - id: check-json - id: check-merge-conflict @@ -19,23 +19,23 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.32.1 + rev: v0.33.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] - repo: https://github.com/pre-commit/mirrors-prettier - rev: v2.7.1 + rev: v3.0.0-alpha.4 hooks: - id: prettier - repo: https://github.com/adrienverge/yamllint - rev: v1.27.1 + rev: v1.29.0 hooks: - id: yamllint - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.7.1 + rev: v0.8.0 hooks: - id: flake8-ros - id: prettier-xacro @@ -45,12 +45,12 @@ repos: - id: sort-package-xml - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.8.0.4 + rev: v0.9.0.2 hooks: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.5.1-1 + rev: v3.6.0-1 hooks: - id: shfmt args: [-w, -s, -i=4] @@ -61,19 +61,19 @@ repos: - id: isort - repo: https://github.com/psf/black - rev: 22.6.0 + rev: 22.12.0 hooks: - id: black args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.6 + rev: v15.0.7 hooks: - id: clang-format types_or: [c++, c, cuda] - repo: https://github.com/cpplint/cpplint - rev: 1.6.0 + rev: 1.6.1 hooks: - id: cpplint args: [--quiet] diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index d0a6487db0744..d4a01d67a1732 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -21,8 +21,8 @@ tier4_debug_msgs launch_ros - rclpy python3-rtree + rclpy ament_lint_auto autoware_lint_common From d90bb435ca6be7a88ef4b617b33dd67dbbc5c7e1 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 31 Jan 2023 01:20:23 +0900 Subject: [PATCH 05/72] doc: update occlusion spot doc (#2774) * doc: occlusion_spot Signed-off-by: taikitanaka3 * doc: update Signed-off-by: taikitanaka3 * doc: update * fix: pre-commit Signed-off-by: taikitanaka3 --------- Signed-off-by: taikitanaka3 --- .../behavior_after_safe_margin.drawio.svg | 929 ++++ .../behavior_after_safe_margin.svg | 454 -- .../occlusion_spot/collision_free.drawio.svg | 621 ++- .../docs/occlusion_spot/da.drawio.svg | 596 +++ .../occlusion_spot/detection_area_poly.svg | 567 --- .../maximum_slowdown_velocity.drawio.svg | 3830 +++++++++++++++++ .../maximum_slowdown_velocity.svg | 1235 ------ .../object_info_partition.drawio.svg | 669 +++ .../occlusion_spot/occlusion_spot.drawio.svg | 1053 +++++ .../occlusion_spot_partition.svg | 145 - .../docs/occlusion_spot/occupancy_grid.svg | 263 -- .../possible_collision_info.svg | 146 - .../occlusion_spot/raycast_shadow.drawio.svg | 1049 ++--- .../occlusion_spot/safe_motion.drawio.svg | 897 ++++ .../docs/occlusion_spot/safe_motion.svg | 646 --- .../docs/occlusion_spot/sidewalk_slice.svg | 179 - .../occlusion_spot/use_object_info.drawio.svg | 143 - .../velocity_planning.drawio.svg | 1276 ++++++ .../occlusion-spot-design.md | 118 +- 19 files changed, 10399 insertions(+), 4417 deletions(-) create mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.drawio.svg delete mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.svg create mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/da.drawio.svg delete mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg create mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.drawio.svg delete mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.svg create mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/object_info_partition.drawio.svg create mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.drawio.svg delete mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot_partition.svg delete mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/occupancy_grid.svg delete mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/possible_collision_info.svg create mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.drawio.svg delete mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.svg delete mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/sidewalk_slice.svg delete mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/use_object_info.drawio.svg create mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/velocity_planning.drawio.svg diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.drawio.svg new file mode 100644 index 0000000000000..640483a4ef2ac --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.drawio.svg @@ -0,0 +1,929 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedest... + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedest... + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedest... + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedest... + + + + + + + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedest... + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ + safe margin + +
+
+
+ + safe marg... + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + +
+
+
+ + collision point + +
+
+
+ + collision... + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{0}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{n}` + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.svg deleted file mode 100644 index 45106037c084e..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.svg +++ /dev/null @@ -1,454 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- pedestrian -
-
-
- - pedestrian - - - - - - - -
-
-
- pedestrian -
-
-
- - pedestrian - - - - - - - -
-
-
- pedestrian -
-
-
- - pedestrian - - - - - - - - - - - -
-
-
- pedestrian -
-
-
- - pedestrian - - - - - - - - - -
-
-
- pedestrian -
-
-
- - pedestrian - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - -
-
-
- - safe margin - -
-
-
- - safe margin - - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - - -
-
-
- - collision point - -
-
-
- - collision point - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - - - - - - - -
-
-
- - - - - - -
-
-
- - t_{0} - - - - - - -
-
-
- - - - - - -
-
-
- - t_{n} - - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg index 8a785bd919726..7c30818b54e77 100644 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg @@ -1,110 +1,547 @@ - + + + - - - - - - - - - - - - - - - - - -
-
- ego + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ ego +
+
-
- + + ego + - - - - - - -
-
- pedestrian + + + + + + + + + + + + + + +
+
+
+ + collision free + +
+
-
- + + collision free + - - - - - - - - -
-
- pedestrian + + + + + +
+
+
+ pedestrian +
+
-
- + + pedes... + - - - - - - - - - - - - - -
-
- collision judgement is done by polygon consider  pedestrian radius + + + + + +
+
+
+ collision +
+
-
- + + collis... + - - - -
-
- collision free + + + + + +
+
+
+ pedestrian +
+
-
- + + pedes... + + + + + + + Text is not SVG - cannot display + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/da.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/da.drawio.svg new file mode 100644 index 0000000000000..3609eec53ff15 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/da.drawio.svg @@ -0,0 +1,596 @@ + + + + + + + + + + + + + +
+
+
+ wall +
+
+
+ + wall + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedes... + + + + + + + + +
+
+
+ TTV +
+
+
+ + TTV + + + + + +
+
+
+ lateral max distance +
+
+
+ + lateral m... + + + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + +
+
+
+ offset +
+
+
+ + offset + + + + + + + + + + + + + + + + +
+
+
+ detection area max length +
+
+
+ + detection area max length + + + + + + + + + + +
+
+
+ TTC +
+
+
+ + TTC + + + + + +
+
+
+ + TTC>TTV + +
+
+
+ + + TTC>TTV + + + + + + +
+
+
+
+ TTV: time to vehicle +
+ +
+ TTC: time to collision +
+ +
+
+
+ + TTV: time to vehicleTTC: time to col... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg deleted file mode 100644 index b2e6772a6c02e..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg +++ /dev/null @@ -1,567 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - -
-
-
- - - - - - -
-
-
- - t_4 - - - - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - - - - - -
-
-
- offset -
-
-
- - offset - - - - - - - - -
-
-
- area which obstacle is coming  before ego vehicle pass -
-
-
- - area which obstacle is coming  befo... - - - - - -
-
-
- front bamper -
-
-
- - front ba... - - - - - - - - - - -
-
-
- lateral max distance -
-
-
- - lateral m... - - - - - - - - -
-
-
- calculate  lateral distance from TTC top consider case if obstacle coming out from here -
-
-
- - calculate  lateral distance from TTC top consider c... - - - - - - - - -
-
-
- - - - - - -
-
-
- - t_1 - - - - - - - - - - - - - - - - - -
-
-
- interpolated polygon -
-
-
- - interpolated polygon - - - - - - - -
-
-
- - - - - - -
-
-
- - t_4 - - - - - - - -
-
-
- expand detail -
-
-
- - expand detail - - - - - - - - - - - -
-
-
- collision point of obstacle -
-
-
- - collision point of obst... - - - - - -
-
-
- place where obstacle  might be coming -
-
-
- - place where obstacle  m... - - - - - - - - - - -
-
-
- area which ego vehicle pass before hidden obstacle is coming -
-
-
- - area which ego vehicle pass before... - - - - - - - -
-
-
- area which ego vehicle pass before hidden obstacle is coming -
-
-
- - area which ego vehicle pass before... - - - - - - - - - -
-
-
- detection area max length -
-
-
- - detection area max length - - - - - - - - -
-
-
- max length is calculated from ego current velocity and current acceleration -
-
-
- - max length is calculated from ego current velocity and current acceleration - - - - - - -
-
-
- area which planned velocity is not effective -
-
-
- - area which planned velocity i... - - - - - - -
-
-
- area which planned velocity is not effective -
-
-
- - area which planned velocity i... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.drawio.svg new file mode 100644 index 0000000000000..82a5d7b6eb30a --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.drawio.svg @@ -0,0 +1,3830 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ + `t` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ + `v` + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{2... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `const\ jer... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $$const \ accel$$ + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+
+
+ + `t_{1}=\frac{a_max-a... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{0... + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{0... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{1}=\frac{j}{6}*t... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{2... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{2}=\frac{a_{max}... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `a_{0... + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `a_{1}... + + + + + + + + +
+
+
+ s +
+
+
+ + s + + + + + + +
+
+
+ v +
+
+
+ + v + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{3... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `s_{safe} =v_{1}*t_{1} +... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{2... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{3... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `const\ jer... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $$const \ accel$$ + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{3... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+
+
+ + `v_{safe} = 0`... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + , + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+
+
+ + + `t_{3} < t_{1}`... + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $$delay... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{2... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $$delay... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `const\ jer... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $$const \ accel$$ + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{2... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.svg deleted file mode 100644 index 0d9f4f63ec203..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.svg +++ /dev/null @@ -1,1235 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- - - - - - -
-
-
- - t - - - - - - -
-
-
- - - - - - -
-
-
- - v - - - - - - - - - -
-
-
- - - - - - - - -
-
-
- - t_{1} - - - - - - -
-
-
- - - - - - - - -
-
-
- - x_{1} - - - - - - -
-
-
- - - - - - - - -
-
-
- - x_{2} - - - - - - -
-
-
- - - - - - -
-
-
- - const\ jerk - - - - - - -
-
-
- -
- - - - -
- -
-
-
- - const \ accel - - - - - - -
-
-
- - - - - - - - -
-
-
- - v_{1} - - - - - - -
-
-
- - - - - - - - -
- -
- -
-
-
- - t_{1}=\frac{a_max-a_{0}}{j_max}... - - - - - - -
-
-
- - - - - - - - -
-
-
- - t_{0} - - - - - - - - - -
-
-
- - - - - - - - -
-
-
- - v_{0} - - - - - - -
-
-
- - - - - - - - -
-
-
- - x_{1}=\frac{j}{6}*t^3+\frac{a_{0... - - - - - - -
-
-
- - - - - - - - -
-
-
- - t_{2} - - - - - - -
-
-
- - - - - - - - -
-
-
- - x_{2}=\frac{a_{max}}{2}*t^2+v_{1... - - - - - - -
-
-
- - - - - - - - -
-
-
- - a_{0} - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/object_info_partition.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/object_info_partition.drawio.svg new file mode 100644 index 0000000000000..64a5d99b28baf --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/object_info_partition.drawio.svg @@ -0,0 +1,669 @@ + + + + + + + + + + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ lack of pcd +
+
+
+ + lack of pcd + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ fence +
+
+
+ + fence + + + + + +
+
+
+ we can tell there is no pedestrian +
+ run out from here +
+ if there is fence tag +
+
+
+ + we can tell there is no pedestrian... + + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + +
+
+
+ lack of pcd +
+
+
+ + lack of pcd + + + + + + + + + + +
+
+
+ we can tell here is occupied cells +
+ if we use object information +
+
+
+ + we can tell here is occupied cells... + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.drawio.svg new file mode 100644 index 0000000000000..8acecba2f558d --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.drawio.svg @@ -0,0 +1,1053 @@ + + + + + + + + + + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + ped... + + + + + + + + +
+
+
+ fence +
+
+
+ + fence + + + + + + + + + + + + + + + +
+
+
+ will not run out +
+
+
+ + wil... + + + + + + + + +
+
+
+ colliision +
+
+
+ + collii... + + + + + + + + + + + + + + + + + +
+
+
+ occupancy grid +
+
+
+ + occupancy grid + + + + + + + + + + + + + +
+
+
+ + occupied +
+ 58~100 + +
+
+
+ + occupie... + + + + + + + + + + + +
+
+
+ + + unknown +
+ 43~58 + + +
+
+
+ + unknown... + + + + + + + + + + + +
+
+
+ free space +
+ 0~43 +
+
+
+ + free sp... + + + + + + + + +
+
+
+ free space +
+
+
+ + free space + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ v [m/s] +
+
+
+ + v [m/s] + + + + + +
+
+
+ this module plans + safe slow down velocity +
+ to stop before collision with strong brake +
+
+
+ + this module plans safe slow down velocity... + + + + + + +
+
+
+ s [m] +
+
+
+ + s [m] + + + + + + + + + + + + + + + +
+
+
+ ego velocity +
+
+
+ + ego velocity + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot_partition.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot_partition.svg deleted file mode 100644 index aedeb20bd703e..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot_partition.svg +++ /dev/null @@ -1,145 +0,0 @@ - - - - - - - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - - - -
-
-
-
- partition lanelet -
-
- - fence -
-
- - gurad_rail -
-
- - wall -
-
-
-
- - partition lanelet... - - - - - - -
-
-
-
detection area polygon
-
-
-
- - detection area polygon - - - - - - - - - - - -
-
-
- occlusion spot inside partition is considered -
-
-
- - occlusion spot inside p... - - - - - - -
-
-
- occlusion spot outside partition is not considered -
-
-
- - occlusion spot outside... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occupancy_grid.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occupancy_grid.svg deleted file mode 100644 index 196f5a05bad0d..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/occupancy_grid.svg +++ /dev/null @@ -1,263 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
pedestrian
-
-
- - ped... - - - - - - - - -
-
-
- has collision -
-
-
- - has collision - - - - - - -
-
-
- has collision -
-
-
- - has collision - - - - - -
-
-
-

- - Occupancy Grid - -

-

- - - Occlusion spot is where N by N size "no information" grid cell has that doesn't have any collision with "occupied" cells from occlusion spot to ego path. - - -

-
-
-
-
-
-
- - Occupancy Grid... - - - - - -
-
-
-
- Occupancy Grid Cell -
-
-
- free space : 0~45 -
-
- unknown   : 45~55 -
-
- occupied   : 95~100 -
-
-
-
- - Occupancy Grid Cell... - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/possible_collision_info.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/possible_collision_info.svg deleted file mode 100644 index f3b14e972a8ae..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/possible_collision_info.svg +++ /dev/null @@ -1,146 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - -
-
-
-

ObstacleInfo

-

- - Information of obstacle to consider as possible collision - if - there is a hidden obstacle that darting out from occlusion spot. -

-
-
-
- - ObstacleInfo... - - - - - - -
-
-
-

- Intersection -

-
- - position that ego vehicle can have a collision with hidden obstacle where ego vehicle baselink is at collision point. -
-
-
-
- - Intersection... - - - - - - -
-
-
-

- Collision Point -

-
- - position to inset safe velocity to path. -
-
-
- - Collision Point... - - - - - - - - - - - - - - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg index c6e1943c7bfbf..6dedeaac81142 100644 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg @@ -3,680 +3,749 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="647px" - height="682px" - viewBox="-0.5 -0.5 647 682" - content="<mxfile><diagram id="Doxya9YoADHyABFa7ko3" name="shadow">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</diagram></mxfile>" + width="655px" + height="824px" + viewBox="-0.5 -0.5 655 824" + content="<mxfile><diagram id="Doxya9YoADHyABFa7ko3" name="shadow">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</diagram></mxfile>" > - - - - - - - - - + + + + + + - - - - + + + + + + + + + + +
-
+
ego
- ego + ego - + + +
-
+
stuck vehicle
- stuck ve... + stuck... - + +
cruising -
+
vehicle
- cruising... + cruis... - - - - - - + + + +
-
possible collision
+
+ possible collision +
- possi... + pos... - - + + + + +
-
-
- not possible collision +
+
+ not possible collision
- not p... + not... - - - + + + + + +
-
- occupancy grid consider possible collision object ray cast +
+ remove possible collision candidate behind predicted objects
- occupancy grid consider possible collision object ray c... + remove possible collision candida... - - - - - - + + + + + + +
-
- +
+ passable -
- collision -
+
- passable... + passable - - - - - - + + + + + + + + + + + + + + + +
-
+
stuck vehicle
- stuck ve... + stuck... - + +
-
- passable +
+ passable
- passable + passable - + + +
-
+
cruising -
+
vehicle
- cruising... + cruis... - - + + + +
-
passable collision
+
+ passable collision +
- passa... + pas... - - + + + +
-
passable collision
+
+ passable collision +
- passa... + pas... + + + + + + + + + +
+
+
+ this pedestrian will collide with cruising car +
+
+
+ + this pedestrian will collide with... + + + + + + + - Viewer does not support full SVG 1.1 + Text is not SVG - cannot display diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.drawio.svg new file mode 100644 index 0000000000000..94951bec84f2c --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.drawio.svg @@ -0,0 +1,897 @@ + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `S_{safe}("TTC")` + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedes... + + + + + + + + +
+
+
+ Obstacle +
+
+
+ + Obstacle + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ s +
+
+
+ + s + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `V_{safe}("TTC")` + + + + + + +
+
+
+ TTV +
+
+
+ + TTV + + + + + + +
+
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + is calculated by emergency stop jerk and acceleration which ego vehicle can stop within TTC. +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + is the distance to stop when the car is running at + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + . +
+ + + TTV means Time To Vehicle for the approaching pedestrian to own vehicle. +
+ + TTC means Time To Collision of the approaching ego to collision point.. + +
+ +
+
+
+ + + `V_{safe}` is calculated by emergency stop jerk and acceleration which ego vehicle can stop within TTC.... + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ collision point with margin +
+
+
+ + collision point with ma... + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.svg deleted file mode 100644 index 7ee5bb292b90a..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.svg +++ /dev/null @@ -1,646 +0,0 @@ - - - - - - - - - - - - - - - - - - - -
-
-
- - - - - - - - -
-
-
- - S_{safe}("TTC") - - - - - - - -
-
-
pedestrian
-
-
- - pedes... - - - - - - -
-
-
- Obstacle -
-
-
- - Obstacle - - - - - - - - - - - - - - - - - - -
-
-
- s -
-
-
- - s - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - -
-
-
- - - - - - - - -
-
-
- - V_{safe}("TTC") - - - - - - -
-
-
- TTC -
-
-
- - TTC - - - - - - -
-
-
- -
- - - - - - - - - is calculated by emergency stop jerk and acceleration which ego vehicle can stop within TTC. -
- - - - - - - - - is the distance to stop when the car is running at - - - - - - - . -
- - TTC: TTC means Time To Collision of the approaching pedestrian with the own vehicle. -
-
-
- - - V_{safe} is calculated by emergency stop jerk and acceleration which ego vehicle can stop within TTC.... - - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - -
-
-
- collision point with margin -
-
-
- - collision point with ma... - - - - - - -
-
-
- collision point -
-
-
- - collision point - - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/sidewalk_slice.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/sidewalk_slice.svg deleted file mode 100644 index 5540ab7236750..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/sidewalk_slice.svg +++ /dev/null @@ -1,179 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
-

Sidewalk Slice

-
create sidewalk slice polygon inside focus range
-
-
-
- - Sidewalk Slice... - - - - - - - - -
-
-
- focus range -
-
-
- - focus range - - - - - -
-
-
-

Only Consider Lower Bound

-

considering all occlusion spot  cost very much, so consider only  longitudinally or laterally closest hidden obstacle as slow down  target.

-

-
-

-
-
-
- - Only Consider Lower Bound... - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/use_object_info.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/use_object_info.drawio.svg deleted file mode 100644 index a8afe81d7f497..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/use_object_info.drawio.svg +++ /dev/null @@ -1,143 +0,0 @@ - - - - - - - - - - - - - -
-
- ego -
-
- - - - - - - -
-
- use object info -
-
- - - - - - - - - - - - -
-
- ego -
-
- - - - - - - -
-
- without object info -
-
- - - - - -
-
- obj -
-
- - - - - - - - - - - -
-
- obj -
-
- - - - - - - - - - - - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/velocity_planning.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/velocity_planning.drawio.svg new file mode 100644 index 0000000000000..55693e2a2688c --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/velocity_planning.drawio.svg @@ -0,0 +1,1276 @@ + + + + + + + + + + + + + +
+
+
+ v [m/s] +
+
+
+ + v [m/s] + + + + + + + + + + + +
+
+
+ s [m] +
+
+
+ + s [m] + + + + + + + + + + + + + + + +
+
+
+ + min velocity + +
+
+
+ + min velocity + + + + + +
+
+
+ + + + - -  reference maximum velocity +
+
+ + ---  safe velocity to stop before collisopn + +
+ + + + + ---- safe slow donw velocity +
+ + + ---- min slow down velocity +
+ + + ---- motion velocity smoother velocity +
+ + O  current velocity +
+
+ + +
+
+
+ + - -  reference maximum velocity... + + + + + + + +
+
+
+ x +
+
+
+ + x + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ al +
+
+
+ + al + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md index b4670eef0345a..b05714cac1352 100644 --- a/planning/behavior_velocity_planner/occlusion-spot-design.md +++ b/planning/behavior_velocity_planner/occlusion-spot-design.md @@ -4,104 +4,108 @@ This module plans safe velocity to slow down before reaching collision point that hidden object is darting out from `occlusion spot` where driver can't see clearly because of obstacles. -![brief](./docs/occlusion_spot/occlusion_spot.svg) +![brief](./docs/occlusion_spot/occlusion_spot.drawio.svg) ### Activation Timing -This module is activated if `launch_occlusion_spot` becomes true +This module is activated if `launch_occlusion_spot` becomes true. To make pedestrian first zone map tag is one of the TODOs. -### Limitation +### Limitation and TODOs -To solve the excessive deceleration due to false positive of the perception, the logic of detection method can be selectable. This point has not been discussed in detail and needs to be improved (see the description below). +This module is prototype implementation to care occlusion spot. To solve the excessive deceleration due to false positive of the perception, the logic of detection method can be selectable. This point has not been discussed in detail and needs to be improved. + +- Make occupancy grid for planning. +- Make map tag for occlusion spot. +- About the best safe motion. + +TODOs are written in each Inner-workings / Algorithms (see the description below). ### Inner-workings / Algorithms #### Logics Working -There are several types of occlusions, such as "occlusions generated by parked vehicles" and "occlusions caused by obstructions". In situations such as driving on **road with obstacles**, where people jump out of the way frequently, all possible occlusion spots must be taken into account. This module considers all occlusion spots calculated from the **occupancy grid**, but it is not reasonable to take into account all occlusion spots for example, people jumping out from behind a guardrail, or behind cruising vehicle. Therefore currently detection area will be limited to to use **dynamic object** information. +There are several types of occlusions, such as "occlusions generated by parked vehicles" and "occlusions caused by obstructions". In situations such as driving on **road with obstacles**, where people jump out of the way frequently, all possible occlusion spots must be taken into account. This module considers all occlusion spots calculated from the **occupancy grid**, but it is not reasonable to take into account all occlusion spots for example, people jumping out from behind a guardrail, or behind cruising vehicle. Therefore currently detection area will be limited to to use **predicted object** information. Note that this decision logic is still under development and needs to be improved. -#### Occlusion Spot Public +#### DetectionArea Polygon -This module inserts safe velocity at the collision point estimated from the associated occlusion spot under assumption that the pedestrian possibly coming out of the occlusion spot. -![brief](./docs/occlusion_spot/possible_collision_info.svg) +This module considers TTV from pedestrian velocity and lateral distance to occlusion spot. +TTC is calculated from ego velocity and acceleration and longitudinal distance until collision point using motion velocity smoother. +To compute fast this module only consider occlusion spot whose TTV is less than TTC and only consider area within "max lateral distance". -This module consider 3 policies that are very important for risk predicting system for occlusion spot. +![brief](./docs/occlusion_spot/da.drawio.svg) -1. "Passable" without deceleration - If ego vehicle speed is high enough to pass the occlusion spot and expected to have no collision with any objects coming out of the occlusion spot, then it's possible for ego vehicle to pass the spot without deceleration. +#### Occlusion Spot Occupancy Grid Base -2. "Predictable" with enough distance to occlusion - If ego vehicle has enough distance to the occlusion spot, then ego vehicle is going to slow down to the speed that is slow enough to stop before collision with full brake. - If ego vehicle pass the possible collision point, then ego vehicle is going to drive normally. +This module considers any occlusion spot around ego path computed from the occupancy grid. +Due to the computational cost occupancy grid is not high resolution and this will make occupancy grid noisy so this module add information of occupancy to occupancy grid map. -3. "Unavoidable" without enough distance to occlusion spot - This module assumes the occlusion spot is detected stably far from the ego vehicle. Therefore this module can not guarantee the safety behavior for the occlusion spot detected suddenly in front of the ego vehicle. In this case, slow velocity that does not cause the strong deceleration is only applied. +TODO: consider hight of obstacle point cloud to generate occupancy grid. -#### Occlusion Spot Private +##### Collision Free Judgement -This module considers any occlusion spot around ego path computed from the occupancy grid. +obstacle that can run out from occlusion should have free space until intersection from ego vehicle -![occupancy_grid](./docs/occlusion_spot/occupancy_grid.svg) +![brief](./docs/occlusion_spot/collision_free.drawio.svg) -#### Occlusion Spot Common +##### Partition Lanelet -##### The Concept of Safe Velocity +By using lanelet information of "guard_rail", "fence", "wall" tag, it's possible to remove unwanted occlusion spot. -The safe slowdown velocity is calculated from the below parameters of ego emergency braking system and time to collision. +By using static object information, it is possible to make occupancy grid more accurate. -- jerk limit[m/s^3] -- deceleration limit[m/s2] -- delay response time[s] -- time to collision of pedestrian[s] - with these parameters we can briefly define safe motion before occlusion spot for ideal environment. - ![occupancy_grid](./docs/occlusion_spot/safe_motion.svg) +To make occupancy grid for planning is one of the TODOs. -##### Maximum Slowdown Velocity +![brief](./docs/occlusion_spot/object_info_partition.drawio.svg) -The maximum slowdown velocity is calculated from the below parameters of ego current velocity and acceleration with maximum slowdown jerk and maximum slowdown acceleration in order not to slowdown too much. +##### Possible Collision -- $j_{max}$ slowdown jerk limit[m/s^3] -- $a_{max}$ slowdown deceleration limit[m/s2] -- $v_{0}$ current velocity[m/s] -- $a_{0}$ current acceleration[m/s] +obstacle that can run out from occlusion is interrupted by moving vehicle. -![brief](./docs/occlusion_spot/maximum_slowdown_velocity.svg) +![brief](./docs/occlusion_spot/raycast_shadow.drawio.svg) -##### Safe Behavior After Passing Safe Margin Point +#### About safe motion -This module defines safe margin to consider ego distance to stop and collision path point geometrically. -While ego is cruising from safe margin to collision path point, ego vehicle keeps the same velocity as occlusion spot safe velocity. +![brief](./docs/occlusion_spot/velocity_planning.drawio.svg) -![brief](./docs/occlusion_spot/behavior_after_safe_margin.svg) +##### The Concept of Safe Velocity and Margin -##### DetectionArea Polygon +The safe slowdown velocity is calculated from the below parameters of ego emergency braking system and time to collision. +Below calculation is included but change velocity dynamically is not recommended for planner. -Occlusion spot computation: searching occlusion spots for all cells in the occupancy_grid inside "max lateral distance" requires a lot of computational cost, so this module use only one most notable occlusion spot for each partition. (currently offset is from baselink to front for safety) -The maximum length of detection area depends on ego current vehicle velocity and acceleration. +- jerk limit[m/s^3] +- deceleration limit[m/s2] +- delay response time[s] +- time to collision of pedestrian[s] + with these parameters we can briefly define safe motion before occlusion spot for ideal environment. -![brief](./docs/occlusion_spot/detection_area_poly.svg) + ![occupancy_grid](./docs/occlusion_spot/safe_motion.drawio.svg) -##### Partition Lanelet +This module defines safe margin to consider ego distance to stop and collision path point geometrically. +While ego is cruising from safe margin to collision path point, ego vehicle keeps the same velocity as occlusion spot safe velocity. -By using lanelet information of "guard_rail", "fence", "wall" tag, it's possible to remove unwanted occlusion spot. -![brief](./docs/occlusion_spot/occlusion_spot_partition.svg) +![brief](./docs/occlusion_spot/behavior_after_safe_margin.drawio.svg) -##### Use Object Info +Note: This logic assumes high-precision vehicle speed tracking and margin for decel point might not be the best solution, and override with manual driver is considered if pedestrian really run out from occlusion spot. -use object info to make occupancy grid more accurate -![brief](./docs/occlusion_spot/use_object_info.drawio.svg) +TODO: consider one of the best choices -##### Collision Free Judgement +1. stop in front of occlusion spot +2. insert 1km/h velocity in front of occlusion spot +3. slowdown this way +4. etc... . -obstacle that can run out from occlusion should have free space until intersection from ego vehicle -![brief](./docs/occlusion_spot/collision_free.drawio.svg) +##### Maximum Slowdown Velocity -##### Possible Collision +The maximum slowdown velocity is calculated from the below parameters of ego current velocity and acceleration with maximum slowdown jerk and maximum slowdown acceleration in order not to slowdown too much. -obstacle that can run out from occlusion is interrupted by moving vehicle. -![brief](./docs/occlusion_spot/raycast_shadow.drawio.svg) +- $j_{max}$ slowdown jerk limit[m/s^3] +- $a_{max}$ slowdown deceleration limit[m/s2] +- $v_{0}$ current velocity[m/s] +- $a_{0}$ current acceleration[m/s] + +![brief](./docs/occlusion_spot/maximum_slowdown_velocity.drawio.svg) #### Module Parameters @@ -222,7 +226,7 @@ stop @enduml ``` -##### Detail process for predicted object +##### Detail process for predicted object(not updated) ```plantuml @startuml @@ -274,7 +278,7 @@ stop @enduml ``` -##### Detail process for private road +##### Detail process for Occupancy grid base ```plantuml @startuml From 5bb6357f4f533b295506d85e027adeaef8567b98 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 31 Jan 2023 11:11:31 +0900 Subject: [PATCH 06/72] feat(detection_by_tracker, shape_estimation): add trailer filter to properly track trailer shape (#2461) * add trailer filter in shape fitting Signed-off-by: yoshiri * fix trailer_filter compile error Signed-off-by: yoshiri * use adaptive range to merge objects Signed-off-by: yoshiri * add search range for divider Signed-off-by: yoshiri * add trailer corrector Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../detection_by_tracker_core.hpp | 5 +++ .../src/detection_by_tracker_core.cpp | 37 +++++++++++++++- perception/shape_estimation/CMakeLists.txt | 1 + .../shape_estimation/corrector/corrector.hpp | 1 + .../corrector/trailer_corrector.hpp | 43 +++++++++++++++++++ .../shape_estimation/filter/filter.hpp | 1 + .../filter/trailer_filter.hpp | 33 ++++++++++++++ .../lib/filter/trailer_filter.cpp | 25 +++++++++++ .../lib/filter/truck_filter.cpp | 2 +- .../shape_estimation/lib/shape_estimator.cpp | 8 +++- 10 files changed, 151 insertions(+), 5 deletions(-) create mode 100644 perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp create mode 100644 perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp create mode 100644 perception/shape_estimation/lib/filter/trailer_filter.cpp diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 165b9f9929894..577e0ff12c367 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -44,6 +44,7 @@ #include #include +#include #include #include @@ -78,9 +79,13 @@ class DetectionByTracker : public rclcpp::Node std::shared_ptr shape_estimator_; std::shared_ptr cluster_; std::shared_ptr debugger_; + std::map max_search_distance_for_merger_; + std::map max_search_distance_for_divider_; bool ignore_unknown_tracker_; + void setMaxSearchRange(); + void onObjects( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg); diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 226aba129da21..3c29f63ea2366 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -159,12 +159,39 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) ignore_unknown_tracker_ = declare_parameter("ignore_unknown_tracker", true); + // set maximum search setting for merger/divider + setMaxSearchRange(); + shape_estimator_ = std::make_shared(true, true); cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); } +void DetectionByTracker::setMaxSearchRange() +{ + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + // set max search distance for merger + max_search_distance_for_merger_[Label::UNKNOWN] = 5.0; + max_search_distance_for_merger_[Label::CAR] = 5.0; + max_search_distance_for_merger_[Label::TRUCK] = 8.0; + max_search_distance_for_merger_[Label::BUS] = 8.0; + max_search_distance_for_merger_[Label::TRAILER] = 10.0; + max_search_distance_for_merger_[Label::MOTORCYCLE] = 2.0; + max_search_distance_for_merger_[Label::BICYCLE] = 1.0; + max_search_distance_for_merger_[Label::PEDESTRIAN] = 1.0; + + // set max search distance for divider + max_search_distance_for_divider_[Label::UNKNOWN] = 6.0; + max_search_distance_for_divider_[Label::CAR] = 6.0; + max_search_distance_for_divider_[Label::TRUCK] = 9.0; + max_search_distance_for_divider_[Label::BUS] = 9.0; + max_search_distance_for_divider_[Label::TRAILER] = 11.0; + max_search_distance_for_divider_[Label::MOTORCYCLE] = 3.0; + max_search_distance_for_divider_[Label::BICYCLE] = 2.0; + max_search_distance_for_divider_[Label::PEDESTRIAN] = 2.0; +} + void DetectionByTracker::onObjects( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg) { @@ -222,7 +249,6 @@ void DetectionByTracker::divideUnderSegmentedObjects( { constexpr float recall_min_threshold = 0.4; constexpr float precision_max_threshold = 0.5; - constexpr float max_search_range = 6.0; constexpr float min_score_threshold = 0.4; out_objects.header = in_cluster_objects.header; @@ -232,6 +258,9 @@ void DetectionByTracker::divideUnderSegmentedObjects( const auto & label = tracked_object.classification.front().label; if (ignore_unknown_tracker_ && (label == Label::UNKNOWN)) continue; + // change search range according to label type + const float max_search_range = max_search_distance_for_divider_[label]; + std::optional highest_score_divided_object = std::nullopt; float highest_score = 0.0; @@ -357,7 +386,6 @@ void DetectionByTracker::mergeOverSegmentedObjects( tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects) { constexpr float precision_threshold = 0.5; - constexpr float max_search_range = 5.0; out_objects.header = in_cluster_objects.header; out_no_found_tracked_objects.header = tracked_objects.header; @@ -365,6 +393,9 @@ void DetectionByTracker::mergeOverSegmentedObjects( const auto & label = tracked_object.classification.front().label; if (ignore_unknown_tracker_ && (label == Label::UNKNOWN)) continue; + // change search range according to label type + const float max_search_range = max_search_distance_for_merger_[label]; + // extend shape autoware_auto_perception_msgs::msg::DetectedObject extended_tracked_object = tracked_object; extended_tracked_object.shape = extendShape(tracked_object.shape, /*scale*/ 1.1); @@ -383,6 +414,8 @@ void DetectionByTracker::mergeOverSegmentedObjects( const float precision = perception_utils::get2dPrecision(initial_object.object, extended_tracked_object); if (precision < precision_threshold) { + if (tracked_object.classification.front().label) + std::cout << "Rejected by Perception Threshold:" << precision << std::endl; continue; } pcl::PointCloud pcl_cluster; diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 139c9e6d28f67..5ae6002cd7c3b 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -24,6 +24,7 @@ ament_auto_add_library(shape_estimation_lib SHARED lib/filter/car_filter.cpp lib/filter/bus_filter.cpp lib/filter/truck_filter.cpp + lib/filter/trailer_filter.cpp lib/filter/no_filter.cpp lib/filter/utils.cpp lib/corrector/no_corrector.cpp diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp index 73e6770c32170..d52bdc21f916f 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp +++ b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp @@ -20,6 +20,7 @@ #include "shape_estimation/corrector/corrector_interface.hpp" #include "shape_estimation/corrector/no_corrector.hpp" #include "shape_estimation/corrector/reference_shape_size_corrector.hpp" +#include "shape_estimation/corrector/trailer_corrector.hpp" #include "shape_estimation/corrector/truck_corrector.hpp" #endif // SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp new file mode 100644 index 0000000000000..6dd885353c78f --- /dev/null +++ b/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp @@ -0,0 +1,43 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +#define SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ + +#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "utils.hpp" + +// Generally speaking, trailer would be much larger than bus and truck. +// But currently we do not make large differences among bus/truck/trailer +// because current our vehicle classification is not reliable enough. +class TrailerCorrector : public VehicleCorrector +{ +public: + explicit TrailerCorrector(const bool use_reference_yaw = false) + : VehicleCorrector(use_reference_yaw) + { + corrector_utils::CorrectionBBParameters params; + params.min_width = 2.0; + params.max_width = 3.2; + params.default_width = (params.min_width + params.max_width) * 0.5; + params.min_length = 5.0; + params.max_length = 24.0; + params.default_length = 8.0; // Ideally, it should be 12m from average Japanese trailer size. + setParams(params); + } + + ~TrailerCorrector() = default; +}; + +#endif // SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/filter.hpp b/perception/shape_estimation/include/shape_estimation/filter/filter.hpp index cfbfd39a08dcc..b205cbd6791ba 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/filter.hpp +++ b/perception/shape_estimation/include/shape_estimation/filter/filter.hpp @@ -19,6 +19,7 @@ #include "shape_estimation/filter/car_filter.hpp" #include "shape_estimation/filter/filter_interface.hpp" #include "shape_estimation/filter/no_filter.hpp" +#include "shape_estimation/filter/trailer_filter.hpp" #include "shape_estimation/filter/truck_filter.hpp" #endif // SHAPE_ESTIMATION__FILTER__FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp b/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp new file mode 100644 index 0000000000000..83c9ab0d8a944 --- /dev/null +++ b/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp @@ -0,0 +1,33 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#define SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ + +#include "shape_estimation/filter/filter_interface.hpp" +#include "utils.hpp" + +class TrailerFilter : public ShapeEstimationFilterInterface +{ +public: + TrailerFilter() = default; + + ~TrailerFilter() = default; + + bool filter( + const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose) override; +}; + +#endif // SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ diff --git a/perception/shape_estimation/lib/filter/trailer_filter.cpp b/perception/shape_estimation/lib/filter/trailer_filter.cpp new file mode 100644 index 0000000000000..368985d5303db --- /dev/null +++ b/perception/shape_estimation/lib/filter/trailer_filter.cpp @@ -0,0 +1,25 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "shape_estimation/filter/trailer_filter.hpp" + +bool TrailerFilter::filter( + const autoware_auto_perception_msgs::msg::Shape & shape, + [[maybe_unused]] const geometry_msgs::msg::Pose & pose) +{ + constexpr float min_width = 1.5; + constexpr float max_width = 3.2; + constexpr float max_length = 25.0; // maximum Full-TRAILER size in JAPAN(normally upto 18m) + return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); +} diff --git a/perception/shape_estimation/lib/filter/truck_filter.cpp b/perception/shape_estimation/lib/filter/truck_filter.cpp index 12dd066c2ca41..672082fe305fd 100644 --- a/perception/shape_estimation/lib/filter/truck_filter.cpp +++ b/perception/shape_estimation/lib/filter/truck_filter.cpp @@ -20,6 +20,6 @@ bool TruckFilter::filter( { constexpr float min_width = 1.5; constexpr float max_width = 3.2; - constexpr float max_length = 7.9; + constexpr float max_length = 7.9; // upto 12m in japanese law return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index ac2024e4787cc..9577b1e2d3197 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -92,8 +92,10 @@ bool ShapeEstimator::applyFilter( filter_ptr.reset(new CarFilter); } else if (label == Label::BUS) { filter_ptr.reset(new BusFilter); - } else if (label == Label::TRUCK || label == Label::TRAILER) { + } else if (label == Label::TRUCK) { filter_ptr.reset(new TruckFilter); + } else if (label == Label::TRAILER) { + filter_ptr.reset(new TrailerFilter); } else { filter_ptr.reset(new NoFilter); } @@ -114,8 +116,10 @@ bool ShapeEstimator::applyCorrector( corrector_ptr.reset(new CarCorrector(use_reference_yaw)); } else if (label == Label::BUS) { corrector_ptr.reset(new BusCorrector(use_reference_yaw)); - } else if (label == Label::TRUCK || label == Label::TRAILER) { + } else if (label == Label::TRUCK) { corrector_ptr.reset(new TruckCorrector(use_reference_yaw)); + } else if (label == Label::TRAILER) { + corrector_ptr.reset(new TrailerCorrector(use_reference_yaw)); } else { corrector_ptr.reset(new NoCorrector); } From 202f283c289800c9c1055ecba6bdbbb435212dd1 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 31 Jan 2023 11:58:08 +0900 Subject: [PATCH 07/72] fix(behavior_path_planner): fix pull out module when enable_back is false (#2779) Signed-off-by: tomoya.kimura --- .../src/scene_module/pull_out/pull_out_module.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 9e96a502561c2..ac8241d691675 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -389,13 +389,13 @@ void PullOutModule::planWithPriorityOnEfficientPath( status_.planner_type = PlannerType::NONE; // check if start pose candidates are valid - if (start_pose_candidates.size() < 2) { + if (start_pose_candidates.empty()) { return; } // plan with each planner for (const auto & planner : pull_out_planners_) { - for (size_t i = 0; i < start_pose_candidates.size() - 1; i++) { + for (size_t i = 0; i < start_pose_candidates.size(); i++) { status_.back_finished = i == 0; const auto & pull_out_start_pose = start_pose_candidates.at(i); planner->setPlannerData(planner_data_); @@ -412,6 +412,9 @@ void PullOutModule::planWithPriorityOnEfficientPath( status_.planner_type = planner->getPlannerType(); break; } + + if (i == start_pose_candidates.size() - 1) continue; + // check next path if back is needed const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose); @@ -439,11 +442,11 @@ void PullOutModule::planWithPriorityOnShortBackDistance( status_.planner_type = PlannerType::NONE; // check if start pose candidates are valid - if (start_pose_candidates.size() < 2) { + if (start_pose_candidates.empty()) { return; } - for (size_t i = 0; i < start_pose_candidates.size() - 1; i++) { + for (size_t i = 0; i < start_pose_candidates.size(); i++) { status_.back_finished = i == 0; const auto & pull_out_start_pose = start_pose_candidates.at(i); // plan with each planner @@ -462,6 +465,9 @@ void PullOutModule::planWithPriorityOnShortBackDistance( status_.planner_type = planner->getPlannerType(); break; } + + if (i == start_pose_candidates.size() - 1) continue; + // check next path if back is needed const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose); From cd546b50c72111a352be18b4b48783dbbbab4584 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 31 Jan 2023 12:23:49 +0900 Subject: [PATCH 08/72] fix(behavior_path_planner): disable avoidance if ego is not on preferred lane (#2781) * fix(behavior_path_planner): disable avoidance in preferred lane Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): use odometry Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../src/scene_module/avoidance/avoidance_module.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 3c8ffe389aca6..93c20fd25b1b1 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -93,6 +93,18 @@ bool AvoidanceModule::isExecutionRequested() const return true; } + // Check ego is in preferred lane + const auto current_lanes = util::getCurrentLanes(planner_data_); + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet( + current_lanes, planner_data_->self_odometry->pose.pose, ¤t_lane); + const auto num = planner_data_->route_handler->getNumLaneToPreferredLane(current_lane); + + if (num != 0) { + return false; + } + + // Check avoidance targets exist const auto avoid_data = calcAvoidancePlanningData(debug_data_); if (parameters_->publish_debug_marker) { From 35670121250a32168aef3b054e2583e1599f0d1d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 31 Jan 2023 12:33:54 +0900 Subject: [PATCH 09/72] chore(freespace_planner): add maintainers (#2767) * chore(freespace_planner): add maintainers Signed-off-by: kosuke55 * remove kenji-miyake from author Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- planning/freespace_planner/package.xml | 4 +++- planning/freespace_planning_algorithms/package.xml | 3 +++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index 614049e1e5222..62e3cbe73fa2b 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -4,10 +4,12 @@ freespace_planner 0.1.0 The freespace_planner package + Kosuke Takeuchi Takamasa Horibe + Takayuki Murooka + Yutaka Shimizu Apache License 2.0 - Kenji Miyake Akihito OHSATO Tomohito ANDO diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index b8740d435897c..ea0813c5b57ea 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -4,7 +4,10 @@ freespace_planning_algorithms 0.1.0 The freespace_planning_algorithms package + Kosuke Takeuchi Takamasa Horibe + Takayuki Murooka + Yutaka Shimizu Apache License 2.0 Akihito Ohsato From 224831b91771867973b890fd9fcca897d69760c6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 31 Jan 2023 12:34:21 +0900 Subject: [PATCH 10/72] chore(tier4_planning_launch): add missing params and sort params of costmap generator (#2764) Signed-off-by: kosuke55 --- .../launch/scenario_planning/parking.launch.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py index b6400b568837a..acf0b7546e27f 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py @@ -63,10 +63,7 @@ def launch_setup(context, *args, **kwargs): "vehicle_frame": "base_link", "map_frame": "map", "update_rate": 10.0, - "use_wayarea": True, - "use_parkinglot": True, - "use_objects": True, - "use_points": True, + "activate_by_scenario": True, "grid_min_value": 0.0, "grid_max_value": 1.0, "grid_resolution": 0.2, @@ -76,6 +73,10 @@ def launch_setup(context, *args, **kwargs): "grid_position_y": 0.0, "maximum_lidar_height_thres": 0.3, "minimum_lidar_height_thres": -2.2, + "use_wayarea": True, + "use_parkinglot": True, + "use_objects": True, + "use_points": True, "expand_polygon_size": 1.0, "size_of_expansion_kernel": 9, }, From 5940ba77d5e60ce0928bbf888f740e773515ac40 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 31 Jan 2023 12:46:25 +0900 Subject: [PATCH 11/72] feat(tier4_state_rviz_plugin): add planning API visualization (#2632) feat(tier4_state_rviz_plugin): add Planning Visualization Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> --- .../src/autoware_state_panel.cpp | 277 ++++++++++++++++++ .../src/autoware_state_panel.hpp | 19 ++ 2 files changed, 296 insertions(+) diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 0b9225bf94238..d90f2769b36d5 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -67,6 +68,8 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa h_layout->addWidget(makeFailSafeGroup()); v_layout->addLayout(h_layout); } + v_layout->addWidget(makeVelocityFactorsGroup()); + v_layout->addWidget(makeSteeringFactorsGroup()); v_layout->addLayout(gear_layout); velocity_limit_layout->addWidget(velocity_limit_button_ptr_); @@ -206,6 +209,51 @@ QGroupBox * AutowareStatePanel::makeFailSafeGroup() return group; } +QGroupBox * AutowareStatePanel::makeVelocityFactorsGroup() +{ + auto * group = new QGroupBox("VelocityFactors"); + auto * grid = new QGridLayout; + + auto vertical_header = new QHeaderView(Qt::Vertical); + vertical_header->hide(); + auto horizontal_header = new QHeaderView(Qt::Horizontal); + horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + + auto header_labels = QStringList({"Type", "Status", "Distance [m]", "Detail"}); + velocity_factors_table_ = new QTableWidget(); + velocity_factors_table_->setColumnCount(header_labels.size()); + velocity_factors_table_->setHorizontalHeaderLabels(header_labels); + velocity_factors_table_->setVerticalHeader(vertical_header); + velocity_factors_table_->setHorizontalHeader(horizontal_header); + grid->addWidget(velocity_factors_table_, 0, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * AutowareStatePanel::makeSteeringFactorsGroup() +{ + auto * group = new QGroupBox("SteeringFactors"); + auto * grid = new QGridLayout; + + auto vertical_header = new QHeaderView(Qt::Vertical); + vertical_header->hide(); + auto horizontal_header = new QHeaderView(Qt::Horizontal); + horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + + auto header_labels = + QStringList({"Type", "Status", "Distance.1 [m]", "Distance.2 [m]", "Direction", "Detail"}); + steering_factors_table_ = new QTableWidget(); + steering_factors_table_->setColumnCount(header_labels.size()); + steering_factors_table_->setHorizontalHeaderLabels(header_labels); + steering_factors_table_->setVerticalHeader(vertical_header); + steering_factors_table_->setHorizontalHeader(horizontal_header); + grid->addWidget(steering_factors_table_, 1, 0); + + group->setLayout(grid); + return group; +} + void AutowareStatePanel::onInitialize() { raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); @@ -259,6 +307,15 @@ void AutowareStatePanel::onInitialize() "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onMRMState, this, _1)); + // Planning + sub_velocity_factors_ = raw_node_->create_subscription( + "/api/planning/velocity_factors", 10, + std::bind(&AutowareStatePanel::onVelocityFactors, this, _1)); + + sub_steering_factors_ = raw_node_->create_subscription( + "/api/planning/steering_factors", 10, + std::bind(&AutowareStatePanel::onSteeringFactors, this, _1)); + // Others sub_gear_ = raw_node_->create_subscription( "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1)); @@ -513,6 +570,226 @@ void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg) } } +void AutowareStatePanel::onVelocityFactors(const VelocityFactorArray::ConstSharedPtr msg) +{ + velocity_factors_table_->clearContents(); + velocity_factors_table_->setRowCount(msg->factors.size()); + + for (std::size_t i = 0; i < msg->factors.size(); i++) { + const auto & e = msg->factors.at(i); + + // type + { + auto label = new QLabel(); + switch (e.type) { + case VelocityFactor::SURROUNDING_OBSTACLE: + label->setText("SURROUNDING_OBSTACLE"); + break; + case VelocityFactor::ROUTE_OBSTACLE: + label->setText("ROUTE_OBSTACLE"); + break; + case VelocityFactor::INTERSECTION: + label->setText("INTERSECTION"); + break; + case VelocityFactor::CROSSWALK: + label->setText("CROSSWALK"); + break; + case VelocityFactor::REAR_CHECK: + label->setText("REAR_CHECK"); + break; + case VelocityFactor::USER_DEFINED_DETECTION_AREA: + label->setText("USER_DEFINED_DETECTION_AREA"); + break; + case VelocityFactor::NO_STOPPING_AREA: + label->setText("NO_STOPPING_AREA"); + break; + case VelocityFactor::STOP_SIGN: + label->setText("STOP_SIGN"); + break; + case VelocityFactor::TRAFFIC_SIGNAL: + label->setText("TRAFFIC_SIGNAL"); + break; + case VelocityFactor::V2I_GATE_CONTROL_ENTER: + label->setText("V2I_GATE_CONTROL_ENTER"); + break; + case VelocityFactor::V2I_GATE_CONTROL_LEAVE: + label->setText("V2I_GATE_CONTROL_LEAVE"); + break; + case VelocityFactor::MERGE: + label->setText("MERGE"); + break; + case VelocityFactor::SIDEWALK: + label->setText("SIDEWALK"); + break; + case VelocityFactor::LANE_CHANGE: + label->setText("LANE_CHANGE"); + break; + case VelocityFactor::AVOIDANCE: + label->setText("AVOIDANCE"); + break; + case VelocityFactor::EMERGENCY_STOP_OPERATION: + label->setText("EMERGENCY_STOP_OPERATION"); + break; + default: + label->setText("UNKNOWN"); + break; + } + label->setAlignment(Qt::AlignCenter); + velocity_factors_table_->setCellWidget(i, 0, label); + } + + // status + { + auto label = new QLabel(); + switch (e.status) { + case VelocityFactor::APPROACHING: + label->setText("APPROACHING"); + break; + case VelocityFactor::STOPPED: + label->setText("STOPPED"); + break; + default: + label->setText("UNKNOWN"); + break; + } + label->setAlignment(Qt::AlignCenter); + velocity_factors_table_->setCellWidget(i, 1, label); + } + + // distance + { + auto label = new QLabel(); + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << e.distance; + label->setText(QString::fromStdString(ss.str())); + label->setAlignment(Qt::AlignCenter); + velocity_factors_table_->setCellWidget(i, 2, label); + } + + // detail + { + auto label = new QLabel(QString::fromStdString(e.detail)); + label->setAlignment(Qt::AlignCenter); + velocity_factors_table_->setCellWidget(i, 3, label); + } + } +} + +void AutowareStatePanel::onSteeringFactors(const SteeringFactorArray::ConstSharedPtr msg) +{ + steering_factors_table_->clearContents(); + steering_factors_table_->setRowCount(msg->factors.size()); + + for (std::size_t i = 0; i < msg->factors.size(); i++) { + const auto & e = msg->factors.at(i); + + // type + { + auto label = new QLabel(); + switch (e.type) { + case SteeringFactor::INTERSECTION: + label->setText("INTERSECTION"); + break; + case SteeringFactor::LANE_CHANGE: + label->setText("LANE_CHANGE"); + break; + case SteeringFactor::AVOIDANCE_PATH_CHANGE: + label->setText("AVOIDANCE_PATH_CHANGE"); + break; + case SteeringFactor::AVOIDANCE_PATH_RETURN: + label->setText("AVOIDANCE_PATH_RETURN"); + break; + case SteeringFactor::STATION: + label->setText("STATION"); + break; + case SteeringFactor::PULL_OUT: + label->setText("PULL_OUT"); + break; + case SteeringFactor::PULL_OVER: + label->setText("PULL_OVER"); + break; + case SteeringFactor::EMERGENCY_OPERATION: + label->setText("EMERGENCY_OPERATION"); + break; + default: + label->setText("UNKNOWN"); + break; + } + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 0, label); + } + + // status + { + auto label = new QLabel(); + switch (e.status) { + case SteeringFactor::APPROACHING: + label->setText("APPROACHING"); + break; + case SteeringFactor::TRYING: + label->setText("TRYING"); + break; + case SteeringFactor::TURNING: + label->setText("TURNING"); + break; + default: + label->setText("UNKNOWN"); + break; + } + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 1, label); + } + + // distance.1 + { + auto label = new QLabel(); + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << e.distance.front(); + label->setText(QString::fromStdString(ss.str())); + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 2, label); + } + + // distance.2 + { + auto label = new QLabel(); + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << e.distance.back(); + label->setText(QString::fromStdString(ss.str())); + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 3, label); + } + + // Direction + { + auto label = new QLabel(); + switch (e.direction) { + case SteeringFactor::LEFT: + label->setText("LEFT"); + break; + case SteeringFactor::RIGHT: + label->setText("RIGHT"); + break; + case SteeringFactor::STRAIGHT: + label->setText("STRAIGHT"); + break; + default: + label->setText("UNKNOWN"); + break; + } + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 4, label); + } + + // detail + { + auto label = new QLabel(QString::fromStdString(e.detail)); + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 5, label); + } + } +} + void AutowareStatePanel::onShift( const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index b3e9b08152b16..52af59c241fe3 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -30,6 +31,8 @@ #include #include #include +#include +#include #include #include #include @@ -53,6 +56,10 @@ class AutowareStatePanel : public rviz_common::Panel using MotionState = autoware_adapi_v1_msgs::msg::MotionState; using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; using MRMState = autoware_adapi_v1_msgs::msg::MrmState; + using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; + using VelocityFactor = autoware_adapi_v1_msgs::msg::VelocityFactor; + using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; + using SteeringFactor = autoware_adapi_v1_msgs::msg::SteeringFactor; Q_OBJECT @@ -80,6 +87,8 @@ public Q_SLOTS: // NOLINT for Qt QGroupBox * makeLocalizationGroup(); QGroupBox * makeMotionGroup(); QGroupBox * makeFailSafeGroup(); + QGroupBox * makeVelocityFactorsGroup(); + QGroupBox * makeSteeringFactorsGroup(); void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); @@ -150,6 +159,16 @@ public Q_SLOTS: // NOLINT for Qt void onMRMState(const MRMState::ConstSharedPtr msg); + // Planning + QTableWidget * velocity_factors_table_{nullptr}; + QTableWidget * steering_factors_table_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_velocity_factors_; + rclcpp::Subscription::SharedPtr sub_steering_factors_; + + void onVelocityFactors(const VelocityFactorArray::ConstSharedPtr msg); + void onSteeringFactors(const SteeringFactorArray::ConstSharedPtr msg); + // Others QPushButton * velocity_limit_button_ptr_; QLabel * gear_label_ptr_; From a662b83467ca2602e1d1d83ea9130511d5c5db2b Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Wed, 1 Feb 2023 19:09:49 +0900 Subject: [PATCH 12/72] chore(rtc_manager_rviz_plugin): add code owner (#2792) Signed-off-by: tomoya.kimura --- common/rtc_manager_rviz_plugin/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/common/rtc_manager_rviz_plugin/package.xml b/common/rtc_manager_rviz_plugin/package.xml index 8af550e05ac69..6987e63c3c6ee 100644 --- a/common/rtc_manager_rviz_plugin/package.xml +++ b/common/rtc_manager_rviz_plugin/package.xml @@ -5,6 +5,8 @@ 0.0.0 The rtc manager rviz plugin package Taiki Tanaka + Tomoya Kimura + Apache License 2.0 ament_cmake_auto From c8983a404ebcfff4dc38c66aad116be0a7a2b5bc Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Wed, 1 Feb 2023 20:08:23 +0900 Subject: [PATCH 13/72] chore: update CODEOWNERS (#2788) Signed-off-by: GitHub Co-authored-by: tkimura4 --- .github/CODEOWNERS | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 091110b5f5148..43af74eb8b9c2 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -24,7 +24,7 @@ common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satosh common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners common/polar_grid/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners +common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners common/signal_processing/** ali.boyali@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners common/tensorrt_common/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -117,8 +117,8 @@ planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4 planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/freespace_planner/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/mission_planner/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp @autowarefoundation/autoware-global-codeowners planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners From f45077587504a46565a84844615b83b905ae09ff Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 1 Feb 2023 20:20:14 +0900 Subject: [PATCH 14/72] fix(behavior_velocity_planner): clean up expired intersection/merge_from_private/traffic_light (#2790) * fix(behavior_velocity_planner): clean up expired intersection Signed-off-by: Mamoru Sobue * clean up expired merge_from_private Signed-off-by: Mamoru Sobue * clean up expired traffic_light Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../scene_module/intersection/manager.hpp | 6 +- .../scene_module/traffic_light/manager.hpp | 4 +- .../src/scene_module/intersection/manager.cpp | 96 +++++++++++-------- .../scene_module/traffic_light/manager.cpp | 27 +++++- 4 files changed, 85 insertions(+), 48 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp index c42a311ffa623..735f807949492 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -43,7 +43,9 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - bool hasSameParentLanelet(const lanelet::ConstLanelet & lane) const; + bool hasSameParentLaneletWith(const lanelet::ConstLanelet & lane, const size_t module_id) const; + + bool hasSameParentLaneletWithRegistered(const lanelet::ConstLanelet & lane) const; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -61,7 +63,7 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - bool hasSameParentLanelet(const lanelet::ConstLanelet & lane) const; + bool hasSameParentLaneletWith(const lanelet::ConstLanelet & lane, const size_t module_id) const; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp index a27d8b8d9e3c7..b93205e5acdb8 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp @@ -43,7 +43,9 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; - bool isModuleRegisteredFromRegElement(const lanelet::Id & id) const; + bool isModuleRegisteredFromRegElement(const lanelet::Id & id, const size_t module_id) const; + + bool isModuleRegisteredFromExistingAssociatedModule(const lanelet::Id & id) const; bool hasSameTrafficLight( const lanelet::TrafficLightConstPtr element, diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 8f4ee1c91cee2..70813af895d82 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -97,7 +97,7 @@ void IntersectionModuleManager::launchNewModules( const auto lane_id = ll.id(); const auto module_id = lane_id; - if (hasSameParentLanelet(ll)) { + if (hasSameParentLaneletWithRegistered(ll)) { continue; } @@ -177,21 +177,20 @@ IntersectionModuleManager::getModuleExpiredFunction( const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return - [this, lane_set]([[maybe_unused]] const std::shared_ptr & scene_module) { - for (const auto & lane : lane_set) { - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - const auto is_intersection = - turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; - if (!is_intersection) { - continue; - } - if (hasSameParentLanelet(lane)) { - return false; - } + return [this, lane_set](const std::shared_ptr & scene_module) { + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; } - return true; - }; + if (hasSameParentLaneletWith(lane, scene_module->getModuleId())) { + return false; + } + } + return true; + }; } std::function &)> @@ -201,24 +200,42 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return - [this, lane_set]([[maybe_unused]] const std::shared_ptr & scene_module) { - for (const auto & lane : lane_set) { - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - const auto is_intersection = - turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; - if (!is_intersection) { - continue; - } - if (hasSameParentLanelet(lane)) { - return false; - } + return [this, lane_set](const std::shared_ptr & scene_module) { + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + if (hasSameParentLaneletWith(lane, scene_module->getModuleId())) { + return false; } + } + return true; + }; +} + +bool IntersectionModuleManager::hasSameParentLaneletWith( + const lanelet::ConstLanelet & lane, const size_t module_id) const +{ + lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); + + const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(module_id); + lanelet::ConstLanelets registered_parents = + planner_data_->route_handler_->getPreviousLanelets(registered_lane); + for (const auto & ll : registered_parents) { + auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); + neighbor_lanes.push_back(ll); + if (hasSameLanelet(parents, neighbor_lanes)) { return true; - }; + } + } + return false; } -bool IntersectionModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet & lane) const +bool IntersectionModuleManager::hasSameParentLaneletWithRegistered( + const lanelet::ConstLanelet & lane) const { lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); @@ -237,20 +254,19 @@ bool IntersectionModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet return false; } -bool MergeFromPrivateModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet & lane) const +bool MergeFromPrivateModuleManager::hasSameParentLaneletWith( + const lanelet::ConstLanelet & lane, const size_t module_id) const { lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); - for (const auto & id : registered_module_id_set_) { - const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id); - lanelet::ConstLanelets registered_parents = - planner_data_->route_handler_->getPreviousLanelets(registered_lane); - for (const auto & ll : registered_parents) { - auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); - neighbor_lanes.push_back(ll); - if (hasSameLanelet(parents, neighbor_lanes)) { - return true; - } + const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(module_id); + lanelet::ConstLanelets registered_parents = + planner_data_->route_handler_->getPreviousLanelets(registered_lane); + for (const auto & ll : registered_parents) { + auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); + neighbor_lanes.push_back(ll); + if (hasSameLanelet(parents, neighbor_lanes)) { + return true; } } return false; diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index 363cb191d3405..45ac7025e89a5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -125,7 +125,7 @@ void TrafficLightModuleManager::launchNewModules( // Use lanelet_id to unregister module when the route is changed const auto lane_id = traffic_light_reg_elem.second.id(); const auto module_id = lane_id; - if (!isModuleRegisteredFromRegElement(module_id)) { + if (!isModuleRegisteredFromExistingAssociatedModule(module_id)) { registerModule(std::make_shared( module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, logger_.get_child("traffic_light_module"), clock_)); @@ -143,10 +143,9 @@ TrafficLightModuleManager::getModuleExpiredFunction( const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [this, lanelet_id_set]( - [[maybe_unused]] const std::shared_ptr & scene_module) { + return [this, lanelet_id_set](const std::shared_ptr & scene_module) { for (const auto & id : lanelet_id_set) { - if (isModuleRegisteredFromRegElement(id)) { + if (isModuleRegisteredFromRegElement(id, scene_module->getModuleId())) { return false; } } @@ -154,7 +153,25 @@ TrafficLightModuleManager::getModuleExpiredFunction( }; } -bool TrafficLightModuleManager::isModuleRegisteredFromRegElement(const lanelet::Id & id) const +bool TrafficLightModuleManager::isModuleRegisteredFromRegElement( + const lanelet::Id & id, const size_t module_id) const +{ + const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id); + + const auto registered_lane = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(module_id); + for (const auto & registered_element : registered_lane.regulatoryElementsAs()) { + for (const auto & element : lane.regulatoryElementsAs()) { + if (hasSameTrafficLight(element, registered_element)) { + return true; + } + } + } + return false; +} + +bool TrafficLightModuleManager::isModuleRegisteredFromExistingAssociatedModule( + const lanelet::Id & id) const { const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id); From d6c0c096b3492930f740163ec8ee5d02beddd919 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Wed, 1 Feb 2023 20:34:44 +0900 Subject: [PATCH 15/72] feat(rtc_manager_rviz_plugin): add the number of rtc status (#2791) * feat(rtc_manager_rviz_plugin): add the number of rtc status Signed-off-by: taikitanaka3 * chore: simplify layout Signed-off-by: taikitanaka3 --------- Signed-off-by: taikitanaka3 Co-authored-by: Tomoya Kimura --- common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp | 5 +++++ common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp | 1 + 2 files changed, 6 insertions(+) diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index 8647d87558cb0..2daa837ffd073 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -159,6 +159,9 @@ RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent) } v_layout->addWidget(auto_mode_table_); + num_rtc_status_ptr_ = new QLabel("Init"); + v_layout->addWidget(num_rtc_status_ptr_); + // lateral execution auto * exe_path_change_layout = new QHBoxLayout; { @@ -327,6 +330,8 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg { cooperate_statuses_ptr_ = std::make_shared(*msg); rtc_table_->clearContents(); + num_rtc_status_ptr_->setText( + QString::fromStdString("The Number of RTC Statuses: " + std::to_string(msg->statuses.size()))); if (msg->statuses.empty()) return; // this is to stable rtc display not to occupy too much size_t min_display_size{5}; diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp index 0a16a84b38ca4..8bdaef94b6254 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp @@ -121,6 +121,7 @@ public Q_SLOTS: QPushButton * wait_vel_change_button_ptr_ = {nullptr}; QPushButton * exec_button_ptr_ = {nullptr}; QPushButton * wait_button_ptr_ = {nullptr}; + QLabel * num_rtc_status_ptr_ = {nullptr}; size_t column_size_ = {7}; std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; From 231f70c1d4920fe3c5bd822bf969a7669d284aad Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 2 Feb 2023 18:00:02 +0900 Subject: [PATCH 16/72] fix(freespace_planning_algorithms): set goal z to path (#2797) Signed-off-by: kosuke55 --- .../astar_search.hpp | 1 + .../src/astar_search.cpp | 24 +++++++++---------- .../src/rrtstar.cpp | 2 +- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp index 9ccec85bfc185..6f1810fabec88 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp @@ -144,6 +144,7 @@ class AstarSearch : public AbstractPlanningAlgorithm bool setGoalNode(); double estimateCost(const geometry_msgs::msg::Pose & pose) const; bool isGoal(const AstarNode & node) const; + geometry_msgs::msg::Pose node2pose(const AstarNode & node) const; AstarNode * getNodeRef(const IndexXYT & index) { diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index 8d419c3daa13f..5686d7eee26e0 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -61,18 +61,6 @@ geometry_msgs::msg::Pose calcRelativePose( return transformed.pose; } -geometry_msgs::msg::Pose node2pose(const AstarNode & node) -{ - geometry_msgs::msg::Pose pose_local; - - pose_local.position.x = node.x; - pose_local.position.y = node.y; - pose_local.position.z = 0; - pose_local.orientation = tier4_autoware_utils::createQuaternionFromYaw(node.theta); - - return pose_local; -} - AstarSearch::TransitionTable createTransitionTable( const double minimum_turning_radius, const double maximum_turning_radius, const int turning_radius_size, const double theta_size, const bool use_back) @@ -356,4 +344,16 @@ bool AstarSearch::isGoal(const AstarNode & node) const return true; } +geometry_msgs::msg::Pose AstarSearch::node2pose(const AstarNode & node) const +{ + geometry_msgs::msg::Pose pose_local; + + pose_local.position.x = node.x; + pose_local.position.y = node.y; + pose_local.position.z = goal_pose_.position.z; + pose_local.orientation = tier4_autoware_utils::createQuaternionFromYaw(node.theta); + + return pose_local; +} + } // namespace freespace_planning_algorithms diff --git a/planning/freespace_planning_algorithms/src/rrtstar.cpp b/planning/freespace_planning_algorithms/src/rrtstar.cpp index d710b74111b1a..f03dc81faba05 100644 --- a/planning/freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/freespace_planning_algorithms/src/rrtstar.cpp @@ -141,7 +141,7 @@ void RRTStar::setRRTPath(const std::vector & waypoints) geometry_msgs::msg::Pose pose_local; pose_local.position.x = pt.x; pose_local.position.y = pt.y; - pose_local.position.z = 0.0; + pose_local.position.z = goal_pose_.position.z; tf2::Quaternion quat; quat.setRPY(0, 0, pt.yaw); tf2::convert(quat, pose_local.orientation); From a3de48643f5b76a1ea64771b94c1eeedd3df5837 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 3 Feb 2023 00:17:08 +0900 Subject: [PATCH 17/72] feat(interpolation): add option to enable akima spline for xy (#2802) Signed-off-by: Takayuki Murooka --- .../motion_utils/resample/resample.hpp | 40 ++++++++-------- common/motion_utils/src/resample/resample.cpp | 48 ++++++++++--------- 2 files changed, 46 insertions(+), 42 deletions(-) diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index 6a987dc269e76..cfb945270ed7f 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -45,7 +45,7 @@ namespace motion_utils * @param input_path input path(point) to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -53,7 +53,7 @@ namespace motion_utils */ std::vector resamplePointVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** @@ -63,7 +63,7 @@ std::vector resamplePointVector( * based on the interpolated position x and y. * @param input_path input path(position) to resample * @param resample_interval resampling interval - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -71,7 +71,7 @@ std::vector resamplePointVector( */ std::vector resamplePointVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true); + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** * @brief A resampling function for a path(poses). Note that in a default setting, position xy are @@ -81,7 +81,7 @@ std::vector resamplePointVector( * @param input_path input path(poses) to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -89,7 +89,7 @@ std::vector resamplePointVector( */ std::vector resamplePoseVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** @@ -99,7 +99,7 @@ std::vector resamplePoseVector( * based on the interpolated position x and y. * @param input_path input path(poses) to resample * @param resample_interval resampling interval - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -107,7 +107,7 @@ std::vector resamplePoseVector( */ std::vector resamplePoseVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true); + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** * @brief A resampling function for a path with lane id. Note that in a default setting, position xy @@ -119,7 +119,7 @@ std::vector resamplePoseVector( * @param input_path input path to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -129,7 +129,7 @@ std::vector resamplePoseVector( */ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); /** @@ -141,7 +141,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * and is_final are also interpolated by zero order hold * @param input_path input path to resample * @param resampled_interval resampling interval point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -152,7 +152,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( */ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_lerp_for_xy = false, + const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); @@ -165,7 +165,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * @param input_path input path to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -175,7 +175,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( */ autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); /** @@ -186,7 +186,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * forward difference method based on the interpolated position x and y. * @param input_path input path to resample * @param resampled_interval resampling interval point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -197,7 +197,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( */ autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, - const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); /** @@ -210,7 +210,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * @param input_trajectory input trajectory to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -220,7 +220,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( */ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true); /** @@ -233,7 +233,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( * method based on the interpolated position x and y. * @param input_trajectory input trajectory to resample * @param resampled_interval resampling interval - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -245,7 +245,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( */ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_lerp_for_xy = false, + const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, const bool resample_input_trajectory_stop_point = true); } // namespace motion_utils diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 5417da86f5359..14f44b7892645 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -21,7 +21,7 @@ namespace motion_utils { std::vector resamplePointVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { // validate arguments @@ -60,9 +60,12 @@ std::vector resamplePointVector( const auto spline = [&](const auto & input) { return interpolation::spline(input_arclength, input, resampled_arclength); }; + const auto spline_by_akima = [&](const auto & input) { + return interpolation::splineByAkima(input_arclength, input, resampled_arclength); + }; - const auto interpolated_x = use_lerp_for_xy ? lerp(x) : spline(x); - const auto interpolated_y = use_lerp_for_xy ? lerp(y) : spline(y); + const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x); + const auto interpolated_y = use_akima_spline_for_xy ? lerp(y) : spline_by_akima(y); const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); std::vector resampled_points; @@ -82,7 +85,7 @@ std::vector resamplePointVector( std::vector resamplePointVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy, const bool use_lerp_for_z) + const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { const double input_length = motion_utils::calcArcLength(points); @@ -102,12 +105,12 @@ std::vector resamplePointVector( resampling_arclength.push_back(input_length); } - return resamplePointVector(points, resampling_arclength, use_lerp_for_xy, use_lerp_for_z); + return resamplePointVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } std::vector resamplePoseVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { // validate arguments @@ -120,7 +123,7 @@ std::vector resamplePoseVector( position.at(i) = points.at(i).position; } const auto resampled_position = - resamplePointVector(position, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePointVector(position, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); std::vector resampled_points(resampled_position.size()); @@ -148,7 +151,7 @@ std::vector resamplePoseVector( std::vector resamplePoseVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy, const bool use_lerp_for_z) + const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { const double input_length = motion_utils::calcArcLength(points); @@ -168,12 +171,12 @@ std::vector resamplePoseVector( resampling_arclength.push_back(input_length); } - return resamplePoseVector(points, resampling_arclength, use_lerp_for_xy, use_lerp_for_z); + return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { // validate arguments @@ -245,7 +248,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( }; const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); const auto interpolated_heading_rate = lerp(heading_rate); @@ -279,7 +282,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z, + const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) { // validate arguments @@ -339,12 +342,13 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( } return resamplePath( - input_path, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, use_zero_order_hold_for_v); + input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, + use_zero_order_hold_for_v); } autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { // validate arguments @@ -390,7 +394,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( }; const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); const auto interpolated_heading_rate = lerp(heading_rate); @@ -420,8 +424,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, - const bool use_lerp_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, - const bool resample_input_path_stop_point) + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, + const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point) { // validate arguments if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { @@ -473,13 +477,13 @@ autoware_auto_planning_msgs::msg::Path resamplePath( } return resamplePath( - input_path, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, + input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, use_zero_order_hold_for_twist); } autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) { // validate arguments @@ -542,7 +546,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( }; const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); const auto interpolated_v_lon = use_zero_order_hold_for_twist ? zoh(v_lon) : lerp(v_lon); const auto interpolated_v_lat = use_zero_order_hold_for_twist ? zoh(v_lat) : lerp(v_lat); const auto interpolated_heading_rate = lerp(heading_rate); @@ -579,7 +583,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z, + const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) { // validate arguments @@ -632,7 +636,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( } return resampleTrajectory( - input_trajectory, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, + input_trajectory, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, use_zero_order_hold_for_twist); } From 515e1b258b6fb203928d404846d833416fa00b79 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 3 Feb 2023 10:33:45 +0900 Subject: [PATCH 18/72] feat(interpolation): add curvature calculation (#2801) * feat(interpolation): add curvature calculation Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * add test Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../interpolation/spline_interpolation.hpp | 20 +- .../spline_interpolation_points_2d.hpp | 22 +- .../src/spline_interpolation.cpp | 27 ++- .../src/spline_interpolation_points_2d.cpp | 86 +++++-- .../test/src/test_spline_interpolation.cpp | 188 +-------------- .../test_spline_interpolation_points_2d.cpp | 222 ++++++++++++++++++ .../src/mpt_optimizer.cpp | 4 +- 7 files changed, 353 insertions(+), 216 deletions(-) create mode 100644 common/interpolation/test/src/test_spline_interpolation_points_2d.cpp diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp index 9640bcd6c5120..f5befefeae9da 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -70,9 +70,11 @@ class SplineInterpolation { public: SplineInterpolation() = default; - - void calcSplineCoefficients( - const std::vector & base_keys, const std::vector & base_values); + SplineInterpolation( + const std::vector & base_keys, const std::vector & base_values) + { + calcSplineCoefficients(base_keys, base_values); + } //!< @brief get values of spline interpolation on designated sampling points. //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x, @@ -86,9 +88,21 @@ class SplineInterpolation // return value will be dx/dt(t) vector std::vector getSplineInterpolatedDiffValues(const std::vector & query_keys) const; + //!< @brief get 2nd differential values of spline interpolation on designated sampling points. + //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x, + // meaning that spline interpolation was applied to x(t), + // return value will be d^2/dt^2(t) vector + std::vector getSplineInterpolatedQuadDiffValues( + const std::vector & query_keys) const; + + size_t getSize() const { return base_keys_.size(); } + private: std::vector base_keys_; interpolation::MultiSplineCoef multi_spline_coef_; + + void calcSplineCoefficients( + const std::vector & base_keys, const std::vector & base_values); }; #endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index d44d16d88dd04..567a1873aaa5c 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -37,20 +37,19 @@ std::vector splineYawFromPoints(const std::vector & points); // SplineInterpolationPoints2d spline; // // memorize pre-interpolation result internally // spline.calcSplineCoefficients(base_keys, base_values); -// const auto interpolation_result1 = spline.getSplineInterpolatedPoints( +// const auto interpolation_result1 = spline.getSplineInterpolatedPoint( // base_keys, query_keys1); -// const auto interpolation_result2 = spline.getSplineInterpolatedPoints( +// const auto interpolation_result2 = spline.getSplineInterpolatedPoint( // base_keys, query_keys2); -// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaws( +// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaw( // base_keys, query_keys1); // ``` class SplineInterpolationPoints2d { public: SplineInterpolationPoints2d() = default; - template - void calcSplineCoefficients(const std::vector & points) + explicit SplineInterpolationPoints2d(const std::vector & points) { std::vector points_inner; for (const auto & p : points) { @@ -63,9 +62,22 @@ class SplineInterpolationPoints2d // std::vector getSplineInterpolatedPoints(const double width); // std::vector getSplineInterpolatedPoses(const double width); + // pose (= getSplineInterpolatedPoint + getSplineInterpolatedYaw) + geometry_msgs::msg::Pose getSplineInterpolatedPose(const size_t idx, const double s) const; + + // point geometry_msgs::msg::Point getSplineInterpolatedPoint(const size_t idx, const double s) const; + + // yaw double getSplineInterpolatedYaw(const size_t idx, const double s) const; + std::vector getSplineInterpolatedYaws() const; + + // curvature + double getSplineInterpolatedCurvature(const size_t idx, const double s) const; + std::vector getSplineInterpolatedCurvatures() const; + size_t getSize() const { return base_s_vec_.size(); } + size_t getOffsetIndex(const size_t idx, const double offset) const; double getAccumulatedLength(const size_t idx) const; private: diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp index bd92af1007b50..1275b47346d78 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/interpolation/src/spline_interpolation.cpp @@ -85,10 +85,8 @@ std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) { - SplineInterpolation interpolator; - // calculate spline coefficients - interpolator.calcSplineCoefficients(base_keys, base_values); + SplineInterpolation interpolator(base_keys, base_values); // interpolate base_keys at query_keys return interpolator.getSplineInterpolatedValues(query_keys); @@ -266,3 +264,26 @@ std::vector SplineInterpolation::getSplineInterpolatedDiffValues( return res; } + +std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( + const std::vector & query_keys) const +{ + // throw exceptions for invalid arguments + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + + const auto & a = multi_spline_coef_.a; + const auto & b = multi_spline_coef_.b; + + std::vector res; + size_t j = 0; + for (const auto & query_key : validated_query_keys) { + while (base_keys_.at(j + 1) < query_key) { + ++j; + } + + const double ds = query_key - base_keys_.at(j); + res.push_back(2.0 * b.at(j) + 6.0 * a.at(j) * ds); + } + + return res; +} diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 71e2629044f11..82c3841c424c5 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -73,14 +73,14 @@ std::array, 3> slerp2dFromXY( const std::vector & base_keys, const std::vector & base_x_values, const std::vector & base_y_values, const std::vector & query_keys) { - SplineInterpolation interpolator_x, interpolator_y; - std::vector yaw_vec; - // calculate spline coefficients - interpolator_x.calcSplineCoefficients(base_keys, base_x_values); - interpolator_y.calcSplineCoefficients(base_keys, base_y_values); + SplineInterpolation interpolator_x(base_keys, base_x_values); + SplineInterpolation interpolator_y(base_keys, base_y_values); const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys); const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys); + + // calculate yaw + std::vector yaw_vec; for (size_t i = 0; i < diff_x.size(); i++) { double yaw = std::atan2(diff_y[i], diff_x[i]); yaw_vec.push_back(yaw); @@ -94,10 +94,8 @@ std::array, 3> slerp2dFromXY( template std::vector splineYawFromPoints(const std::vector & points) { - SplineInterpolationPoints2d interpolator; - // calculate spline coefficients - interpolator.calcSplineCoefficients(points); + SplineInterpolationPoints2d interpolator(points); // interpolate base_keys at query_keys std::vector yaw_vec; @@ -112,6 +110,16 @@ template std::vector splineYawFromPoints( } // namespace interpolation +geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( + const size_t idx, const double s) const +{ + geometry_msgs::msg::Pose pose; + pose.position = getSplineInterpolatedPoint(idx, s); + pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); + return pose; +} + geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint( const size_t idx, const double s) const { @@ -142,18 +150,64 @@ double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, c throw std::out_of_range("idx is out of range."); } - double whole_s = base_s_vec_.at(idx) + s; - if (whole_s < base_s_vec_.front()) { - whole_s = base_s_vec_.front(); + const double whole_s = + std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back()); + + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); + + return std::atan2(diff_y, diff_x); +} + +std::vector SplineInterpolationPoints2d::getSplineInterpolatedYaws() const +{ + std::vector yaw_vec; + for (size_t i = 0; i < spline_x_.getSize(); ++i) { + const double yaw = getSplineInterpolatedYaw(i, 0.0); + yaw_vec.push_back(yaw); } - if (whole_s > base_s_vec_.back()) { - whole_s = base_s_vec_.back(); + return yaw_vec; +} + +double SplineInterpolationPoints2d::getSplineInterpolatedCurvature( + const size_t idx, const double s) const +{ + if (base_s_vec_.size() <= idx) { + throw std::out_of_range("idx is out of range."); } + const double whole_s = + std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back()); + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); - return std::atan2(diff_y, diff_x); + const double quad_diff_x = spline_x_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0); + const double quad_diff_y = spline_y_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0); + + return (diff_x * quad_diff_y - quad_diff_x * diff_y) / + std::pow(std::pow(diff_x, 2) + std::pow(diff_y, 2), 1.5); +} + +std::vector SplineInterpolationPoints2d::getSplineInterpolatedCurvatures() const +{ + std::vector curvature_vec; + for (size_t i = 0; i < spline_x_.getSize(); ++i) { + const double curvature = getSplineInterpolatedCurvature(i, 0.0); + curvature_vec.push_back(curvature); + } + return curvature_vec; +} + +size_t SplineInterpolationPoints2d::getOffsetIndex(const size_t idx, const double offset) const +{ + const double whole_s = base_s_vec_.at(idx) + offset; + for (size_t s_idx = 0; s_idx < base_s_vec_.size(); ++s_idx) { + if (whole_s < base_s_vec_.at(s_idx)) { + return s_idx; + } + } + return base_s_vec_.size() - 1; } double SplineInterpolationPoints2d::getAccumulatedLength(const size_t idx) const @@ -174,6 +228,6 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( const auto & base_y_vec = base.at(2); // calculate spline coefficients - spline_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); - spline_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); + spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec); + spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec); } diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index c2bb8eb177ba1..c95fc902ade44 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -221,203 +220,18 @@ TEST(spline_interpolation, splineByAkima) } } -TEST(spline_interpolation, splineYawFromPoints) -{ - using tier4_autoware_utils::createPoint; - - { // straight - std::vector points; - points.push_back(createPoint(0.0, 0.0, 0.0)); - points.push_back(createPoint(1.0, 1.5, 0.0)); - points.push_back(createPoint(2.0, 3.0, 0.0)); - points.push_back(createPoint(3.0, 4.5, 0.0)); - points.push_back(createPoint(4.0, 6.0, 0.0)); - - const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } - - { // curve - std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - points.push_back(createPoint(5.0, 10.0, 0.0)); - points.push_back(createPoint(10.0, 12.5, 0.0)); - - const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } - - { // size of base_keys is 1 (infeasible to interpolate) - std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - - EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); - } - - { // straight: size of base_keys is 2 (edge case in the implementation) - std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - - const std::vector ans{0.9827937, 0.9827937}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } - - { // straight: size of base_keys is 3 (edge case in the implementation) - std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - - const std::vector ans{0.9827937, 0.9827937, 0.9827937}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } -} - TEST(spline_interpolation, SplineInterpolation) { - SplineInterpolation s; - // curve: query_keys is random const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.075611, 0.997242, 1.573258}; - s.calcSplineCoefficients(base_keys, base_values); + SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedValues(query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } } - -TEST(spline_interpolation, SplineInterpolationPoints2d) -{ - using tier4_autoware_utils::createPoint; - - SplineInterpolationPoints2d s; - - // curve - std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - points.push_back(createPoint(5.0, 10.0, 0.0)); - points.push_back(createPoint(10.0, 12.5, 0.0)); - - s.calcSplineCoefficients(points); - - { // point - // front - const auto front_point = s.getSplineInterpolatedPoint(0, 0.0); - EXPECT_NEAR(front_point.x, -2.0, epsilon); - EXPECT_NEAR(front_point.y, -10.0, epsilon); - - // back - const auto back_point = s.getSplineInterpolatedPoint(4, 0.0); - EXPECT_NEAR(back_point.x, 10.0, epsilon); - EXPECT_NEAR(back_point.y, 12.5, epsilon); - - // random - const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); - EXPECT_NEAR(random_point.x, 5.3036484, epsilon); - EXPECT_NEAR(random_point.y, 10.3343074, epsilon); - - // out of range of total length - const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1); - EXPECT_NEAR(front_out_point.x, -2.0, epsilon); - EXPECT_NEAR(front_out_point.y, -10.0, epsilon); - - const auto back_out_point = s.getSplineInterpolatedPoint(4.0, 0.1); - EXPECT_NEAR(back_out_point.x, 10.0, epsilon); - EXPECT_NEAR(back_out_point.y, 12.5, epsilon); - - // out of range of index - EXPECT_THROW(s.getSplineInterpolatedPoint(-1, 0.0), std::out_of_range); - EXPECT_THROW(s.getSplineInterpolatedPoint(5, 0.0), std::out_of_range); - } - - { // yaw - // front - EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); - - // back - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); - - // random - EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.7757198, epsilon); - - // out of range of total length - EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.3593746, epsilon); - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.2932783, epsilon); - - // out of range of index - EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range); - EXPECT_THROW(s.getSplineInterpolatedYaw(5, 0.0), std::out_of_range); - } - - { // accumulated distance - // front - EXPECT_NEAR(s.getAccumulatedLength(0), 0.0, epsilon); - - // back - EXPECT_NEAR(s.getAccumulatedLength(4), 26.8488511, epsilon); - - // random - EXPECT_NEAR(s.getAccumulatedLength(3), 21.2586811, epsilon); - - // out of range of index - EXPECT_THROW(s.getAccumulatedLength(-1), std::out_of_range); - EXPECT_THROW(s.getAccumulatedLength(5), std::out_of_range); - } - - // size of base_keys is 1 (infeasible to interpolate) - std::vector single_points; - single_points.push_back(createPoint(1.0, 0.0, 0.0)); - EXPECT_THROW(s.calcSplineCoefficients(single_points), std::logic_error); -} - -TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) -{ - using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using tier4_autoware_utils::createPoint; - - std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - - std::vector trajectory_points; - for (const auto & p : points) { - TrajectoryPoint tp; - tp.pose.position = p; - trajectory_points.push_back(tp); - } - - SplineInterpolationPoints2d s_point; - s_point.calcSplineCoefficients(points); - s_point.getSplineInterpolatedPoint(0, 0.); - - SplineInterpolationPoints2d s_traj_point; - s_traj_point.calcSplineCoefficients(trajectory_points); - s_traj_point.getSplineInterpolatedPoint(0, 0.); -} diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp new file mode 100644 index 0000000000000..a7d7012c19e22 --- /dev/null +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -0,0 +1,222 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +TEST(spline_interpolation, splineYawFromPoints) +{ + using tier4_autoware_utils::createPoint; + + { // straight + std::vector points; + points.push_back(createPoint(0.0, 0.0, 0.0)); + points.push_back(createPoint(1.0, 1.5, 0.0)); + points.push_back(createPoint(2.0, 3.0, 0.0)); + points.push_back(createPoint(3.0, 4.5, 0.0)); + points.push_back(createPoint(4.0, 6.0, 0.0)); + + const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // curve + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(createPoint(5.0, 10.0, 0.0)); + points.push_back(createPoint(10.0, 12.5, 0.0)); + + const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // size of base_keys is 1 (infeasible to interpolate) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + + EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); + } + + { // straight: size of base_keys is 2 (edge case in the implementation) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + + const std::vector ans{0.9827937, 0.9827937}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // straight: size of base_keys is 3 (edge case in the implementation) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + + const std::vector ans{0.9827937, 0.9827937, 0.9827937}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } +} + +TEST(spline_interpolation, SplineInterpolationPoints2d) +{ + using tier4_autoware_utils::createPoint; + + // curve + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(createPoint(5.0, 10.0, 0.0)); + points.push_back(createPoint(10.0, 12.5, 0.0)); + + SplineInterpolationPoints2d s(points); + + { // point + // front + const auto front_point = s.getSplineInterpolatedPoint(0, 0.0); + EXPECT_NEAR(front_point.x, -2.0, epsilon); + EXPECT_NEAR(front_point.y, -10.0, epsilon); + + // back + const auto back_point = s.getSplineInterpolatedPoint(4, 0.0); + EXPECT_NEAR(back_point.x, 10.0, epsilon); + EXPECT_NEAR(back_point.y, 12.5, epsilon); + + // random + const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); + EXPECT_NEAR(random_point.x, 5.3036484, epsilon); + EXPECT_NEAR(random_point.y, 10.3343074, epsilon); + + // out of range of total length + const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1); + EXPECT_NEAR(front_out_point.x, -2.0, epsilon); + EXPECT_NEAR(front_out_point.y, -10.0, epsilon); + + const auto back_out_point = s.getSplineInterpolatedPoint(4.0, 0.1); + EXPECT_NEAR(back_out_point.x, 10.0, epsilon); + EXPECT_NEAR(back_out_point.y, 12.5, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedPoint(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedPoint(5, 0.0), std::out_of_range); + } + + { // yaw + // front + EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); + + // back + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); + + // random + EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.7757198, epsilon); + + // out of range of total length + EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.3593746, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.2932783, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedYaw(5, 0.0), std::out_of_range); + } + + { // curvature + // front + EXPECT_NEAR(s.getSplineInterpolatedCurvature(0, 0.0), 0.0, epsilon); + + // back + EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.0), 0.0, epsilon); + + // random + EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.2441671, epsilon); + + // out of range of total length + EXPECT_NEAR(s.getSplineInterpolatedCurvature(0.0, -0.1), 0.0, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.1), 0.0, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedCurvature(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedCurvature(5, 0.0), std::out_of_range); + } + + { // accumulated distance + // front + EXPECT_NEAR(s.getAccumulatedLength(0), 0.0, epsilon); + + // back + EXPECT_NEAR(s.getAccumulatedLength(4), 26.8488511, epsilon); + + // random + EXPECT_NEAR(s.getAccumulatedLength(3), 21.2586811, epsilon); + + // out of range of index + EXPECT_THROW(s.getAccumulatedLength(-1), std::out_of_range); + EXPECT_THROW(s.getAccumulatedLength(5), std::out_of_range); + } + + // size of base_keys is 1 (infeasible to interpolate) + std::vector single_points; + single_points.push_back(createPoint(1.0, 0.0, 0.0)); + EXPECT_THROW(SplineInterpolationPoints2d{single_points}, std::logic_error); +} + +TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using tier4_autoware_utils::createPoint; + + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + + std::vector trajectory_points; + for (const auto & p : points) { + TrajectoryPoint tp; + tp.pose.position = p; + trajectory_points.push_back(tp); + } + + SplineInterpolationPoints2d s_point(points); + s_point.getSplineInterpolatedPoint(0, 0.); + + SplineInterpolationPoints2d s_traj_point(trajectory_points); + s_traj_point.getSplineInterpolatedPoint(0, 0.); +} diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index ceaaeeb865030..75466d4362b13 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -1472,8 +1472,8 @@ void MPTOptimizer::calcVehicleBounds( return; } - SplineInterpolationPoints2d ref_points_spline_interpolation; - ref_points_spline_interpolation.calcSplineCoefficients(points_utils::convertToPoints(ref_points)); + SplineInterpolationPoints2d ref_points_spline_interpolation( + points_utils::convertToPoints(ref_points)); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); From 66753e807fa321c35840f0e689ac3f83fb4d110b Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 3 Feb 2023 10:54:43 +0900 Subject: [PATCH 19/72] fix(obstacle_avoidance_planner): check if bounds_candidates is empty for debug marker (#2785) Signed-off-by: kosuke55 --- planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp index 2f1ba9f3a58ef..30a74c0392a0a 100644 --- a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp @@ -445,7 +445,7 @@ visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray( visualization_msgs::msg::MarkerArray msg; const std::string ns = "bounds_candidates"; - if (ref_points.empty()) return msg; + if (ref_points.empty() || bounds_candidates.empty()) return msg; auto marker = createDefaultMarker( "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, From ded1d94524e673d871f60b7fd4268d7a417ce304 Mon Sep 17 00:00:00 2001 From: Hayato Mizushima Date: Fri, 3 Feb 2023 13:03:20 +0900 Subject: [PATCH 20/72] docs: add mkdocs material setting (#2784) https://squidfunk.github.io/mkdocs-material/setup/setting-up-the-footer/ Signed-off-by: Hayato Mizushima --- mkdocs.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/mkdocs.yaml b/mkdocs.yaml index cc2b7513e7470..d448ab04c9a41 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -15,6 +15,7 @@ theme: - navigation.tabs - navigation.tabs.sticky - navigation.top + - navigation.footer favicon: docs/assets/images/autoware-foundation.png icon: logo: fontawesome/solid/car From b026fc3281ab30c796a36896366a00103322d69b Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Fri, 3 Feb 2023 17:31:15 +0900 Subject: [PATCH 21/72] fix(accel_brake_map_calibrator): set method to none as default in view plot py (#2808) Signed-off-by: tomoya.kimura --- .../accel_brake_map_calibrator/scripts/view_plot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py index 3fc263d990cbb..1ec5f6a328361 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py @@ -281,7 +281,7 @@ def main(args=None): parser.add_argument( "-m", "--method", - default="None", + default=None, type=str, help="calibration method : each_cell or four_cell", ) From f7c7e0df608d28a81ff6d4095eec22563e5bc319 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Fri, 3 Feb 2023 18:14:54 +0900 Subject: [PATCH 22/72] fix(behavior_path_planner): do not access empty path in pull out module (#2805) fix: do not access empty path in pull out module Signed-off-by: tomoya.kimura --- .../src/scene_module/pull_out/shift_pull_out.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index d5fb98aa8e980..58bf256f5f202 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -183,19 +183,25 @@ std::vector ShiftPullOut::calcPullOutPaths( } // get shift end pose - const auto shift_end_pose = std::invoke([&]() { + const auto shift_end_pose_ptr = std::invoke([&]() { const auto arc_position_shift_start = lanelet::utils::getArcCoordinates(road_lanes, start_pose); const double s_start = arc_position_shift_start.length + before_shifted_pull_out_distance; const double s_end = s_start + std::numeric_limits::epsilon(); const auto path = route_handler.getCenterLinePath(road_lanes, s_start, s_end, true); - return path.points.front().point.pose; + return path.points.empty() + ? nullptr + : std::make_shared(path.points.front().point.pose); }); + if (!shift_end_pose_ptr) { + continue; + } + // create shift line ShiftLine shift_line{}; shift_line.start = start_pose; - shift_line.end = shift_end_pose; + shift_line.end = *shift_end_pose_ptr; shift_line.end_shift_length = shift_length; path_shifter.addShiftLine(shift_line); @@ -208,7 +214,7 @@ std::vector ShiftPullOut::calcPullOutPaths( // set velocity const size_t pull_out_end_idx = - findNearestIndex(shifted_path.path.points, shift_end_pose.position); + findNearestIndex(shifted_path.path.points, shift_end_pose_ptr->position); for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { auto & point = shifted_path.path.points.at(i); if (i < pull_out_end_idx) { From 3bc370cde4cc41532a12aba75f474924c644db82 Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Fri, 3 Feb 2023 17:33:16 +0800 Subject: [PATCH 23/72] feat(autoware_auto_perception_rviz_plugin): improve rviz performance (#2780) * add choose box element to control type of visualization polygon for detected objects Signed-off-by: Alexey Panferov * WIP: add switching logic and defininition for 2d poligons functions, working Signed-off-by: Alexey Panferov * add function for making 2d polygon from cylinder shape Signed-off-by: Alexey Panferov * add code to make 2d polygones from unknown objects Signed-off-by: Alexey Panferov * wip, add creation of milty dummy objects by one click Signed-off-by: Alexey Panferov * modificate empty_objects_publisher for testing purpose Signed-off-by: Alexey Panferov * feat/2d_obj_rviz: cleaning for pull request. Remove dummy_empty_obj.launch and revert testing modification. Signed-off-by: Alexey Panferov * feat/2d-obj-rviz refactor, update names of functions to more meaningfull, remove unnecessary code which was drawing additional lines on circle, remove TODO Signed-off-by: Alexey Panferov * style(pre-commit): autofix * feat/2d-obj-rviz correct spelling mistakes Signed-off-by: Alexey Panferov --------- Signed-off-by: Alexey Panferov Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../object_polygon_detail.hpp | 18 +++ .../object_polygon_display_base.hpp | 28 ++++- .../object_polygon_detail.cpp | 117 ++++++++++++++++++ 3 files changed, 157 insertions(+), 6 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index 404ef468a50b3..47c704c97709b 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -84,6 +84,12 @@ get_shape_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_2d_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param centroid Centroid position of the shape in Object.header.frame_id frame /// \return Marker ptr. Id and header will have to be set by the caller @@ -131,10 +137,18 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( const geometry_msgs::msg::Point center, const double radius, std::vector & points, const int n); @@ -143,6 +157,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( const autoware_auto_perception_msgs::msg::PredictedPath & paths, std::vector & points); diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 2a5c7e53f484c..76cdee1839b01 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -14,6 +14,8 @@ #ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ #define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#include "rviz_common/properties/enum_property.hpp" + #include #include #include @@ -59,8 +61,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase explicit ObjectPolygonDisplayBase(const std::string & default_topic) : m_marker_common(this), - m_display_3d_property{ - "Display 3d polygon", true, "Enable/disable height visualization of the polygon", this}, + // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this}, m_display_label_property{"Display Label", true, "Enable/disable label visualization", this}, m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this}, m_display_pose_with_covariance_property{ @@ -79,6 +80,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this}, m_default_topic{default_topic} { + m_display_type_property = new rviz_common::properties::EnumProperty( + "Polygon Type", "3d", "Type of the polygon to display object.", this, SLOT(updatePalette())); + // Option values here must correspond to indices in palette_textures_ array in onInitialize() + // below. + m_display_type_property->addOption("3d", 0); + m_display_type_property->addOption("2d", 1); + m_display_type_property->addOption("Disable", 2); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { const auto & class_property_values = map_property_it.second; @@ -150,14 +158,22 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase const ClassificationContainerT & labels, const double & line_width) const { const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); - - if (m_display_3d_property.getBool()) { + if (m_display_type_property->getOptionInt() == 0) { return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba, line_width); + } else if (m_display_type_property->getOptionInt() == 1) { + return detail::get_2d_shape_marker_ptr( + shape_msg, centroid, orientation, color_rgba, line_width); } else { return std::nullopt; } } + template + visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + /// \brief Convert given shape msg into a Marker to visualize label name /// \tparam ClassificationContainerT List type with ObjectClassificationMsg /// \param centroid Centroid position of the shape in Object.header.frame_id frame @@ -366,8 +382,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::list m_class_group_properties; // Map to store class labels and its corresponding properties PolygonPropertyMap m_polygon_properties; - // Property to enable/disable height visualization of the polygon - rviz_common::properties::BoolProperty m_display_3d_property; + // Property to choose type of visualization polygon + rviz_common::properties::EnumProperty * m_display_type_property; // Property to enable/disable label visualization rviz_common::properties::BoolProperty m_display_label_property; // Property to enable/disable uuid visualization diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 2ef88880987be..b5d891f186338 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -276,6 +276,38 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->ns = std::string("shape"); + + using autoware_auto_perception_msgs::msg::Shape; + if (shape_msg.type == Shape::BOUNDING_BOX) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); + } else if (shape_msg.type == Shape::CYLINDER) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_cylinder_bottom_line_list(shape_msg, marker_ptr->points); + } else if (shape_msg.type == Shape::POLYGON) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_polygon_bottom_line_list(shape_msg, marker_ptr->points); + } else { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_polygon_bottom_line_list(shape_msg, marker_ptr->points); + } + + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = to_pose(centroid, orientation); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->scale.x = line_width; + marker_ptr->color = color_rgba; + + return marker_ptr; +} + void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -392,6 +424,49 @@ void calc_bounding_box_line_list( points.push_back(point); } +void calc_2d_bounding_box_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + geometry_msgs::msg::Point point; + // down surface + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); +} + void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -435,6 +510,21 @@ void calc_cylinder_line_list( } } +void calc_2d_cylinder_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double radius = shape.dimensions.x * 0.5; + { + constexpr int n = 20; + geometry_msgs::msg::Point center; + center.x = 0.0; + center.y = 0.0; + center.z = -shape.dimensions.z * 0.5; + calc_circle_line_list(center, radius, points, n); + } +} + void calc_circle_line_list( const geometry_msgs::msg::Point center, const double radius, std::vector & points, const int n) @@ -521,6 +611,33 @@ void calc_polygon_line_list( } } +void calc_2d_polygon_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + if (shape.footprint.points.size() < 2) { + RCLCPP_WARN( + rclcpp::get_logger("ObjectPolygonDisplayBase"), + "there are no enough footprint to visualize polygon"); + return; + } + for (size_t i = 0; i < shape.footprint.points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; + point.y = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + } +} + void calc_path_line_list( const autoware_auto_perception_msgs::msg::PredictedPath & paths, std::vector & points) From 9dcb99cc8904d818d09b4ba4d7d4718d7a763138 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 3 Feb 2023 19:08:17 +0900 Subject: [PATCH 24/72] refactor(behavior_path_planner): common duplicated lines of getExtendedCurrentLanes (#2809) Signed-off-by: kosuke55 --- .../behavior_path_planner/src/utilities.cpp | 20 +------------------ 1 file changed, 1 insertion(+), 19 deletions(-) diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 156bca488cc58..229fa2977b4eb 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2026,25 +2026,7 @@ lanelet::ConstLanelets extendLanes( lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data) { - const auto & route_handler = planner_data->route_handler; - const auto current_pose = planner_data->self_odometry->pose.pose; - const auto common_parameters = planner_data->parameters; - const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); - - lanelet::ConstLanelet current_lane; - if (!route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("utilities"), - "failed to find closest lanelet within route!!!"); - return {}; // TODO(Horibe) what should be returned? - } - - // For current_lanes with desired length - const auto current_lanes = route_handler->getLaneletSequence( - current_lane, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length); - - return extendLanes(route_handler, current_lanes); + return extendLanes(planner_data->route_handler, getCurrentLanes(planner_data)); } lanelet::ConstLanelets calcLaneAroundPose( From d695f7a651f6bd898dd42aa6d53cd7b95c0e5fb7 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 3 Feb 2023 19:25:17 +0900 Subject: [PATCH 25/72] refactor(motion_velocity_smoother): remove duplicated implementation (#2798) Signed-off-by: Takamasa Horibe --- .../motion_velocity_smoother_node.hpp | 4 ++ .../src/motion_velocity_smoother_node.cpp | 47 ++++++++----------- 2 files changed, 24 insertions(+), 27 deletions(-) diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 262473565daab..bb656a30f3a4d 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -208,6 +208,10 @@ class MotionVelocitySmootherNode : public rclcpp::Node Trajectory toTrajectoryMsg( const TrajectoryPoints & points, const std_msgs::msg::Header * header = nullptr) const; + TrajectoryPoint calcProjectedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & pose) const; + TrajectoryPoint calcProjectedTrajectoryPointFromEgo(const TrajectoryPoints & trajectory) const; + // parameter handling void initCommonParam(); diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index fdba04a625661..d27294b24e23b 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -399,12 +399,7 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar // calculate prev closest point if (!prev_output_.empty()) { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_output_, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - prev_output_, current_odometry_ptr_->pose.pose, current_seg_idx); - current_closest_point_from_prev_output_ = closest_point; + current_closest_point_from_prev_output_ = calcProjectedTrajectoryPointFromEgo(prev_output_); } // calculate distance to insert external velocity limit @@ -884,11 +879,7 @@ void MotionVelocitySmootherNode::publishClosestVelocity( const TrajectoryPoints & trajectory, const Pose & current_pose, const rclcpp::Publisher::SharedPtr pub) const { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory, current_pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose, current_seg_idx); + const auto closest_point = calcProjectedTrajectoryPoint(trajectory, current_pose); Float32Stamped vel_data{}; vel_data.stamp = this->now(); @@ -898,11 +889,7 @@ void MotionVelocitySmootherNode::publishClosestVelocity( void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - trajectory, current_odometry_ptr_->pose.pose, current_seg_idx); + const auto closest_point = calcProjectedTrajectoryPointFromEgo(trajectory); auto publishFloat = [=](const double data, const auto pub) { Float32Stamped msg{}; @@ -938,12 +925,7 @@ void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & tr void MotionVelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result) { prev_output_ = final_result; - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - final_result, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - final_result, current_odometry_ptr_->pose.pose, current_seg_idx); - prev_closest_point_ = closest_point; + prev_closest_point_ = calcProjectedTrajectoryPointFromEgo(final_result); } MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorithmType( @@ -968,11 +950,7 @@ MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorit double MotionVelocitySmootherNode::calcTravelDistance() const { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_output_, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - prev_output_, current_odometry_ptr_->pose.pose, current_seg_idx); + const auto closest_point = calcProjectedTrajectoryPointFromEgo(prev_output_); if (prev_closest_point_) { const double travel_dist = @@ -1027,6 +1005,21 @@ void MotionVelocitySmootherNode::publishStopWatchTime() debug_calculation_time_->publish(calculation_time_data); } +TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & pose) const +{ + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + return trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, pose, current_seg_idx); +} + +TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( + const TrajectoryPoints & trajectory) const +{ + return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); +} + } // namespace motion_velocity_smoother #include "rclcpp_components/register_node_macro.hpp" From 355783ac6180427af19ad69e43c60ec2ff7aecc1 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 3 Feb 2023 19:29:01 +0900 Subject: [PATCH 26/72] refactor(intersection): divide stop line function, cache lanelet object (#2799) Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 2 + .../scene_merge_from_private_road.hpp | 1 + .../scene_module/intersection/util.hpp | 34 ++-- .../scene_module/intersection/util_type.hpp | 43 +++++ .../intersection/scene_intersection.cpp | 96 ++++++---- .../scene_merge_from_private_road.cpp | 49 ++--- .../src/scene_module/intersection/util.cpp | 177 ++++++++---------- 7 files changed, 235 insertions(+), 167 deletions(-) create mode 100644 planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 90bdbea51297a..80280a0946c0a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -114,6 +115,7 @@ class IntersectionModule : public SceneModuleInterface bool is_go_out_; // Parameter PlannerParam planner_param_; + std::optional intersection_lanelets_; /** * @brief check collision for all lanelet area & predicted objects (call checkPathCollision() as diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index a2d884240747d..6db961152d80e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -95,6 +95,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface // Parameter PlannerParam planner_param_; + std::optional intersection_lanelets_; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 085de0c834ab7..6fa4d184239da 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -52,17 +53,10 @@ std::optional getDuplicatedPointIdx( /** * @brief get objective polygons for detection area */ -std::tuple getObjectiveLanelets( +IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on = false); -struct StopLineIdx -{ - size_t first_inside_lane = 0; - size_t pass_judge_line = 0; - size_t stop_line = 0; -}; - /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, * read it from the map; otherwise, generate a stop line at a position where it will not collide. @@ -73,13 +67,27 @@ struct StopLineIdx " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane * @return nullopt if path is not intersecting with detection areas */ -std::pair, std::optional> generateStopLine( +std::optional generateStopLine( const int lane_id, const std::vector & detection_areas, - const std::vector & conflicting_areas, + const std::shared_ptr & planner_data, const double stop_line_margin, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interaval, const rclcpp::Logger logger); + +/** + * @brief Generate a stop line for stuck vehicle + * @param conflicting_areas used to generate stop line for stuck vehicle + * @param original_path ego-car lane + * @param target_path target lane to insert stop point (part of ego-car lane or same to ego-car + * lane) + " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane + */ +std::optional generateStuckStopLine( + const int lane_id, const std::vector & conflicting_areas, const std::shared_ptr & planner_data, const double stop_line_margin, const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, + const std::pair lane_interval, const rclcpp::Logger logger); /** * @brief Calculate first path index that is in the polygon. @@ -136,7 +144,7 @@ bool isBeforeTargetIndex( const geometry_msgs::msg::Pose & current_pose, const int target_idx); lanelet::ConstLanelets extendedAdjacentDirectionLanes( - const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane); std::optional getIntersectionArea( diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp new file mode 100644 index 0000000000000..604cefd373496 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp @@ -0,0 +1,43 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ +#define SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ + +#include + +#include + +namespace behavior_velocity_planner::util +{ +struct IntersectionLanelets +{ + bool tl_arrow_solid_on; + lanelet::ConstLanelets attention; + lanelet::ConstLanelets conflicting; + lanelet::ConstLanelets adjacent; + std::vector attention_area; + std::vector conflicting_area; + std::vector adjacent_area; +}; + +struct StopLineIdx +{ + size_t pass_judge_line = 0; + size_t collision_stop_line = 0; +}; + +} // namespace behavior_velocity_planner::util + +#endif // SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 0096b922ea75f..83099c46a6336 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -96,16 +96,19 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* get detection area*/ /* dynamically change detection area based on tl_arrow_solid_on */ - [[maybe_unused]] const bool has_tl = util::hasAssociatedTrafficLight(assigned_lanelet); const bool tl_arrow_solid_on = util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map); - auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - tl_arrow_solid_on); - const std::vector detection_area = - util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length); - const std::vector conflicting_area = - util::getPolygon3dFromLanelets(conflicting_lanelets); + if ( + !intersection_lanelets_.has_value() || + intersection_lanelets_.value().tl_arrow_solid_on != tl_arrow_solid_on) { + intersection_lanelets_ = util::getObjectiveLanelets( + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, + tl_arrow_solid_on); + } + const auto & detection_lanelets = intersection_lanelets_.value().attention; + const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent; + const auto & detection_area = intersection_lanelets_.value().attention_area; + const auto & conflicting_area = intersection_lanelets_.value().conflicting_area; debug_data_.detection_area = detection_area; /* get intersection area */ @@ -115,25 +118,34 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - /* get adjacent lanelets */ - const auto adjacent_lanelets = - util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); - debug_data_.adjacent_area = util::getPolygon3dFromLanelets(adjacent_lanelets); - - /* set stop lines for base_link */ - const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine( - lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin, - planner_param_.use_stuck_stopline, path, *path, logger_.get_child("util"), clock_); - if (!stuck_line_idx_opt.has_value()) { - // returns here if path is not intersecting with conflicting areas - RCLCPP_DEBUG_SKIPFIRST_THROTTLE( - logger_, *clock_, 1000 /* ms */, "setStopLineIdx for stuck line fail"); + /* spline interpolation */ + constexpr double interval = 0.2; + autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; + if (!splineInterpolate(*path, interval, path_ip, logger_)) { + RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "splineInterpolate failed"); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); setDistance(std::numeric_limits::lowest()); return false; } - const auto stuck_line_idx = stuck_line_idx_opt.value(); + const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id_); + if (!lane_interval_ip_opt.has_value()) { + RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + + const auto stuck_line_idx_opt = util::generateStuckStopLine( + lane_id_, conflicting_area, planner_data_, planner_param_.stop_line_margin, + planner_param_.use_stuck_stopline, path, path_ip, interval, lane_interval_ip_opt.value(), + logger_.get_child("util")); + + /* set stop lines for base_link */ + const auto stop_lines_idx_opt = util::generateStopLine( + lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin, path, path_ip, + interval, lane_interval_ip_opt.value(), logger_.get_child("util")); /* calc closest index */ const auto closest_idx_opt = @@ -149,7 +161,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const size_t closest_idx = closest_idx_opt.get(); if (stop_lines_idx_opt.has_value()) { - const auto stop_line_idx = stop_lines_idx_opt.value().stop_line; + const auto stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line; const bool is_over_pass_judge_line = @@ -189,29 +201,35 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * closest_idx, stuck_vehicle_detect_area, time_delay); /* calculate final stop lines */ - int stop_line_idx_final = - stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().stop_line : -1; + std::optional stop_line_idx = std::nullopt; if (external_go) { is_entry_prohibited = false; } else if (external_stop) { is_entry_prohibited = true; - } else if (is_stuck || has_collision) { + stop_line_idx = stop_lines_idx_opt.has_value() + ? std::make_optional(stop_lines_idx_opt.value().collision_stop_line) + : std::nullopt; + } else if (is_stuck && stuck_line_idx_opt.has_value()) { is_entry_prohibited = true; const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(stuck_line_idx).point.pose.position, + path->points, path->points.at(stuck_line_idx_opt.value()).point.pose.position, path->points.at(closest_idx).point.pose.position); const bool is_over_stuck_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx) && + util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx_opt.value()) && dist_stuck_stopline > planner_param_.stop_overshoot_margin; - if (is_stuck && !is_over_stuck_stopline) { - stop_line_idx_final = stuck_line_idx; - } else if ( - ((is_stuck && is_over_stuck_stopline) || has_collision) && stop_lines_idx_opt.has_value()) { - stop_line_idx_final = stop_lines_idx_opt.value().stop_line; + if (!is_over_stuck_stopline) { + stop_line_idx = stuck_line_idx_opt.value(); + } else if (is_over_stuck_stopline && stop_lines_idx_opt.has_value()) { + stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; } + } else if (has_collision) { + is_entry_prohibited = true; + stop_line_idx = stop_lines_idx_opt.has_value() + ? std::make_optional(stop_lines_idx_opt.value().collision_stop_line) + : std::nullopt; } - if (stop_line_idx_final == -1) { + if (!stop_line_idx.has_value()) { RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed"); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); @@ -226,23 +244,23 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * setSafe(state_machine_.getState() == StateMachine::State::GO); setDistance(motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, - path->points.at(stop_line_idx_final).point.pose.position)); + path->points.at(stop_line_idx.value()).point.pose.position)); if (!isActivated()) { // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block is_go_out_ = false; constexpr double v = 0.0; - planning_utils::setVelocityFromIndex(stop_line_idx_final, v, path); + planning_utils::setVelocityFromIndex(stop_line_idx.value(), v, path); debug_data_.stop_required = true; const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx_final, base_link2front, *path); + planning_utils::getAheadPose(stop_line_idx.value(), base_link2front, *path); // Get stop point and stop factor { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx_final).point.pose; + stop_factor.stop_pose = path->points.at(stop_line_idx.value()).point.pose; const auto stop_factor_conflict = planning_utils::toRosPoints(debug_data_.conflicting_targets); const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets); @@ -250,7 +268,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck); planning_utils::appendStopReason(stop_factor, stop_reason); - const auto & stop_pose = path->points.at(stop_line_idx_final).point.pose; + const auto & stop_pose = path->points.at(stop_line_idx.value()).point.pose; velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index d6084a5d8ce92..a395fc00d5153 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -64,32 +64,42 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* get detection area */ - auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - false /* tl_arrow_solid on does not matter here*/); - if (detection_lanelets.empty()) { - RCLCPP_DEBUG(logger_, "no detection area. skip computation."); - return true; + if (!intersection_lanelets_.has_value()) { + intersection_lanelets_ = util::getObjectiveLanelets( + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, + false /* tl_arrow_solid on does not matter here*/); } - const auto detection_area = - util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length); - const std::vector conflicting_area = - util::getPolygon3dFromLanelets(conflicting_lanelets); - debug_data_.detection_area = detection_area; + const auto & detection_area = intersection_lanelets_.value().attention_area; /* set stop-line and stop-judgement-line for base_link */ - const auto private_path = - extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); - const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine( - lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin, - false /* same */, path, *path, logger_.get_child("util"), clock_); + /* spline interpolation */ + constexpr double interval = 0.2; + autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; + if (!splineInterpolate(*path, interval, path_ip, logger_)) { + RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "splineInterpolate failed"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id_); + if (!lane_interval_ip_opt.has_value()) { + RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + + const auto stop_lines_idx_opt = util::generateStopLine( + lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin, path, path_ip, + interval, lane_interval_ip_opt.value(), logger_.get_child("util")); if (!stop_lines_idx_opt.has_value()) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; } - const auto & stop_lines_idx = stop_lines_idx_opt.value(); - const size_t stop_line_idx = stop_lines_idx.stop_line; + const size_t stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; if (stop_line_idx == 0) { RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); return true; @@ -98,8 +108,6 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR debug_data_.virtual_wall_pose = planning_utils::getAheadPose( stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; - const size_t first_inside_lane_idx = stop_lines_idx.first_inside_lane; - debug_data_.first_collision_point = path->points.at(first_inside_lane_idx).point.pose.position; /* set stop speed */ if (state_machine_.getState() == StateMachine::State::STOP) { @@ -109,7 +117,6 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR /* get stop point and stop factor */ tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = debug_data_.stop_point_pose; - stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point); planning_utils::appendStopReason(stop_factor, stop_reason); const auto & stop_pose = path->points.at(stop_line_idx).point.pose; velocity_factor_.set( diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index c047c7814e018..e4ae13c6ce7ce 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -141,13 +141,12 @@ std::optional getFirstPointInsidePolygons( return first_idx_inside_lanelet; } -std::pair, std::optional> generateStopLine( +std::optional generateStopLine( const int lane_id, const std::vector & detection_areas, - const std::vector & conflicting_areas, const std::shared_ptr & planner_data, const double stop_line_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, - [[maybe_unused]] const rclcpp::Clock::SharedPtr clock) + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interval_ip, const rclcpp::Logger logger) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -158,81 +157,15 @@ std::pair, std::optional> generateStopLine( const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithJerkLimit( current_vel, current_acc, max_acc, max_jerk, delay_response_time); - // first inside lane idx - const auto first_inside_lane_it = - std::find_if(original_path->points.begin(), original_path->points.end(), [&](const auto & p) { - return std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end(); - }); - if (first_inside_lane_it == original_path->points.end()) { - RCLCPP_ERROR(logger, "No points on intersection lane %d", lane_id); - return {std::nullopt, std::nullopt}; - } - const size_t first_inside_lane_idx = - std::distance(original_path->points.begin(), first_inside_lane_it); - - /* spline interpolation */ - constexpr double interval = 0.2; - autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; - if (!splineInterpolate(target_path, interval, path_ip, logger)) { - return {std::nullopt, std::nullopt}; - } - const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); - const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id); - if (!lane_interval_ip_opt.has_value()) { - RCLCPP_WARN(logger, "Path has no interval on intersection lane %d", lane_id); - return {std::nullopt, std::nullopt}; - } - const auto [lane_interval_ip_start, lane_interval_ip_end] = lane_interval_ip_opt.value(); - - /* generate stuck stop line */ - size_t stuck_stop_line_idx_ip = 0; - if (use_stuck_stopline) { - // the first point in intersection lane - stuck_stop_line_idx_ip = lane_interval_ip_start; - } else { - const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons( - path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas); - if (!stuck_stop_line_idx_ip_opt.has_value()) { - RCLCPP_DEBUG( - logger, - "Path is not intersecting with conflicting area, not generating stuck_stop_line. start = " - "%ld, end = %ld", - lane_interval_ip_start, lane_interval_ip_end); - return {std::nullopt, std::nullopt}; - } - stuck_stop_line_idx_ip = stuck_stop_line_idx_ip_opt.value(); - } - - size_t stuck_stop_line_idx = 0; - { - /* insert stuck stop line */ - const size_t insert_idx_ip = static_cast(std::max( - static_cast(stuck_stop_line_idx_ip) - 1 - stop_line_margin_idx_dist - - base2front_idx_dist, - 0)); - const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; - const auto duplicate_idx_opt = - util::getDuplicatedPointIdx(*original_path, insert_point.position); - if (duplicate_idx_opt.has_value()) { - stuck_stop_line_idx = duplicate_idx_opt.value(); - } else { - const auto insert_idx_opt = util::insertPoint(insert_point, original_path); - if (!insert_idx_opt.has_value()) { - RCLCPP_WARN(logger, "insertPoint failed for stuck stop line"); - return {std::nullopt, std::nullopt}; - } - stuck_stop_line_idx = insert_idx_opt.value(); - } - } + const auto [lane_interval_ip_start, lane_interval_ip_end] = lane_interval_ip; /* generate stop points */ util::StopLineIdx idxs; - idxs.first_inside_lane = first_inside_lane_idx; // If a stop_line tag is defined on lanelet_map, use it. // else generate a stop_line behind the intersection of path and detection area @@ -251,7 +184,7 @@ std::pair, std::optional> generateStopLine( if (!first_inside_detection_idx_ip_opt.has_value()) { RCLCPP_DEBUG( logger, "Path is not intersecting with detection_area, not generating stop_line"); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } const auto first_inside_detection_idx_ip = first_inside_detection_idx_ip_opt.value(); @@ -262,7 +195,7 @@ std::pair, std::optional> generateStopLine( } if (stop_idx_ip == 0) { RCLCPP_DEBUG(logger, "stop line is at path[0], ignore planning\n===== plan end ====="); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } { @@ -271,19 +204,19 @@ std::pair, std::optional> generateStopLine( const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); if (duplicate_idx_opt.has_value()) { - idxs.stop_line = duplicate_idx_opt.value(); + idxs.collision_stop_line = duplicate_idx_opt.value(); } else { const auto insert_idx_opt = util::insertPoint(insert_point, original_path); if (!insert_idx_opt.has_value()) { RCLCPP_WARN(logger, "insertPoint failed for stop line"); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } - idxs.stop_line = insert_idx_opt.value(); + idxs.collision_stop_line = insert_idx_opt.value(); } } const bool has_prior_stopline = std::any_of( - original_path->points.begin(), original_path->points.begin() + idxs.stop_line, + original_path->points.begin(), original_path->points.begin() + idxs.collision_stop_line, [](const auto & p) { return std::fabs(p.point.longitudinal_velocity_mps) < 0.1; }); /* insert judge point */ @@ -292,7 +225,7 @@ std::pair, std::optional> generateStopLine( std::max(static_cast(stop_idx_ip) - pass_judge_idx_dist, 0))); /* if another stop point exist before intersection stop_line, disable judge_line. */ if (has_prior_stopline || pass_judge_idx_ip == stop_idx_ip) { - idxs.pass_judge_line = idxs.stop_line; + idxs.pass_judge_line = idxs.collision_stop_line; } else { const auto & insert_point = path_ip.points.at(pass_judge_idx_ip).point.pose; const auto duplicate_idx_opt = @@ -303,24 +236,68 @@ std::pair, std::optional> generateStopLine( const auto insert_idx_opt = util::insertPoint(insert_point, original_path); if (!insert_idx_opt.has_value()) { RCLCPP_WARN(logger, "insertPoint failed to pass judge line"); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } idxs.pass_judge_line = insert_idx_opt.value(); - idxs.stop_line = std::min(idxs.stop_line + 1, original_path->points.size() - 1); - if (stuck_stop_line_idx >= idxs.pass_judge_line) { - stuck_stop_line_idx = - std::min(stuck_stop_line_idx + 1, original_path->points.size() - 1); - } + idxs.collision_stop_line = + std::min(idxs.collision_stop_line + 1, original_path->points.size() - 1); } } RCLCPP_DEBUG( logger, "generateStopLine() : stop_idx = %ld, pass_judge_idx = %ld" - ", stuck_stop_idx = %ld, has_prior_stopline = %d", - idxs.stop_line, idxs.pass_judge_line, stuck_stop_line_idx, has_prior_stopline); + ", has_prior_stopline = %d", + idxs.collision_stop_line, idxs.pass_judge_line, has_prior_stopline); + + return std::make_optional(idxs); +} + +std::optional generateStuckStopLine( + const int lane_id, const std::vector & conflicting_areas, + const std::shared_ptr & planner_data, const double stop_line_margin, + const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interval_ip, const rclcpp::Logger logger) +{ + size_t stuck_stop_line_idx_ip = 0; + const auto [lane_interval_ip_start, lane_interval_ip_end] = lane_interval_ip; + if (use_stuck_stopline) { + stuck_stop_line_idx_ip = lane_interval_ip_start; + } else { + const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons( + path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas); + if (!stuck_stop_line_idx_ip_opt.has_value()) { + RCLCPP_DEBUG( + logger, + "Path is not intersecting with conflicting area, not generating stuck_stop_line. start = " + "%ld, end = %ld", + lane_interval_ip_start, lane_interval_ip_end); + return std::nullopt; + } + stuck_stop_line_idx_ip = stuck_stop_line_idx_ip_opt.value(); + } - return {stuck_stop_line_idx, std::make_optional(idxs)}; + const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); + const int base2front_idx_dist = + std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); + size_t stuck_stop_line_idx = 0; + const size_t insert_idx_ip = static_cast(std::max( + static_cast(stuck_stop_line_idx_ip) - 1 - stop_line_margin_idx_dist - base2front_idx_dist, + 0)); + const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; + const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); + if (duplicate_idx_opt.has_value()) { + stuck_stop_line_idx = duplicate_idx_opt.value(); + return std::make_optional(stuck_stop_line_idx); + } else { + const auto insert_idx_opt = util::insertPoint(insert_point, original_path); + if (!insert_idx_opt.has_value()) { + RCLCPP_WARN(logger, "insertPoint failed for stuck stop line"); + return std::nullopt; + } + return std::make_optional(insert_idx_opt.value()); + } } bool getStopLineIndexFromMap( @@ -387,7 +364,7 @@ bool getStopLineIndexFromMap( return true; } -std::tuple getObjectiveLanelets( +IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on) { @@ -461,9 +438,9 @@ std::tuple getObjectiveLanelets( // get possible lanelet path that reaches conflicting_lane longer than given length // if traffic light arrow is active, this process is unnecessary + lanelet::ConstLanelets detection_and_preceding_lanelets; if (!tl_arrow_solid_on) { const double length = detection_area_length; - lanelet::ConstLanelets detection_and_preceding_lanelets; std::set detection_ids; for (const auto & ll : detection_lanelets) { // Preceding lanes does not include detection_lane so add them at the end @@ -480,10 +457,22 @@ std::tuple getObjectiveLanelets( } } } - return {std::move(detection_and_preceding_lanelets), std::move(conflicting_ex_ego_lanelets)}; - } else { - return {std::move(detection_lanelets), std::move(conflicting_ex_ego_lanelets)}; } + + IntersectionLanelets result; + if (!tl_arrow_solid_on) { + result.attention = std::move(detection_and_preceding_lanelets); + } else { + result.attention = std::move(detection_lanelets); + } + result.conflicting = std::move(conflicting_ex_ego_lanelets); + result.adjacent = + extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); + // compoundPolygon3d + result.attention_area = getPolygon3dFromLanelets(result.attention); + result.conflicting_area = getPolygon3dFromLanelets(result.conflicting); + result.adjacent_area = getPolygon3dFromLanelets(result.adjacent); + return result; } std::vector getPolygon3dFromLanelets( @@ -574,7 +563,7 @@ static std::vector getAllAdjacentLanelets( } lanelet::ConstLanelets extendedAdjacentDirectionLanes( - const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) { // some of the intersections are not well-formed, and "adjacent" turning From ace34c6fca9ed6289cfef5096113ef130fa40aa2 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 3 Feb 2023 21:11:09 +0900 Subject: [PATCH 27/72] feat(ndt_scan_matcher): dynamic map loading (#2339) * first commit Signed-off-by: kminoda * ci(pre-commit): autofix * import map update module in core Signed-off-by: kminoda * ci(pre-commit): autofix * minor fixes. Now map update module launches!!! Signed-off-by: kminoda * ci(pre-commit): autofix * debugged Signed-off-by: kminoda * revert unnecessary fix Signed-off-by: kminoda * minor fixes Signed-off-by: kminoda * update launch file Signed-off-by: kminoda * update comment Signed-off-by: kminoda * ci(pre-commit): autofix * update comment Signed-off-by: kminoda * update comment Signed-off-by: kminoda * ci(pre-commit): autofix * update comment Signed-off-by: kminoda * ci(pre-commit): autofix * update for ndt_omp Signed-off-by: kminoda * changed parameter names Signed-off-by: kminoda * ci(pre-commit): autofix * apply pre-commit- * ci(pre-commit): autofix * update readme Signed-off-by: kminoda * ci(pre-commit): autofix * update readme Signed-off-by: kminoda * ci(pre-commit): autofix * simplify client implementation Signed-off-by: kminoda * remove unnecessary comments Signed-off-by: kminoda * ci(pre-commit): autofix * removed unused member variables Signed-off-by: kminoda * set default use_dynamic_map_loading to true Signed-off-by: kminoda * changed readme Signed-off-by: kminoda * ci(pre-commit): autofix * reflected comments Signed-off-by: kminoda * use std::optional instead of shared_ptr Signed-off-by: kminoda * ci(pre-commit): autofix * fix parameter description Signed-off-by: kminoda * revert launch output config Signed-off-by: kminoda * change default subscriber name Signed-off-by: kminoda * remove unnecessary setInputSource Signed-off-by: kminoda * add gif Signed-off-by: kminoda * ci(pre-commit): autofix * minor fix Signed-off-by: kminoda * Update localization/ndt_scan_matcher/src/map_update_module.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * update literals Signed-off-by: kminoda * update map_loader default parameters * update readme Signed-off-by: kminoda * ci(pre-commit): autofix --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> --- localization/ndt_scan_matcher/CMakeLists.txt | 1 + localization/ndt_scan_matcher/README.md | 62 +++- .../config/ndt_scan_matcher.param.yaml | 17 +- .../include/ndt_scan_matcher/map_module.hpp | 4 +- .../ndt_scan_matcher/map_update_module.hpp | 107 +++++++ .../ndt_scan_matcher_core.hpp | 6 +- .../pose_initialization_module.hpp | 4 +- .../launch/ndt_scan_matcher.launch.xml | 6 + .../media/differential_area_loading.gif | Bin 0 -> 89542 bytes localization/ndt_scan_matcher/package.xml | 1 + .../src/map_update_module.cpp | 299 ++++++++++++++++++ .../src/ndt_scan_matcher_core.cpp | 17 +- .../config/pointcloud_map_loader.param.yaml | 4 +- 13 files changed, 508 insertions(+), 20 deletions(-) create mode 100644 localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp create mode 100644 localization/ndt_scan_matcher/media/differential_area_loading.gif create mode 100644 localization/ndt_scan_matcher/src/map_update_module.cpp diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 0f5b761baf6f0..0a04f1bb60466 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -34,6 +34,7 @@ ament_auto_add_executable(ndt_scan_matcher src/tf2_listener_module.cpp src/map_module.cpp src/pose_initialization_module.cpp + src/map_update_module.cpp ) link_directories(${PCL_LIBRARY_DIRS}) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 81fac59e4b8db..7c63785aebcd2 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -64,7 +64,6 @@ One optional function is regularization. Please see the regularization chapter i | `max_iterations` | int | The number of iterations required to calculate alignment | | `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | | `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result | -| `neighborhood_search_method` | int | neighborhood search method (0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1) | | `num_threads` | int | Number of threads used for parallel computing | (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) @@ -172,6 +171,67 @@ The color of the trajectory indicates the error (meter) from the reference traje drawing drawing +## Dynamic map loading + +Autoware supports dynamic map loading feature for `ndt_scan_matcher`. Using this feature, NDT dynamically requests for the surrounding pointcloud map to `pointcloud_map_loader`, and then receive and preprocess the map in an online fashion. + +Using the feature, `ndt_scan_matcher` can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error) + +drawing + +### Additional interfaces + +#### Additional inputs + +| Name | Type | Description | +| ---------------- | ------------------------- | ----------------------------------------------------------- | +| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) | + +#### Additional outputs + +| Name | Type | Description | +| ----------------------------- | ------------------------------- | ------------------------------------------------- | +| `debug/loaded_pointcloud_map` | `sensor_msgs::msg::PointCloud2` | pointcloud maps used for localization (for debug) | + +#### Additional client + +| Name | Type | Description | +| ------------------- | ------------------------------------------------------ | ------------------ | +| `client_map_loader` | `autoware_map_msgs::srv::GetDifferentialPointCloudMap` | map loading client | + +### Parameters + +| Name | Type | Description | +| ------------------------------------- | ------ | -------------------------------------------------------------------- | +| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (TRUE by default) | +| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | +| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | +| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | + +### Enabling the dynamic map loading feature + +To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node. +Follow the next two instructions. + +1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) +2. split the PCD files into grids (recommended size: 20[m] x 20[m]) + +Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of + +- one PCD map file +- multiple PCD map files divided into small size (~20[m]) + +Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) + +| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | +| :---------: | :-----------------------: | :------------------------: | :------------------: | +| single file | true | true | at once (standard) | +| single file | true | false | **does NOT work** | +| single file | false | true/false | at once (standard) | +| splitted | true | true | dynamically | +| splitted | true | false | **does NOT work** | +| splitted | false | true/false | at once (standard) | + ## Scan matching score based on de-grounded LiDAR scan ### Abstract diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 1fb8fd003b4ac..56c5baa347aaa 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + # Use dynamic map loading + use_dynamic_map_loading: true # Vehicle reference frame base_frame: "base_link" @@ -41,10 +43,6 @@ # Tolerance of distance difference between two initial poses used for linear interpolation. [m] initial_pose_distance_tolerance_m: 10.0 - # neighborhood search method - # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - neighborhood_search_method: 0 - # Number of threads used for parallel computing num_threads: 4 @@ -63,9 +61,18 @@ # Regularization switch regularization_enabled: false - # Regularization scale factor + # regularization scale factor regularization_scale_factor: 0.01 + # Dynamic map loading distance + dynamic_map_loading_update_distance: 20.0 + + # Dynamic map loading loading radius + dynamic_map_loading_map_radius: 150.0 + + # Radius of input LiDAR range (used for diagnostics of dynamic map loading) + lidar_radius: 100.0 + # A flag for using scan matching score based on de-grounded LiDAR scan estimate_scores_for_degrounded_scan: false diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp index 871ff2760acf7..5253879a28937 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp @@ -19,9 +19,9 @@ #include +#include #include #include -#include #include @@ -30,7 +30,7 @@ class MapModule using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: MapModule( diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp new file mode 100644 index 0000000000000..fb9577ca42934 --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -0,0 +1,107 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ + +#include "ndt_scan_matcher/debug.hpp" +#include "ndt_scan_matcher/particle.hpp" +#include "ndt_scan_matcher/tf2_listener_module.hpp" +#include "ndt_scan_matcher/util_func.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +class MapUpdateModule +{ + using PointSource = pcl::PointXYZ; + using PointTarget = pcl::PointXYZ; + using NormalDistributionsTransform = + pclomp::MultiGridNormalDistributionsTransform; + +public: + MapUpdateModule( + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, + std::shared_ptr ndt_ptr, + std::shared_ptr tf2_listener_module, std::string map_frame, + rclcpp::CallbackGroup::SharedPtr main_callback_group, + std::shared_ptr> state_ptr); + +private: + void service_ndt_align( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); + void callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr); + void map_update_timer_callback(); + + void update_ndt( + const std::vector & maps_to_add, + const std::vector & map_ids_to_remove); + void update_map(const geometry_msgs::msg::Point & position); + bool should_update_map(const geometry_msgs::msg::Point & position) const; + void publish_partial_pcd_map(); + geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( + const std::shared_ptr & ndt_ptr, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); + void publish_point_cloud( + const rclcpp::Time & sensor_ros_time, const std::string & frame_id, + const std::shared_ptr> & sensor_points_mapTF_ptr); + + rclcpp::Publisher::SharedPtr loaded_pcd_pub_; + rclcpp::Publisher::SharedPtr + ndt_monte_carlo_initial_pose_marker_pub_; + rclcpp::Publisher::SharedPtr sensor_aligned_pose_pub_; + + rclcpp::Service::SharedPtr service_; + rclcpp::Client::SharedPtr + pcd_loader_client_; + rclcpp::TimerBase::SharedPtr map_update_timer_; + + rclcpp::Subscription::SharedPtr ekf_odom_sub_; + + rclcpp::CallbackGroup::SharedPtr map_callback_group_; + + std::shared_ptr ndt_ptr_; + std::mutex * ndt_ptr_mutex_; + std::string map_frame_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr tf2_listener_module_; + std::shared_ptr> state_ptr_; + + int initial_estimate_particles_num_; + std::optional last_update_position_ = std::nullopt; + std::optional current_position_ = std::nullopt; + const double dynamic_map_loading_update_distance_; + const double dynamic_map_loading_map_radius_; + const double lidar_radius_; +}; + +#endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 30356bc61cee0..1bd7a509a3a7b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -18,6 +18,7 @@ #define FMT_HEADER_ONLY #include "ndt_scan_matcher/map_module.hpp" +#include "ndt_scan_matcher/map_update_module.hpp" #include "ndt_scan_matcher/pose_initialization_module.hpp" #include "ndt_scan_matcher/tf2_listener_module.hpp" @@ -34,8 +35,8 @@ #include #include +#include #include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -71,7 +72,7 @@ class NDTScanMatcher : public rclcpp::Node using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: NDTScanMatcher(); @@ -200,6 +201,7 @@ class NDTScanMatcher : public rclcpp::Node std::shared_ptr tf2_listener_module_; std::unique_ptr map_module_; std::unique_ptr pose_init_module_; + std::unique_ptr map_update_module_; bool estimate_scores_for_degrounded_scan_; double z_margin_for_ground_removal_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp index 7745ecf8a2148..0e1a6e6816413 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp @@ -27,8 +27,8 @@ #include #include +#include #include -#include #include #include @@ -39,7 +39,7 @@ class PoseInitializationModule using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: PoseInitializationModule( diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index 9e3d966bd770a..b442ac1b3d843 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -7,10 +7,13 @@ + + + @@ -23,6 +26,9 @@ + + + diff --git a/localization/ndt_scan_matcher/media/differential_area_loading.gif b/localization/ndt_scan_matcher/media/differential_area_loading.gif new file mode 100644 index 0000000000000000000000000000000000000000..4283b738a1dfa092f5af98e4e1c88b7c93743626 GIT binary patch literal 89542 zcmdqIWl$VZ)aTt}Ffh2g1q%|KkOat}!9swbK{6z0@Zdg!dvJFT9$dqa0Kp|e26qXP zpn({(*=MWX_jx|-Q?>hXf7M;pea`8+eY)!Qy{GTLAE~R#+_U6>d;kFez==m_tLQ&f zQqWNmzat9y57SR5VD-->^lu0LXILZ%1cE}L^pq4&X^E1VAjK@8EOvrGZb}_tUTFyl zMMXuO$B&JTj7&|<%*`zS?mrk0kLj*gDr-d-#g+t=6k@jvMA?;ji-9Q{v>{U@fTrl$Y*eE$3y zhr=!WSN>W47xVM;b8~b5j{iUZ%F4>de`0HE>)_zv>gwv}fAHtepPT;*9*_U8hX4QU z|5ew&4FB&^`|mIO@4f#`i~sA1{~rbX3x?wXN+K4uhQhv3IJJP;P(#tjNQzq;xoYo< z2Vxl{UM&y3EBW+>Ln($uy|HvSh5u=l*>GdoXol#^kzDnr^6?xQ_k-o(ri#e|WGFGK zMswwKiCT()`AGBIFBLi^8hIKmRkPK`O|Mo)TB_&kQ6FPiHCt;In(V$*nUA*CF15LC zjpS*z)va{-oFA-=w$-ooq9F)2t@ehEk5SZu7Gv%2wmv1^(#+TDXxtgilyF!b>uB1W zEL4hRd)(Rl^-JZ`YK!sCmc#jmm!tWQyIPNz+T9OV$Gh6TuVF(G?AqP!r&~iQg3l+q zJI?o}N;C_!dpa)M+q47D_*-+zl{w-mRX;|QE zNmT&s!8yf<02&mFiF}##GG9jEY4*x9R0XKYn{7tE*0zR7R@Y=lgjK86X+WzAnBM|e z7}U~|zZy>ao3bVa+*RGs6Le8s-xne&)H?gxUs&z){jdnIA#Wizm7t~YL)Xa%W{Be3 z3C+YbQ$)u1uyOVl5%1NASfrn#fqXMxAiFfaqm<1F?n+wk;9=tK5cn=c;&k55VkJlJcR}bZ~%jkI0tozfv_KjgSQp#1rJlDRR%$MJ; zCV_21ohp$iVTyG+qM(zk7?=L*O%L|`PSvRD)y|`YF0u~QQg-DI1qO$1w@-i>NF2By zCEvRJm-n&EjMk*=^}={{px-f5EnA057yS%`blH1Njt(vbAon3foJhcmq2Pf^xdeIO z;&d-QU}@sXq}PTo*LR&C^eq4u0_^|+G^p;aq|!h)rIlmx7d>(PWM1_z*N>|1D#Xc7 zVJQ36AQFx5LmUBseI3p?VOuXo6JvMqWo@8RzO@}P5fPLi)}-OxFa7XnsMcP<|tOq+vDnEv1RWGr8BZzO!P9S!eGgW{Q!~2v%3S;ovsdyX zTGL9xzstUPWmNZr9*fU=_1Y*K_3n;LDOi4+Bm}uZY}?;EHhdT7Uh|Uv9M>;%;}#Vvv`zlKeTaju z&->O6*%8ZG7$I-RX3Y!jNH%5R z$E}0Tv$uq7CAvB$ybLAs2ekI&ygMd?R?qSv<2!_zF~h}XuNAd_&w#&QV9Dg~giao4 zu50^MMrli8m;;RZs=iETUN+xTh+o8QN`DbGXjaUNHB;}(_kcW(oooDrr= z3-W3$re&{e=#}3OMYO&h|9u?l$jrGF;oY{v>Kxi_x_mCLX003Vtj%n^pnKV7s`A&) zHdDwlvq-5giaY6GN=-?grm)>|yTbJUyKhdpZRA;i<) zQyi^5nSR$@UNcyS<-9qu3JqwGGP)WdYCLuCHQQis^Bnq#IMKeFyK>L_dipfyAU!x` zLqf#!b5FKQ+>=}m#tNU=miOlvtGqqayV+l2JHF;H_v{xWbdEo^F#pl{ciW2n_sHm- z3$^O*<4F}??6mwz8GF~l>qya=@f(+C-+R8<-j$u0t#enKJloqPyFdI?+hc&9?X<+} z&kkR(PYr=Z2ZwL#XT~R&o`-FR;IH>^x^JTD$(1j8LS>tH>b<*+@{f7;{%rnad(~^& z^|gS$yXj^A@5PF|E4{0pwu%IcxsR(mee^fgQBQuhZLxmyZM!LRd;a(6W$?G@g17i1 zVa>lq4#9KFcYCkuCvP?j@N)?Ahq7Pme)m`Oo)515IsX3b=F$cK^=#gJi_foNC&2Q` zj}Yf)wC9P)^dpLbPwx4FIWZL1Xx2!(=BuEC`Jnxb;K0ZLl!_m$(ihBtKELaed4);O z@C~a7xoY#jjq~RiGU7E3;WrNcb@%n3`M}!N(38l}JNu!VMxo1+=p9>sCU~ewX6Vg4 z=66Qe55BNBkHTQqVOx(dG}i&NLw+XLXi6NK*v`*fJm`;2xEVa+*>(8uvd|~EFxl&1 zw@2Yg))6Cw&M&V0J=I*i+XJ|80o=x6uhl~D?qkH;Ltylw!*@d)(;}(bBB$|+5p0hl z1AY90)!b9~of7Qa;_N)LhTIaPFv+-(@b)mFs3-@%@K3fe@fA@ObWyc30Y>l$b6m7B zU##7JI8ju18$6(Hh=OrHf*Bs^2#-^{4rF17`|BO2go}O85K|@N7hxA&oEbIE5c0(* z#t9xYR>?mB4=ag^Dcg^qQHzP>Pr%83`oZv0 zKK%`?;hP`(Z_cCM{9%BCv)%wiZwMC>U|DZ8$`gn;tilGZjmxbmO{~#|NqSyM3`Eu_ zRWRGnBo4-8u6xPc>R?urWd5vV&JGZHVe*}~$wEJq#nqGf?xje7O$x=gCh3+ZF%PFO zWTo66PKw-3{52OJCX)b4f#@`(YAvK{|4h|kOw%a@Y1~dT^i9*g4dFqjDIsk#&#gF7 zU_o@6=G!zY-*iLuG`o)UCvQO=cCpR;Udkq^7v8CR?J@?#>3(n1{f5(p*8z?b08yyI zf0BA{{Q=W0KxY_Cp9mJ=fLeE?1rDbN+{?22nr_RG@$PPh$Gs?>p|GlyjCA$%w0qgQ zL|`JUR8$A(Ne3h)Nn7cCyn z(e20?=}0Xs7As8#5LoaNdzc&wrWFm9D}+djLUghO>WKhu{8a8}R_^vf>U-a499^!p zWNyyatbT^@A>aI+d)bN&U=j{M=_FMs5go&db-h1wlpa09^K0O|Nz zw3n4X-ySo|A4R{I|Aju5hAErxAXSn$O#%(-apwUQ0(Uslc$rF*7}J`fd5Dl;@%vdy zzY3{ep}T#GdU2s%zQ!yX1x5#{n~)qbELhDo?eRgW!(ysX17Mgerd|cM*T}MbR$}ri z2i{)xZJ|uuyTrn?V*6hC6rj#2!Y31th0(mWgXKh%40yeF9&l4*rZk|5~a!oFPgf&R7Bni58Zt*KNJ3 zOYcwuH2`BV`N#LOr+<}Asnz;kMt5q|pAu!yHZ`QHr-@OM zI-<>7OHGFU`64>};3??O&el|cR_)Pe*&|tErg4vBGx&(BpI0}UJjjmhN*5A+tCI81 z&-C369Ia`Zo9Iw)5Zb024Wmy4y!_iD5N&%|?b7}&q+M-ah!fCuwZVb~<;<0@W5osz zI<5se1J%3y-=<2Qyi<7AnHHP60QbxBYwfw;^d_t$!4FiG1t0{v9Em$QG&}F1>p+R1 zum_cInA=BW!m9+k`w{K*)wvRQv{=7Jd%S7a#8PgnL&Z~n9-_ns|XLHMR#4(9!V|U_1wAN4s78 zTfQ<^Yeqv+?mQ$q6-`yxkFmjSG94#GeLMl`g9u2e_*n`A5N`OGSrsWS8-X0_5$_rlIvS8&8W1=vGGHD!@vna9IDmUL7~fvx_!jbVxK;g6 zhjT!2clC#-ED6aE>dEnO0}*E34)-8?5`!!(L#aolJIpP5V}ts+Lw#LCN<#gTvFWe? z9&%vFOlZh$xlU-kH#KW`^)QSm@{`=NPai(i3KwS60}Zym!xf$VpuJ9TdW8a)|)mafH!v`nV=T z5Gw_4=w|slPJ$N-{6>_dz-Y~wXnn6I1vU3YXE%j5%Z5>BMm2CoEpJAnXNGYdpvTVW zu+FOgwbnDA)q6Z^m^Z6&JoC(H=8pitpfjsYI%DeAiWME zqQFJUEu@a)gpn^G3vcrOE?K^ta?&>?ZoH=-1 zGyJ)E@yl|}3ypEDktIL-@h+0(Cb_N_*52W&NE5B_A>q#^gqc@>Vml zG6Y?Dz{sl5`)iLG*Jj7o23B&p8>aE2JQS< z>y=^Zn-MyzE&I2r>x^A2+FNW1)8if67xBvqH=(&lZLb`+lft%-eoaJt*m@i^E%+P&xtIa%fz;C@Yk^Zr9*uS}9?tqjo2& ze`geL0vU?kwUpm|AwRXp2sUa2g2~n#EWV8H?g}<j?d7#Yd%~A8szEr}| zDBHK$_kN$h9ek8;9bhXpJOP;V59sTT_H(}d`L=m7F&1ppI-+$9U2_<$6S`qJxodGa z{QjGc{K-Uot+DV%=UvdZ6ZLQ3!6Zx9*`P(T?dhP?i-Z%}hmMUmxu#!FKaGET zf?xft{R+m%0f;Bm^jb_xGkm^W?F^SXkOWlQiO$u8&Rg%C@l`=krK9O=`x-tw?)Klf zB9;WxH33ic`iL8u{k04@LH)ceGS z{kuiv{-4f`wftkVf*jwV3vJ^3Et5^1-eEqoAC_*5&(FTHU7Hq+uyMR~7x_lV|8D2oPqx5)hlf8q-n0+l z$$OO6>$Ts!?{nFSesdZd{G;vrRr1;;)su6wpzA2Jo7=%xM3ZL=1%M)Yg$9Fj6*U{s;_17%8i?GS%jWa!gcWDmqzrOErP@p2caUu2MoG&*N+sU%ed- zMh2tzLJL&8V+ptISHoUV^Jy_s3Es#uQ10u5a3)fyRE2Qrl|Rmt4`(ts(62J67HD~; zv+=nG67I$0>*W-uuYS$`-Y=gtSO)%3v7%`})`XI> ztL7S?z9?acazwgJ~J3;@@O>7}d5zW01|R{FT&>R-L}kDDs8H=pqozpGg}9bb zA5GI3Plu$$ILVZjxXk4Yi@Vyby?$M_``>F$F^H4xbIctJsHBd9Wez7?2~Vyfsr#={ z4*~*WYe_wI#z4+cb&&zdD0=yZqNh(D!lY)j&D)Pax^g{0#8V^dv{`-AaH%;unfR7D z1KSpJr5e2>4O7RfowjFzC7VAK{N9F8R6qf> z!a6TqQ{xuTS*a^C_a!b`bfn8cy~1HaNc>{ZLjuOB;38sNKc+$;vz(WKVjUcQt5u}V z<7XE`XN1!YTm!Z%HD*zLGX82iF_O1ax+qVeV>P8r<0r3BsnXZo-}ut>j@>SsLEjf7 zj1*+AvN94z_NvgW2m8g7s$E-s$-E4XmDyfK4j#4Th{Ly*gNfmzhV6^T75(KgN7b_0 zU9KNz3F4em|9TNSN1WzAc+vGW=H8vIL<$ZN-LG$1r{&O!k)w}~_yYN-lk{@!C$nDX zHjS+K1=b8#N0@y}XyABv9#8t9$73z{Zsk>x9=X3~>9fhj4><4amD=6;XF(PFV$%8i z*a~o6!UMmzjTr~OzkPqQ6cqCT&wNub zLat6>2c&#mDMd@Dv!BlqcKr}L3G8Gh`CDB4@Q;D06-@vf&$QQzj{&A`@kMI%D`iY21amf95iM}kQQGbfL6e>|uA$R-JNtuiL%7;#z5Zh{yu+T2bP8~nheg=wJ zqX#>jNxY1QbR>L=A1n4LB8^B=X~{n*&8eSqvK)UW=Q!B2S#S% zyjUsVnnvKhO004Jt&nfV6nEG}5-4*;`pfYH$tKtACba`AD`QdT%tyUXLS}yMQm0 zN{tAFC5RAuNfhe)0m&DzTP)XrrmTju#K=l!Wt36>Hg~zfPjl z+n|oMAG8EFadWlD!L1|5L&m0Wr#i+7mED^Z8gVy8xY<~Nhc(=@#pSn@Igmug+AdzD zFG|fS1a+aBpy@9~bH(LS_)ilJm8ZmC?Xk!=L}DtPdxqOyuR_XCX}O{$3=UREOAJluh|Ev7l6)u6!s z@#^gd*|~4t*GPcW0IE5#3Z|o_$W5ij>3xjBTIy^){>iMM$?2>2i4;k-Ob^}~Qg&4d!KMDl;K9CsefGPFcI=PJ%NU~9eyL69k5xat(cvtHHq*8W+ zJkEYi;TFXlcOyn-%gLw;0pgXJj(fuD6k3I7nBzKB;9mu&I|U-($3kTG%O7bR>hn)Y zcDGoQ1*9R&mK%(E=)Ke&jr`}c#@5C8k%CUFbX0=PNDxb9A1xILjB^T0Y}g(`rdEii zHBP^UxU=5}Jz4J_eDtA)M<60xRsjJS=%IGQ+mfdlzZ+keFDVH`v^VI4z=qISl4=<1 z=dPg9XMF^u?bs;QFXr9Ndnnz_cXsUXx$swBLoNw>iw6Qr4HL>PfSQDwN`MrZ6bM4< zQ$l;kH3aIiFc>leKxW=jvK-#KWvlA`=7FNM+WW-rD{+^F7!q4fhxP`R2zBB?+RfiF zHun@8lIQIj(06V6r`k;Wz-A3Ee%4??ZG)b5o;?m053oWV5udHOYHd=WkATp>ZZ<{3 z_BK!Yzwo{kc&y=?#hmzSIJzH1V$lFl;$$Ga#;4T}UW7XpU>naUiyLNpoW0tXR*qN2 z-f1?@RR>8|k2-t3CqHv6Q}O5JY>(^4=Z+cie=3C<;=b3K095ff^q??WrvDKZ9c3iD z$)|>eS&2dg3xBV$6Z#Hq9V1h)hq%jf_ngyOmvm~IFSPg0=r^jB$<27-bG+BfVTAyz znZf|eI}CxSs7LY3b+F2#UsEEk7_bBfgw@zh^)u`1Dx<&Rx7bJB**I9Np8vAVVlx{Z zkf#W08()KV$ITOGyEh)7K#_Apu{-kKo%4SOjC`0#?j)iK+R+<&ZGP)-6FhV6@(%ge z8gy1?0uP?L_|3Mh zUJBum2=O4$bl2*SJX-|<1%)xj*ZXkgqa~>xH>8EXoQTc=F6tD zXR;KIt&OI+OW)j3pZXeECpAs>az1~4}Rc-cOh0|z23RA6paU~w!;!F2-ro$US1 z95jmBw-jf^KBP^7@C>e?b|qo%8YWnSkPuD;rUOvuJjg7>5H={q#DX#^CHX5Agh^!Y z94V&wfT(3KqE01OJv(2`cL|8bU~9 zKoL#g7c?M#ELdnJO2JVj(O<#2|+F<8dG+(VanU;%6Kxjnn#*EcCP!jh`(Q zdV|A^?gOgLZS}H3#f6{%tBKv_M@)(1(?#RjjpJ2dbRK5xd7Mz2><8*GI0KG(ORM99 zmiCtgkO*?}LVxno>dD12h&F2CW7dROmG&EBbe!fRc*@x7h4#FC=7Z5DKCUsbQWabO zdt&IxYoXkq-Q~Yh+rBMNz*in0AHhAq=w-$3Cpjt~&FU+VIxzDooEDsFMu$2~rz}-x zRd$+aHcI4A7Y)-?{jHA$4F%*d-DbE2mMge%xOj2!s<~aX+&nb#_h$UMC zP-P7T%mC07+5`y=E4mFrYsZ{~K>4G9c73iqL%CNEHPE! zQ|j<&+V(i$vhLO8v_PPt)bYq%Zo$0doS_pjvmD%oHj2szzzb0T5)AM=D188}uWXE_ z<$%)-u@Kk+%A&DACCJk0-_4o$_sqX^>L(;BZSE&o>DII3!AV_CO8J{G3Qs{zJ{uXJ z>3u-Pc_0P~!_X-lW*?1UfQwQ9s^brn4L~ZibA>8UhDxLK_@@48BgMShoRizxQaGLC zr@30i>LXDG`n;rHw-1&=}W$T`9`omWd!T43!rSG~i4q zfZAH_fSr;$sV=izA}(4}J)R}WvgOg8EQi`5$1-@E3OtH6N(_#UmjhAj%wyEh^y^D7 z3OFNb1==u!QVIR1ds9Z54XiAFa$V*MGs9=DD*%N|t&Vp_%d%#YKReBs&2O7QRAweU zfzWm|45iOZfo8=SaG!vMhS1OkV+;cX!~hnbdYBwv+6C^4lcL6+Po3g7>Yqc2lA1JW!)&%XgmKUXfw5Fg1r?PgDz4dmh*|g=bnTDZ-rYc;^YsV;k z^VOochErrJqJaXb3E3kEh8jAS;JcdQKluCvKm=(d_Trw^^|Ji2*o z`p{x2{c+5>ol=ieM-%IOJ921UV0|RSbL9q-~Z37 zw+-c|lK1LK-QnppqwaQijp^>4-`y~vX7+(xO!@xSm;I&Co&<~cZwS6FZS5yH_asZ( z&p5n~DyP|h`BgdVeTE^;q0+v|$a}Isl;1vnWe4@--RL@wH&$fMo+)`#y=h#$GFC?@6|8Ze`{s)Y^s(_h4|@ zDq%O6cMDB+$jo&3IBiGu^G@@e?V_cFUfuKHIU9rY!;ZSb&WZBIK_a(4iJrli0>=(* z5st5Sx4rN0ZE_vSiXV9?9_0*SF0S8B|FIIbu#q%4blx3)U0yO`u{=6hG8VMcLea&kg$8wYKR8rMFx zd4GH|;WEnU0-ONbe_h-<%pvo*7>qo2<1x!y7|Y*9j}l#oN(ilxR8%xII1kvJx)Zu;hd{c7w&3c*4n1 zn2<~uoz4Of0H_UsVL>=?jyrmHol`0^FZqAyw_WrJxM@Y|&ZJ*nkg@$MVWMhhsgYp> zuIR;MFdkHi`N9LCp+GO@!6mREmdA!93^5g+QHT@c5DF?a6$riP6}irJXyvt^tw%dA z!?XuA9nSkzkIN^5>km&oRS(zhvLDHKBbb6`_i8r?DIkli@L5@Nj1Jm^J*x9DK+UmE zFhtJEupov)LjlmGIViy;GH(!&{i(5-h7 zbgm$JESeX&PLINrncipMzyLacucxqouuEGzNU#^3d)Mp4`Er*HK~K>X$X63+K6?(n zEHFAVJ`cH5h=JOH7~xkKtf?|Z=M*iPmtt+c2hFh$K9|FosDWxf0pcgKCNdyAjKPSZ z(A09)RI(vX%oPNix`a+)m@^H;8015AfDM-*Rydko24bn=t2_lNqFs!8{&Pe8PrWcH zMSC0+6%h2$>reSv-Vg}7$}+O{(<0)WM8XGyHb^rE=rUo`52HTMqe&Q|oE%o>VNtwM z-P}kTGzpgkm_kgs85~LqBbj8$mFgb|4+nR1Wb>d%AOr&XLf7kkP$d#Uru5sHC_*LB z3me)$u3+5^&ij#33fzF@$3n$OD&bcb{GUrTGQ=NO72AHPBpDDz3$)x$rZ6hwx((V9 z#5KJ5l+5bS^H27DXQa_-$xw-`?e*Ts$^EN4OPxMf3E`vk^&!ndh4|~gqec;PwZ{L{ zrc7Yyh&wf%VT9G8doFi2fnGH%}*&JYp#vH z#rn17B9({&lAAK;1T!9P9W=cBWu1$E~5j+O#6>Yq9h{G8Hw-8ic6?uM*%E}QH_1U%|QX@rwo6A4>$K?TnMMC+p`WcANhnb zkrs-wF?MJq3J-RPp{>Y8bqp7ffdLLQ8Syj2JtMN@VdHQbKSrCUjJs`?y1AsYIRFv7 zfyEwdT!p1jY)M3d5$*h@3Q8VTSVdB~FgPNQ7X{%$c>KaXF^~I2$w=&41u)vmVC}(A zT028Y!2BneK*>Z*#GCi|t(zI+_q%9{PvY%)i>UI6DhqInKluthyqGy!y(er{P(Wcr zZL6!{Z&X&eq8Gn^^lJx(65aTXa7K;z4X~1sna&I&O3R3b5rvbUb)}AWJ8>s^x+kji z*lb~nv8kaD?PPT#L&HSz@VD3yiOSMyGi|GbFuolf6T3SC^E2HWeck$J9u9B4SDzcA z_q(jfJfSAF_+Ha-7SH+iZd{nN~k{r&yJ+K-1jo91_&SGPRr zq?N$pxP(r0Mpy3HH@i)g$MbGH{r#HYornh*gvo$U@= ziWP@L|L@DaKYyAVqjdi)HDy%D1$b!M$>DV;@MZTee;)SYealnKW((izKR7k#5GHQ; z@H6fc`x)zGK(Z#iB@O)kRT5#4`z`a8KZKG*>9F8Gzd7JWk9r4DlWsIDlK}N~nqcZ7 zSb(JfB22F|=H@ED-$im}Rna|fJ`faeih_soEBD)rIsnUzSvJOnf1wIb0G&RFC0v2v zY%e0?-RASxnLX4okAldVHfCSR&~cExi?azMXK2$?Tyzr?za96Y@PY>2GIiplA;b=S z2TvEQLEZ7wGG%6d*>~YX8Ly%|LM)36Q4w!UN(k8Ik}Ir{&}&TIR^Kw1yMB)p*o_y; zF+$Aoq8Vp&W4y*n7(Q+SfG8@^1+TtiDbtarkX?~@+{djnv>tAL)|m1BkF`jJ&9EgE zwNj-1GUXO8_Q8$qV_r2)utv17KuAKCK^Tu^;X)|Ll3L`Mt<|Gk zy63B!OVyXOU5^TRDf`F4ucn)!g`m5nRXYD<#EIf*C2URt`s}~Z2<4V4^8FCSrYeKP zwDH%s@+X5I8J)F^VwbN(!|digZjyQ9Iq&NYd_9F`9}ot5GV=pD=; zG)@;`+W$re<2Nm*X_NgVEP@{WxmJoblx<$-cG~+4w)^o_qzyJja0=^2QCY*0j| zSFq^DLa!Ti(_l!5k4pu2x%Fz^6ZI{9K+m132}rxTI{m(7$ufZxCI4|NVxe-))y7p& z5(!HDCp6`xHW`ux#FFv6neo zd}R0Z6`N??$A)_t3oi_p1$I_So}7 z=V;BpyCjN=U_B&V-q!DfuZwTIK&~J(IqQzp8yU{O|0bPLfk)K*T{;$1d9WH39jkg; zg9kS88$TWk3HR5Bgf-?wj{)**XmwpZj?i+n6~C1zsGbcS9ub$%|1$AUWQHS6HezP? z)WP##A3b#(m_P8|qa(iWE#LR3Y=LJ@3P)~n{q7dqUBr|+Db-0HIX{4~h0h&;Tthkc z|4BNAjP!<1z(rG)ocD3vzI0PRi_O*_XuVCar78q}DZ~(TC_x!Dq-dxEfc{1i;$zyi zM#k_{r-7!^PsB1wzy)RJ1>4vO%UulP`d5ZC&O9rjwa-!oZ$02#nOI1i0${9yU@@^5 zg)(ttucg9hWzi5U);kl8kVH}!LK#y5NIUYUSQrG3^`1gPB(X_S{aQ(@jNb*r6#1@+#Zm^v@I&JZR|6kYrpR$~_dwxY1<$ z*g#1rJP1o61Q2nAfa8$#K1hiEC!r%G5J%ASnA2ej${?xy+u#!d)#sACj_~M%c@RJx z0UA*>qu2A4R3tTyn9C7OB8f@$!N7C3?j-J36T*m&RNKjk!wqp@ic7v~b`f4!dv3y(OiLuwvtCCvZg3 zD(LiM9ZQs#Ih3%-M|zLmwxb~=7-b8!7gB1A5dK$h^i zI1;Rt2F5;D%-n!L`v{MUy#fd!a0UHMr1G;7MlI_jgEBocYtzNm<;pSyHxd}bBF{=z ziqxq+uyWdI&svF06&cNl2MnoZJd0U3#JIn7EEABa)NdzWB*B%oqYY4fSDav?01dVG z{m9f(Wi;{?R|sKL@hfqt^(x%c3}9-|QNYD)zh&pjWoIE!`8|M5zK8-(6>pi7FgZfG zF3^lNSdvO?uYM$jEB8&7rA{sq53`+7-|TWS_I#nRC%I=VUx7wph-7p*lc{3s%4Dmk z-;9%+hwPQDl$F3?G~8$~b_Ak@f5f>M17OiQj_4{Zp8`h+2{)QBQA5WE158;uSRld1 zFh@OJu0nJwFOonk4##>h@IPjV4r#CGYpRWJM~Uq z-@!Q0LovX(50JPjC>@2-ijKL%F$_Kmnp|MI?O4VdHm)cPNgU?i$4?q4h$|JO6|$y$ zaljySOB)N+k%igEfebgGIzm`dp9uT05HgAh_kJ{maoCn`U)>!+u3QYb9m$AN_+$)X z6vbXg_mQ>>D3Rc|n!XjeQe_s$FqTm1d0M=pE}F$D3z*pD8tI%dl8XxLQ0fd@sabD4h5eV6W{w^W+28Xgib>5eZEjD zG@)$O-gb3}l|YlHfRdx_(|}hF;Z>EZ*1jGXqEuj?HGw`A+f4_fYe4@igb)q4@-U7{ zQAha;LR{-Y!NWpaLE)Q}TmUzg2wUUK!)0-l-=n_XbLUl`aLt+2(MI&%LeKFmi*U-U zU;>w8fdMN`Zd&v4FeFj9reohs5<|?t$LY1aGA#tstDU`1uqjPGc~&|3;dn{$Sbxgt zL;kVs3w&)3JUx!iIiuDwb`?f~CFYa|T3PE>f^t6mz$r>QZfn#n=sMRutHa#BGwZd3 zNP^L!?Ipu&AqfK{R?vB3Gz4X`P<&^1&PlQH&d1KWr4Qe>M~;uC>kfG9Q|<43$1NH= zVu`MgdqbR!%_cCePT)cSr^kGj&A(J8d|p?3t|;;++bBb=wmjKU*s|V|1k=nIk2o>E z4iPanBNOvFCb&q6w|CxpU-oz6C3t>vD`f9*@cSnX5kT)dwydsc_R$s}r&($ii~}_ zg32Wxv0xlPY|bF5`Dx zW9(fq``FlVbwX-`(rXulehEYr?cy2(gVOG5}E)=d?i!=U@@t0Fmu6yQ$0ZTm$FZMlt&HKhIn_GS2=EzFzQw zQ4IlzM1VjBU4eTITaj6P4A4riL;mcEwMh4v+l@)C{G zlV+`!8HLbP1>0s-C&@=oXY>OvRL#@mGbA%h)9eRkO^Q4;nhfk(nl*cAwLa3?k4rwz zNPpaxrnT3sect?Rw)sg4tq!55agU^KaGEarr4IY0QBk`3!^?FZclo>hn+Zn;L3yG^ z-*3CS89r~|SQ6EbY_Zbv+)4JFWji(cG%2?x^vtE@S-tzQxs>tq0s6>y=7cH6^vw5j zJPn>4%Mh}eA=aPIe|}crDOd2!^45oYwde}T)|U^ZtsY8K>`6syc%nYxUp{+2W&Lso zC2?XyUtxAjB7y-4N^P|-kbYI+WzUQ~*ZWbF?J4MBZTiyFrsSB>!OJe^%n8y8Q$*NT zOS@J}J9Pc{^1aFXtsMzYZy9zP7XbZ3-rvE`)8K^P`m0}=w};-n#stg6zie;fX`L8p%M?`2dP?AV zD1&y10AIWYCs%u|5 zoMH z6rlOm2MX?5qV83A%^E`?(6{N=k5#5l#nuw4XclA0GVxf7F@T;MsSO8+;y_w+zZp{h zjr@*Lls-Q5-(xjREU#L6C)?$&{JHSI&Gb|g4t*-OR3i&PkTCS$z!x7_NC%)u#$p19 z#~?%&i!ehgTZJJ=z72AK@3kW1TCo=50C6RnAh(b(SFQ1ips6ATA$t2=K0JByXX*r) zNT@J9xg0R-v#rENuLD;3|Ax;3L@6>v8_MDwgAl^1&U9K&{C=6?|DOCh#-+0-#J6wW z$BbI`1#`FHE!j$eKNTfjA9FNH)kYk^+yi%&3GJ-f!GNMH_AyjJVGLl*1d$5$!H(zAQojbpq&(Hazmt8BsbPnj#S!F-O2v=|Hqxu_FjgKj%qA z-J{Q5gKa;5vQ5PGKgj#bptj=nZ}d$oev*($;&pdDTn=^BMvnE+HnXJs3HJMD-^}Rk9HZqO{F^L_b z{I1-rMJOUQ`_S)($R0_(uDK13aR8r^{jHx z@Kv;2XtJzodV>N%pGj;4Kg;3By}o6#KI!fW3FKva ztsM)1=0W&yJABjqKoK7We_yDfyFsg48k&qdR&k!@+oou8WQ}*e_a4;iMt;cyQWO1lp`~JROKRmW0K>t z?`f47e<;%v4uR~KwL|-*!nN$r5(1A{uQ$tnaos*|*}#nMv&;=9H&iPQAu|snjwZvM zL`MFyhJTBEqTr8=g6bINhWz89dL2dn-hM4a(2@f?k`Bh14-q)R*IReg_-*CyhVPCkN9H9q?d_ip_KOw> z!nwZNnJ=FFAi`Q4Y&1A}f5dgJ@8CLcCw1;xvR_DjHuM{>HNOU@Ghw?&nSc3jB)KBz zdGz7-z0-)%>b-jrt*e=P3Y@SVP^)>oe6>o1Bf;61p8la|aMcsNd_3*YxO_90D{bJH z6O8`gwp_JAdwtk!(0ZF27TV%HpY2NLy=xbG_u*^BeL((_>}5sJd7 z{Nf013kM0l{JdEn8^Y_kUd~4$OGy9;c{-7!eTfxML(KY-`Q8%8->~i9vP~#mct(fB z3t3h^LHB3D1NcM2a&gK;u55o(NG)`F^0g*xWJu_-eg(@<3C2a)vSdE70Lkar$1yuE z^%JRr6lveZxtilKQ(P!2CiZJPC=36l2hK}LZ)ke4kmS*#1?~Fz-@74vml&9zi1foX z;^T$CZmLgl@JrGW#woAoD=rnsX+4*Z65(WJr@d+6%B_j7@GO|)bNKP{-$^VJIFIRk zPWe^Zmn0vOO>QLR@RPo?@Y>BS!FeoYdZzQF!E#nUNk5eoBHff}&jQ>xFtE$pg!JXV z+b_CAF)(eOWNZnui&(@WJt`A2Wt8m1*KtPi+8DAfHn$}X9magNYqRb)*(FYIG!c`! zIR}63WXAlof|H)+K%W(17dxnjW1fMjPEhiKX^(x-U-N0F0T?_B+Ho;;`Q*L;W$iQ_ z!W6v%-oqUXzBQfHHzb8mGdWdVxO6kN>k6K2IcUDVMgRJGq*u(s>ZmQhr~57DFYrg& zj&Am#N}^4?dHQBHUow_DjWH=pws4;CACV4)f5fG>6@_}LFJCdeyI{`m2{RA}85C5L ze)WvzHf)WUZHagmTozSq1ay+AiG#f-{$$EofVnMiqX3|N5+AS7niC_{~J6i}NA%#qG470#R8~`XQA?8Bue`aaGX; zucN=J;p_L8H960Ygu?e#?INDnh5y}m%@zMKm1fh_U;p;?7n7w;F`v4H_R4q6)Jr=t z`Ay7@(MRNM^R`Fce}+~R2Yr&`>W+~} zJM-wA4nNH3wSX;sp4I=2|xvbGP<)MAZO6lb6bC2`-u!+TN)Gb_S{^b+RmQ^9a zU}b$xoOJqcwjbm}a+mLt{djBzXLp)vUP4+RsKfhlkw!2KH)N?KG-jo{g|eXaD{p~Mb%|3F$7E4%SuK}TLC$xF`WPw(az8lE@4;B|5D4E;r0`MgwF z;`fVu@zK4Ni$=}pLxC^pD~z01kz*Cgwid=yqjX*ktq-oQcWHH3l~)mFna8C^>1%5% z-fhe3JE4T8E9+9%y-(xag6!C5C{pX?9o1D{*OJZq)A_xheXxnDT4JJ-_6yQ=IrC%} z-!}Mg6T0f6oK(BC^%U(f$SviL3`*U3dDU3?somrKRok*K*83ji_LDUF#XqT;w-kv1 zZiPWqD?tADv!OA61E@0(?mv6a)OWZya4hYrqZ@Cgt;{d@x@rHIy83$PM_>MW`>>$% zsbzTi?L}v&zxb=D;sr?t4*K@My3%yT#XF-{F`3ZD;73lUQb%zf>fcgqim`;7)yqc5ufH^%@X7#iCR_!teNt^Rk! z4`Pi4{iy=-#y+U6{T(MPwFf}p0$_PJ@Ue0=SRDqISp&avgQ$;#4No8z;}9!1OjRvR zhZ9WMaZL9UOfNSqyK1bDu@IyHwt*XV_z5`r1lvgq=XETOg%-rJ7^A=qx1<`k;sm!w z09x+`ZLWs4pFq0=@Os_w2CDH!PVgoK@TcAI=d1CTPw>|T2)5h^cB=^vPY6y02rt|S zZ>k9&P6&a5Fo-)0rv`?93L_FEB6TOCtRbR3C1MmLW_2g#tRdz-B^DHX^345-Xw4JJ z(Ngqz4l7|cY-kB^B7`j43g0)eowum}hUNXeOdX7l=%>Lgt4bSgy-8}wFsQuskG=G2p|N3eFCDf2e)_zv^z`)d^78if_Tk~-zc$zZ z*T??@*!BPWFa5v4%Ki^$_y6Xw|L6Dq_Y(Z?AN{!hXrLMW|D}P(7aIM)479B1G+xKS zG7Elle5%Wn`PMI**%E1gFU~)8R#~9$|J^)bF?_toS7Hdj9K13QJi+PDc@L2)UZHL| z*p#PP6a@zpNP0dRXbhh@Hp0a8Q0rkFg(7+Q6vaCmk&@NezoVF~Eis}+0_8nq#w=LqrRe+*dg5(wRp}cJi4-7dY}6 zStE)w<8-Kqb4hAo9EvSq;7(B>Q;&nv=NUh?LPq7ZlG17obi|*6vQe(d5@Np=Rz)kh zSU3hO*0@xrDc~#Dx5V$b@nsc`w{Zmn3k%NWb$rG;H3ZW{G8i}#u&zW*{L>#zoALf~ zFGX1%TW?wj(qRp>v?jdYhs5rPYR9ToODBVW3`BN zu%{|pvATxh+R&5!Rqb@^`$TmN$Tq=F`h63j0Xn0l@g6Kv#-1U8ALTuR-8ww6gH%>D z-v?;Jmjr*(`Yk;jqq(6G9;d-E6P}=^%oLuaUT=d9(P_J!kAQ-{op=1Zgr2lnYTkME zS@}!GPvIJk#4S9}|2Q$j?jw0NCqApvIB)YO`l`@K5#7@O7q4cxUJE)F^BQrvIwV?> zUF6+gc6YjeI;blC&k;rTFrw0p_w+zOHjQw!w``IoDjou-7g{Aq@92snq~p+%K`G~XP5UKh6}bZ?S9c_4f*-KGqq z6p|zKu4baIQ^J;MlHn$E57&^hKJQKu;!vnz(=}Bht?`wQu&H4(nX)FIzwV{sstH%V zUwJY%mmth#6aCitx7>u_eeUu?qSPgxTmYrBB(g8{H`D>Z%ZsPe`9iro78 znzJ$di3{;&`Y*g9QP~xU0-Po(l_b(SEGq-}kLTs5uh@%nZ;#`gSaZPHkqXDmp z(XjbUOfK!N{ij`{(eL`c6$Q4|*`kIh^vos0rwb@XaBwnbReifN%TDx~qOcht#V*1)TpU5X$Vq%tXfPx4qRlT1bTDfew zYUe8kykAvaz3oVbkXKg)TybOEpIvnxw*#O6wY3Y}CKTk7KM<#U4T8Qx5d@qdnA=t{ zb>}+BIs`s4mapGhGvIH%8X$~!3n5Ow?%~j=i7?0r#*Ovu6^J>F2pG@vYS2m_ zQmM`eotbugRxk3b*09ytjPEB5UFRov?mNw6EE+0Y?iF+H;PB_obGzCxz2I|-3!Zt& zS=~0D!dk~eiS5kWc@_%=PfyYZQz81a$zQg@)~6Y9>r!HqNuwUQiY^N>2~_pLe21ww zmpfs3D&wVm=Qai|dxlPAziz$?>kcgL33ZbV3zME#K3g_5Wfz++V5_L|KiqlQB{n}^ zcu`^-?d-sv{(WMGvn;jUrB;f1ac!m{SKh_R$Be#=}fnNKshYB=38z4869wjTx{ECsS=*ZG`~3J z)8FkLxs{2HTwJDNX>4nES^D_zEVk&wCeGFE_lcN`P({gMZ|UbNPyczfEy=%oI1qim6RmOy{|=Xm8t{fi_Mjos5f?*Fbc*rjG) z{`|0IqUrx*KjS#!>TX@W%6+ny?jV*h;E+iAPQxi{xr|WqK#|{n%fI7lB)RRcS*OpE z0n5!V`i|p^qYuBHc22ZfNTL5;xI8R9mAoAL^zkn*+J9Uz>tUJjeeICuzjL(Y91^SZ zH&XNCkzF>L)yMzuPt~7TS^%t1=syA7SO~mg#6vQottjB)1p-{`f2a82Un7Eo;*-|Z z#}l%E+hjiy5u&+J1RDi{_!>#x^zpdp<3@5IsYqb40fN*E!Iu)yYv*536ezlbz^6b; zrg#g91d%lb+4#KI3k_I#5g^|f$WIYGs~FU-ANZmvSYQsR5f(&wjbOhH+W+}Zb_b@n z6Jk6U$T1fHZwj&NN9z9!7Nq!W#uVaw<^9$G=4J9Tv-{t4a34DDeG zeXjIbA3KCG?6YR^XEAhtm~Am~TQMxiI!wnm)Y~u=TodZc;gdQSD#Yr6C=Na=diT{A z_GjAag4E)?AmS*|sU2p$QBL^ly79mPDF2U#1G#{ zdhdv!Gi#$hYre}U0e#y^U+XEuXx14Jq&j+OE_&rUdW|AxQ3)tN4&L^S*-44nYl=A# z0SlvIPAI;d?!=rKez}|j^Jv9fHGSFd2hqbo0Loa9Xe`($7IPQ)>~l2dqaG7xi|b%* z(iep*_$aU;-ShyU*f^@@IGXvmM*|Hc0MtTYvKYm)`TfU0;|d3h!Q+jq!HU&j&Rq}> z0{A=}7>NE25^#fXP-0rU#VehF^|Q0DIt5O=8)Y&rXS}{q@+(eEO)an# z3MA;3Kmh}D8YKkmCYvDQt0}6D>v4zKNz;<3uQJITD8_B`Ns~u(G>yCV$6C_QXkR2*;EaNC{}h;N%3$0G`Ug zz;FOq;v|8c6Wqm=iu(kJe@=YeyZQ?p*SgummR&!GHToPt|)}deCadW1L z7Fb#r6tT2;9*1--OAJ^J z0M7&HHDAb?3Vu$BAyW;uVJOIQh?jxq@*==Gj`4bq1=JA*SL7ZH=$14ls&F&xm)2s1 z-;E0OOTZ3$$=5fp^&E=4!ZY3(=McqWXd5SHz0U~R1-*3pwzT_=VqBE)1f-!=92!x) zT2j1Y6anfl*xq$X7D)nWCt+A&B&d|QJCwW%FM7TUlF0%0Ig|?DmOy_2$OXXv91@G( z$Lk1yMFdiWPx2aXiddB~-kM;$@jn@ zszinJC;9TH8!+nhVPcrWLCu@9J{)lS;Ugnfy{j=G(U-F);puY)v|XT3g9W!D4RN zvX&VK{^?dZ7-Xus`z_6bPMv1qoh-MxK^$=p;vQ2_!x`>+CS^>~oDRIUCVUhKb=Qq= z+M%*mO`!$Og^r0gmTPtA`*y!i>E>FjS{Z8E;_f9MY!QTG zfK0mZOCdbf0QgXs>~Nb8w_)i(nF^k-1{4JxjssW6X+?eGQ0{H!{z=Y-xvAPIv6Q1} z`PjiIBnZ$eYG$l&Z8k=?mICmmw zN_d()RsH+eo%(%K`rjD=NG%gcs~i2C)8$a21hBefng-#vX6Dpy{oG9y)j+G^T=|So zEZFVY`u)vf?L*ds49)#7lz}lzgJ{YDlEI&BqC+|=BL}z;>+xhktLouv7BFVRbX*q+6S+3b)Y;`TFQ z2=o{xpn`+eB}T&xM}vk72+|!cO#sIbAUL>!QSzr}!(WOtJ~Q)iFwJvPOAo53k5C`FZTrE=2@w&1G^gjX=6cJ1l%mDt zS%aKVE;W`c6NxWSq9eyQ)D{h{?S`Wl9~&X|=CjPBflm;5=N}+l#6XZ)QGwaW#mA8b z(N6E?fuYEy*p|-{Pf&<5HA>>v#LL^ zO3ko%w6}WYINfKkVno}&$0I(Y3Md&~fsU>vr*%!HE|Yd-F16Q6siUL?R;k}Ena%@o zVT;tz4R*ebk3Ng6M*{<9(#0kKGRuS$i9*+M@VW_5Ok?$j*^eKY1MDS`%1EHN)|Qh= z8g-5U{B7CGmDvc2VU~{B=IFkP_(rv(%|6^-WmpGA&bq$T(zBS{pmxYYBv1^n{c-rW zspvM&qga8q+}_CC_8aNMc)MYrwcQzo;*V>@FdHep~b?m3@T&D=(!@bP|mDH-6QvY~)7)Ckor0zTh0sI?Y z)ODuV?Q+F+5#ddoYtIM5%K-F}Bk5gQcAkb-g5dZ~m*zMQY^a^^K4aGGGqJ!d%zwcfcE1Vnq6G4qBUX5{n_W(dD ztf7!LhIouAFnnSHP8z9T3hfQK2u`Uv%f$aN&?EzFY=1zN;#npC`M!QM(2{t~I)W*6 zx7E^~xvYT0iP`&Kkm?-$-GMNV=R5(^$fxP;J{o8cH3XBE8UY>!jH@T@*?uG|jF=@h z_tn8;*10j^q^|jmkW?JIa)0FA;lv?NUzGA%!h# zzVCctJ>Td9rN@Qh@zzZp?i+i7@Or4}u-vM1sZ1(s_6lVMWFX|GC#dd9C!SX9#{J5d z1v!&}Qe@u{UvfAolq``fVt=8}iB%f%%RUy<_?OV^V3p9)2qhx?cH+6*LFCG>_#rM^z2nekY|yV~tZ-+< zkKlJpf+#vl?pQ&V=x?+8pKF`6m|KE$i8H#I~u)W(ZP zE->fDprpCDUWRSSv)}oA`M{$_gnZ~+LswJ&*>@e+g(re+f*!JuRp zhw-!g=enBVrS=rZlfNp5t<*K8C6Cm#KM4!;>lGC=7-#6$mrQq?dp*-Pl7y-o2uFpC zO*|Q4ja!-vwnPq6AfsI(Ft2r!jI`*fNd=uNwi+!yy;w+Qa|z$@HgWCr;xaXxenq%U zK>PX2`mdg*F&*F6)Pa?}$@S#TT0}#Q)d)0evyHA|l7nRqF59(baH@`>cDQR$Q+wKU z`b|T|>({lfQ5g_QW6^?V&Gz}na-wXBKGuG=$rc?a8=mL_zS&t4S2ou_b9GV`cG;Yu zmg?AWm0SDTv83{Yyt zI&wP+_dKNa9b4n#XX%XL9+HEl0U-8JQPHxzB?oU8n?&3)dE zT+};*snOxx05{q@uoQPLLxQYB<%8oZ|J%KEbp&ahYKnoiN`t#t#Lc_L6+;M2?@A z!bH#hy33HPGY6mF$9W;Y5isI657siqvqS{rFq7~0k7?y_Ip{HeX!{Oew-gHRG&N)c z`gBVa_H*OHkerNx@)`E0Y?U^)!kUngFDUdG>-Q*DUP)a5u9khgFHSec?eC#+_wQEQ z!)XGkbHU2(@UMp@n|EK0+9}>>$HpYQks>S^Of~$)I@ebCT>2^E+lO=EhL_f&FO(I< z2&`0IiMas@K>@EdLwX?FEiv&(NRN=SUaaj}JT#R<#Yl*cKB*o-v`Pwzpts(uoVTtfP z2Yuzfi5RdKt1PLbcB`yT1}$l@y4MJCy4_42%e3^X`qFprTI4Hc5-m)(!VF%4W~w5* z^qu7+P27X^)k{gLi$%-*%az~}3q5)0XI*7xtWfcvJJT#5mdh-pW%X<15;NRQi>)ZB zT6%S=YM%M=s56Nh&Fs~Em+Bf-_ra>qcdG?}A<;%2CjeUC8DCsT;V>~bS=VhrU70wq zx{i3lbi+(@>BH`VB{k!G{Feq$bpgbUx_)u07}$|$;pYEr!E*1gDOmMLEr~l}TWYqQ z-=WG@HE@3DmNXvgx3hlK2`O2%XHyR&kNH%@6uK{zL2KM_kIp)fFh!_c$jDV~;m#bl zjHTn3nT{8yl8Nwh*3K=3KxFUbBMmS1x*C-+5CYtwIK?mcJdSJCrh_Ls#}QH zeH=Z@I<7S4yVwv38zyp59MTiCdt74MCexE#h}!3OU#sB3I3%b+gT8l3^-)_;%N&qN zgAnAiC0HnVe)thHBZB`028mB$%dnIBaWnD}(=!#VwoQ7+ zF#e9%~xZ>*9IN0SNNWoQ`3#DgRSNbt4Y`OS;P`MVNC zZRSU$jq2jUY~OPN&jgpZJ|>G4C*DmJ>Mrid>GPX!o_|ES965C|V9+2M?`Qz7?`IcR zuKIl1pSn4jU8PSy;sTv$NR}hsH>)Wu%`GR{2W|X2m}5sP%G-mQx}C=o8pe7$4QkUr z!?o#kWzmdr{DvShty$m8+Z41^TkS(k`1@ydepl1ssUm{1M@_f~)r5%3O zgA3WM*A(~y!m#We&&m|awlsg*z1~zMz8!5Wjvcn_o)Y~92IT&slqMh7LZx56=$(OwLkj5>O+G~-z!;t(}OZh+zJ*I9yW8!V%%DGU@!I*-_D53C7;Fz0y zC)|SAo`l3x5gWJVd_Z_M5h|GhmDFE@e^5g3RrK&WX_2Z-O(hU@Cp8XgEpdnUm6eix z=om;Br~E2I)z(UQpMV?oZDf!I%Mju2$c;`wVGGM(ez=o?hx`0Hir*$&z%*Qt<;Mr1A2+f;aJyPM10xx(ai^(U zC#m^~?nx_R0~tkH>e8_>Ri*#tbxH`o+}ZnqasMN+xu+vO5qc{@SjLQpT@$G)N3YkT zVAF%7z{gvY0eUb>rPToLj=zRdAx6o<{45J&eWE5Vn#L1);3TfK%8oXtSz4Sn<>hYi9o@)o^E$Z zvcC%P{qo7b`dpTJE90d#@#K!QWHDer>+A!oa*#H9y+Jf$XefV0&X<>dKf?rokk}%p z58~+Z7RCAB3InN%v2%EEO1YOdP+2vAoLaBEoFapBFETd%w|{?_8v<-E9`To`skMJ_ zsDEj`|I3aNw50%nS6*db!}axtsEYE}Vccd*VBHfM?hR(uo<5mU<)K#P(}97!6D8bY zK)xfC#utEP$;3U}-KC?rz>rMVh2XST4zsTV$8zwO55^QLVh#Q*o+>K&n$x%$mI>|( z)KMZ~X{(lt%R7*+fsv7@sLtHsmd5`qiV|xH7Hi#PY+J%^Um7wc9>&6};1!mvuvaV- zQ40kiee1|2Ov9T#NdJry@26oNn8F?`SIaP=s~#R6nNka3z+mJ6)ooI83jwiGxX`J= z32HW#&2sAcIwSJdBT;T6?ZN7(xRG3*k{Y!#KAMqDJe>8mwv7)hzoS_9O*LT7qgBD9 z9`U1dazWf$gYR-R-Y}}0_GsWRYaH^h9JR3>Pw_l&Q#+#>dqYxzU5)rl@-p38Z8lyr zIDhQZ_p!mvp)(Qz)O~X~0|q-bh^!cZTRje~0FYsW*a7?^LXzz{-J3>Znhu%?0wV() zS{Tt|Q7K|EYFZq-g>efEWC(zVu+n3p6o~*)^p7)e1hN1o8F&M6DgcyjAi%|Va2@f# zmNX)l1Y+LsCtEe7>JwCATA%k7-A;ulw-{19M<3KmkT49!901Ll7H=36ny#2a&@GS} z_VnazXvJi*3L8q)F1!DAG9hP@%QlCbw3M8d6OTcmLJisuS9mn&MM-|q=OCB_q6MlE zIZ+4>25dnF5bike>{mD8Lqrt{sZs#q2$OH1$eAruD*FA4_WV~BXbQKcfPS!w>V`&c zWWtstOrRQxYX~x^LvW<&g&2Ym34wpV5^-__zLuY{c{O8eJ7d!hSo#~G(HoDn4=hz; zG%61xv`0XSd-iCwT2x-qnoPGlP9M3wG+h2qSGjTTg#2)9Er~8}7$@@jRmfwN5H3{k zHI6%OCg5WB6J!qY3K04=uv8#$d3dI3NB^?t*HurAB>wCnv}7trwk`e{G$Cg- zE`l-fsY9=DCgN#i+=+s2n*z1l@rC9BS=Nxa2n0Hb7JGPlY3*VbQq3CwcPSjO3{Ej= zyw^|UHoIB?((WL43s&~(SN3~X4yKW~u`34`E5{?|)F{9S>+0Ddkb39Y>!;8EYFk*z zHn=79V_UxPeBJ1_*l>&ANPWG4ZP@6RkEASq{x$(wWs9^eBU+bVTQ7JVDPZ9<^ukyC z3s#b){}AHz*($Wp9IqOPx4pJ!3k>o%{}Zu7aI|)KXinT=c`9#3(17H^s3rfrPX2a^ zEoYsoVV#;FkbxnPdV8JjZJ_t*Is@4T109HpVuM-1nlTJPcCF3QuzpwpV1`>W1DYs` zt=81nz@GsOZX0xTAOdWp0NHOrf*8T)ztK+>ehXXzpypQR(`(N=)(8o#{)m}nE}3BB zjZsINnN0x67b?N0N8Fwa6)vi5OUmhrNy>WG^<>UAVR#MQ(}A0 zTb+mo0enIf#M+<22A`}s31p@SWMVK8hGCEcKu@}?Uo!XD~VV}#~F~cR)@xlsr@@h6?ip5Gjz8P%U ze~+F1vhrG`?cQs_J^|> zGrNp`jVAH;DC_LGcpYOrSyn0aI!U3jwvO+Qh-tJy#R~Gsra)}?zHI@JA}o*Io}A?N z&zm08hPO_@!!741r)CX1i0L2r$Y4I}V**vh+r@D_ZP`94LMq?@sTElB7N4^iArbMb zZQWoX%h`5g_sfX`0eBBR^e;JgR8>ub0fFf195Yv8v5;O zNI}qQ{o#0dg_2VuOqH6j0MWrkxlOKt74y41~1$0l-J8E_)i z^#{x+gaA>+E>UBvG{7)I^AL9Ri3bGy?Ky`AYA!TJ<*%856ZoUquUw@woV>DPrI9SA zAmuth3^=Th4!pZrIie7?_m|`e`L)`)DgYBOY8U60qy-YmGG|S3%N}$lTymDVryZk% z>O9htSt!`56eg)A7eI^iMlZM^EIDdGNx`)>r0TH*$XRs8)p*98ea3l>q*6@7ojrhl zivP0YR(~CVPRp_*qF?H28y4R{l4v6Rtfa7g+?zgkCHn{Q{y7t~I-c=#Eu;Y8RU^u& z&uhj5v0zUpF83$&b-IMH^Htr64LisP)5uq4#ebelyoi@H7?ry6;Mi5Y*Es*RE}af} zr&FJosg6i{cZOZ$Az0_B_VWq+WEqtc491Pv>_ntMreNK`qVOlLIVM=YRR& z$YxJNdSZ3Ekd~Q&yAOYe6g?OXJRIzHOPc~oiu%CVKxH3Vm05WkgG(kWZ@b1p+4Vw&{L0vWETK7bG%{9c6$KZHFUvaviOO5y0=G@u|s--S?r+ zJ72FWtkSvo!`#O!Yz9ljxf1|;+*c$FAnSu5ABX5`AH zPa|qf7|Rmd4hEk3AN(%9-mqtHzAo~O1%ROaVPf#-`mw7~u|Zf2puFUuC?yakhhOwK zL~JcgtjS;B?bcoMCh9Hj8Yg{5D&nNXYlai&s@wwjKm`Unu;eb8rPM0j*=$ z!sEUW-@B+B0K;4ugBya1^^VF6QE3?>;0u`ibW4r)kp_}z(|CRtkcUt>!^Upq6eZ9; zSh;R9af{ER-eLOx)J%<7#byvVU;+eU!lbcJut#RYC@g`|_Cc8AzB0BUSl38V!}oQr z{>*d!w#WV@v+j5i4>d65-}q;XKW~>bKd|~Zg-dm(eY#KgL6ij`*(^g?EN^4n{zX{= zDFE2|pin(naWJS1%f_(8FUJW;tKUL_U2cEM)E7nd;@!c{DRX};lVUuN-|0{41g2Fz zr4r(Aqmrf~5LPxW|*i9GAB;o~M|=C%I6y8^L2%NJddx-jZ*_La{XlCRl0(7YmK@mO-fjC_DM1F-8m@7^& zPzbfl6HB))010l+q9F+j3PE8Z$iA%G7pIgM&Mx&VFTz$maSA4-&S+-r>JWjwMcY#y%lUqt;aTR>e}iPqP3)ke6~ z&6LmDv>U1m7!_zYj6b-S5SH1zb=>hTKWd@2){^<$G;uqPhTxlORkh!i?wp0+A3^!4 zWUlK%P%bj4uQ6D5$!<>2#zMiM)ff`NK4tFqzrG3yeKihriuh+O;HjG-<5>XB*vYl^bC?*L8NIP9Gn~2)m+re8+#8@6{;=l)YhDElAI*+hLS3OPea!HM=hV z@{>nAI(mPv-qUm}>nlvCaCs&g2a$8*oiTJM`Xk4#^uF+0_CJZg`Go|Nd*ABcKI~T; zDES{P(TBFaTca-$4`*f*`!VELi_dxL-s`fkRCpu#5xp|8`03Nx+AHaWi+j7Ok6Idq z+-D(U-oeE5?%2f{MB$7$N=gpV%aF z?F$<@QS}49x)_%IU?mO+F&46`X5Gj>1%5oOg3GDQjlOGPAHaR;R1$e zeArO+UIS5T)kNsGF_G04%aiL_Mi}2SQPlSbLW3U-w68!YmjLqF_nPRpADC%=iy(2U zb0FSr%yj3)isHs=AphZf$lmo&8RD}quX$LQp3Dcza($2U$NR?29ImLOA{5V44aXEJ z8Bl-sJt6DfhHb#NPy5X&h=+ldt0D|346jM7L{BaLCv0dm`8}zjjg`+MH5j*F=syM; z-(%R&lDIa-fT0_Vn=@=HRGT_#vkiXyb+Y410Lj~fp4XO)xV-bgIST~5XdVc3`dpi_ zEn)ZMGaSS7du`^fX_5GE%2D^h+N_0dMUwlXV1nSB?0@&{a15?-q_JH}p9BY=$)f`; zRG0L^a)q0jOB+%GGvP5!gqmT=W*A~)fK#IJI<0{;oLyXE5>SGH_{oY>w@|C}QNr9r z?MduYVOS=IhDT{v`l!vPyxmnz^i`?ofC>k%d<4;3+sXXWDFo7Z*EK~YDWP;KRIq+c zO;Pt3$(de}`Gcb}*8)-YXUt+-(miw;2LcHFs>aUv5Ya{jV=C1m<<%IOHM6_asiCxm z+KA-5s@h9}PLlY^?sFMFDVvK9OJug0jx=J7oDm_qaD1Css{Ic2l|iqdvUi|t<8p7l zm@EYEX_yRARw)^lgr#pFt+T#-?ew^?cTGd0z00#R!&bSZ#)Ro{z+-*Uhw_!Eu_dZa zEYfHl(cm<~yZUbbf*%iCrXH*cfLIKXbvsU1d&(ZT{D96Zy|HP0x_jvN$%$Qz)IgNN zQpNEH@p1(`u{kgESbz9C23|G5?q&nv`tI8@UGH<@+G(g?Z<*yiD}^h$qydt(Ron1J z&dl{;kT^vi*s?Lpb+I)VdX8RPFUYgPl@guN4f<0KbbZn7E$WtrYF(r3{N6<-c&s{} z{`#VFHb0qF8@xc4Ng7QdKUT5)F~RvMp3*wvOa+ByxU|9W-XK8ec|UP&`4*jUT%Wl@ zLiDTiwU}m+_9vst!1#IdEfOm%57skP*>Y0RgzEyGi9pR5wIoJ?As3wn(^L!4@6_Qh z88tPK|0Ao)LRKn*{r=jUfu(hiVL+*FVTF+=Q^(AuL$re z&mWmKDl=w@e;)4a<|2f){2}V>SvlA!%45uM{q0+D3qV&T)A?vHX&%t>@2c4%2MJrGYpcY*WNcYk$1CU7ER^Oy+o#fq zcXp!wwxls=FGF)OMj+wLSmbJnHT)K%b+K?X=|vqU-#MdE#(Rp!tjD1or!lKG0E6?Z zEcdNEv*Tw=XS{w@RK0F`y8Bx{Q?G{eSA-q0j^N_QzCa!XQVdATBQ*YN9BBSGu6Ed* zJ=ZymN3cd4CiIM z+ydS_YZwPu#^Aw9t7;#CH8$`ipof84b zg)o;$D^{ZrhQXTm=CpWS7z~*BJPHZ|2s?_^)8)mL2ffvao-<(bj2U44V+eUQKE^P% zML6xmu5lF}k|aD_v)^=j!FqIhG~bA!`EV2YyV0UYwmQ1n>H%K>&@ z$q^r95sae&+=g%h2?PlYlSU5!9!LGdkwXJ5QVapsu|UW{*2zE@L<bqaG(9y=u=<3AQloTHH#dhZM_Dadcr| z1BoTz3c?EVZ3y(bB1SOqlPMJvtyzjG&Lnbr3%%1FIlC1w)za{_g zgCiIEO(!F!4M~=)b~p-zC}PmW@Q`D1RQj|fxDf?^_ny2JNdgTg@@CEQR304(!D$R7 zCs&m026T8%osc>j7AP_&E`u}Djc9G3jKu(N6n#i1VM3!swGMjJ3}*f1-sW;mok(x{ zk_QMQ9A}^hz$b4{}4pv4mfMu z?rT~ek}DqahMvA)<6j#9E@p~@SZXASsHo`oriNLlS$u46VX|C_HIj^6i9lfA@sGOY z{hVd$h~m@yuRzTEKlqMH&hOL2kk3UT50%iA=Hc*0!?#PEDCa>O&o~XTQVb*nM+c1~ z2IO^RhLpwAU)p9Y71G<|pMI2MFh>8BLq5VCCr5A~kID<#{7kCjNWve=R6Ce6ZFHYX5r*VoG_IdLS}bjr%qUTr*tmYhii zRZ7#0Ql68e1K280%hh0uJ77G#OM`S-JoLu+!Jb@aGH4P4FvEd7ToE}s9a3c&zCp_4 zLCX`){Jf+-d$rHm2zm5MohL>yL_4{>T!g^%c}0@f_cEc)?@o74 z=``gTW(#(SB-EL};d!Cz>Ja^FCE80d=GO+UZw^~x(S!|cc}{MkuW_=t{i}Oth)=a@ z&{1C`&!5pe=jlOGIIzw%o|ZZU&L%guc5E5MHL`#F`s7&o^2LX{7oI}j>tXw7+*neb znRazN3Pk|!JLqtG(r_erWP!dbF`_H9cKS=r`128F_YF@GtH$1JOSO=d4dS+kPD?D1 zDLuCo;gk0Qo6T>WQevy;b{Xak^TVZ&b7FuxGjMW$4v5&1zLwjS9OteyW9Lsk)l1j$ zdp=|E$)^>w&m7yIQ|rD^L%M8G)0S&9Y$zDAZc3SVLmpm{Q>*;h?Ld`}NbkqinCUhn z=4c!AQ)^=a3xH|wA55zP*SXLycb9i13)(;{8}jbTs_tODeQryEQ3v-HcY*Hp!bp(cLQl6Fc;Tr)FNH?s{AK_{}buWh!FQD69BNK*Xr)c&pXBp z?3)OhFGSD+Y@ihSM(OrC#Wk@@K)I;49Uo5f52f9?dXte6b%}eilDtl!4rjvfz2pO) z3h>}1jl${aa7HqtBpSXU8&uHs(%nLYVLyOQ<{+GUSj=ETtN@rviTZ~oj@aN??9NoA zQ`3&~?}SlOMCM^AHTggSxgVHnSPgvt||x5e`-|HYu`$TQDx09w#dqF2cw|Y=J!_m8ansXABv0 zVG4KOFRr>&#!tWce(NbPFA03{5}->Hx)M>i6A@xEAjH;nF^P(pEf!CtN%D(|4yTEB zJa{sZMq1kh?|eWQ1dxNg!^>*MbrpR?w7P+YEfZTMCt>^y&COVHgTpz zUsqw80eWy++I*_d#^n>EI{RZB+DPQfpviDiBW*Uq@S>ce;}ybDc*bN?E-*^wRa0xV z$5eg2VcxgHZ=YmO#}kmKS@O#X<<{rwg2nt|3Uq}E7YvGc^NRZ8DpzzTFyQ*bbV%um ziL;!Rpo+?AbT_G(S_?=iewoxO<_1SWG?8X?aRY})kO7*+YFjHU zA^%D&E~a%J*GFDzT9@@|np@mc#Hal6m22g)wr1OcX#~}ovWIlzn=coTxD2Bja~%@# zS<1HcDpOK78XG3v%!PoU1=9Bw`E_`RtO)YU%qm=) zcO$1xyAfsKkzkaBPUFv;N&F3+GBuX*D^tAo<~8>DaOPvD<99l;6Y}frg92)7W5z+Y z(g445=^*9V1)<^~=&BVuCyt{?lRSC7^3dXq5j^<2pZ|%U^z|dHJ3k#bh1u6NqaWy| zXK0Jkm**^_WUVOe9n=knj*4H)pS9~2l#lX_&t`@%_is!@;;Cv>V5eJ%e}7D;VTyl@ z85+ezAzt5+;^wo#2%*v8!TpCcYxvkN#`T1)5}0!SzgttY=wNvU{k%iJZy)8#5G0jqL# z?CCrr5WwvO{2Pl;j0SiKqqXC@N?XidC4DG%QYqoRH4+Xmij-WJ3&^{nZ)J=kJF{!k z3$Tn6hA;X5xs1nqjI6>DuhHJBG@EMpFj`gf#f@Nm6Q38n;X>}e=QfEZa01gKTu{bf z2B?+GG(aCKiX+uj$ANaH;q(W{JWF(gVWjd1CWL%CtNJ*vXsUgn-)BcXidq_tol<|n z(Fs4ksE<3#CAn?fe$g~M+H}G6&OPK&$`%X>7)1>7i$p$f4Iwv2y$K4ZCl9$dQ6#tK zaM3}vJAr*-6obvfBkoalOCe-P)Y-?u4AT)}gpF;3gwar7V_0F$uo#zinqL~UP3tJn9{>z`8bR5|+JE&i5{jJF6pc;<0Qe7FuQB+n+9yFx@9v%YZ!sT>lLWm8} zWM<(IG4u)l;4Q;L-swSNxFXCyl70gK*+%u9zwETz>_WWTD2}gix!HsqXjsVtA-R9hST!_h+O5s`$*Idl6D#ixvUV-lvCKMpho?J zBftjja!NW>u4Ez9o{kKkZD`FGvW5W2qN+L6GdV*&fu)#aR?Tp{T1y1}Fd{yOj!JOG zCIpx$m&7C&NiLCUua?HCcJCjS>VPdRHEe%pG8dPS{piJ3o=YhTP$(OMOCh=R9fePw zY|14BB%rO(E>=$G5y!v?A^2osfL-#K#44-7$Tw@xTuqwHP>8GN87KPO76E%#i9Y31|3x_59Hj3rB`zDr* z}q(%@UjkrwH2p zA}K?I>E{|ivb!;r5E@uDfFOBjCzNf?wk8^{Q5^x|9w}*yT z78S8;1|o@;yY;OnE(vlH(g~swb&`hrsvgmVt=6cBM<&U3c#JHndm+r0)?XzIvFmMN z%+lTNcvJ+Tr9ep@e*g|8i%=^<61{&xK32)>e*|6Pv^BE7DZL}vIWzH{(Y6Shb5Qsf5=jFK5#nAZ@ z&KovOgL}|BHC5*e9z8Zhq8K!kJL$ph#0;@*QEBLqCt8}mdBdx1@x66K!e>~jtr#uT z0izTdJa2FTk}$PnC#F1t;crGJi&PO|hz zf%>%5u9?~BlbJ&^i1fljXmS*eLVR!poIl<$W_<}!*Ty-C#0^;2r&5ybep(E*^aR8{ zrbQJ)qJrY2LsgT|MO_5ACF|lAw3UFpq)1P|T-Iw65wjedoZoACgd1Jo_ukrK)DIi8 zEE%i6`B8wlDI-;NkBTSILO@(wC~I}yEaSql!QjlisqTcb%letVDqY#*&(N4eu2|%7 z5&s$IuyoS?9Ee!F>;r*tRur)<7dhE3JTj^13qfsB7}>kVfzTzn5Zo6EatqdA97;u^ z`WOY;$IAC%6^TIDX98TttO=X=gU)1^H;^KaEYf3ANW zHW~Y@KoN(=3(gF~I&Wh~$>gn>D2R?V)C86iH6#>Xn*9~x$TwBiY*=ivpeNX_sKars z@WFzoCDUIzm$4#SCqm$z-}2I|I3`8|12B+X-{%PH{qvD6TvT2Z0iskY;}ro9KBox) z5VvB>6PrizkwWuaqk6OL!4&hi}>%^URx0f?uRtqTg8tn%RcPzq>eE59iLG7gbrQbT;f4a;6rD=JUcz!zBP@qlI0>L4fNQTIzlKG_>t3*Ta zt-IKbhlw*gRj*F0$fa~QANqDBMcCTYiLF*9GvjMzsXuU(N`F=olFgk!KvDlW-zh$)|&Vq z)wukl*T)WVl$32aEVVCPiutFA+dGoIV%c46#|0?lyS|XLiT5}+rbD84c>k=m_v5

ji4ZJUFzms{6dYQKuIrCT+GIJbCG@-Ncwr?*IsqlyaC=j+hc4%6Qyi z%f}7<0wGmhdG~flsje-1H0q&u-uwrn1gf>TBQID}1`te`-#>lTwWD(w>TSvSx z(dp}{vboEQi@qxc@8S>f+x+JpHUTpi`5pV8=lCCU`!t5D`g!v>6iDL6!yRFt4Hk{N z&<3rWS704#9*d_JSML2se||X}@?q2@7mxXiN=Wj5`ef3~`FMGQI~dqKnBviRTRW`^ zCWIP~L#0E6rVTX8@jSfbV9rHhOkg=EvGNdMG$7pine{jJO!W%d9aM@wc4lj1mw|!Q zjo+!*7AE8Q#+|5N&uqc(BaI;n&%(gX(}L^1>o;UPo`KsWg*W}2@`SO4L0_%D+)VlP zpPE^tK$?ZO%N_SuO@()OM1#=H>-+0Lrr`4=S*Rd2=Ffc6{jUnFf%XV=wZVirZ$aZQ zHDd103*X+I7GfBX_ZL8+>l{GIvZ#f|w*jG|nK}Lbwi|V2egvtux%0LSPq9 zJSnV!b_Vhhf=a+~V#k3gLiklKKqvXr(D>7*L#H|{W8S#GcQpMiNmjC;gb5!Bt`vU1 z6M{t%!?cF*edGyB;RITt1aTh-Es{Pr3lVp~iF?kzk6;b7GnK5y?MOG`J0aKBxl{l# z{HqSUtI>4h?5B6=igz6;L&~RLLSztEl7xB^q7-6MVR+La)=484J|`0uUhxJ&Ig0*o z15LYWmaT~*FbbwI1hYzb@R5_0X^OJY6aGT?0joMy%^b1EE*^m7HNno9`8l^A9k=saJ1Ny?$DMMNg)3UB2OFjt>j;+07!E zzZk;K5f)1PhkT$|#04z?KWG>jjU@Cf0!qpQBT-P*SdhDlLC%glEcCr}C~gq8i5&rg zLy4mSAYVLCJA$osN0WAuXUdc7ZeE1*f+P18LU569N<=d07q?UzTf#i+%6o*DwKdle z!nr7_XkVNjkZfE`Obo}1M-ubYiyl}*-FNWJ6$plk@B)y+L@W~FsOWu^paZY?J_71n zqz(fSvlM|~mH60k92{eaIctIdv>Nr2oQVzL5CZBAAT9&o0q}_fka!r__>C6mUB!sKc9N*)gfU?fxeF5fB5dNKqB5x>7Mdbqtq77~ zQ8S2`NwL&%5y%yZPj4++5(BaPrb}G}iYx-!)_`^t5N1 z;*u`;Lfr5rg;a;G6l)leaR=BdMjVS2MV3gLFykWkbTMQ_q5^Rg^+?g8Fyagpq9C1u z1t4QmghvnxvL}3X6BZZ0j3nBH_&zY=S{AD+lcYxw>?4U8k%XQAv0ZE6bQmZWiD!y_ z$pz3OTePq}w&)VG5<*BC$`e5433tLCE67{X&OMU@iRA0@DunXN4e_4i*j*E=#gR+8 zrK>uSQ3tkhb}S-LD9}<-*uQ3=qdtrrYQk@g2MZ-W`zmQXNGPTNRh|LWwng25^}E|N zjRzt2$IL@*Vp;o735U#01}zCov|%i$oD6ps1gHpr#}naA5UN@98zD9f;S$hRu7A#2 zBp8Ll%Rn2aqs4mTDWYG!3Kci5K->5kNMP(p4U@?Eq<$O98XBf8+4(F>3a5#xJmk^H zm>FW1VL)2=FoW#drNf4U8Ot3%LY!>u*oV-jAucA;d$?R}&V(#a+fa5v3N9u<+?lpe z1VE;I1sJ>dXczE;h(l&v5ijEzRI`*BfksVch_Pi-P~#KCw}HHo#0W$%^+ShrKjJd9 z@e2}L4X8&0T=n|5{0g;c)>8>Ri(cV4)P!R(9GaPa&_FNdH{&NH}XW-lB zs~KIS>jy9?Dnd|xQ>RBE8Bi}VYem}3MGyfrkbcOC6%C^PfEWk7I5OlqF=XX7REZZ0 zg!lyxG)I0-uv0KpTaHqr8&Mm@R?&cn#af}i@ub3wpurWyzMLOn-=GG2p?fIkJ^&{O z0R0yB7L`VT?d1$Igz2$9d5eH$px%a}ZOW|iB6s8nK}7WL5yl2kNu0Oz&VCt);9UeX zXb0MWBsA{CLs>_BV1w?XpbJQY?II|T5)rNhTYQnAnRq0Vet@5`M{Zh7et8vtHrlOd^JYkJBpIMTt#@BM85XVlIhC2 zTZyc=2=KVc>!ac?ugtFcm9LPQ8fZ|!n~^Cqlqn-`X9LT=B261z4ZxwwSCE)%PP6~= zr%0(JmvA@NVxfQtRJa^g=pbJxERvT*lQlsTCAb!)4GJQnEtWGSvH4uM?U^Ull_zSN zk6Fn|nJFd=EqS1jJ^ZLl&a`-TI8`I3{F zc3$@-hoUEk2Q!dU;NoBX!8B^hwDh98`dYHGDaXhml$4>V(a)_uWv#x{sOdDjiutyj zo%W}yWbIW>wT~nf@4ApHZIc9@nKEs|qlYbiw5`JH4gBl1n1CWkPm4)lemZHbLT+mm zUo&1ExyDpqOsgW~pY@t9`Xzrf=4j6xdeY(;N1yt*WQ0PpW4kr`dt7x-F1sk1nq%uw7}T z_na=~$$DoUZBe#a?_atuj9J~E+nyN70aEGW2ohi>$qi-RV44Sj+58V%&`{hgfZzO& zko0g&!w^QI_gebju%E}UQt!2D?}(4!h;H7Wdyi3{(-Dh*1mr@4Ejc;Ba7P$DLcaeQ_O0KVgVRIQGJ*SdCr<>3GEPu>|p;3EtPGfULiDAXcVp>LK zVP5(ZrZn(V*xiCt{*u<+>g?^xi#KZ#GBqiiz3H0^F@1Ay^VhQT*In)wR(1S(=1WiCz~*M@o2?Lw&Fj96+R! z))g#vwEJi50`}AHw(87QSQ%$^@AsZuXoRVdnj;0u~tFI-HRC zF}k&biMc;+>;G=XxJMd%V$%QP{C?X``)H35eyK7{N`wC@(Eb**`e!n5`GV=9LTI)p^*(U5 zY4czIGA4gj_NH*)0vMW(4gnL=N{>%QQia3lL@sTIi$`K=5qiDRI?Q7UU~#VvXKb-4 zw*N&qfKLruZTgPStUCx_^!;?Um_v1~+L=~F`eR7s50ezOxip!Wm4B-bxSy7aXU`Y} z7&y;IAWb%FW5sJ$>dbp$d)M?iXUp_n%9kH{JFaCp6=t}p`#77oc(}BucRl1VZV38& zdq{*?-ap`L@F80AFQ)IkK7o1f|ieLV)2q>2kt*^;Gc>~Gr68lK#E>J z!?<*ihEg>}k%m!m`_I4mtEFla0aN=-_F9)lbtmL%;=>mMoD8F)C}SIEn^>N2Q7r1T zl9ViVtnS9PQ4dq0+wtOUowi9&B5|<_Doo8jMVT?iK2_2HVW0Z+Np=|#HB!wXT}IW# zAw$j9IxNGIa^h>Il`Z%CO#6&%hb-s!t6y`T6&!NB^Bg-Y%kh3P0iu`vIpO5z)d+Mh z3ME!|E{<$29;19tKj~Z=UL6uynkq?fSe8K>J64kEPV4+03p5>8+<_V!&>q7B= z48T~ijB{&WbDwhS*a{|e@7zt)aPRt75a-@~R6ph3^Ru7OqxaX0hDYD;tvHYV+n-Y& z1NZlYo@fw>rsp6oQ@rO8f#9^~FtIG`wwhKQO+9d5yCqX?jg? z6vlf^@-$3)O$iLZUQLV4YQFmTa6A6hjO6L`tJxG@5QIG z*xUpV_&=%D#Y~VaPIwR#Op*i_2fHf(fCB~-6O&L<(eZI|sy-AoQg~voreLP6WnyS( zYGPu6#j35W?Cl-QZJrxgd1+e(XxpOHY@+1svSnTBWZk=;cn&`C8k6>!{eMW+YCiqX zUj})4zQo2WFR%ZFRlokv_%E#bzr5d?^8@bGY~R~-`*gY~MhfhDVx{xecj zQ!_I&^YimdN=p8BwYt5%y{oJ1{|Hx)jg3uAO#JU~HI}UY?=ki_=yzYy!Ktu3rw{l5ZhSYrk2qoX6NV109Q^M4eq|GS|4U#9Z^di?+XrvI6U{vWRY ze?IVk-ugeE_J2MWyY>IE1NLnHzg4T&Tt%tN&7y5PQd*?)aorQy8m0P4$A$1 zSF7oJcfvnVo6a?DD#&i2hyP2hX3Ovo12bk+?#8mu=m6t{#>T_qL{^M#A)=ov?V=z5 z>|{-Nas;(c7XPhkpCZ9XvzM&iJZ_)HLyP}4mcrRNg-p}i9*=+kvBQp|?mC`K@+w&@ z3ha;w9)q#hnJmDpdub|iTvPWs)I29$obtVo)X1_azoJlZ`Y-bU5bX2WFc^;w6`n-^ zlw8PmiS>3W^q-N?E6DQYA*;xJPPJ*W@0u|r8P30?J6Ei4{sP{^;Fc@Uu^s!Dv~w?D zy|D|w)U3Yyts!kn&zT6~RBkEYu;~M-w3rGA|1-0v9Qc*}*$7mywowHZs3@Q++5>Bv zCf*BbQQ>~HCoC%DXD3zK5sH*`M=4Q%L6>uY=@T{bfNNV;P7 zqL1{GRbZd)EbNY+jIix?>JsVf1il(3Q_(yh^e+s2H1&ENBcr!LP<5iqSSw(CvhFEW z$DiY~Xz^o9mzA(*`nqwoW;TqMbkA3-Owg8Po)YaCgDL;Q0;7N0wJJHX8Rg zKQg2QTt_M0o}7H>jmm>FEQwgNAAzdeNsmpYf~8JUdbY%W*u~Y6uF{2kOgYCpe5ymC z>5YAbqL?mm+u&K6LA#D3TVBW0R_=i(4caro+Yk3FQ+`vf%JD-w3X<;2j!L)xU9R*4 zA2y)QA6Wm||Ijim6YeIo1FfOmi6k(cBWjkX|IHQJ`&pvFbuPEyQbb-R!MGv7k@X8y zSZ^pkPluI`4_b$%91->-#P~a#ABM{0@T&PxXJ!Zxrm*7)v%qOIo&f@SqCzU&hyucH z{e114+9(e@6^gT1?PzoVz%(in@3N6lXAQzr-6S^Rg=${PtH=@XQwbx z6SMu9A{kSr0Gb%0uq=z9eqBtcZ#*d%pqH2@&BnS2oy?1FW-gO1=jIQcjH?t$uU#tV z701^gp?sN9So>AWeInWXmscjmVYyQ9;gt20S6SnHjtogLLp~CE*}OL2ob?oQ zD2l&{)tNkX_0>;$z5gD6)r9nA=&Pi|zwd=)Z!j~=?hle`V8w_(651YHEgZi!p%Sw$ zGciSJEJvE0G7M`vG1A^Fq!9;-{HF8K<_0V@`Kt0Hu_-Bx_Qm8rPI6x)bYA1WE3aca z(4bRW$j(|qaDA;*cgj(<1CeKe5~?&pCP#Am_uoS%s#K&1Qj(O}$`G3edVcB#Z^tew zmc=S{nX6{^wDdAC8*IX&hx$#9X|*=X-&VW3m+fN}tF#=B6!Hj2E1x{8ep&w9(58E~ z_-FciHy?aGe9)(^$z^8mH_;Zyu|~s3KKXJdyoStXOXzC*MjUQFTaxwU5lQ=|ShpXJ zOuX~smiq6Xm6to1gqnmnx3}auL_5)e)@CuWlq22D8c~st4&S$+ENOn#3C}Z^N;mh3Ijdas=bUytHoIi1?^tC$>cw%EX zr{)&|Gh+yue`Ca>X&b^)6o`SiDcb7n3u*4>A#&tkj1k`#@|LVo_TckaJ3hMyJ)L6@ z4TTexH1`<%IzK$wI!g$f{zA|hGI;k4M42(Y&mr|^LMO4AjIC&&%k0mjE=MRqz2?x+>UOc7<=DeH;=f&^=&H5Ku0@g!C7p>)m}64Yf+Vg)6|BIui_2YAF<2J z`Ap_JLP^(Xw@uG5%=$MOjKqw7;$`Wc=z;cI-jCToUsas;9vT=)&b}(}t_h<*(%oKO zGD|#9lS2X=)>gC_}v?oxbwjmD-*OmdgpWy{P@>?2P7aV21 z2Miv&#|58y9(A8aJqg?aVtl(qx0(`XAI}r^yS3Tn|KPz$eidXnZ&ukl;KT%VOEvf{ zKC=A9LLvQGBgtzncjMxjQ{a)u>~(7g<4$Qq#Sc>Xn_)Ue=4frz0IKGoy-%4KXwYmeUiHF|S-4R56i_WvHd{r%0K3e-RpD(_Knnh}chC8~?+?aqfl z{DZgc5L8xFuy2PIIhXtIDE*sHZ`-a>*y>)siqKzpZ;3j6*`I|nIfV#Chw@gwUcCfv;g!guv(UXVhUj zkHhp-!@qZgm8QQwz7EsQ3d5?^oeq%{s$oEt2pBZnJUYS>8uB7K-1~FH6Y5tsk0U&@ zOgOV5sX8NkJELAbMv-A#496`@F;`J3WYP8F(JEP?Zz`k055grshowhHG2i$lXGx~2 zTD%*NR?dpD@{fw}kEx1|HXV<>po-Pf55N5sBj+ETJsuT%fa=PM&Q(P6<{7k+24}QCLafe{Z)#ZG#o8zg8w} zKTZ@(PTcWN{B)4GIG(uUY(JRA`|P)qp0dM@vqNBelGaMneW!geWwNs}0QYq=G&`A~ zD;egJEW87T)1;6|q>!7WP#%Ki&yuOIYPDJlmkt(Gh2&8Xx03X`pn4}qF;Lcak{H4Z0<9~lb*3tTaqckZ#Pj7jW* zQC#JXxbo_|fLzWb09ktAksSBrYonW3i8YZOo0YH27u+^UbXxW?HFzS*T* z@8oE*AsFFoJYk%C{G0>)9BuCWc?oc1SB|80dY76Q5&)J(!IvyCkj>A{R?jOT%%vlEt8&0Do?vFaW!d3}Kf{S7~(7CSiV{kg~Ou*)YZvRKxJsrrfUPhTywdvB-72&Aoa5f{Xb>8n6=7i ziSj)EH-@*ede`~8YVaNI>{z3BR%=z(Ywt^|APSy1jV6`G6Yqb}Kt$&NcxS@oh^#s8 zbah0AC;~iIRp|z=(W3!d=>Zi`xNj#b6W1z}Cj-7cs;WgqjT^^zj+YnQmfP@TsaxYT z#(*Sr9K&L&%ecX{x55NFfQLKa#_kGz8gS!kp2=B7H*GrMkuaeSP*taFNU~PPv^I{5 z6#l_#axK35CT@G7e73rvh&HQ*CxZtL)<`Zat*R@XsIhDYkR!m{lJy3!OXRT2_Axjb zI%UVS>1vH2*jzQ^FhCAae4*a(;CX`?Pt|yL!}Lva4^$LD*H}PP7lM(%6@!B{0kuCS z8p9-TVLCuXbj$o|lb!_RpGZcGX^Ap#x*igTL6nPwx0oxpIa3n8UsXJADosZa3iNMa zQg2~?-r|M^YsA&)9=7PIf!X8QyVUDL4#7wv9L2vyS{msZC=gLmos+>!YapMR$|rHSa-Li;p)Rd}j#3FoP+WSRRF0w$B=oT3 zEpK_q6hT;EIsbZF!hr= z(uV_E)T%2g)Y~eKvv}fS9(?W|9*=C)C~wy20mXp@g)+D>aPZ+#yUA-D=QAAL8URIO z<`7{AiD@GR(s6<}eJU&G^!fp6Uq>6Y%<^;F_r+|sVN91m)zwJMd72L z-$a__)(X~Uo2vIwD-E~47?yyI=uNyQLV@;p$JC`q6wh)fJm2eQk7>}i5eHz${q*tn zR=s;Y3U-Y(g}uQ}+l$v|N7lAG|LA|qgDY2LTZke2VEnpYr>s$zZ`2?t&f2)sL^D2! zZ`h^=ZN1is{E;3f*%$#EG_f8>zy|FQ^>*7(#d;k(1pasvk z77s2(GQcgjy#zKAKV2~TqpWsgGVMn^$%lMWzlqNpertbCPGbr(ur-{lpPJQhi`^N5_jMqyE1(8 z`@gaGwZnGs45Uz#WIUJ}Ac*CEr0lYzHx(dp*%VJB^uuy&t_&*~BYC;rA* z9!PaiwM#<#zGluGK5mZ*kY8xv7eOCwvI1e_Ov%S(7Uyy+hT-{M|F_c)z6%!KDD;=JD?wb+7)Y+Zp89-jbO|RW3Huuk3vs70oNa znVTossXCNNH}>Qr-dS(u-#n4YVi#B?uFqmcHJ4gU$#Z?Sb*h@Ze*2Lq2L}$c#Rmce zN*EZ^b?1bM5V>#Sc5F#S8 z>cJumKo)^j#hE5&OyWVP8u(HCh{ls*M#SRLk4n9t4Z+4=Df>2NsCt&Bzgs+QIpjaE z)B`g0Hb%{~#v~r8*0;JpDP}?}4$FLp*ISYm97^X;Q1(x-u6=*oKd;Gu1l0j*J^!Yq zwM*a*uH9V+eYvriwfOO_P@p~G4UT0fXfWUi970z|9GQIf;O#l}+w;eH=k(j>%8loY7@SAaV8(_sq$iLe`PYA_)rG$T z?%3()eu-eVfpo~U$K1(2z84aQ)Bo5Lj-WQY$u_biHfO5#N=)|TLTBm&mzvv`PcfJ3 zOh6$Woc~a(-(DFPUYS~rm~LNLo?e+_u54w0QwUwzz5Q*W4RlQUZJP~pw!*Q&{1%2w zi+cX%v&Jgd*8v09LEG2Cm}?p#;O@=EW%OZ1{&vJQ$M?-DlH?;ifz1*3)w#M6MNb@i z%kJH$5RQat(z-0F#%>ak=| zm6;U8!h01VQQzPne-`&(AV56hf2q{~M#cH&5_t$aex2p$8tw#9h#n_V?@XeRZC5q!SDd!pghg$uN_G!Gn({525 zFE7WM!Ns?7l7i1!lKTRtEq1gB%7YXHnbBXHYQ%x)Y%~~djP47$+-n$%jMaz9;rQ)0sW90roO43x?BnG| zYSV<&0JvEM5N`E6-G~PAN&@xEBJ7YfuD0G6@zn3D$rOHjA?reZflKC#m7#+kOm zMQ4~cTU}H~;-z8QL&*jrLwN6bKpJ8L@(4UROv8qrGGABk3j%T;?6d^o)i6LC^$g+D zkK%Vzq(})_h`7PGgS?3uX$Ceg;U|B469EK4aPp4?)F_0ZqK5tDu=y7r+iLomB_73# z=1vm)2$5y)#}Bdncm_^|5i8}6b|UFQG7;UsE1sEr{Q=~#CCVUFYwCSAVzDaKQMbl} zu+{q=`8gR#*e3sQ6`djDy5@ZSwPQQ}@=cYgt8QP~&T#uz!+p)5VSg023Gf$|HDS8K+G?M7iFPDg%w4!%XrU=Vf$JGQN&hR3e;5?r-6z zWw=E}p?Hr4WgZIkKSx|H`Veol?W@rJlUgHGQYsXJ~S{!OYP zs!9>z@r{n?fhC*Z-ZJbp{2eu}ifAMT3%o(E!U$=3Mp6F`8(0>msD1-Fw^`u4fle(#?k!j7lvj?X^_) zIhr2zHemmwiLaw8vcMLT;>zp5q_=skry-Kge`b?eCsYK0d=^WhNSILe>t6oqEYA9b z=(*}!o|SiSim0dT33`wUL#q<;Vt0oGf+3e0nCDK`_y|vO!Njxv{)rlDAg_9OK=dSH#{DYc8;KFwPF$vFFZRNwx6gMXkjHubSv8wi5 ze&ZDH7oTT&VgRIbc|#ikIJi52&RrOb#3-F*H^-;^DL40kz{`!#>iXRU=_X(vWXO-s zbfD1f&v;G3%(3>-PIu;?FDI7QmiyY-GoE>b>JC|MynHC^nD6k$&knp)WZl+7|I_Z7 zWU?^$vmMD7?9VZ0>kNePASWY!nAG|X>ock-{g>xBexg8Z_hKskp`e-9KSQEdT8|Ww zK!RTyoticPae5&PbBvL#E%~Et4t2EryK-p<9;4MsBg0T|QILax-=9fEosi5?X-7U( z_Ow%fGl58l)t$pSy31)aFT~U+)W@5cjF|t-bfizSF)_+TQhb ztzExfjEQv!j9eN=U&MciqTe%;HweV#={xeu0{CcpZ8v|xwN~cV^4X|y%G!?%ecp0F zxVdp>=xxI@y`KV1nh==1LPiQ{W5~p=*h|_0{~3)ETRfGHy;@r`YK5P)LZ9d3IXDF; zWomzw_wZ)v`cOAN5gN=H(Eo1qopnav)+~*@oz0SSLUrKaB~2+*oS32RAOQ9dR>};a zI50js5-%t1T=`Z%9I$nj^OCZVp#RMgC!}5Cav2Z;BskMq6Ry2%i%URcKa~!Q=6J1InZ?+$-${nvSopkU^ zJ(-#$WC^~b1Vd$^`n0m`Aw)qte-6@v{}C$OrS254Su^3^%MO_-QCg+GpKV;kLGG&y zum{3!(`3o_dXF!9@fM$Ket9x+@q{54$@l=tWXFFYf_`V+4Bn{5!rh5~d$$PdMhW}# zy3nPQXi#r&?FE_&j1TU`KPHs>QImsbKmdaFZ5?s2M@z3%qFVs6na_KFnhpA{q1VFX zNWg<3q;hx$srYb7M*0MZDj<|>2mu~q38%);$&a!K4XX{M9iZ9cD>%yXI9;1CZ1~-8 zj|O^@K}$Lzt$ID=VpM`l@YkQvLIjeiX?#c$hE|at ze0U&ar#lb>6~=`tJelT^{iiTZFoHun@`INrS)sNEo&-k7l*Nrm?GK5q(IQaqnC%M4 zx&cd_yvZ~QX~YevQ2Q{$t!j~Cc-U3s{CK%-E~kKZ{I)qBqR`myF+ld z7KhT{ZpF2MLUEVkP0(O1UW!xPiWVtQf)r?h0>!0RacQwqpu)-bxu0{N=lssh{rmSf zbM}vGGPAQg*_mYL+Wma4*IV_TCNma|s^MjCPJq67c{k5yMOJ@H?cJFgBS_tof3S3q zMLK+-4S^@0h;o!rqIOg#*2VAvGPN_I^C|!eK}L731luw7`%6Qh1NG7CA#QZjD>p9Q zM(O|s*$sEK{>5Bz7rx+3%>7z*y_K%;neN*NiL%I;mroT29UPP37OK&@axtwcoUJfM zL+SWr%ubT#Ug=0}i>7k_2>3?hbg|DjoMa{klxm|k&7eitok-o(=#r25rHy(!pq5Z4 zT zD+=XXe2f9gOc&~u7d-)v!>=9RH&A1^auS)Sm_OvI=gMnmJL)94YyV8wd0I9~Iio{! zq}9+gDwP^l?LxPkCKOVp5!N)kFR1$>KqsZK_xE(qFQ)Od_4`xHTIqv2g~VE(IlXiS@-Htci{I}L)55*?$~Xzjc$gU?r@lHZ{Gmtpx_&Y#xG-f5svz1P`zS4 zz5T78b&13p6HO=T53?6qXeBi*$6UHrJRE0Xx9XkYJbqCNzt8?fO6mvU(Dn+I~1N%E1nv~ zi~+zA{D4x!8O;=;_Xf9TLZ89%pI5XW;3BCVs7Kml)c=M~d_LDEC5n>EBikxfU4>Fd zBccdVKq0vC#sNDndV#$hO0fqZ^|?PjHp#s_DOq1A)l)D1rowxShw{ybmX3zaw9gwy z%2DKaEEp8$2`NV^K!pVggPKI8Mrl}rP88B`K()>6tnK!!-H%zDKS1h9)7(4Lv`8Se zX76lpA>}$?hJ4I%aqKx(L&ug`_xSDp7NN&6MkF1@zoluYhAJ%`;1@PU1ET43P*yb1 zIB?Wmv}r8>4|i=GSFfqGOzys)51*Zn*q#r+2Oy|I&2oX^XKZH=Xbjng#mv+nf(*#Mx=IzR$wbJe}6 z_>85RSQHu*#aRWCu&{jbW2r%M8m|iL^an6AqMKJRgEwO-74fO?82aaYv1ov_q`YnW?Lls~jr&ezR58kgH+pc;{ zx4F3&f2Voq)zwboii$XIM|HJdm_7`t1rWFZ2y##nc|dv;kP#ydOGV)uN_%&Su0W6x zdH`ydhd3H0?yiiH$~r{9gD?m9Eq47Ychx%0>TU}1o2MPoJLJ*qD#^|&=_T@rrAU@@ zgHmLJfDgoLxIy!HgEj~R2Y@(yHW(h;vtxjaLmSMW?3v~^STF5)gV)$`eEZb~&0jQx z2E-OO#e_o9M3w>K9PZYmID+iiu>dmcrqG;2h$Bk)a#M75lPGwdoOVO17oz*Yb}L zGb_6>%k8sf&t&un7zNliW%v3+I*<5Kd-T`FY#$^qb;1wye!QlooKlp zSU%Ra^1^J+BE| zDH1|0(THefu{oTl-`(RJ?&TD*>TIFD@N}m}f_7YTj#vN391dkR9AA#ZXXdg%-oyToe(?&Ft^2~TVd?13WZ1j4RT#}m|&PE4_u%{}sW1?!a7s)%#UEWRPo$c0t zdyg90>E}_sZ~wVTorte~zhy^DA{b3iKmG7E0Ecb!=a_%TE&QZ!{mSeLJ~`}tSJS$U z)u!=yYVUeb=fth%al>fURxA#2Ihu(Xd!2)#;fvx#Zr*ZDx<`vyvLT|_QPjeFQ?LVe z^T$!_Vo`yQQ|6Wc_(+ts>M~Z*bN9#%{ko6hD7%`k+MAo`LKcKPx|uqm5fDca~AcuwN#>!djj?a~;t&u0AxuXhfmj*fU-J}qc|XC-1rko20= z$&Zr}uu*irNZ;8uI5jZ=P*M>;x%8=jgw5XWt5iRE%#4Bp65CZ>ZgbFfTwO>rD>tUd=(u` z-vuX%h2p<O@ML{mBddy)gm36PuujBrwqeqL*Z{7w`~)lV`9BF`n>)Iq5+1xHb&;U z4;}_M#hzqFT^!q}ZM+7U&-x~$%95c1*UM-0D+5!S0}X9FWw?JlX!jQu{UqYtAH??X($f2qMLa+&XusL*(fikz3L1eP_Yb+Dpw_SFm@B{~lYzLX=i~2Cd7tRW z_UM#gJie;v`-^ud`}Jh=r-V(AaRUCga!@-2oFR>c$|C_B!PifnW4R0gvGY(nY;?*9Ub8bK z-}}n>_Pxv6%&S)bS>dU+Jz!7OLdk6)?VGS1m3<4cpU=szQ-W`jG1cORsHS0z6#W(6 zdZ-x+#Wo2Ya0d{gLaFrtJT3q;ayN(zWO@)G7Mdgy zDS?d5-Hip)huImTc6JxXV}oo6aB4LHrI5?yI_{Mx?H!LjpA8g@hE<_%L0bb z!w3cf^DCr#L~p5o$P_RwwAnv%VxncR|IEeT!t*Ib*ndYgy#zd2tQ=1%?AAI65hHkU z$5U={n7LB7L_1J!m|&WX3N~kC=v@4axsG&!VF5LQ;2;O{SdL;z1f_}3xH<+BUqsb} z{yaNA{3*JQjk>*kz#k}NTL+4lfM!Ai0;ED*LXswC(!9?-`fX#IR5qx5&{jdkhK(Z5~7!WWFm&W z+xQ725(Mvv6?{PhhSw!>U3vahxVbneAT@Yyae6ay2>DoKV0pd+qXw_{%24rwp?qRP zIloFo3noJWJ~l`EW-rV+p%rxMbJiVuvEVas^1T|J+0Fs>Ah zkgY4`Dv4!e+$?-9E6bL7AeqaWWeOrUdTdpwWlYUp0^rGIP2;b~b%_(GaDSwEftP=u z1fmRriWyeMKY9IP&*X$6el|2jW*q<~UI^uD?1PQc)yD}lmZv>`MOR%$$;G>$Eh~;f z;N=H>oT?;$20JKskE>3jq-04GFk*^n1=g{Atg&k%cr$8Jbg1=WxbeqQn77h*{Ghth z2X_v>hmN_)0@bpgsiSMbuQQKh*X+9L+rDJ)Tai?XJ%&e5dpGz`Xk^ zH~h}+$E4WbbiMohVXLD%gez-+bP;M{eH|`}+<}5Sb?`wF{D@+AArnKq>Kd4&^DwJm z?A}Ody^Y}qe(MOyIUPMu2L}VtN<-S+Wsj3LjDK_kCSGaV>`b;~@x-bsbKRpRV7H&V z@M0@%|1pPWS|GWOE?W6%CtON*?v2smxm>u~IPzbj()i@L$n<91%9Ywo?EQ+eZ6o`t z#(o6rn$h`>_Vw!8NWP8mpg-Qt4^?uVzCODD&2#&)0juw~$2)ep#57p3rq2j*mbXTw zdFD6Bss@}|9i00&Z$oS}^6JN~xLuf2yUyyb#TMI7OIHzEL?@%PHxNcH%qChnNtyb4!qF|0jNz{8B^aJ$jtcf_a+5`xx z2p*DrES};KUP`yYr$Mc|4*Hi`J$&!%6hw&tQs7xzDeyRf7UlC~ylHS8E%~34&^Gb+_>n*JJ*rR&NV& z;$}4K>DkH{CDCYpUHiX&I9;~3E*F9bxYpr1r~x4 z`{*l|jt^GllACs|nlpWmN*G-QZd^0ryX^oiYIc`g?%>|SAqn~WkZu60zVNfxejpaT ze5nzos_K>!Ql~GnbYJy9@J))*un^x9_Al2Cd|oUn9#!#|K?%Z=$xACytuckKNFpE+ za+YVR$|f-Vk}<9zK7^&Yn^tPcR}^&9x9d^y1`r$KRwgk1Maw_k3YIX3L zbSSgJtfywCMj$7F$FPw4$HAjJHB(_h0aDUX!dr6k8I!SkNlBgh57r@ZaZCWTzoKzg zk_6oJ72kJhsrLd%r>PwY<$qhB*gAD zUz5^F?t|_jEpJF?@IV?T>CcFiH_}3C?d{!j-UmIeBb}+!5gTs- z3&CM?b!1SH1$dTrsUqt1!pv0AHn;f_{+dPC0hsZncXEz{NqE7NY%s=5skl9xM&yAn z(!bxY(nf9mD}+CU+FRy*X4}*eHGLEr>*D;p+>ZGDeWrl zkM4an@5N+z8Z-R`sQ%S-9NJ-N_{8(&OuiGvU-M0F!Tmw-qBpYbkdFT+H}*{!JfC^Y zg!u(YzX&i&H3vCKOkg=O!2A zpyhhylta8R>?GwML3qCVtnqsMSzJregQGk0x3M3?`&y1^Q!>(}IwlIvnzaSYfKSat zAiHuB4?cCb?}=zlGr3-rKI%Cj|Iqn4v+%6Gc4ZSEG+P|K^RjZ*tW#jC7_v@9Sz}z_ zve9A1*_bWanlR>;t;HDDw9Ma7GV5`RJii`V4f@_dH29f>%+hI^7DLwd=g6f+UQGYe z`c3^4Pg>_L4lv;Rc$+PyNU`=4sn^U^=9E5ZuO${g#z{2IeAx-Qo3FaNvovt+u-~I= zBbsyh+7np3I|c2#d+_z5V&FI(T^X~r^XE&)uJyNEMTIYQmOt&z3~6ar`$)T8uIhG; z-EAh86}vuNs&h`Jrkp!=6W&c2q00`)%30yL@f!bp`|@n(b+}l#v5aw4F{)m)7!=fr z2cyNJAW$W^wK*2_vG#?GL@}HQu1Rh(NQXZb&Myj8Rtlzw!Iwa2(pvbPJ@s`1l^;Vg2g-77SrmA{=S|eJpa{b{}F8Gz;0|LB5EKKzkbO^75u_)|WF~My zn{^f;EagcQDRlxKt^&h{W`3*13I&1|`p9KaEHgkn%|0rwHD)2~31JN0WFNT%ibYci z$4j%0Y@9XWz+|)#tr9?vjY6HR-e^tUvM5xhWXlw#tZJ`ctb`XcNZh#g!Jhr*<&aJ6 zB3&Y(_!$*ND*IkF<9@Nd86Ueg)1#HfLUOJuquSr2~@CEjV(^eW~4-T z^H$03qL3>25o;z~)TNI+hDA^a%d`@EtcC(#p~z$a#8?2%mWCy7va}Z9!3IHNK+%p@ z79D6>n=)FVc;Qut`YjGhWqhiG_h0q!FK3t4^{@cwBYm+xM`6I{fV?|it7vkxYT4{v z>ZTiNC)yfU~T6fGyB^rV~9)<4r<(kOV?!BZ9B7ua6RgnYlF> zRX3aZH-*_nKk4_F4yk`xrVRoU$Eb$%aqS9o8aVdRR--&t%AypMbZXcnU)?Slz%&e> z4H;S?jbJgBRvLV)XuysVIiLtJ*ar$%^SdZ{GGdQc{FsJ*6XGyzt_u7z`#!jv1-xFs zboG$L1ckUl@|-ENN?`fg9M}?-Sh)yTaGj$c5z10)$3MzPg0~X;NPB21)DQ?U9v+S4 zzT|Ff@4X7$3Vg;^Ey&T>jnX4=4wRnPPD$Y7hskzQ*@Z5Qs^;%TK#kiL`2~1BN=o3q zWCY-O_j~!~C|WU;`KMCI4Qte8A{A;_`T>Hl4HzSZI$lISyC`7U>I0v!_)~(x$!vJ^ zj?yB17+N&wH&zKsxbC_ebdWC+Ig1q0(TZ|@XZnfSf_+A*Vlw!4jl6X}D&d0z2Lg=L zG0>)oq#SiVQ+1JxW$8eh%BissqY!<9*5hF zJa^?W$S#o-naJEx%~BjMI*rG<{Q+>IE>clZ>0a!HhPOOgWp}@{RC?e+cQ^%_JrqA06C}2{coJxO7v%Af}HLtaL{JLso^VB(7DaajzA)Wo&*Rc=5*x`M^e#hrR7X4ax~j zM?U-0Bc}K9%Tm@6ETqn9m8L?(I&!tBo0YZ4;n3anP{G&T-T_Qig(AbUeQO2M`w(mq z*IlJL$Aq&wKdyP`DWD(VHZZg_U?pgr>}ZS?GQN8CRTUkEF9>#x0mpzt|GMk`J#tQ%kCvLiOQi4(w&u+7RzV30(^@ zQfjK=ZhooHhRB!!Ana;CxyTv4h{)eqmXqf{Bfm503VxHz{iZ1Lo$~RPoN8+^n(S!d zL_|O%0I4a1{S{fe8@W^zXa*?;P}{KUQ|F>4Ee^~|`=2!c@ycrp8{R}X-`r?~v=QUaJ-FsAU&I~wP28E9gxkQF!M255{)-i#W)0>LGhms{Mo!PvZ_vJLb z_u1vp%jq+^nK+uoxUBDQb0&HC*J$~5G3Y51)JiJ;WoVpBxemdK;8sd=r*E@J7tyrU zJny_EpD4M=gc-j-88H8>Y}NbqyTLRopKE(>f`CWEGky!iEwew)M5yeq^{vDvvL*58 z-_y3JFrSzH^&GJmdptErnd9$*y!9pwMob%?XNLR#aca4j1eGeaP^5|(6H&X*Hb^{q zUf_F}wZ6QNdafhX2hv3RX$IeMSDhBdjL22@eQ1e*N`P>yM!e5K@*-u()`wz~0OXLW z&zh3-Y&eeBUuXywIKN@eRU;tQ+}$0Bu)ut2C3J0#@NMmG@)Z4aQSN&PxH9p}X)S}d z>YO~*lmJ|~73xF_YbARjesIO}ndsgI05hNjs&~x&QJV)6fyZ~d6C@)R?mWr~GzJH} za~lF6P^y{;an3h7$iDD9sP&ugyKMo#VM<_m(^WXi&U4cAX@Hpyi9h=ULAay^5v?WO zj_8D?^do9R>5`d>GF8J{1pnw89bEE6OblpUXk9UI4dn5Qm1L@n8S0V@+Qa`%eA%pf z*##T6kJon)ZF4&f_;D>^f7j>fg7J`R+~cm8>KIkfI-otFO~5~fmbx;}BIhC1dxDW> zkV_7-^v78L9@RY`2VnNUFp z6wW{%Pxd<6E|EVsK9fa0(R~soZH8`tmiRL=<4MSUK=Ou$57iO~>f_Qb{nri5$Xu;@ zg;4R%#kWV)cEH@N-vo-rS~a}3_clZkb(m5FmoRpWok(j*HD<}=n8-`WJs;A;^pP6U zyWD?*tHTX(VZR*RW<|CK+jFiytsG4#z0$1?Zfm|$=nS?>^8YH)F`?bjh(^OqurSP` zj=^Yv^nB|5khH@OX#?8ng4ZhIA%k+)n~3W%?am&1wyftv^ub~RoYIqHgOpp?(B1&I z7qVwDA$iYb!vJsN^RE%qN_KKsvX%VRkSaV)Apg!b5vDHhPIb-sYWb)->03C0l?hodl55l-`GlIHQ z*otc9ho*;a&4h3HsN{TbO;GT+5M`{|9U$I13$X;2ehY{zaRFGdR4!=T19_lHEDLqt zH;Nc)m)jd}1fWt>4MJUPH_5G}TS=Cw#ww(QN4(0S>7sCRl6zeRc#Aq-^NqM!8L={m&0ft~W1jT@dDLIH0B~pYF(HJWU0!y+2q+hpXt-_kq zqp3T($Tj5z$-bQZ|FHY`@OSl*BD*{(0GZ|{^PDZL5sTp3B=_l)zO=_BB{J$dKw6a`^sjJJRHl;( zoI-6&7Q@1YAa((Q7XZd{C?+flj3f{>R03rdv>D%fuItE5tlIsEfFiliyheed1IW^( z4DL<<=l39zyI$jj?NN3jIGR4Vi)j+|tLy&4A_mLbcs+OPrQ&4|LwO%+WHU$VV}J+~ zO%$Lnip?{1#ib?HUL~H4UEO1VBT+SpZrSD9s05UjKqDR(&d1#k1QXxMH0(@w(s?0j zpS>p*ZK{_i?YlG6nK4T&rhvwu-GB6KvOrOB(B$J;G+E(&N0#GZg)35kHx zTDW?=h6|8yR(uRFEa8!(#d3o11rU8`IsL(}gSi4&4typzDaOmP@z2fBpMiC^RP4S!oSxW+78ipE!B^iDlZQH#dcom_-zQU7!e-dH ztQJHF1eWl$KF6W>p2G>U5&gXotw1fry`N*o22%dUjV3P`SN7uxyp%1-zcFgU@XlVjtV>P=*z5J3z2M7iYxQSo24jL}hi*FVKv_ zpShsu%o1G?F#I`4KU#s7yeU!8(nyJrt=r-DP7+stx?UwS5;40jVOf5z7k#Yy#{S;R zyEzm0`QLfrEY`aL#t9V|oGK_a#P?b6WtF`=ZMuugmR^Djwi|-7(3~-MHQP5a;Z&F0 zShlIREVl^Em8@iCsX_r^mvY5G0(uK9G~WUV098Myp%6ApMXo~#R+~SNK#&&6ocNIu zO;%<>NB};*S5*SA!Nme8${vEIto(R}0z_4n7N^Hj=j`Zs5}e#q7RTqnAkpQEn@(ux z+}!yhzkPhh^!bLoe;obWM6F)^uc@5Mc#bpNR@a6S4(rfZ_935<9~y#i zO*HKw(&cfHP7i~F>G!dfs=^5TTe(WP)k#7!{zR7n9^opD=+?ZWktkB>m?~73xaPp4 zWbU*o72^CQY*dzfw#DX6?wL$)y^NdR0Os`2d6bl5Vu>b-cV(u7pUd7nP)39C;ceU%tHf_{j)ZOla<(&hZjn=DV6rh2nnNwuNL=4*q>m*1je0@% z20oX`G(8Ft%}Cs5^D1B)N^+?#H{PVzYLdadEFrG2L!n@W>I)KB$Kxp$NpBYi?uNM*ulOVlKg~jIngViEyGS1GS7`qa4S~XD22ZS|MMjwq#?=Y=0b> zM?}wV6r1P?s%2P`%sj|`M_mz-?yAxXkD&>0G?w+$q{iJ&!6$o0V?rQOlqH&(4@g2f zYoZErOaCJShsM$*sFk}3JZYY(fu2`*N?Z%1MrE}WKx!=_ROTc~0ug1N&#b1(M%#ui zDoC4n?=ykoxM#jK)dxZavi8Y2Ce0k-bvrvhfh+q=W zLcfniMt6LX_ci zAqdE?Ptm2h>sbmv3z&N9j3QBJV%?V?P20}0DwEM9mN}!gZOn1rr5m3`g2y)N9YfTZ zRp=hZ5UH)3Blj6UNIrCg$)iw_4#@y$E1Kx;ky1Iz70l$Yp+Rneav-DxvT|^J(9R}& zX|TM$qp(2g-FQ(hUf`XRp6_ATVC zBF%{>Q(0;>EgT3-btGfNG=g4J#6Udvl#mRQ0B9Lv@+ZYWnr|vD0|XaG4()$Mk&omt zp8HUIdD-4c{CGL_?LewX`%CV)9kUFJvbvG8@AP_^20>rRgzzOc|X+vPO za!h4)VKkabqO9EdGSzBrWuEwD6y-Aka#u2H4pMqKCDKiDFUB4PZh?){AoKdwU7RQPT~DKP)sO4(-W8X~@|x-|Tk7psD|@R{!}< zXb7OCM_;{{{(G8+Q>&SP(1C}4&r6<%Q$yKrW}pAPs4x1ng|`7>Ir(q3y688{0_woi z``2RS+NDJC@9hn?+pk+li0J>LRtu&g{#=U!AgQ;8C=mV$fZ_xK6T>Yzy5&QzGKzt| ze+P0Kg6JASoF^dhR3N_(o-8@&-U&cb417!A1J)7)$e(~Aji4)yzcwcj-sgv_4KwhD zZMTzcE`uY#Nqo}-eE&yBiXH@k1KWPlP+vS~{MQ4d522s|AzFD`f1JQ~0P2lL$T@Ks zDz+6zK-5X@9OI++IF;}{IetMZZ}Fsb)uhztR79&!b{VUS(hCnjwjQ8i}P5g=y1= z&?$O09c zn#`z`#xi{hzdmJM7GV2%%HB}JeD{dCg@V1wh-GSuqkRfK3WK8^Sn)oS`VP?dOx5>O za2VYo9z+g3aI#cdQ?mx!;PF9}N}d zTZc2YdSA|Yan2G-&ZJ7_=M;P)4upINLklFKVHvJj3=+dBNaYQc8$o8`lxJm5FA#2B zZ+<;gxAMDoy-oJjlp=VIMyL^$;(qVUbqo#SBDUY(#Q>vP|^eV+Im0a9F zMJxSi2Y9rYV?^@^&l&2{4GTS5CkR3co**Qu z#AE%WC_}Oq_KL>eBAOu#ybAkYJ;AYXNCN98T8Red> zSD^fl1oQx9cV%K)IMfFLJTnGzAqm2fkh5Z-9RfpmSJk+uUzsg%xb1R=E{G z5Lf_-8vDghP=xxWmV&d&ZXif-F%Jj){x*KU8Nliogy?jTWB@$1ndXVYs?j50M_5vN zIFTDx@Bk@cQU>(`Sj7N9b!DVgU@|xyx*Dj{btazZ0Mq9r5_=?tBkR3dO#Myxf~ZK7 zE?}~E%mjm^)2C(QjCq|*`IAm%>mQiGu_SV(Jk3a;x<7G0!ra3Fz6^#{wxywI1p+C(<2HLy2@{#sXgz<1fW&yD5V< zk9CG1f+p((kpRtfPd65Tu&g7lHB&MhE);{(dEDYfJaJNiwYQx#z4dl*2t0k{JuBu; zBd$biZu{9Rp^1j!vw3-jTu%m2AcH2j59kmif4eDS$*wa|{`kHmFp-0}CaV?^E9g?p zL+?%;69j1n*j;V9ShBl5SBqo>SPP%KGN6c#UXj*i(==w&w9*n}nnD34#BZI#;aHE- zVV*HQn1u?+2TRIW96~?mSn!i{k5-gZitmJ$_c;}BB9*K>EuWMbYlFG|yLq0sFId~> zMF-k>n(26lej-$}#aXy`^HBo9@Dun2;FYR#yXj-W`N#E+1aT<$Gn8)hy1`mJDLo3p zhtyMR=h@R>-dT5p)uGyiP+4uV!zzi!q3))0sznh6!fSV6uM5}fER{rfx*T|gU#;GYe2&h z#5sVlJ%1ZNBy@5{q_3Q(0vXdDMdbLCbS#@Nw~`c9OkhP$)C`CZStszKe*(#duB?;9 zAW2Vt2r*moCSyI=v*TmwA3eSDj-J7l_gKPm_(kWMLroLP{gZO%R9TS3nW(#+#HP4@ z@f9SY02X-gfcSWhr)ECvN084oZRY*OOqIo)Ta6G*QB?%Ve7=b;@7X#52MRRSKbnt% zaI6bvASL8aU$}GRB%Fu4XXnqq2pgT3iD!73B#U|RGKc1F!V}ko_~ydr&Y?x*W?won zFFP}d&N9t~3j#Ou$Db519BEo}YfaQ<9e0*^gYtD$ii{YF?eApq-sF%iy}D&6#bYed zcu-OuUC0tz#>QAQZum<2reN_U2Jc0wpxn!!o#mEq^UOjD8FI^x8D70#d~`PmE}4r@ z^0xHp%}VGE0VG#C|7DhMXtmB#hUv{KXRFL>%a_mT3!Or1%-_CrXMC=jTg_xuX~bBS zWF?blT*sfwGn!j9AqTy8BgG?5wITOfdaCwTgRxHUZSk;`Y<#ZldG4d>+=lrjO_e$6 zi&Qd_yy?Br>f^T!^*@;QEnObg)fCoVnpqeSlbo8 zJp%{?7i(-d^m792$$aahFnlZu31Pks|2<4I35c~B#;Mg|%-}SIk*DFqAuAvJ^FKVz zhdk{57}DR;+0}N=g#C5-ZnmqS1Thr}kHRe=)r!`7=4o zoO77h6Te*bC0}}@`|Y+(=MUzYubrQ3nWs4Kjj^#z64=ff|C;^LHOqbvs~3SavYk_l zn6msbVH`1MZ~OUC#OH#RIfFl+2V|#jIe$%e$Mz%VCUa+PSu~;3kUevWt^LMrA-g;@o8hhk2%eyHp z+gfhcZhpnDUv0nWz1t-DyT!1&nfGUY_uf`S*p~IXEsDag83p6OmTCT==?`z;*spdv zv94RLu9@5J8Nb^}diVAA&sWz!yXNwHS$1C~+nV3MI2g5DthC!br`<(QAFwB{6#OMC z`MVQh*D#-V)RN!vi{+S2@i0R1sHkw|Haf3+bhW`Z@AzKi*9_M0FYLavQ+x{xJ9?w= z{mb7Yg5K{$PmjqS9?U9k0*hWl*m^o6i+c)-_Z5$2|9qn>+A;sT&9+v{c{|SgFzn5{ zCXJ$IorgIFYzC{p&VA)B?nV5lRs1o`aLBZ_BmHzkboCskR&TQ|`aj)!xw>ui?&=BK zzHHH!c;rgh{U-aTyW+jOBoEIH?Zjw`Zc5jVK6GD4tW^`T{i1qzp7M0(#ly9XqTi;4 z*PTVxMNbcF*SaOye)rx#uY7p1w$`&2`RDNF=ZAZ3Q%~CdtyW`v@ag|itHF2wZ?!td z2d7r!(fDn-oPHfj<+v3M6$+UeX8vE)YM*B@IJG+c=$dNxLt$OqCb1P2zh0@bBq&9* zmBZwfuKQ~8|4Xf|w8;8Y_jYD~s@k#i)xBCN{^d6oYfH?Bk`1dp;lKRANyZ)~Eq4fq zTf<~T^ajKjdR2+3nstUVcIPsWC>z(t^X0>MYF<_#Uu9ZnKTha$a?7l=wAQyaKOLLn zc1^NLxpT4mr914GdX@FgUR&A2fs}g-!S>q;X>_8a3e86^`<`0&KD*}~m@}}?^F%)| zRCk%}ga4o{WyJNK=(xAZt#8=n^~GUt=J)06uaA|nj8@vuc$XXXx>V#J2AudN|J?#1tzZR2OT z{lYqqzaq;n+qELwMO$kpSFdej>)D&$>mid z{((AElHyyJ(o}sp7beVu#Ok8#$GO#I`3?vBl^I?F2URb!8LLaH6d4a{64DRcUiqKA za;|S%P5j@g)dm0Os@3!O;Qu66moWni*`YZ+q?!Cwse*JUUK(y<0?9`S|+b&}u(FKYxG!z`(#KPo4w^2mi~h#*Go-;s0{0 zBO@dKO;<-pNB^6y#<|tco;}0K)i}30CnpEzR+p5NR8&;d*Vi{SH8nRkx3;$aFKBgt zfB)d%Adaj4`0*o-s~-EWZG3zjr&Rw7r~dbdBdX`-=KiNr{V$gKU*po!63(Yy`>$>N ze{49V`s@GLc6N5Yef##~$B*ml>tDZq{rU6fUug2b$N%6j|DV6>f6<@+eNq4V@E^Ya ze?IU(@BPoG{m;k#f4>;+Z2$iit1JI4R=-Qh6?FSYthO6x_;0cL_sjpSSY3z{s{=IX zY!`51wbIf0zs2f~Xvn|B>Lt`aV)fpCiPaqVQvZ9gnqks+ic&AYQ3FOQ)QG}kaOv1k zMvN$>Gt=hCrqfDg4nEeBasgK6ySHh5&f|J7yIT-6*U78pWHmSdQuWEirNpXE;*nDd z!GWxg1gQ5kU26C#3nO>5b~QsEA5?1=_`P-2%&}YIttyR^;8)`(|7b+)PVvDGK< zhy2{{j>%+LKNuwu&YPrA1q-)M>WU2xKu9G3gCB(+5`7*ebgw-hHyTzr9iw?PmiC^( zc!gqu`1&Y~g;Qk`uS}N=9HeeoYJJ{l(k1+6%=i9psYa9>OWFdi+57TGN(hC=c+{L|F_-QsA)K^5-TtB_eR4~uXUT$x43j!=BM6Tq1h7uhF21VjYT0PO|_RwuD9-@+F9DWy&#@Fh4v3>Jge zA4EAXNAt;PqT*>)0U)IXsAvovjnDNF5(sc0pHNk$QG6cjVpGWQGH2-SYl^6jX)V^l zsy2SRm>7KtcBY4W>XNThpVzx{P>WYm5Pmk|j*Ee^-c{~ZU~P(w8Pw!txK>S8HR61c z>Bud5M@vVYk~2YJOG`PV`OZz#bKc*a0(gWY_g*(g=={!QQCp;ZAmx~rJHyFrz29&A z7AIC;Y>O;P>R7!V%qH&R=9P9$)3@WwC=4i~T!!je!0zOR+ zddwiw_=RTjw!~V_CmFYMo)r^}^Z=oLBBq67i@8#EKjBtivwtE@Yj8_ZkaFV5YD3QM z-EF}z%W->r|B}DqT-?9-4fh#lcpz#c3dfcc2`d>K*Mqx=drL+M;Q<`%kbO-(tIwEx z6Au0YO?6S2c8tDbDOdR({8rY;)y(8o=f%E$i`MkZu^G6GQ?-6mwLZ$OC0~KW4H3_% zlt)Vw=h#}KV+~7tq0UtyX1j0p0%lq)l2Lclx@Y>xZzhT>uuj=YK(hYBCydmaO5-d+ zvw2uX8|7BL?e>96H;ke&(zFh(R;yVWI@gi?rY8Kj){LflGTv&gq_?bAZ27}zpx4~% zZ^<6QyuZk?q-Kqz>_;lZ))s?>W=#T4N3tw=X0L7&v$-Q#Fs{Z)OL0|m>~*&F?jc0h znWS_b!%idK5zE$b;Cc4W9^sdo*4hVm+B@vCgzvj{S+snTYIsgkA7D(0t1n)@T}i2b z(oVFxv}#&o5ZDkrEU$gY)6$$9yJxVryl@%xqIWIj`_o;Whi?+Mx<7h;Q_enI*^-j! zk+uDfx_>zHNxKQlzoA6rfZZSsyBdJw#A=))LXvbf$Vcl76G`2q?UKP4jBH9)3&*9x zUk$Oo7Juf;{FP-5Cst=SJ&P7{WTIMpk5KeW%&6OSvafed(bhTCfj$6#6JV?joQCW?!yt z(F5A43gWGM*!{^g`<0Yfn{o;{zNkeXEW}GCAj18KHE5RNQnZGU+1)V1YN0PuvhsY{ zwMJlhx_(YjJc-0o+sJBqJo~buq~Q3(YGF3;i)d4IguVOp4ZO`v^DR~XVQi~B`{8TJ z*Nexw-F?k)ub1u(NItRAndH8y3boEZ$ia7Q%+1Y`AGH%v?c`}Gs_{BInf2X%icz85o z-QAz*Yq79wm@lh;-h{#m_{r6$5JmT#-nl(l-kWI^MUTW%n?0~?XRq})|LCIdO@d*m z$@eLNXlq=~62Es{=j$KnL8dR)iX&b7#{rwlJ$nz@`y{nLrI>m;q4D&9F5vGj3Yzou>Itf~(hrss zSSS5d+B_sWKJs=K-AjjSsR}=?jBu}v5UKRqGACcvAm4N*7uO^g-3tjsMwdII-v30==b*!XK7sS0>2hLdI$|_kp1v`Ug>*#fLn)N1 zV(>;{Ombo!W#XtU;>=`X7v^JgE2C^*#93Lyn9IbF;D8I6!0s3BvM$0N9VoA=2uVJ2 z^a$)OCVF}}YKPY*9^;)r?;3Ht7yGmWb2ArB))pNc9G~ct5K4gJy~22Qpt-I>RQdeW z>7#fw6Oo$n{tF3(d(mlC390c3p9zw3=$^jJiBHx{D4k0rK8^Jaj&bXVb)k=|#U%E+ zB$kdm}%`^;my+Tyo)6a1W0^ahf87LtwrFP`o@oa*-v{P>xig=24y zy|-kgb7ZejgzOMylhr!s7$Icu9kLx#c8-_UR z_v^mz$J2^9ATent`nEj%sV=7 zUvLM#IE5wtD1Y&f4Gd{{0TO!w-%g-zd13xGA(`JT?!?teDd|p>J4=NtTUL_QTX!z& zyTp?uYc~j!DL^usX87h+GHSx^q*L5@+;MpJq<$w@`)u|ZMvhg(C?JvD%s-vtyGvW0B4nS7wdGl z&~$e(05u8hI+^bME8Ukp!(S!CbrNLi2sKWG7-KRd6M;{*GYWb@(xy~3mxX4RXJuDG$E;@5D0f0Z?68i27m16ETQ?7O1}^_ZB)sClM$` z0&AK=jL48{vOq=*kPa(vj?TC|sU~;_=zk6e(xQQki6Ch*=*6#`J@)LEE!l_c1?7v` z4e4nRz8v4dw0;3G7kJe=0uP> z7Gfk@WNJfYhXG$4H$amCBI0Ft3^Jqn4n;Cl+8l|@L>aBP>ekaG$6rxJX9o)`(u3Bi1g|LwF1bP4~ zF`1_5>?glK*CrvmDrLFbS^O`r;U;s-f9F?KR@SuMdSLT%F*rdwBphrO{dp(bd;V3^ z?}C<{EW@?BQt zGUC-rW#C6u9VL#~(BYWZ;`%$SwXKV_fh~}yLm*9bS?6}$yG79pt%soyYy!v@zD;F@ zg(#D96-W@=xoSE)JQ}&5 zH9kPb1*;L)w#*-nG_`LxI~76>e}kqtnq@el?2|%Rbn}CjYOmjrg`Kh#wK97^^S(!m zY;4PK-KOAs^%Z}i2BBwAg>3Y~l*@{YL>WZS=C2(x1`BcP)JS?Y8MV#h!vR5R|5 zRN&6Z07l31wvjeD@utYvaYL-_529c8Sv}bFZy$VKm5>FIJIoN1g{&ak>UYykF8~s8 zpyna;{c;)REaQVj#>eF{{?~x@sv;ki#!ZP=TZZ-7BkxK@BmZQ*924yP_7ET?3w~Mw z(mE{pvz#6i3T4A~X!~~my-)7=opH^v+pC2bsuwV$a-eTp1{dBP z>$VrFb#g!N)W-Hm=VsbZk{KK!(sAAM%VkC+u#se$k|%j)SIDuUnBh-P*>;;Pti8~^ zx96-S|F#@YTsw6kSk<)K0+sQ6SX;_=KnB%jB>R>fUvcSC8=(yny)Hy=+NxpHUwZD* z57&FCRo(R)-s!1sLcBT@P7ll%J#>OX5*^gVWYbm zYxP(4dOZ%Br}=?UB(s)Q}t7R4!z~Lx1e) zMe)~hLi!Pe<^w>iu$&?}y85Ba01sX|14(g?A1neL)`4)Z@%z#J%vJq4=L_QxTZNe( zr$b~X*zrJWSwPdHZYM05l>?}UY38dL5Rj@-qzq_b$q)XN(ToBZFrdNqNz~uL@eiY3 zTqF9UP1JbclbETyp+jJCXyo3MXY1^p;rH=pP(Th)889mHr%ZqhWJLgZO~ zy?2Le2wn4%VWq3s5XkB~=Hc=Ft3G614LbaLLeFdYl_$ym0!~NGW!tsz;~Ns3=h``! zs<-CXS{qx+Uzt3gDK-SWEFa{Cq)*nN0>rQYW2 zJG%I$lw2DXuPV}5=fX<<(cc^;E~ql)O=nFu@5MnN*{=Q?g0u0N1k&ovk? zv|E+W`!F4yz$HC$wEPlcI>z*O)ejCy{k0(dd_5brkzKQ1VYnERJA7Ufhw|z&;QaK- zAb#Jo9nd=SBy<6;QK(c{MuUdzZBra1HV6{qOyUsj@qEcA*?eT60I5>{U&P?b#>ePY z&b>LiAN^2r5xe8(TjTjP$3j}B>&;^mYpAv5JRp{`?0t@W#XotCX|+xI$r>oX0Gz)x zX6!UoxZ1Y!@^M*zKNI#p7l>{8ghCZ58ETHr7sbZv+R{4O`WG6zv|hWHq!x5P z6tuML!fMme|8~)2Nb;4PT!TH$s&zZrw)M!6S`x zD2cZqiN_;N;5h;luCgug}B|!=Ii; zUp-~RfMY(MCcFSYKR->DIgPyf>&eFqY0R%QpI?u!g86#@QBP0VNI)*r-(^pKmlyo5 z?EKB;3%tQXnKPmIn^c+J1Q`5TLKGbKBpiB;oyBvO*c{Hf<)6iH0&VvjFo~1pL%^4I z2aVoM4FzB6@0>LiED*f+knhf#$m%)Wt4W;2x7absSMbL07%{qTt(~7O; zx9p9cj7pE=hA)X?up9pqt3w@i+0$JU+7qYEH;u9{-4(Z1`DOrNcJllOSpp$Ui?v-5 zm$JW$n3NiO$UoqCV}wKtSKIexTkKla3Ml^@OD?v-3J@R8CIt1Dc?Pg%Wm}O6xja_J8^gYS-Qe{dF7YygP)(j-agmw zcJSwtCbp(4a}o(qOte_*D1^yJm5O@LTaJ%bpj&uXTA`Z&H81pNKh~JtX+O~stk+_6 zdU(C=)H|j8Ez8j-_ol>?mq~)%QOc<3uMp3@7-|+@0368r=&*&~wbiszI%)(D-!ESF z08&Mv>%`G@?FPM!O&bk{G71Jqwaf>VFKXoHTd&vsxmRLVDny%8)Iwc!tVtYv;+Qzd z|HDOAzoJmt2r7~A(gaR(&g;G|DpB8VJ|e>#sc6pFQ*z(vx4nzs8^wT}q%kEAy)}|( zLWFqapm<2)ggUq6^!183WzT2a*8UIbdDBw@Dz(}FPMYaIg(@w)6O9}(ZyvviTx?V- zy>EQ|u^J$}Cr-HmRTe8NOO!BoeK6sD7I#tT1GJ0Tj!8zx^=t%vPE&V?jJ#|gdWAn- z>dshi>gc1Kc=}G03++0R6<_{-DXn#oOb~3BS{NkWkH4pHmZbQzL7M;KaM!&DUaY;r zU!W>3N!As?H4o>_S!z-zoZr8D3S==VmDZ?wTW`khZ8fJUVIK!q?ol}H65vqwy-la4 z@%2+uhMVei@|>#5;^XrddD%7THBZ<#yFwCZoxcQ@(RzMqPq-J&q*Ru6e>gQ93r|Q2 z3a%~tQr0%|Y9>~-CT(n~t-5Mz)aL%bS)~NesosCTZ}Y%|ho9rbq>qv8Zy$4df7_^4 z3mBVkj|J!Mmp%6Cd-mwv?j~zWM^KOt9eu!R(-#e?k7Jq?v?D^Ro?bFCM#~Y_jY|DS?@Jj-Y2#aVmI2lUlk#IP4c4wiUMiVDg z$xgJ6UHA0Ybm_>rp0eYvd3wJfnmZtR&1iP?=|lw3)`#*dsSRrKy#I=X}|q!@zMQpL}yTLY@rfv&6`AP2RCAT+lW5-hbf)cd=^B zOtNs^NbED*c!KiHL?|8WC3;{PWEYtX-cKZ_E|!|rqAQsJU-hI;lfatjVaXy=>V#sc z!qHnaYt4CC8gpo=RjyF8wp-y1m%s#i6XwR;zsd1c_u1&n(iCVVpJiFH3n_k9X}A$4 zm}V`x!Thaa>`8@K`ebowJeNMB+9ixx*UgtI5{vzWf4xY_K`b&ZOB{{G+%ajiRsYgr z7CQG@;OE}pB{BZ#7?;CIV^yn5e-Xf!7fECjOdF8KBf5^qpE;^dxby-C&iG} zE)AO%gGOGm1H~kMRt%Sh^_G}udyI;k8h&0&6*3zngT13nUrx@7*;^oPJx_*s=r`Jw z)VqUaV;hTD=4xK3it8lD(kE+_)Y69Tu)JK#IeF8lrzjvo?PYT#3SkC+HbkZ$w@E9I z#+08IdG6j8YAbEFbg5--kLjXVX_qJNS}E zK+Sq7p}c{qWl!&N)lK6EZ>=BJn7OlFwplMuDKZ_jxL&gkq?KfBn6CB;ydJn5vk}Cf zr40+BOP-JO{Px=7lH@%#7n{+%jw>2fWU9ZTso9Cw*l;b|)*T z(M`5}%$`orQQXPuc*mjkA*t|8?cOkTXY1FRGW>E`lnIk2K%*-3@+XJE&(&mhJ71#q z2aaE=5$|ZHYH{Ya=>1mz?%^uH*>6s687(r%`^-m0?t?h3!$Qx2`dW14XghL{!)dI&!HNJ zPkivRWje|_A(KQl#Sd$A&q8{IG>%xT@N0|%A^jrxM5OzNPrNUH12~Q27zYegQ1@4l z3>KK)^x?D2v(OO__JEAL0P4#Fq2mwU1>~K>K;s^3&92%C~40citQcG+%Yq3s$$4H+wYcAWqYhIc^+++1nV?{6i~?8#(S zG}a}(8)cYwVro@IFd$nb{#-P6V=_k$hGx#D()PV?Icgn*+su z6IA29(X&mX?qBQ6#>ZqYQOAv2XYC9NYgx&E@yceyNimd5o}n?Y*9wCDb{_0aV>Keb zh@-NSt?KzzmxnyW?^t&}id9_2xM~c>9{Ibq%P$22)UZ%l-=8BA6(Bmtu6?x^uRn4& z357q^T=+u`>V3HQW7MJT8xr)G`3CP$@-2nY=8u2-LU73S0mX@L9Y!fJ;>sv-GqgxU zIkl=N>e$m=UQX6aB8+(;|tM7zUyN6zik~O z++hRI8C{aT+%iqgi^&I$8lJQoBJ3ibol3bV1tO>{(my zt&{6c0QQ+asxx!ZI`-45BJ@iC=3hRGM{BZD>q(D)BCnod=>)Yd-_&yW(|51B))A8D zq?O{VC8#u1q%=dvC5ebj!W*^sNyPQ@RL6*l5~m+>pCAad$~gO8o%_{Nj_tZk*ZNrZ z23{QMSUIP77i;}9jFrG-qp$;aje3p)^s*+!$PM|^Fv9tD=c>oOx-C42ssxBtcH7G@F# z^{!6NXhDy9Y+Q7+Ng)52p=mN3RpUU8ZF{bfVqR{?pJh4_w%?@$bv+FK&0whH4?N9w z@L6h%DPQKRz1UA+MV)&6%0P*#xVF5A?tF--AD8~HtZu#faIcg8ly4d(g%F|`Pa7S# zQKcW3YM4-LIQZI7esdOSd@c@ZWqqKWQ=N=G~EPNVDFEK_ko*u)I9bq_sbL?)| z=)&_+-CjdZ)Ns1dfU)g>WMj4-w~wKN7sIsc>CHYr^~H^?%M20<9(`C?jA^j4XyIAUgg}KN}W}pS@_ap75P=Bjwpk=gaQ-5Nv-wy?FAvL+?rer@&d3`tg=44jxZ^k2TO847P zN)}XBg_r(e{&fZ@0MNYqNKZ%^ojI$0PH#LWTsp~>ca4dempRQG)6rFdkp7)SgeBq; zM}~A#{DoT}ei9zJ@E@2hguSU{LFAt#Ae#aF?)~b|bri0kMXb#4xyx^3O+F}Nk6LXvl zF}G)o?23%zXG}_qu79n4Y9p8gG5Ad{lA<;DQ$5!Q9xi;b)g6+{M<#Id5%Wc{tR%cp zW}3awjr%~$0cVhC;^5TH8<#J=|BAEJOuliwbS62Z;nw%zh8UE_uCe9AYtyF^SmK<|g#c{XN2fVA+uZy|2ZDxQYe+r5CXnumtqk*4MhzeyJ*#iO{% zQ0Dru$fAdK?;i=lk%fh7X|cDh@yXiy_d5!*PKo@Pcs{^9QnoCr)JmWnz*ES6RcJrV_s7+0zOf9ou-6cViopybVwfO@C zXEe&`lEIs=ixSNMW&j9-y^*d=gy|8cgjNulfN5>J={`J6Z;3aNpl5N5oVK!P2Y`p% zvRAdnsMz+VNuT$*iuX(L43xqSKyd}BAIhQ$gXP3VDRZ(55rJQkPXzH7uI^lj)qVCp zm+Vk5kf`#lCFsY_Jp2}SJe@@O%IB3wfj7(>qn}n^d+|q#LvGc*reH97Zp3T#svQwg z4w66{Pw5d^MTt!1cy23u8~8^^!V;WBs+Ip3hx_NVAIw3BW^I5MJ2Ov;-l`0UYShx(O~K zjzoSoB3yPOxp&>#WZF#0ry*ve3%n;AhD#{x10a_I6{&i}rO#LuNs%VO{bmbjdv7b`frzA%<0tuEmBpN)o^EPqxQL;XU0iST*^|v2e#NT*m@xIQ^Ez{%fc?EX< zh`;v8HAdEoh$P`H`v5jK5i$Ux$i*7F887$at^w5jka>&XW*+fGB4D#7%KO7JmKdO@ zo>d@16a(Tbzgx;=Wl@8VOd}%DL}?gUYKwr_@+f8_BCJ3%r`GU8U>__z#l$1+pIdtW z&5Zn!A9hi4La_hVnU*7?M6$ zmA{haHkrP8ub8-fX0S=cu$uh))#EqTPwfEsJ_n0AIT!)u4lY?f*PxdB) zX5l0=n=S&JZ5BZplP_P0cm4YQ?LxwsRifQ3GVNXs&t62-i@N=#bCTuekWodVr{}B8 z_oJ)UcQszgCK3hpfF}NN?9ssa_f`U#`0a~SK=gt@$WG~p&B4O2OTw0+9bSoIo}-#y zPAt9%FVmUP?3c;T4QdrL;5DTWvpn=bA7dbLsady@o+xuE@t)4$c=qk+fK0p3)aYwG;{XUb! zwx0*ViMjh?9S~jy6E44lbOXu!WRTT;%@thP`vkUb98*~uFlBvk-RJhzrW^?C_W+Fpo#huG?Dqyc)F2+f_jAm~ z;ToG%IO`zbXW8{ytkFi#x0rHC6#(eQ7Y6K?fgYZ~?9E}M@9U200yr696C7~+Szu!z zYWiisY;;_cXX;PR6X(k(+nNum+YR!+d%XhOkKsk|D1$ zx2RM?f;G&u*xJGSKWZ}dqp2Amv;K@}s)_9kd7d#Ecey8?wJ7*)5MUrE9X1v<5F4v# zoH_uDx%(^q=LHJx5jSxy$n7Wc?(fUNztbl{!f1y4&R8@B4;u_LrVdW?iB>fYud53u zx`Rav!wIKhmqZ`Nk;7F)VVBr~T{^D@bdCf%Oq4$T&PHSMRge^NZf~XX-Yh&);l9mpz5O0>Zgw_%OBl&cBZBnc8fo- zJ+>0;sPelsALa;HH@P(>G8u6`>G5a(-POXh%dk0K7&jRpzxHUvF~5wIJtOg;nj*<8Al1aKHh75+6$xGLD8gzc1yxPW4|(grx|8bd4@<4CI0 zSn<{N|1VZk7o8*eB?dHSyN%y0m`bMA-+86fsO8;?7TAnqqH}ptK6Q5hh7|1GgD<&_ z&6GP;rhBwL7?~&C0vX>sJDDnd3a2i+cd0uPDV0mIVVHMZAc)zbB%N`XlsELsFYQ%_cYI8}Q0Uwm-U{@C43-#z3j*yx?DZ@X270e_9?)-0 zk-3-re)7a$S_=7uWg^~PeYh9%}xnJx6;j1Md;fp-iv^5mzsaS@ZO0F9zFKVV*C|MO&C>B+^ zR&u*q>(Ddl)4Q4K#mJgN+=PXk9A51AmEp8U9VfZ}8wWuQ;@EHy1{aJ7&cukaXTvi+5*P_Uf&VuWs)^y4*|OhPkGl%AfAMJKn!& zdNvAwCZuq0#WK}BiqE-dT@Lw$W7Qkc(tXUgjV8Mj(w^J5Zcc|(glM%II#A{IEoYxU zp~1H>bi#K=C?iyFCBLQHn+o>gojLjlDQ|deZ+3eWx42oXf0pYiq;L0=n=w|%u6O3V zu_QOZTlc)8jn1Ml{c+)Mric_Rl48Jk)#m-6&Km#J&kRgLNv|h#rq0L{Ei`$ovp1M) zcsLw3))Xm(&Wt8U^QX5ef}?|}{S&{sT}guGyKVk3F;~Xm=1O+{)X)-I{~VR{Mtee^*XfKHBp+Z%vnnxWjE-ibu&yD{Gi7?jP0$Az#(lzs;73(+tZ5 zj#YgmMq{eB`@BS(%*3-Rs z@D;are0EYUZt8cmrnlNpJN%TqBE~pwGaAg9*1ZTiLoz-p9JXj^NUJ?}5n|*J8?j#3 zc9v@vk`AaCu{&A!(pIAMU z&1MBu?hzaJOHRoZE61wbN+kG{z0TYB6jA&rHW6Bj$^YIiqB0*d@i_KI!JmyUP$kSH zfuXUG>WZi)^d3-gvZ08!h7Fx=8X05WSj^HPs-w^fLFy^Ke#5dJ$S0v7DW-2YwMn^^7kE_DmPpkAS zy^?p1xrL#!Nhq{qh+nEcla4t7OjA#);rQ%EEoZNvz*-z~Am_9q0jo^fpEF+U1OL-y{tfMTHh+k4D>+)^y3|TzxvnFzB zX*^`)Q`V<0`Rw9hTxwPDeFNbn%Y5hd?!bb1Q!C_=;Qqa;w&jxqyU;nKp~=jp%n*Y8 z%&$TJr~XUtADO>lNA;lW0YYNi)nN1%0C0QRa`$+?_4TE=AfM=E8Cqp|7b9MuuL=MZ zM!%iP0JkuywmQ?8(Xng17lBJ*6gJiJ{2tHQ!F5j7{p;M+Dg0#XX$_krC?F#_hNg^x%GU#W3g|r*r!5R(u2;KQmxYK+j5`sy1$4T&7Rd0o zbVJFh`n+`|AfzLiA*B0r&dtnJb+`RYFCv4OsUU3fN0vLAsqprG@K8{YdrHo_wrJC7 z?dsoJ`}xIrO4%<#lp}yRU6mq#<8<_@bmRRX*;bU++L$H~Sep>%a`RooO#TT@@<8$) z=Vz6m>*@ZfJ()XlJ8y}27CcZC^GQm(SGBZ+yZ3Kkxez z>*O$S5D=L5{o$n*%TEh%&Q6=p^Pg#+#?G$T{b=d=^EKd2n|<>>Ysb&N-8+!5_f(Dz zaU~rBmV;P^VGaMJk%)~K|4#2seO<8cJI~us&xq|q62H~lfBYryK;_*C+X>hf(;jho zj(IDsy8CJG(awre)qCmC`O4dy{=o)3s*QSTvjo=Z9L9R|&g4dxRkF+8Cksr<2DJ>(w z*=Lh^QB3M&kUIW5lO0Jezk|(gV{A3h>!U}Y1Fjh^F-w5pqoKBdUwJ@?A}%^QlKVTr zhSJ9!g2M^MUOWNfun)XE zp;S~0z$HNHDilqo%I~L99&(JqG+7r^Fm=(ZXRN=8V1nQRrxus{xPGqWBO5?;wq#D< zL98N?52e>@Za35NHXXdkDc@kz5mx`9UyE+qh0eG2(8B$iR_vizp*8hM(>9 zpylUX?6Nor8ppt;i;KRXtg-aJ;gCQ=p`I?{CXl078+r(k`GXf`DB{Kf7%vd+K%^y; zt}Vcb2i*ml#nBr94z#gM>V$uJeUigaZmbhGB%Pkpb<2oPLJMc<(#L00Bv0GVqH1j{ zt;4!+XL7P~WNp2m57EYue{>n_=#c{mFF6f{RGw(DZ2`2Sm(8;wI@z~Rww-1d^DWe| zuLt>MM7#cce!2e!fJF7iz#?KveSFta0^&SEA#BaWfO6N| z@%q0H*OelUUCB_CEBn>}h>g`en`Lh-ivGjs5-Fc0f{p5?P>h``uZ?yTx))>5C&5HG z-~I$?UiU553nW3Gllz{#P)p2y6s{yQ1R~GoE}%*P9UBm`CDgd}5mEx6k9&171EM1j zNKSV_+6O=W(KoDx8omWgW5$Xc-P|?@jk9f@@JA^6I|Vx#dk{13zm26=#<$eO)e4n1 z(daRkV5vytVXe=)j`);OEYE%!f=Gqz{_Mk`1uek_4FVy8Kz@4{Iz+E^3ACuZ6q2~b zBw2!(>;ezMnYMW2NTA>bP3Z;@ha&cZ1C?sfyy+zUfId6UE>`&>v%4kT*hx_Rsdo+4%e+;P>%)Bi1=}Sx?7e<|u zr5bI&+FSi+v|8LofCel%GzxaALWbDla46tw6e55nmRSohttTRa0KU`#_{BLx9r4Wh zz?TL98(1vB--X8(4=^C>nc$g3fiK7LU@ZXSERKh*3k>PPc_JCcH{2RD@9vbOo1ioP z+2OOxl$;mLvOS2|g@j%MDp9)}c|GQcOzyD?vAJ#e)ouG(0*9#MjntCAt^iF+7pLGS z+ybtNwwG(qfu|i9h=6copBIeYV-?)SQqF-gu&%K5f;Y5U`(|QK?iKZ0rxyU->;Bz) z5qXEcYYm@x2Z?cK*AvwZSp+TM4Y!2z_KO;y3~*~<5u<=Y*)e7m1$9bBD37GBOCYy* z!JqGW``LPcJcq=}zn~@Yxm;PT!wcMm6eF^k>U~~Zm7B?4@B)s3t*hD+#eo6ROX4PD zJy(*d1=FgZ|6S~is<_w6L+&fPhyLm(1S?^Ijq4*s0PO~iiCrJ7Evi+~*tY1(@^_D;BR4CF(NNRp$WUJV(SzxUZ|esggC z{r>9}=^4v|H!4yG{{|RMwGh>{he29w5${2^fL+8c@wXOzh|CCfiJ_gQ+j#$*s_l1! zx^lCF>D7aNC!b+{y)IWgGIjiCP%krT8P%Lbrsa0;H9$n?Y?5`DE&zK z7(nYnqVkcY4)&!Q0aQ;P^e+0?S^NCk-~acd7j4>0gQdS z>ZKK4ckd#l^+E=`$KRdjadRcp^ZIc{RKs3yz`sfSb~^Q2lj7)%dyiilvABs&=;KF%DNYf(+kNFAg=l!w534QkMv zn#`}|cxN?n4f35Px*e$oUN;rgUh5a7DpoOFy7gMWo=NFds*;a_K06S4mkDMOtzLV|@y@YtuXqEED;Z}vU+L3j^;g5f?+jn~H%>ce)Lm^-BDa!bZ(t^F(dyJvgmyd~5e{0$QATjLuWHwbarj!!HB8{sVd&`6(=#pBn|M!wQT^K%nLE?XY#nu(!DHv;L~C54E=60>JZhp+13-Mmt1lSa5ag z%cj<<35M#2Wz}seH6IgenqENHR1IOkB8N9FGhVDPUS50PgKTvc8GF+6&PT7YZ*0f084K^zg>d)RVkGGDfk zX6{DJCAHeODplHNtj20o`KQje+YHdRAd0nyf+4#x?W3DNHaH&k6{+o(tJ%`-Fe~C9 zryo&FK0Jb5U@m^JdKa@+m)Ls=%>`TxDneL?@E~Xa@<&b6h0MPRy5{5M-M6aw@^BT~$`fTOj=GVfuzweZ}wKT(riAv)(=FwFGBQ>*12eKobd8(A&S9{-ey zT4ap@mMbNPAO>=@SY7Z5x>)+b4vQ3ZMhcd`e3M=aTg+>{Iow4 zP83uX9is1njYxyYFs7r61|_;V!`Y)|362{gFVyMd3ibU6=N3>_S4pfg+h@&uxN55r zW`SRymD+ChgK5=!lUP(BZGW0thPPc?@X_WNQY*o}GvKq;)>(!xPD%+OqLJH7qLGS& zt3~bILPCKVa(CaiXT82SMX%)6?2EnFs&zGeW!aJa;QpKrzoZozLu8n5^84fs6be3A zzD4wO@mmV^o4**?hG4q20=`TX>1Iej?)fwn_Dh}$M3xl7fnWl7sVeI={NwSxV zaI9h|;VjchSRd?$RR!NEwuqbh_LmV%G%>2MvQU+4Fr1lRI;4jnobA{RR1@5Ceuk7S z562YpRDxrFC2$mziLKEXI>nvx^+@=>FM!T+b@F@|!g6rpA;8pl3ZFk?uSJ>< z@>vfh=(U14_w-s=Zu{2?=fxQ|h?X`c81zW)8jVQFDHIYXMPbQ}IP3jb4d~fgh%w^#dTSw!kj~}K`=2AoW6nRQa*T=(eJ?{o7 zvidp_>6ykdRV9!XKkKwE{gYO2CHl@{;H|Ea-F%W#da=|LemSC=RackBC9N7ggyqud zAm*lZ5udfCv-=j=>TaY(k*fUQCnWrRxN2->x+8QBwx#F zDqWr67|P2}Knc&AzdBn+wdcM0t|TO{1gw=d9EfhmXWZ$0-8=6)1}nsjKI03jA;P=e zZ7;8K_<){G+K_5c&xEdjjb-9Wj3*#sDVU8JYVO?O_WDn>55BMoEp8Bpqa8u0T#G~? zTsRRx6j;C4RU9MUZGF`;Ircm`i>G#*M2DmJqkmbH1jJ#ZV1c5dpi^%=7UvYx zjgc;C&n~=JpqdUtnR{(a&)}8D{9qc1asX(Rc^yhbw6kRJW64&*3XaZJO(h*2NV%`~ zDD5|?n_<#J_Rn4yy8WJOdCz7PUcE-?iXtU3F?As;)^e#L$s%wrr+~7DdHt}+TF5%o zMJ8^=c4soGg~Er>RSw71qI+fAw=fS#AnsW#6T0 zWE{jw$H;?9EaRGuV+YS5w#_a%nNi(b_>u?Cgn@(N~#6;gm$rK)t5hbqo!g z5gZZSYIm(i;#L~&B>c(T^;B-qEFw^=E-Wg3o~GhH{ibJ+?xyj*h$Zr+7Dqdmr_^pY zc{&H=tg16y?zC?9<&h3|8pOHF10sYKY13p17eM@2aNuu={FZG^`Y6ZTu(uwTr@iqE z?iJ_Pu}9ww=Bh`e&E`j9rPC^m^UBP^Q>R3YlZl*fN_1l&=KO?!lGAk;w$ws!>Npuz zSW=;C!5E2(kT$ODzF}6clA-CqNieL&@4`ilk zZd<>;sHYtke3NpEOl3eaBktHHhN+gyr#JUqlBVG{6dbg)NKUm&wtO9H!RL4B^{+73 zd)0ty6l$-=-D`|BN|r!{CBJep_hL^cfm7Y^)P~Wyo^{I{!j9y|1_BmQOyMj%-w!IE z?z`>(mEl7o&Ch12lCR%WwXDxiW?E2hC0h&C>_#nb8RodsSmjM}h@=Ml_xXY7ZEi%M zzHGK@!{U+ArnmC?GcIXUZh(a`5Zd$ab$=pFd^oGPdxV+msZWkx5b$h&ZJv|ov`6^s2#iLUx zpEe;w(B;}!tL7_I$9YHXdMYAOgV%~H!}<0;Vzia@X6=sMByG6_s+*u#Sxu(Zs_bu@ zRPaa1#$Z7_lZ0M2+>otX-+C~c{dJbI;~3$ec1Di-_voF=kxeL?V~mFq!Vaz;H1l( z{Kzn-^VyIH1jDXjt2p~e60?ad&*v2s7;HK_Ex$PJz#tH-am`^UAN4`O9 z;o-O4X0oP`-izwk$4OCa?Vs{g+ve0LSAME!hVExL3$(ZBNzZn3YdA&U(OLIITw~%H z<6yPD4V5F~9IOK%d@w%P6-pP44Jm1%xD0U(eWY^}gAtm&Y>Yl9*&Cy% zvp&KYzQ2pGI4DEXC8`uCb!4;V+AFcT)6ig-abfS3S0m5s*8K|3VtF)V;)8>_pUV8I z63;r0_HNh+x)UiZ2z+HX0Yuw;3cvk^0##f9q9d>o;)dD&>pVo-GV`D8BV-^g9={y= zQ>ePl{N(k-uXs*c_SN0|e?NXAFSbpfe-8@I|NcBb2l~P#G3NjhfCdeScZAaV0@+Cj zhKqaKXuvHpjGqMhFXbZtt4#7BCh_)}}oW2m6NP(EiLM?ou)+A^W1{zI< zUd+R-(NrFXRCi5bMri8LL};WVSmT17_l3ofVDX2rtBzn%S+K+*71j8^LRkbnoZ1FM zRegx4mqotuMYfQjl7)yCSyW#l@|81c>=1RQ5H;mX({uQt3YIe{ z95I}2)8pirlAa$4C%sfY+FE}hmzW^OL$|0{-=HeTV&?aS-;Bwt{@><-^rMM2{0mlx zqZadeS(~YaCr2-dNwQH%Tk7@Y_l_{vgo*(GX_?Le2ogC&WJpm^F8YRz%JNDkJm*y~M z3_t&*5n8Q2|?g`Q2zxHZT* zo6Fe9pGr@&-9P3X@e`nKz|c*L)EukoFz7e>H4N7CzVzoTm=?>KX6`ZPWj0p}^e+iH z7GF%xCLYTen2WZV`@t35L5e-NdUzV2@5s(1hkr=3#6p|PFu`U{O zEG=~`@zY$!%Us+~UaH4K3aX%Ra3B>qE?dOF*dedjWiC&sm(EI7>^l~|+An_zuJDNn zmv(e;{67KC3^DWTXE0y_QXq_C69ZJB2o4Ed#_b@i;2rS*XPn9kR$u}qAO;w*4kjut z6bT9mQ!e7NF%eTGP16^vli98lAG4F;Xb~*|lVm2~2#f$Zl7JP404l722Ot107@!D@ zKnNxxfY8MWfX5(?fFcZn27bV)W?%x6pbqH32qd5g3PJ*|U@ok{RqQW1+sHF95;XG? zG7l3q&5}YtlOHFPGFLM^=b~f`A_xC4z(4eetXf88AYhnSArB}J2adoFx=tbRpa^aN z2oxd;y1)l~pgCE=LWV#J&Y%v)Z3w&oKkPsVdSD3jlaMCB0=|G1oZ#xHkrmEB3Iqa5 zabO5&<_fw%2zG!Dq5ubmi4J^V2cAd@zNia!#zJf~E|e$_q+kcGUhhT-4hZk@F0)ij~vg`rA;3j~eSfQX* z3IafzAOgq@YYYMpB)|?rq6jA70os5@?=(;Ir(V0jPgN8bIa4nN)leOCGBmDp+XDl7fEB7>J(j=@@Sq8_V!Ot* zQVJpkd;n8;HBySC1wNKQmULVC69UR;0#Ft`L8K&@GXkst2inF(s=^3%UDMIZ5YdGT(Hib9nYEPw~nb#9P?6|`VIid6!#Vp#4oa4}_7Sb+t2 z^;3a0KPP2oTcAH1fQv97WzaJN3IfxRa{{0M2f9Zo3c>;wEoGu$Jy;+Jf}kNHfOnA7 zKsTianqWOxfe9#r2w(z1i6UYnRtmTlI=vPz`ZX^al{+W(LN!!i1GZo@lzE*u&MMVI zGUVSgjxeyhbPv z!Ug&>0=`HBayEp#CqdD(4n+4=WW;oLAY<(}EPNs+WY-A5mp*U+3T)Q`c7Q^3NNedf zgY6)7r7%zpqImxq^-x22J5_N}@8WnVGIrK2g_}D5^k;6Ae`X7e4ux-h5^bc zD^@mqhh==76#{y|T_Ys}aJ5;Vbq*rH1uozL;P+*>vYvAP9D(hz4{DOqXH{qDZl*2!Nmqn!p)_U}vI$2a14;3gQf=#$|Y52(l)K zX4zA8s1E;x;0GA73ar2&!oUZDzNyuM%w+B47WU;-+@htOcYCV)q6(Sr*Dgilz651ND% zgKdo$q3r^Nky4@MvYZ#%gmr0p-?ph9Z7%XaZZQBqfXxY7Ab(Oo3Fy-r37QoQ`k@`V zE+85vC7L?T);rPmG!&cS~F96rCqwM**dO`(xL-mh8^R|%!e2bbX*`e{+{8sY#dRDfSG=HGT)`z= ztO2}`ZyYi-8!wKWr4d`mDSXGBT$c!t2^&xVqg=`t@Byv70kJ&G7Z3xvyvvvH%L{PJ z#T)?@u*?h4%hCM*)O^kPkIdbC|KL2%tDw#4yb7G~$@Tm$s4x-5&MZxCmqr+V9~WO(*3;B3w;sy{L_J}$5r~rXS>Kt_`?&L zxDnO0=OW1uTh)=A)#W0kK|L_3yTn}_#+h8k?fSd1Tflj}#{06?iM>cfJ;VQBJH(G$ zzHJ=XcU{=qJIH^1tc!imZ{5^OeZkio+i8-&ja=5xd&9#W)i1i*LH*jDz1fHS+0}i< z*?rpGz1?+v*v;MAkDb)J9oJ8N#M4{fMcmu7UE1Hfsq5Xs(Oux9-PZ>`-L?C{q20k5 zp22gv;GO*7ZVLjCW9ujGO$88?x<^ADhe&=%?={26)CpzdYYUp*m=!rby zlYZSXUg`H8>Pg(`W81!^p1!I6=&K&<7hdZF{^hU!=F48~RXprbeC+?T{^E7s>|vhn z*FNO$p6y-!z~R2a<^JQ1{_O3Z=h1%IJAUr@p75Le@5MXt_kQr}-tB=L@0&jH1;6d} z9xFxN@dvu(e_qM?y|6?7)w%xby}sWu|H3_A>_K1IpPt$KUh^eC@0q^x7k~2ozV#pf z^*28DSKs!@e)t)G^Ov6XbDr~g-}Mpv_q)F5w?6fY`}9d4_MgA--+uYS8}bd`@_l~v zQNHx)V&%sl+zsO7L0tAlUi*8S`;VXab^rF$KKF%x_TfMBlRx{}zqH*S_iO+10fLpl zfdmU0JV?-n7K98NGSp(jp~Q)-AX2=DP$9;R2^VtQXz?S+h$8{*c{(33+w_WXHNXiuU`jp9W5R4GoTIBx>IiZ!d&tz2PR z?WvPzQmjyGQZ?(eW?7nRXP%|`R%TndQqf}l+BL7pI^y}NtjXm(r;_0u(hku?+?{PHXM*shDbRb3rD%f6f0lGBbeG*Pc z;Y1O(1fE(AmbBnS37YhvdmA>lUQzs=XyR2DR>)$D6RwC-hLxe%REgoyI8}(J>B!-T zA9YC4hbiUfVv$Bl2xCby*4AH&PTt6#jXC0|p_DZxd1RJa_J!p~OVZV3jsEdSB9(BF zDUy&A0lB7+2%`B>ms`$Br;0>|wqKNHx&`N(dxANpl}!F=-gR{zis)W)ZuF&eVNMyS zo|20BWR-+IXJ?|CZkj1jjV|WqNP_ZtDSDAsI^(2}0<|couD*Iu5IgulYpu54ifgXA z?qI>Jy8a5Rt~Ov0Y_Z1rFzm6)>WYC4%RXysv(W!So9wjDR%@-Y(q^lzv)mes?YF^( zEAFq`mJ4hKbim4PyByikM!fRQOK-jQ-rI&6SEzw+zy8*%#u@(({BOVnAAIi_{36V- zyb3qmu)z>BEb+t#KWy>E3uj!gz#I=;@yGpQEONhc@ZxUDDzD6P%PzkRbIdZ&OmodP z-;8t4I`7PL&p!VQbkIT%O?1&lAB}X_>Z-5Kdh4#g4twmf Q&rW;ow%?9B*8u?lJIV*8p#T5? literal 0 HcmV?d00001 diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 13b0f186ceb98..581879322caf3 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -14,6 +14,7 @@ autoware_cmake + autoware_map_msgs diagnostic_msgs fmt geometry_msgs diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp new file mode 100644 index 0000000000000..2e8052b41d413 --- /dev/null +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -0,0 +1,299 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ndt_scan_matcher/map_update_module.hpp" + +template +double norm_xy(const T p1, const U p2) +{ + double dx = p1.x - p2.x; + double dy = p1.y - p2.y; + return std::sqrt(dx * dx + dy * dy); +} + +MapUpdateModule::MapUpdateModule( + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, + std::shared_ptr ndt_ptr, + std::shared_ptr tf2_listener_module, std::string map_frame, + rclcpp::CallbackGroup::SharedPtr main_callback_group, + std::shared_ptr> state_ptr) +: ndt_ptr_(ndt_ptr), + ndt_ptr_mutex_(ndt_ptr_mutex), + map_frame_(map_frame), + logger_(node->get_logger()), + clock_(node->get_clock()), + tf2_listener_module_(tf2_listener_module), + state_ptr_(state_ptr), + dynamic_map_loading_update_distance_( + node->declare_parameter("dynamic_map_loading_update_distance")), + dynamic_map_loading_map_radius_( + node->declare_parameter("dynamic_map_loading_map_radius")), + lidar_radius_(node->declare_parameter("lidar_radius")) +{ + initial_estimate_particles_num_ = node->declare_parameter("initial_estimate_particles_num"); + + sensor_aligned_pose_pub_ = + node->create_publisher("monte_carlo_points_aligned", 10); + ndt_monte_carlo_initial_pose_marker_pub_ = + node->create_publisher( + "monte_carlo_initial_pose_marker", 10); + + auto main_sub_opt = rclcpp::SubscriptionOptions(); + main_sub_opt.callback_group = main_callback_group; + + map_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + ekf_odom_sub_ = node->create_subscription( + "ekf_odom", 100, std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1), + main_sub_opt); + + loaded_pcd_pub_ = node->create_publisher( + "debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); + + service_ = node->create_service( + "ndt_align_srv", + std::bind( + &MapUpdateModule::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group_); + + pcd_loader_client_ = node->create_client( + "pcd_loader_service", rmw_qos_profile_services_default); + while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO( + logger_, + "Waiting for pcd loader service. Check if the enable_differential_load in " + "pointcloud_map_loader is set `true`."); + } + + double map_update_dt = 1.0; + auto period_ns = std::chrono::duration_cast( + std::chrono::duration(map_update_dt)); + map_update_timer_ = rclcpp::create_timer( + node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this), + map_callback_group_); +} + +void MapUpdateModule::service_ndt_align( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) +{ + // get TF from pose_frame to map_frame + auto TF_pose_to_map_ptr = std::make_shared(); + tf2_listener_module_->get_transform( + clock_->now(), map_frame_, req->pose_with_covariance.header.frame_id, TF_pose_to_map_ptr); + + // transform pose_frame to map_frame + const auto mapTF_initial_pose_msg = transform(req->pose_with_covariance, *TF_pose_to_map_ptr); + update_map(mapTF_initial_pose_msg.pose.pose.position); + + if (ndt_ptr_->getInputTarget() == nullptr) { + res->success = false; + RCLCPP_WARN(logger_, "No InputTarget"); + return; + } + + if (ndt_ptr_->getInputSource() == nullptr) { + res->success = false; + RCLCPP_WARN(logger_, "No InputSource"); + return; + } + + // mutex Map + std::lock_guard lock(*ndt_ptr_mutex_); + + (*state_ptr_)["state"] = "Aligning"; + res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, mapTF_initial_pose_msg); + (*state_ptr_)["state"] = "Sleeping"; + res->success = true; + res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; + + last_update_position_ = res->pose_with_covariance.pose.pose.position; +} + +void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr) +{ + current_position_ = odom_ptr->pose.pose.position; + + if (last_update_position_ == std::nullopt) { + return; + } + double distance = norm_xy(current_position_.value(), last_update_position_.value()); + if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up."); + } +} + +void MapUpdateModule::map_update_timer_callback() +{ + if (current_position_ == std::nullopt) { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, *clock_, 1, + "Cannot find the reference position for map update. Please check if the EKF odometry is " + "provided to NDT."); + return; + } + if (last_update_position_ == std::nullopt) return; + + // continue only if we should update the map + if (should_update_map(current_position_.value())) { + RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)"); + update_map(current_position_.value()); + last_update_position_ = current_position_; + } +} + +bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const +{ + if (last_update_position_ == std::nullopt) return false; + double distance = norm_xy(position, last_update_position_.value()); + return distance > dynamic_map_loading_update_distance_; +} + +void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) +{ + auto request = std::make_shared(); + request->area.center = position; + request->area.radius = dynamic_map_loading_map_radius_; + request->cached_ids = ndt_ptr_->getCurrentMapIDs(); + + // // send a request to map_loader + auto result{pcd_loader_client_->async_send_request( + request, + [](rclcpp::Client::SharedFuture) {})}; + + std::future_status status = result.wait_for(std::chrono::seconds(0)); + while (status != std::future_status::ready) { + RCLCPP_INFO(logger_, "waiting response"); + if (!rclcpp::ok()) { + return; + } + status = result.wait_for(std::chrono::seconds(1)); + } + update_ndt(result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); +} + +void MapUpdateModule::update_ndt( + const std::vector & maps_to_add, + const std::vector & map_ids_to_remove) +{ + RCLCPP_INFO( + logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size()); + if (maps_to_add.empty() && map_ids_to_remove.empty()) { + RCLCPP_INFO(logger_, "Skip map update"); + return; + } + const auto exe_start_time = std::chrono::system_clock::now(); + + NormalDistributionsTransform backup_ndt = *ndt_ptr_; + + // Add pcd + for (const auto & map_to_add : maps_to_add) { + pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); + pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr); + backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id); + } + + // Remove pcd + for (const std::string & map_id_to_remove : map_ids_to_remove) { + backup_ndt.removeTarget(map_id_to_remove); + } + + backup_ndt.createVoxelKdtree(); + + const auto exe_end_time = std::chrono::system_clock::now(); + const double exe_time = + std::chrono::duration_cast(exe_end_time - exe_start_time).count() / + 1000.0; + RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); + + // swap + (*ndt_ptr_mutex_).lock(); + // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should + // ideally be avoided. But I will leave this for now since I cannot come up with a solution other + // than using pointer of pointer. + *ndt_ptr_ = backup_ndt; + (*ndt_ptr_mutex_).unlock(); + + publish_partial_pcd_map(); +} + +geometry_msgs::msg::PoseWithCovarianceStamped MapUpdateModule::align_using_monte_carlo( + const std::shared_ptr & ndt_ptr, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) +{ + if (ndt_ptr->getInputTarget() == nullptr || ndt_ptr->getInputSource() == nullptr) { + RCLCPP_WARN(logger_, "No Map or Sensor PointCloud"); + return geometry_msgs::msg::PoseWithCovarianceStamped(); + } + + // generateParticle + const auto initial_poses = + create_random_pose_array(initial_pose_with_cov, initial_estimate_particles_num_); + + std::vector particle_array; + auto output_cloud = std::make_shared>(); + + for (unsigned int i = 0; i < initial_poses.size(); i++) { + const auto & initial_pose = initial_poses[i]; + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); + ndt_ptr->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr->getResult(); + + Particle particle( + initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, + ndt_result.iteration_num); + particle_array.push_back(particle); + const auto marker_array = make_debug_markers( + clock_->now(), map_frame_, tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1), particle, + i); + ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + + auto sensor_points_mapTF_ptr = std::make_shared>(); + pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose); + publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr); + } + + auto best_particle_ptr = std::max_element( + std::begin(particle_array), std::end(particle_array), + [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; }); + + geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; + result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; + result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; + + return result_pose_with_cov_msg; +} + +void MapUpdateModule::publish_point_cloud( + const rclcpp::Time & sensor_ros_time, const std::string & frame_id, + const std::shared_ptr> & sensor_points_mapTF_ptr) +{ + sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; + pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); + sensor_points_mapTF_msg.header.stamp = sensor_ros_time; + sensor_points_mapTF_msg.header.frame_id = frame_id; + sensor_aligned_pose_pub_->publish(sensor_points_mapTF_msg); +} + +void MapUpdateModule::publish_partial_pcd_map() +{ + pcl::PointCloud map_pcl = ndt_ptr_->getVoxelPCD(); + + sensor_msgs::msg::PointCloud2 map_msg; + pcl::toROSMsg(map_pcl, map_msg); + map_msg.header.frame_id = "map"; + + loaded_pcd_pub_->publish(map_msg); +} diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 877ffef7908dc..a9c37d7843a64 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -116,8 +116,6 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.step_size = this->declare_parameter("step_size"); ndt_params.resolution = this->declare_parameter("resolution"); ndt_params.max_iterations = this->declare_parameter("max_iterations"); - int search_method = this->declare_parameter("neighborhood_search_method"); - ndt_params.search_method = static_cast(search_method); ndt_params.num_threads = this->declare_parameter("num_threads"); ndt_params.num_threads = std::max(ndt_params.num_threads, 1); ndt_params.regularization_scale_factor = @@ -222,10 +220,17 @@ NDTScanMatcher::NDTScanMatcher() diagnostic_thread_.detach(); tf2_listener_module_ = std::make_shared(this); - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); - pose_init_module_ = std::make_unique( - this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, - state_ptr_); + + if (this->declare_parameter("use_dynamic_map_loading")) { + map_update_module_ = std::make_unique( + this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, + state_ptr_); + } else { + map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); + pose_init_module_ = std::make_unique( + this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, + state_ptr_); + } } void NDTScanMatcher::timer_diagnostic() diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index 8f3ccbff00360..61b02c490c663 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -2,8 +2,8 @@ ros__parameters: enable_whole_load: true enable_downsampled_whole_load: false - enable_partial_load: false - enable_differential_load: false + enable_partial_load: true + enable_differential_load: true # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] From cb3cee8305bac06421beaf7e4eec555cce3aae85 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 6 Feb 2023 14:43:19 +0900 Subject: [PATCH 28/72] feat(interpolation): add maintainer (#2812) Signed-off-by: yutaka --- common/interpolation/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/common/interpolation/package.xml b/common/interpolation/package.xml index 665e26bab9a0a..5a39d818a3238 100644 --- a/common/interpolation/package.xml +++ b/common/interpolation/package.xml @@ -6,6 +6,7 @@ The spline interpolation package Fumiya Watanabe Takayuki Murooka + Yutaka Shimizu Apache License 2.0 ament_cmake_auto From d68966777137cf61e071a29cf17039cc6e3ce486 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Mon, 6 Feb 2023 15:08:33 +0900 Subject: [PATCH 29/72] fix(behavior_velocity_planner): fix misc concurrency issues (#2718) * fix(behavior_velocity_planner): fix misc concurrency issues Signed-off-by: Vincent Richard * style(pre-commit): autofix * chore(behavior_velocity_planner): fix silly bot indent Signed-off-by: Vincent Richard * style(pre-commit): autofix --------- Signed-off-by: Vincent Richard Signed-off-by: Vincent Richard Co-authored-by: Vincent Richard Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/behavior_velocity_planner/src/node.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 9bf21a9d8fbea..126b629362161 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -324,9 +324,11 @@ void BehaviorVelocityPlannerNode::onAcceleration( void BehaviorVelocityPlannerNode::onParam() { + // Note(vrichard): mutex lock is not necessary as onParam is only called once in the constructed. + // It would be required if it was a callback. + // std::lock_guard lock(mutex_); planner_data_.velocity_smoother_ = std::make_unique(*this); - return; } void BehaviorVelocityPlannerNode::onLaneletMap( @@ -367,6 +369,7 @@ void BehaviorVelocityPlannerNode::onExternalIntersectionStates( void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) { + std::lock_guard lock(mutex_); planner_data_.external_velocity_limit = *msg; } @@ -392,10 +395,9 @@ void BehaviorVelocityPlannerNode::onVirtualTrafficLightStates( void BehaviorVelocityPlannerNode::onTrigger( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) { - mutex_.lock(); // for planner_data_ + std::unique_lock lk(mutex_); if (!isDataReady(planner_data_, *get_clock())) { - mutex_.unlock(); return; } @@ -407,20 +409,17 @@ void BehaviorVelocityPlannerNode::onTrigger( if (!planner_data_.route_handler_) { RCLCPP_INFO_THROTTLE( get_logger(), *get_clock(), 3000, "Waiting for the initialization of route_handler"); - mutex_.unlock(); return; } - // NOTE: planner_data must not be referenced for multithreading - const auto planner_data = planner_data_; - mutex_.unlock(); - if (input_path_msg->points.empty()) { return; } const autoware_auto_planning_msgs::msg::Path output_path_msg = - generatePath(input_path_msg, planner_data); + generatePath(input_path_msg, planner_data_); + + lk.unlock(); path_pub_->publish(output_path_msg); stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); From 1d56bbbdd5d869ca1851aff2ed8fda8feda149a8 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 6 Feb 2023 16:01:15 +0900 Subject: [PATCH 30/72] feat(autoware_auto_perception_rviz_plugin): add simple visualize mode (#2814) * feat(autoware_auto_perception_rviz_plugin): add simple visualize mode Signed-off-by: tomoya.kimura * fix typo Signed-off-by: tomoya.kimura --------- Signed-off-by: tomoya.kimura --- .../object_detection/object_polygon_detail.hpp | 4 ++-- .../object_detection/object_polygon_display_base.hpp | 11 ++++++++++- .../autoware_auto_perception_rviz_plugin/package.xml | 2 ++ .../src/object_detection/object_polygon_detail.cpp | 12 ++++++++---- 4 files changed, 22 insertions(+), 7 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index 47c704c97709b..5d99b8a463711 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -126,7 +126,7 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::Sha get_predicted_path_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const std_msgs::msg::ColorRGBA & predicted_path_color); + const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( @@ -163,7 +163,7 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_lis AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( const autoware_auto_perception_msgs::msg::PredictedPath & paths, - std::vector & points); + std::vector & points, const bool is_simple = false); /// \brief Convert Point32 to Point /// \param val Point32 to be converted diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 76cdee1839b01..acf4a389ec6e3 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -87,6 +87,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_display_type_property->addOption("3d", 0); m_display_type_property->addOption("2d", 1); m_display_type_property->addOption("Disable", 2); + m_simple_visualize_mode_property = new rviz_common::properties::EnumProperty( + "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this, + SLOT(updatePalette())); + m_simple_visualize_mode_property->addOption("Normal", 0); + m_simple_visualize_mode_property->addOption("Simple", 1); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { const auto & class_property_values = map_property_it.second; @@ -262,7 +267,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase if (m_display_predicted_paths_property.getBool()) { const std::string uuid_str = uuid_to_string(uuid); const std_msgs::msg::ColorRGBA predicted_path_color = get_color_from_uuid(uuid_str); - return detail::get_predicted_path_marker_ptr(shape, predicted_path, predicted_path_color); + return detail::get_predicted_path_marker_ptr( + shape, predicted_path, predicted_path_color, + m_simple_visualize_mode_property->getOptionInt() == 1); } else { return std::nullopt; } @@ -384,6 +391,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase PolygonPropertyMap m_polygon_properties; // Property to choose type of visualization polygon rviz_common::properties::EnumProperty * m_display_type_property; + // Property to choose simplicity of visualization polygon + rviz_common::properties::EnumProperty * m_simple_visualize_mode_property; // Property to enable/disable label visualization rviz_common::properties::BoolProperty m_display_label_property; // Property to enable/disable uuid visualization diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index aec99ef805f5f..4771589dc9038 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -7,6 +7,8 @@ Apex.AI, Inc. Satoshi Tanaka Taiki Tanaka + Takeshi Miura + Apache 2.0 ament_cmake diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index b5d891f186338..73a524e130776 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -72,7 +72,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const std_msgs::msg::ColorRGBA & predicted_path_color) + const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; @@ -84,7 +84,7 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( marker_ptr->color.a = std::max( static_cast(std::min(static_cast(predicted_path.confidence), 0.999)), 0.5); marker_ptr->scale.x = 0.03 * marker_ptr->color.a; - calc_path_line_list(predicted_path, marker_ptr->points); + calc_path_line_list(predicted_path, marker_ptr->points, is_simple); for (size_t k = 0; k < marker_ptr->points.size(); ++k) { marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0; } @@ -640,8 +640,10 @@ void calc_2d_polygon_bottom_line_list( void calc_path_line_list( const autoware_auto_perception_msgs::msg::PredictedPath & paths, - std::vector & points) + std::vector & points, const bool is_simple) { + const int circle_line_num = is_simple ? 5 : 10; + for (int i = 0; i < static_cast(paths.path.size()) - 1; ++i) { geometry_msgs::msg::Point point; point.x = paths.path.at(i).position.x; @@ -652,7 +654,9 @@ void calc_path_line_list( point.y = paths.path.at(i + 1).position.y; point.z = paths.path.at(i + 1).position.z; points.push_back(point); - calc_circle_line_list(point, 0.25, points, 10); + if (!is_simple || i % 2 == 0) { + calc_circle_line_list(point, 0.25, points, circle_line_num); + } } } From f8f4bd6d4ef306191850f681c1838031561d6473 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Mon, 6 Feb 2023 16:03:36 +0900 Subject: [PATCH 31/72] perf(motion_utils): improve performance of zero_order_hold (#2744) * pref(resample): improve performance of zero_order_hold Signed-off-by: veqcc * fix minor bugs Signed-off-by: veqcc * ci(pre-commit): autofix --------- Signed-off-by: veqcc Co-authored-by: veqcc Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/interpolation/zero_order_hold.hpp | 46 ++++++++++++++----- common/motion_utils/src/resample/resample.cpp | 22 +++++++-- 2 files changed, 54 insertions(+), 14 deletions(-) diff --git a/common/interpolation/include/interpolation/zero_order_hold.hpp b/common/interpolation/include/interpolation/zero_order_hold.hpp index 1142cb544c174..966128321b470 100644 --- a/common/interpolation/include/interpolation/zero_order_hold.hpp +++ b/common/interpolation/include/interpolation/zero_order_hold.hpp @@ -21,37 +21,61 @@ namespace interpolation { -template -std::vector zero_order_hold( - const std::vector & base_keys, const std::vector & base_values, - const std::vector & query_keys, const double overlap_threshold = 1e-3) +inline std::vector calc_closest_segment_indices( + const std::vector & base_keys, const std::vector & query_keys, + const double overlap_threshold = 1e-3) { // throw exception for invalid arguments const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); - interpolation_utils::validateKeysAndValues(base_keys, base_values); - std::vector query_values; + std::vector closest_segment_indices(validated_query_keys.size()); size_t closest_segment_idx = 0; for (size_t i = 0; i < validated_query_keys.size(); ++i) { // Check if query_key is closes to the terminal point of the base keys if (base_keys.back() - overlap_threshold < validated_query_keys.at(i)) { closest_segment_idx = base_keys.size() - 1; } else { - for (size_t j = closest_segment_idx; j < base_keys.size() - 1; ++j) { + for (size_t j = base_keys.size() - 1; j > closest_segment_idx; --j) { if ( - base_keys.at(j) - overlap_threshold < validated_query_keys.at(i) && - validated_query_keys.at(i) < base_keys.at(j + 1)) { + base_keys.at(j - 1) - overlap_threshold < validated_query_keys.at(i) && + validated_query_keys.at(i) < base_keys.at(j)) { // find closest segment in base keys - closest_segment_idx = j; + closest_segment_idx = j - 1; + break; } } } - query_values.push_back(base_values.at(closest_segment_idx)); + closest_segment_indices.at(i) = closest_segment_idx; + } + + return closest_segment_indices; +} + +template +std::vector zero_order_hold( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & closest_segment_indices) +{ + // throw exception for invalid arguments + interpolation_utils::validateKeysAndValues(base_keys, base_values); + + std::vector query_values(closest_segment_indices.size()); + for (size_t i = 0; i < closest_segment_indices.size(); ++i) { + query_values.at(i) = base_values.at(closest_segment_indices.at(i)); } return query_values; } + +template +std::vector zero_order_hold( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys, const double overlap_threshold = 1e-3) +{ + return zero_order_hold( + base_keys, base_values, calc_closest_segment_indices(base_keys, query_keys, overlap_threshold)); +} } // namespace interpolation #endif // INTERPOLATION__ZERO_ORDER_HOLD_HPP_ diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 14f44b7892645..e9b6dcbc34cce 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -243,8 +243,12 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const auto lerp = [&](const auto & input) { return interpolation::lerp(input_arclength, input, resampled_arclength); }; + + auto closest_segment_indices = + interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, resampled_arclength); + return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = @@ -389,8 +393,14 @@ autoware_auto_planning_msgs::msg::Path resamplePath( const auto lerp = [&](const auto & input) { return interpolation::lerp(input_arclength, input, resampled_arclength); }; + + std::vector closest_segment_indices; + if (use_zero_order_hold_for_v) { + closest_segment_indices = + interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + } const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, resampled_arclength); + return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = @@ -541,8 +551,14 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const auto lerp = [&](const auto & input) { return interpolation::lerp(input_arclength, input, resampled_arclength); }; + + std::vector closest_segment_indices; + if (use_zero_order_hold_for_twist) { + closest_segment_indices = + interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + } const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, resampled_arclength); + return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = From e91af6f2277be6197e3f0025554a45e635277f48 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 6 Feb 2023 09:39:07 +0100 Subject: [PATCH 32/72] =?UTF-8?q?fix(bag=5Ftime=5Fmanager=5Frviz=5Fplugin)?= =?UTF-8?q?:=20update=20create=5Fclient=20API=20to=20use=20rc=E2=80=A6=20(?= =?UTF-8?q?#2783)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix(bag_time_manager_rviz_plugin): update create_client API to use rclcpp::QoS Signed-off-by: Esteve Fernandez --- .../src/bag_time_manager_panel.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp index dde3f10e9b554..c2ed9f0b4bc1b 100644 --- a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp +++ b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace rviz_plugins { BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent) @@ -66,12 +68,17 @@ BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel( void BagTimeManagerPanel::onInitialize() { raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - client_pause_ = - raw_node_->create_client("/rosbag2_player/pause", rmw_qos_profile_services_default); - client_resume_ = - raw_node_->create_client("/rosbag2_player/resume", rmw_qos_profile_services_default); - client_set_rate_ = - raw_node_->create_client("/rosbag2_player/set_rate", rmw_qos_profile_services_default); + +// APIs taking rclcpp::QoS objects are only available in ROS 2 Iron and higher +#if RCLCPP_VERSION_MAJOR >= 18 + const auto qos_default = rclcpp::ServicesQoS(); +#else + const auto qos_default = rmw_qos_profile_services_default; +#endif + + client_pause_ = raw_node_->create_client("/rosbag2_player/pause", qos_default); + client_resume_ = raw_node_->create_client("/rosbag2_player/resume", qos_default); + client_set_rate_ = raw_node_->create_client("/rosbag2_player/set_rate", qos_default); } void BagTimeManagerPanel::onPauseClicked() From 69b9085cdb3e8d243655accc1555db57381e0a36 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 6 Feb 2023 21:46:52 +0900 Subject: [PATCH 33/72] refactor(motion_velocity_smoother): remove unneccesary optional (#2811) * remove optional for lateral acc filter Signed-off-by: Takamasa Horibe * apply to behavior_velocity_planner Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../include/utilization/trajectory_utils.hpp | 2 +- .../analytical_jerk_constrained_smoother.hpp | 2 +- .../smoother/smoother_base.hpp | 4 ++-- .../src/motion_velocity_smoother_node.cpp | 22 +++++++------------ .../analytical_jerk_constrained_smoother.cpp | 8 ++----- .../src/smoother/smoother_base.cpp | 18 ++++----------- 6 files changed, 18 insertions(+), 38 deletions(-) diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp index a7fdc337d3dae..ce04fe9d5c7a9 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -105,7 +105,7 @@ inline bool smoothPath( // Resample trajectory with ego-velocity based interval distances auto traj_resampled = smoother->resampleTrajectory( - *traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold, + traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index c3d7af26e4cf8..30f02a057666f 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -77,7 +77,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase [[maybe_unused]] const double nearest_dist_threshold, [[maybe_unused]] const double nearest_yaw_threshold) const override; - boost::optional applyLateralAccelerationFilter( + TrajectoryPoints applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const override; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index 1b75981bc823b..3219a0f41967f 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -71,12 +71,12 @@ class SmootherBase const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, const double nearest_yaw_threshold) const = 0; - virtual boost::optional applyLateralAccelerationFilter( + virtual TrajectoryPoints applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0, [[maybe_unused]] const double a0 = 0.0, [[maybe_unused]] const bool enable_smooth_limit = false) const; - boost::optional applySteeringRateLimit(const TrajectoryPoints & input) const; + TrajectoryPoints applySteeringRateLimit(const TrajectoryPoints & input) const; double getMaxAccel() const; double getMinDecel() const; diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index d27294b24e23b..a5fffbf90a401 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -523,30 +523,24 @@ bool MotionVelocitySmootherNode::smoothVelocity( const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed) const { + if (input.empty()) { + return false; // cannot apply smoothing + } + // Calculate initial motion for smoothing const auto [initial_motion, type] = calcInitialMotion(input, input_closest); // Lateral acceleration limit const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(input, initial_motion.vel, initial_motion.acc, true); - if (!traj_lateral_acc_filtered) { - RCLCPP_ERROR(get_logger(), "Fail to do traj_lateral_acc_filtered"); - - return false; - } // Steering angle rate limit const auto traj_steering_rate_limited = - smoother_->applySteeringRateLimit(*traj_lateral_acc_filtered); - if (!traj_steering_rate_limited) { - RCLCPP_ERROR(get_logger(), "Fail to do traj_steering_rate_limited"); - - return false; - } + smoother_->applySteeringRateLimit(traj_lateral_acc_filtered); // Resample trajectory with ego-velocity based interval distance auto traj_resampled = smoother_->resampleTrajectory( - *traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x, + traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); @@ -593,7 +587,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size()); if (publish_debug_trajs_) { { - auto tmp = *traj_lateral_acc_filtered; + auto tmp = traj_lateral_acc_filtered; if (is_reverse_) flipVelocity(tmp); pub_trajectory_latacc_filtered_->publish(toTrajectoryMsg(tmp)); } @@ -603,7 +597,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp)); } { - auto tmp = *traj_steering_rate_limited; + auto tmp = traj_steering_rate_limited; if (is_reverse_) flipVelocity(tmp); pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp)); } diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index d9b06bcd4da72..fa1479b6f0093 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -277,16 +277,12 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::resampleTrajectory( return output; } -boost::optional AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter( +TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const { - if (input.empty()) { - return boost::none; - } - if (input.size() < 3) { - return boost::optional(input); // cannot calculate lateral acc. do nothing. + return input; // cannot calculate lateral acc. do nothing. } // Interpolate with constant interval distance for lateral acceleration calculation. diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index ca9af83a86864..42d07145c78bf 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -69,16 +69,12 @@ double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; } double SmootherBase::getMinJerk() const { return base_param_.min_jerk; } -boost::optional SmootherBase::applyLateralAccelerationFilter( +TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const { - if (input.empty()) { - return boost::none; - } - if (input.size() < 3) { - return boost::optional(input); // cannot calculate lateral acc. do nothing. + return input; // cannot calculate lateral acc. do nothing. } // Interpolate with constant interval distance for lateral acceleration calculation. @@ -135,16 +131,10 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( return output; } -boost::optional SmootherBase::applySteeringRateLimit( - const TrajectoryPoints & input) const +TrajectoryPoints SmootherBase::applySteeringRateLimit(const TrajectoryPoints & input) const { - if (input.empty()) { - return boost::none; - } - if (input.size() < 3) { - return boost::optional( - input); // cannot calculate the desired velocity. do nothing. + return input; // cannot calculate the desired velocity. do nothing. } // Interpolate with constant interval distance for lateral acceleration calculation. std::vector out_arclength; From 3d41b575721bb68b0aa5d7a9542713bdeb33cbdf Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 6 Feb 2023 22:27:07 +0900 Subject: [PATCH 34/72] refactor(motion_velocity_smoother): remove unneccesary resampling (#2813) * refactor(motion_velocity_smoother): remove unneccesary resampling Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../analytical_jerk_constrained_smoother.hpp | 4 +- .../smoother/smoother_base.hpp | 8 ++- .../src/motion_velocity_smoother_node.cpp | 10 +-- .../analytical_jerk_constrained_smoother.cpp | 28 +++++--- .../src/smoother/smoother_base.cpp | 67 ++++++++++++------- 5 files changed, 74 insertions(+), 43 deletions(-) diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 30f02a057666f..9cddba5ca500a 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -79,8 +79,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase TrajectoryPoints applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - [[maybe_unused]] const double a0, - [[maybe_unused]] const bool enable_smooth_limit) const override; + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, + const bool use_resampling = true, const double input_points_interval = 1.0) const override; void setParam(const Param & param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index 3219a0f41967f..ea85f8dc78615 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -73,10 +73,12 @@ class SmootherBase virtual TrajectoryPoints applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0, - [[maybe_unused]] const double a0 = 0.0, - [[maybe_unused]] const bool enable_smooth_limit = false) const; + [[maybe_unused]] const double a0 = 0.0, [[maybe_unused]] const bool enable_smooth_limit = false, + const bool use_resampling = true, const double input_points_interval = 1.0) const; - TrajectoryPoints applySteeringRateLimit(const TrajectoryPoints & input) const; + TrajectoryPoints applySteeringRateLimit( + const TrajectoryPoints & input, const bool use_resampling = true, + const double input_points_interval = 1.0) const; double getMaxAccel() const; double getMinDecel() const; diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index a5fffbf90a401..9dc88115a85d8 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -531,12 +531,14 @@ bool MotionVelocitySmootherNode::smoothVelocity( const auto [initial_motion, type] = calcInitialMotion(input, input_closest); // Lateral acceleration limit - const auto traj_lateral_acc_filtered = - smoother_->applyLateralAccelerationFilter(input, initial_motion.vel, initial_motion.acc, true); + constexpr bool enable_smooth_limit = true; + constexpr bool use_resampling = true; + const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter( + input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling); - // Steering angle rate limit + // Steering angle rate limit (Note: set use_resample = false since it is resampled above) const auto traj_steering_rate_limited = - smoother_->applySteeringRateLimit(traj_lateral_acc_filtered); + smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false); // Resample trajectory with ego-velocity based interval distance auto traj_resampled = smoother_->resampleTrajectory( diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index fa1479b6f0093..126c1f9be291a 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -279,23 +279,31 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::resampleTrajectory( TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, + const bool use_resampling, const double input_points_interval) const { if (input.size() < 3) { return input; // cannot calculate lateral acc. do nothing. } // Interpolate with constant interval distance for lateral acceleration calculation. - constexpr double points_interval = 0.1; // [m] - std::vector out_arclength; - const std::vector in_arclength = trajectory_utils::calcArclengthArray(input); - for (double s = 0; s < in_arclength.back(); s += points_interval) { - out_arclength.push_back(s); + const double points_interval = use_resampling ? input_points_interval : 0.1; // [m] + + TrajectoryPoints output; + // since the resampling takes a long time, omit the resampling when it is not requested + if (use_resampling) { + std::vector out_arclength; + const std::vector in_arclength = trajectory_utils::calcArclengthArray(input); + for (double s = 0; s < in_arclength.back(); s += points_interval) { + out_arclength.push_back(s); + } + const auto output_traj = + motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); + auto output = motion_utils::convertToTrajectoryPointArray(output_traj); + output.back() = input.back(); // keep the final speed. + } else { + output = input; } - const auto output_traj = - motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); - output.back() = input.back(); // keep the final speed. constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points const size_t idx_dist = diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 42d07145c78bf..b0688fa698f1f 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -71,25 +71,34 @@ double SmootherBase::getMinJerk() const { return base_param_.min_jerk; } TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, + const bool use_resampling, const double input_points_interval) const { if (input.size() < 3) { return input; // cannot calculate lateral acc. do nothing. } + constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points + // Interpolate with constant interval distance for lateral acceleration calculation. - constexpr double points_interval = 0.1; // [m] - std::vector out_arclength; - const auto traj_length = motion_utils::calcArcLength(input); - for (double s = 0; s < traj_length; s += points_interval) { - out_arclength.push_back(s); + TrajectoryPoints output; + const double points_interval = + use_resampling ? base_param_.sample_ds : input_points_interval; // [m] + // since the resampling takes a long time, omit the resampling when it is not requested + if (use_resampling) { + std::vector out_arclength; + const auto traj_length = motion_utils::calcArcLength(input); + for (double s = 0; s < traj_length; s += points_interval) { + out_arclength.push_back(s); + } + const auto output_traj = + motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); + output = motion_utils::convertToTrajectoryPointArray(output_traj); + output.back() = input.back(); // keep the final speed. + } else { + output = input; } - const auto output_traj = - motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); - output.back() = input.back(); // keep the final speed. - constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points const size_t idx_dist = static_cast(std::max(static_cast((curvature_calc_dist) / points_interval), 1)); @@ -131,24 +140,34 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( return output; } -TrajectoryPoints SmootherBase::applySteeringRateLimit(const TrajectoryPoints & input) const +TrajectoryPoints SmootherBase::applySteeringRateLimit( + const TrajectoryPoints & input, const bool use_resampling, + const double input_points_interval) const { if (input.size() < 3) { return input; // cannot calculate the desired velocity. do nothing. } + // Interpolate with constant interval distance for lateral acceleration calculation. - std::vector out_arclength; - const auto traj_length = motion_utils::calcArcLength(input); - for (double s = 0; s < traj_length; s += base_param_.sample_ds) { - out_arclength.push_back(s); + const double points_interval = use_resampling ? base_param_.sample_ds : input_points_interval; + TrajectoryPoints output; + // since the resampling takes a long time, omit the resampling when it is not requested + if (use_resampling) { + std::vector out_arclength; + const auto traj_length = motion_utils::calcArcLength(input); + for (double s = 0; s < traj_length; s += points_interval) { + out_arclength.push_back(s); + } + const auto output_traj = + motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); + output = motion_utils::convertToTrajectoryPointArray(output_traj); + output.back() = input.back(); // keep the final speed. + } else { + output = input; } - const auto output_traj = - motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); - output.back() = input.back(); // keep the final speed. - const size_t idx_dist = static_cast(std::max( - static_cast((base_param_.curvature_calculation_distance) / base_param_.sample_ds), 1)); + const size_t idx_dist = static_cast( + std::max(static_cast((base_param_.curvature_calculation_distance) / points_interval), 1)); // Calculate curvature assuming the trajectory points interval is constant const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist); @@ -163,14 +182,14 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit(const TrajectoryPoints & i const double mean_vel = (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; const double dt = - std::max(base_param_.sample_ds / mean_vel, std::numeric_limits::epsilon()); + std::max(points_interval / mean_vel, std::numeric_limits::epsilon()); const double steering_diff = fabs(output.at(i).front_wheel_angle_rad - output.at(i + 1).front_wheel_angle_rad); const double dt_steering = steering_diff / tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate); if (dt_steering > dt) { - const double target_mean_vel = (base_param_.sample_ds / dt_steering); + const double target_mean_vel = (points_interval / dt_steering); for (size_t k = 0; k < 2; k++) { const double temp_vel = output.at(i + k).longitudinal_velocity_mps * (target_mean_vel / mean_vel); From ba3ae686ef5a115939d4c7814473c0dc57ce6639 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Mon, 6 Feb 2023 22:47:11 +0900 Subject: [PATCH 35/72] chore(ekf_localizer): typo (#2795) Signed-off-by: Vincent Richard --- localization/ekf_localizer/src/state_transition.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ekf_localizer/src/state_transition.cpp b/localization/ekf_localizer/src/state_transition.cpp index 8c87436e60141..88fc9f76d7168 100644 --- a/localization/ekf_localizer/src/state_transition.cpp +++ b/localization/ekf_localizer/src/state_transition.cpp @@ -30,7 +30,7 @@ double normalizeYaw(const double & yaw) * y_{k+1} = y_k + vx_k * sin(yaw_k + b_k) * dt * yaw_{k+1} = yaw_k + (wz_k) * dt * b_{k+1} = b_k - * vx_{k+1} = vz_k + * vx_{k+1} = vx_k * wz_{k+1} = wz_k * * (b_k : yaw_bias_k) From cad63f732e2e20180bf0d6b997bc4d631202a189 Mon Sep 17 00:00:00 2001 From: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com> Date: Mon, 6 Feb 2023 22:59:25 +0900 Subject: [PATCH 36/72] fix(freespace_planning_algorithms): throw error when no waypoints stored (#2505) * fix(freespace_planning_algorithms): throw error when no waypoints stored Signed-off-by: Hirokazu Ishida * test: continue if plan is failed Signed-off-by: Hirokazu Ishida * Return waypoints even when solution is not found Signed-off-by: Hirokazu Ishida * ci(pre-commit): autofix --------- Signed-off-by: Hirokazu Ishida Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takamasa Horibe --- .../abstract_algorithm.hpp | 1 + .../src/abstract_algorithm.cpp | 3 +++ .../test_freespace_planning_algorithms.cpp | 21 +++++++++---------- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp index b59888fa52281..f9ecdbc826d5d 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp @@ -111,6 +111,7 @@ class AbstractPlanningAlgorithm const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0; virtual bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } + virtual ~AbstractPlanningAlgorithm() {} protected: diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index 143f147b79eee..2b5814ee2a111 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -89,6 +89,9 @@ geometry_msgs::msg::Pose local2global( double PlannerWaypoints::compute_length() const { + if (waypoints.empty()) { + std::runtime_error("cannot compute cost because waypoint has size 0"); + } double total_cost = 0.0; for (size_t i = 0; i < waypoints.size() - 1; ++i) { const auto pose_a = waypoints.at(i); diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 08c23c24f73d6..8ea9e0b4af8b5 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -270,8 +270,6 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) for (size_t i = 0; i < goal_poses.size(); ++i) { const auto goal_pose = goal_poses.at(i); - bool success_local = true; - algo->setMap(costmap_msg); double msec; double cost; @@ -284,7 +282,9 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) for (size_t j = 0; j < N_mc; j++) { const rclcpp::Time begin = clock.now(); if (!algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose))) { - success_local = false; + success_all = false; + std::cout << "plan fail" << std::endl; + continue; } const rclcpp::Time now = clock.now(); time_sum += (now - begin).seconds() * 1000.0; @@ -295,19 +295,18 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) } else { const rclcpp::Time begin = clock.now(); - success_local = algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose)); + if (!algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose))) { + success_all = false; + std::cout << "plan fail" << std::endl; + continue; + } const rclcpp::Time now = clock.now(); msec = (now - begin).seconds() * 1000.0; cost = algo->getWaypoints().compute_length(); } - if (success_local) { - std::cout << "plan success : " << msec << "[msec]" - << ", solution cost : " << cost << std::endl; - } else { - success_all = false; - std::cout << "plan fail : " << msec << "[msec]" << std::endl; - } + std::cout << "plan success : " << msec << "[msec]" + << ", solution cost : " << cost << std::endl; const auto result = algo->getWaypoints(); geometry_msgs::msg::PoseArray trajectory; for (const auto & pose : result.waypoints) { From c1c4ab5165128a3ac7f3463369090f60d8674316 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 7 Feb 2023 11:58:01 +0900 Subject: [PATCH 37/72] refactor(detection_by_tracker): remove unnecessary statement (#2823) remove unnecessary statement Signed-off-by: yoshiri --- .../detection_by_tracker/src/detection_by_tracker_core.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 3c29f63ea2366..b9ee6fa9b8b6f 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -414,8 +414,6 @@ void DetectionByTracker::mergeOverSegmentedObjects( const float precision = perception_utils::get2dPrecision(initial_object.object, extended_tracked_object); if (precision < precision_threshold) { - if (tracked_object.classification.front().label) - std::cout << "Rejected by Perception Threshold:" << precision << std::endl; continue; } pcl::PointCloud pcl_cluster; From a00e6710c0bff0096615a33e7ef65a266ff49c53 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 7 Feb 2023 15:58:55 +0900 Subject: [PATCH 38/72] fix(motion_velocity_smoother): resample making empty path (#2827) * fix(motion_velocity_smoother): resampling interval arg Signed-off-by: Takamasa Horibe * fix override variables Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../analytical_jerk_constrained_smoother.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 126c1f9be291a..3b013b77e4b01 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -287,7 +287,7 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilt } // Interpolate with constant interval distance for lateral acceleration calculation. - const double points_interval = use_resampling ? input_points_interval : 0.1; // [m] + const double points_interval = use_resampling ? 0.1 : input_points_interval; // [m] TrajectoryPoints output; // since the resampling takes a long time, omit the resampling when it is not requested @@ -299,7 +299,7 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilt } const auto output_traj = motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); + output = motion_utils::convertToTrajectoryPointArray(output_traj); output.back() = input.back(); // keep the final speed. } else { output = input; From d3918e7ee05b01dbd9c4e4467ecdd5f958dd1f2f Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 7 Feb 2023 16:41:45 +0900 Subject: [PATCH 39/72] fix(ground_segmentation): fix unuse_time_series_filter bug (#2824) Signed-off-by: badai-nguyen --- .../ground_segmentation/ground_segmentation.launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index afc55582853f9..198aef2cdcdcd 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -463,7 +463,9 @@ def launch_setup(context, *args, **kwargs): components.extend( pipeline.create_single_frame_obstacle_segmentation_components( input_topic=LaunchConfiguration("input/pointcloud"), - output_topic=pipeline.single_frame_obstacle_seg_output, + output_topic=pipeline.single_frame_obstacle_seg_output + if pipeline.use_single_frame_filter or pipeline.use_time_series_filter + else pipeline.output_topic, ) ) From 8684d2a1a4cbb707e8dd311c7fe9e96c2713ea8b Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 7 Feb 2023 11:18:59 +0100 Subject: [PATCH 40/72] build(freespace_planning_algorithms): add nlohmann-json3-dev dependency (#2818) --- planning/freespace_planning_algorithms/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index ea0813c5b57ea..4fc302d58de65 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -19,6 +19,7 @@ geometry_msgs nav_msgs + nlohmann-json-dev rosbag2_cpp std_msgs tf2 From fb9b4537363f7adf2ddddcbc7c8b8768369c3dc2 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Tue, 7 Feb 2023 13:20:46 +0300 Subject: [PATCH 41/72] chore: update CODEOWNERS (#2821) Signed-off-by: GitHub Co-authored-by: kenji-miyake --- .github/CODEOWNERS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 43af74eb8b9c2..ee0cd2e75930f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -4,7 +4,7 @@ common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_auto_common/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners common/autoware_auto_geometry/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners -common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners +common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_auto_tf2/** jit.ray.c@gmail.com @autowarefoundation/autoware-global-codeowners common/autoware_point_types/** taichi.higashide@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_testing/** adam.dabrowski@robotec.ai @autowarefoundation/autoware-global-codeowners @@ -17,7 +17,7 @@ common/fake_test_node/** opensource@apex.ai @autowarefoundation/autoware-global- common/global_parameter_loader/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners common/goal_distance_calculator/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners common/grid_map_utils/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners +common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners From 650a35b778a5ac4cb34212957dfbf3147c64a038 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 7 Feb 2023 13:23:51 +0300 Subject: [PATCH 42/72] ci(pre-commit): autoupdate (#2819) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .pre-commit-config.yaml | 2 +- common/autoware_testing/autoware_testing/smoke_test.py | 1 - .../ground_segmentation/ground_segmentation.launch.py | 3 --- .../lane_driving/behavior_planning/behavior_planning.launch.py | 1 - .../launch/scenario_planning/parking.launch.py | 1 - map/map_loader/test/lanelet2_map_loader_launch.test.py | 1 - .../lidar_apollo_segmentation_tvm_nodes/test/launch.test.py | 1 - .../lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py | 2 -- planning/freespace_planning_algorithms/test/debug_plot.py | 1 - .../test/debug_plot_rrtstar_core.py | 1 - .../scripts/calclation_time_analyzer.py | 1 - .../obstacle_cruise_planner/scripts/trajectory_visualizer.py | 1 - .../obstacle_velocity_limiter/script/velocity_visualizer.py | 1 - planning/planning_debug_tools/scripts/trajectory_visualizer.py | 1 - .../test/test_static_centerline_optimizer.test.py | 1 - .../accel_brake_map_calibrator/scripts/calc_utils.py | 1 - .../scripts/new_accel_brake_map_server.py | 1 - .../accel_brake_map_calibrator/scripts/plotter.py | 2 +- .../accel_brake_map_calibrator/scripts/view_plot.py | 1 - .../launch/external_cmd_converter.launch.py | 1 - 20 files changed, 2 insertions(+), 23 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index c54e4ce2f8462..57f65ac523bd2 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -61,7 +61,7 @@ repos: - id: isort - repo: https://github.com/psf/black - rev: 22.12.0 + rev: 23.1.0 hooks: - id: black args: [--line-length=100] diff --git a/common/autoware_testing/autoware_testing/smoke_test.py b/common/autoware_testing/autoware_testing/smoke_test.py index 8c361474034ad..2594ddf7955b1 100644 --- a/common/autoware_testing/autoware_testing/smoke_test.py +++ b/common/autoware_testing/autoware_testing/smoke_test.py @@ -52,7 +52,6 @@ def resolve_node(context, *args, **kwargs): @pytest.mark.launch_test def generate_test_description(): - arg_package = DeclareLaunchArgument( "arg_package", default_value=["default"], description="Package containing tested executable" ) diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 198aef2cdcdcd..c072d4deb3059 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -249,7 +249,6 @@ def create_common_pipeline(self, input_topic, output_topic): return components def create_single_frame_obstacle_segmentation_components(self, input_topic, output_topic): - additional_lidars = self.ground_segmentation_param["additional_lidars"] use_ransac = bool(self.ground_segmentation_param["ransac_input_topics"]) use_additional = bool(additional_lidars) @@ -424,7 +423,6 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con @staticmethod def get_additional_lidars_concatenated_component(input_topics, output_topic): - return ComposableNode( package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", @@ -507,7 +505,6 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): - launch_arguments = [] def add_launch_arg(name: str, default_value=None): diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index aa2d31ff15745..55650f5e6738c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -30,7 +30,6 @@ def launch_setup(context, *args, **kwargs): - # vehicle information parameter vehicle_param_path = LaunchConfiguration("vehicle_param_file").perform(context) with open(vehicle_param_path, "r") as f: diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py index acf0b7546e27f..dca6866a6c811 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py @@ -28,7 +28,6 @@ def launch_setup(context, *args, **kwargs): - vehicle_info_param_path = LaunchConfiguration("vehicle_param_file").perform(context) with open(vehicle_info_param_path, "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] diff --git a/map/map_loader/test/lanelet2_map_loader_launch.test.py b/map/map_loader/test/lanelet2_map_loader_launch.test.py index 75abe2164ca04..24758a46f75aa 100644 --- a/map/map_loader/test/lanelet2_map_loader_launch.test.py +++ b/map/map_loader/test/lanelet2_map_loader_launch.test.py @@ -27,7 +27,6 @@ @pytest.mark.launch_test def generate_test_description(): - lanelet2_map_path = os.path.join( get_package_share_directory("map_loader"), "test/data/test_map.osm" ) diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py b/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py index f657270865c7e..0a9adafc3f641 100644 --- a/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py +++ b/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py @@ -24,7 +24,6 @@ @pytest.mark.launch_test def generate_test_description(): - lidar_apollo_segmentation_tvm = Node( package="lidar_apollo_segmentation_tvm_nodes", executable="lidar_apollo_segmentation_tvm_nodes_exe", diff --git a/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py b/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py index 3153351922219..5a1135379615f 100755 --- a/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py +++ b/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py @@ -22,7 +22,6 @@ def main(args=None): - rclpy.init(args=args) node = Node("lidar_centerpoint_visualizer") @@ -53,5 +52,4 @@ def main(args=None): if __name__ == "__main__": - main() diff --git a/planning/freespace_planning_algorithms/test/debug_plot.py b/planning/freespace_planning_algorithms/test/debug_plot.py index f6c77bec52466..7d6277a2756c6 100755 --- a/planning/freespace_planning_algorithms/test/debug_plot.py +++ b/planning/freespace_planning_algorithms/test/debug_plot.py @@ -202,7 +202,6 @@ def create_concate_png(src_list, dest, is_horizontal): algo_name = algo_names[i] algo_pngs = [] for j in range(n_case): - fig, ax = plt.subplots() result_dir = dir_name_table[(algo_name, j)] diff --git a/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py b/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py index f263e08bf968c..3fa6d152eba11 100755 --- a/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py +++ b/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py @@ -97,7 +97,6 @@ def visualize(self): if __name__ == "__main__": - with open("/tmp/rrt_result.txt", "r") as f: dict_data = json.load(f) tree = Tree.from_dict(dict_data) diff --git a/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py b/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py index 55f7b447fc42a..908cc204da2e5 100644 --- a/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py +++ b/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py @@ -79,7 +79,6 @@ def CallbackCalculationCost(self, msg): if __name__ == "__main__": - parser = argparse.ArgumentParser() parser.add_argument("-f", "--functions", type=str, default="solveOsqp") args = parser.parse_args() diff --git a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py index 00833d4e84e83..5e5cfb207a625 100755 --- a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py +++ b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py @@ -36,7 +36,6 @@ class TrajectoryVisualizer(Node): def __init__(self): - super().__init__("trajectory_visualizer") self.fig = plt.figure() diff --git a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py index 087325c6cc0a8..d26f6133b0b54 100755 --- a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py +++ b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py @@ -24,7 +24,6 @@ class TrajectoryVisualizer(Node): def __init__(self): - super().__init__("trajectory_visualizer") self.fig = plt.figure() diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index 8845bbb52c056..f4ff9b5587312 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -62,7 +62,6 @@ class TrajectoryVisualizer(Node): def __init__(self): - super().__init__("trajectory_visualizer") self.fig = plt.figure() diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py index 517b6eaf9e477..e4bcdeab1232c 100644 --- a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py +++ b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py @@ -27,7 +27,6 @@ @pytest.mark.launch_test def generate_test_description(): - lanelet2_map_path = os.path.join( get_package_share_directory("static_centerline_optimizer"), "test/data/lanelet2_map.osm" ) diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/calc_utils.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/calc_utils.py index a031cc679abaf..ce9667d23bf6c 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/calc_utils.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/calc_utils.py @@ -71,7 +71,6 @@ def lowpass_filter_scipy(x, sample_rate, fp, fs, g_pass, g_stop): def create_2d_map( x, y, data, color_factor, x_index_list, x_thresh, y_index_list, y_thresh, calibration_method ): - if x.shape != y.shape or y.shape != data.shape: print("Error: the shape of x, y, data must be same") sys.exit() diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py index 9b5cfc9186495..a2d82a460aed0 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -254,7 +254,6 @@ def view_pedal_accel_graph( calibrated_pedal_list, calibrated_acc_list, ): - fig = plotter.subplot_more(subplot_num) # calibrated map diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plotter.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plotter.py index 6053d787c5e92..5541dc4f95748 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plotter.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plotter.py @@ -102,7 +102,7 @@ def imshow(self, data, left, right, ws, bottom, top, hs, num_data_type="float"): origin="lower", ) ys, xs = np.meshgrid(width_range, height_range, indexing="ij") - for (x, y, val) in zip(xs.flatten(), ys.flatten(), data.flatten()): + for x, y, val in zip(xs.flatten(), ys.flatten(), data.flatten()): self.plot_text(x, y, val, num_data_type) plt.colorbar() diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py index 1ec5f6a328361..4d713cf4848e0 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py @@ -158,7 +158,6 @@ def view_pedal_accel_graph( calibrated_acc_list, scatter_only, ): - fig = plotter.subplot_more(subplot_num) if not scatter_only: diff --git a/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py b/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py index 4f09c484aa0f4..01029004e6e47 100644 --- a/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py +++ b/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py @@ -25,7 +25,6 @@ def _create_mapping_tuple(name): def generate_launch_description(): - arguments = [ # component DeclareLaunchArgument("use_intra_process"), From 26944a36c1075a3ec92f1f20aa70f546c22e6486 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 7 Feb 2023 19:25:13 +0900 Subject: [PATCH 43/72] chore(motion_velocity_smoother): add maintainer (#2816) Signed-off-by: Fumiya Watanabe --- planning/motion_velocity_smoother/package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index 2550abdd03005..e5e25a55c4e19 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -6,6 +6,9 @@ The motion_velocity_smoother package Fumiya Watanabe + Takamasa Horibe + Yutaka Shimizu + Makoto Kurihara Apache License 2.0 Takamasa Horibe From 2a93ee3de5a906f6267bf1369878a4c72ad46474 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 7 Feb 2023 19:31:45 +0900 Subject: [PATCH 44/72] fix(behavior_path_planner): replace deprecated function (#2817) Signed-off-by: tomoya.kimura --- .../behavior_path_planner/behavior_path_planner_node.hpp | 2 +- .../behavior_path_planner/src/behavior_path_planner_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 07d5621ec7fe9..b1e1c110782fe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -143,7 +143,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node PullOutParameters getPullOutParam(); // callback - void onOdometry(const Odometry::SharedPtr msg); + void onOdometry(const Odometry::ConstSharedPtr msg); void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 9847287793725..322203545183d 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1017,7 +1017,7 @@ bool BehaviorPathPlannerNode::keepInputPoints( return false; } -void BehaviorPathPlannerNode::onOdometry(const Odometry::SharedPtr msg) +void BehaviorPathPlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) { const std::lock_guard lock(mutex_pd_); planner_data_->self_odometry = msg; From 0324e054fdf1e2ff903aa6d3faa4e6752dc11b9d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 7 Feb 2023 21:31:28 +0900 Subject: [PATCH 45/72] fix(behavior_path_planner): fix geometric pull out lane id for overlapping (#2828) Signed-off-by: kosuke55 --- .../scene_module/utils/geometric_parallel_parking.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 15fa153221820..a2e0568379854 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -373,7 +373,16 @@ std::vector GeometricParallelParking::planOneTrial( } // combine road and shoulder lanes - lanelet::ConstLanelets lanes = road_lanes; + // cut the road lanes up to start_pose to prevent unintended processing for overlapped lane + lanelet::ConstLanelets lanes{}; + tier4_autoware_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y); + for (const auto & lane : road_lanes) { + if (boost::geometry::within(start_point2d, lane.polygon2d().basicPolygon())) { + lanes.push_back(lane); + break; + } + lanes.push_back(lane); + } lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); // If start_pose is parallel to goal_pose, we can know lateral deviation of edges of vehicle, From e1069e1019507d70043d51af7345e83a9d9aaa06 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 8 Feb 2023 01:28:34 +0900 Subject: [PATCH 46/72] feat(behavior_path_planner): deal with non rectangle object avoidance (#2830) * feat(behavior_path_planner): deal with non rectangle object avoidance Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../avoidance/avoidance_utils.cpp | 77 ++++++++++--------- 1 file changed, 39 insertions(+), 38 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index 83263cb423a68..84edfd8b241bb 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -232,6 +232,38 @@ double calcDecelDistPlanType3(const double v0, const double a0, const double ja) return x1; } +tier4_autoware_utils::Polygon2d expandPolygon( + const tier4_autoware_utils::Polygon2d & input_polygon, const double offset) +{ + // NOTE: There is a duplicated point. + const size_t num_points = input_polygon.outer().size() - 1; + + tier4_autoware_utils::Polygon2d expanded_polygon; + for (size_t i = 0; i < num_points; ++i) { + const auto & curr_p = input_polygon.outer().at(i); + const auto & next_p = input_polygon.outer().at(i + 1); + const auto & prev_p = + i == 0 ? input_polygon.outer().at(num_points - 1) : input_polygon.outer().at(i - 1); + + Eigen::Vector2d current_to_next(next_p.x() - curr_p.x(), next_p.y() - curr_p.y()); + Eigen::Vector2d current_to_prev(prev_p.x() - curr_p.x(), prev_p.y() - curr_p.y()); + current_to_next.normalize(); + current_to_prev.normalize(); + + const Eigen::Vector2d offset_vector = (-current_to_next - current_to_prev).normalized(); + const double theta = std::acos(offset_vector.dot(current_to_next)); + const double scaled_offset = offset / std::sin(theta); + const Eigen::Vector2d offset_point = + Eigen::Vector2d(curr_p.x(), curr_p.y()) + offset_vector * scaled_offset; + + expanded_polygon.outer().push_back( + tier4_autoware_utils::Point2d(offset_point.x(), offset_point.y())); + } + + boost::geometry::correct(expanded_polygon); + return expanded_polygon; +} + } // namespace bool isOnRight(const ObjectData & obj) { return obj.lateral < 0.0; } @@ -503,42 +535,8 @@ Polygon2d createEnvelopePolygon( tf2::doTransform( toMsg(envelope_poly, closest_pose.position.z), envelope_ros_polygon, geometry_tf); - std::vector offset_polygons{}; - bg::strategy::buffer::distance_symmetric distance_strategy(envelope_buffer); - bg::strategy::buffer::join_miter join_strategy; - bg::strategy::buffer::end_flat end_strategy; - bg::strategy::buffer::side_straight side_strategy; - bg::strategy::buffer::point_circle point_strategy; - bg::buffer( - toPolygon2d(envelope_ros_polygon), offset_polygons, distance_strategy, side_strategy, - join_strategy, end_strategy, point_strategy); - - return offset_polygons.front(); -} - -void getEdgePoints( - const Polygon2d & object_polygon, const double threshold, std::vector & edge_points) -{ - if (object_polygon.outer().size() < 2) { - return; - } - - const size_t num_points = object_polygon.outer().size(); - for (size_t i = 0; i < num_points - 1; ++i) { - const auto & curr_p = object_polygon.outer().at(i); - const auto & next_p = object_polygon.outer().at(i + 1); - const auto & prev_p = - i == 0 ? object_polygon.outer().at(num_points - 2) : object_polygon.outer().at(i - 1); - const Eigen::Vector2d current_to_next(next_p.x() - curr_p.x(), next_p.y() - curr_p.y()); - const Eigen::Vector2d current_to_prev(prev_p.x() - curr_p.x(), prev_p.y() - curr_p.y()); - const double inner_val = current_to_next.dot(current_to_prev); - if (std::fabs(inner_val) > threshold) { - continue; - } - - const auto edge_point = tier4_autoware_utils::createPoint(curr_p.x(), curr_p.y(), 0.0); - edge_points.push_back(edge_point); - } + const auto expanded_polygon = expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); + return expanded_polygon; } void sortPolygonPoints( @@ -698,11 +696,14 @@ void generateDrivableArea( for (const auto & object : objects) { const auto & obj_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto & obj_poly = object.envelope_poly; - constexpr double threshold = 0.01; // get edge points of the object std::vector edge_points; - getEdgePoints(obj_poly, threshold, edge_points); + for (size_t i = 0; i < obj_poly.outer().size() - 1; + ++i) { // NOTE: There is a duplicated points + edge_points.push_back(tier4_autoware_utils::createPoint( + obj_poly.outer().at(i).x(), obj_poly.outer().at(i).y(), 0.0)); + } // get a boundary that we have to change const double lat_dist_to_path = motion_utils::calcLateralOffset(path.points, obj_pose.position); From eb782af48a42e9e27cade063157f0862d5a23702 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 8 Feb 2023 02:26:13 +0900 Subject: [PATCH 47/72] fix(behavior_path_planner): fix path distortion of no back pull out (#2829) * fix(behavior_path_planner): fix path distortion of no back pull out Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp --------- Signed-off-by: kosuke55 --- .../scene_module/pull_out/pull_out_module.hpp | 2 +- .../scene_module/pull_out/pull_out_module.cpp | 30 ++++++++++--------- .../utils/geometric_parallel_parking.cpp | 21 +++++++++---- 3 files changed, 32 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 7c47caf2dc90d..342f2c8983cac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -105,7 +105,7 @@ class PullOutModule : public SceneModuleInterface std::shared_ptr getCurrentPlanner() const; PathWithLaneId getFullPath() const; ParallelParkingParameters getGeometricPullOutParameters() const; - std::vector searchBackedPoses(); + std::vector searchPullOutStartPoses(); std::shared_ptr lane_departure_checker_; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index ac8241d691675..2daa1ecf153d6 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -534,16 +534,7 @@ void PullOutModule::updatePullOutStatus() util::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_out_lanes); // search pull out start candidates backward - std::vector start_pose_candidates; - { - if (parameters_.enable_back) { - // the first element is current_pose - start_pose_candidates = searchBackedPoses(); - } else { - // pull_out_start candidate is only current pose - start_pose_candidates.push_back(current_pose); - } - } + std::vector start_pose_candidates = searchPullOutStartPoses(); if (parameters_.search_priority == "efficient_path") { planWithPriorityOnEfficientPath(start_pose_candidates, goal_pose); @@ -580,8 +571,10 @@ void PullOutModule::updatePullOutStatus() } // make this class? -std::vector PullOutModule::searchBackedPoses() +std::vector PullOutModule::searchPullOutStartPoses() { + std::vector pull_out_start_pose{}; + const Pose & current_pose = planner_data_->self_odometry->pose.pose; // get backward shoulder path @@ -598,9 +591,18 @@ std::vector PullOutModule::searchBackedPoses() p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); } + // if backward driving is disable, just refine current pose to the lanes + if (!parameters_.enable_back) { + const auto refined_pose = + calcLongitudinalOffsetPose(backward_shoulder_path.points, current_pose.position, 0); + if (refined_pose) { + pull_out_start_pose.push_back(*refined_pose); + } + return pull_out_start_pose; + } + // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - std::vector backed_poses; for (double back_distance = 0.0; back_distance <= parameters_.max_back_distance; back_distance += parameters_.backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( @@ -630,9 +632,9 @@ std::vector PullOutModule::searchBackedPoses() break; // poses behind this has a collision, so break. } - backed_poses.push_back(*backed_pose); + pull_out_start_pose.push_back(*backed_pose); } - return backed_poses; + return pull_out_start_pose; } bool PullOutModule::isOverlappedWithLane( diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index a2e0568379854..4a681445594f4 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -163,8 +163,6 @@ bool GeometricParallelParking::planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward) { - clearPaths(); - const auto & common_params = planner_data_->parameters; const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance : parameters_.after_backward_parking_straight_distance; @@ -227,8 +225,6 @@ bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes) { - clearPaths(); - constexpr bool is_forward = false; // parking backward means departing forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose constexpr double max_offset = 10.0; @@ -267,7 +263,7 @@ bool GeometricParallelParking::planPullOut( } // get road center line path from departing end to goal, and combine after the second arc path - const double s_start = getArcCoordinates(road_lanes, *end_pose).length + 1.0; // need buffer? + const double s_start = getArcCoordinates(road_lanes, *end_pose).length; const double s_goal = getArcCoordinates(road_lanes, goal_pose).length; const double road_lanes_length = std::accumulate( road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) { @@ -278,6 +274,16 @@ bool GeometricParallelParking::planPullOut( PathWithLaneId road_center_line_path = planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); + // check the continuity of straight path and arc path + const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; + const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose; + const double yaw_diff = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(road_path_first_pose.orientation), tf2::getYaw(arc_path_last_pose.orientation)); + const double distance = calcDistance2d(road_path_first_pose, arc_path_last_pose); + if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { + continue; + } + // set departing velocity to arc paths and 0 velocity to end point constexpr bool set_stop_end = false; setVelocityToArcPaths(arc_paths, parameters_.departing_velocity, set_stop_end); @@ -286,7 +292,8 @@ bool GeometricParallelParking::planPullOut( // combine the road center line path with the second arc path auto paths = arc_paths; paths.back().points.insert( - paths.back().points.end(), road_center_line_path.points.begin(), + paths.back().points.end(), + road_center_line_path.points.begin() + 1, // to avoid overlapped point road_center_line_path.points.end()); removeOverlappingPoints(paths.back()); @@ -350,6 +357,8 @@ std::vector GeometricParallelParking::planOneTrial( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const double end_pose_offset, const double lane_departure_margin) { + clearPaths(); + const auto common_params = planner_data_->parameters; const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); From 298b2241461c3250e342c7a0e97515da35e8a3c5 Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Wed, 8 Feb 2023 01:23:19 +0000 Subject: [PATCH 48/72] ci(pre-commit): disable autoupdate (#2838) * chore: sync files Signed-off-by: GitHub * Revert "chore: sync files" This reverts commit f3d23de04ac070df8d13d995338050dcd6afc109. * ci(pre-commit): disable autoupdate Signed-off-by: Kenji Miyake --------- Signed-off-by: GitHub Signed-off-by: Kenji Miyake Co-authored-by: kenji-miyake Co-authored-by: Kenji Miyake --- .pre-commit-config.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 57f65ac523bd2..5e57d3b6c0998 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,6 +1,5 @@ ci: autofix_commit_msg: "style(pre-commit): autofix" - autoupdate_commit_msg: "ci(pre-commit): autoupdate" repos: - repo: https://github.com/pre-commit/pre-commit-hooks From 42692911de317fc4a9ef61bf0cc149f7b7d5a870 Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Wed, 8 Feb 2023 10:38:58 +0900 Subject: [PATCH 49/72] refactor(ekf_localizer): add a function to check the delay step (#2657) * Add a function to check delay step * Apply the delay step checker to twist * Test the delay step cheker --- .../include/ekf_localizer/warning_message.hpp | 4 ++++ .../ekf_localizer/src/ekf_localizer.cpp | 16 ++++------------ .../ekf_localizer/src/warning_message.cpp | 18 ++++++++++++++++++ .../test/test_warning_message.cpp | 18 ++++++++++++++++++ 4 files changed, 44 insertions(+), 12 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index dbe44df702af0..f6c664eb26451 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,6 +17,10 @@ #include +std::string poseDelayStepWarningMessage( + const double delay_time, const int extend_state_step, const double ekf_dt); +std::string twistDelayStepWarningMessage( + const double delay_time, const int extend_state_step, const double ekf_dt); std::string poseDelayTimeWarningMessage(const double delay_time); std::string twistDelayTimeWarningMessage(const double delay_time); std::string mahalanobisWarningMessage(const double distance, const double max_distance); diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5243015a64bae..5a62e9c997836 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -404,13 +404,9 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); - if (delay_step > params_.extend_state_step - 1) { + if (delay_step >= params_.extend_state_step) { warning_.warnThrottle( - fmt::format( - "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = " - "extend_state_step * ekf_dt : %f [s]", - delay_time, params_.extend_state_step * ekf_dt_), - 1000); + poseDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); return; } DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); @@ -487,13 +483,9 @@ void EKFLocalizer::measurementUpdateTwist( delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); - if (delay_step > params_.extend_state_step - 1) { + if (delay_step >= params_.extend_state_step) { warning_.warnThrottle( - fmt::format( - "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = " - "extend_state_step * ekf_dt : %f [s]", - delay_time, params_.extend_state_step * ekf_dt_), - 1000); + twistDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); return; } DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index 1088d5c061375..9a7bbf47a94eb 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -18,6 +18,24 @@ #include +std::string poseDelayStepWarningMessage( + const double delay_time, const int extend_state_step, const double ekf_dt) +{ + const std::string s = + "Pose delay exceeds the compensation limit, ignored. " + "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; + return fmt::format(s, delay_time, extend_state_step * ekf_dt); +} + +std::string twistDelayStepWarningMessage( + const double delay_time, const int extend_state_step, const double ekf_dt) +{ + const std::string s = + "Twist delay exceeds the compensation limit, ignored. " + "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; + return fmt::format(s, delay_time, extend_state_step * ekf_dt); +} + std::string poseDelayTimeWarningMessage(const double delay_time) { const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index fc86df0d6cd80..b4a05afa844dc 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -14,8 +14,26 @@ #include "ekf_localizer/warning_message.hpp" +#include + #include +TEST(PoseDelayStepWarningMessage, SmokeTest) +{ + EXPECT_STREQ( + poseDelayStepWarningMessage(6.0, 2, 2.0).c_str(), + "Pose delay exceeds the compensation limit, ignored. " + "delay: 6.000[s], limit = extend_state_step * ekf_dt : 4.000[s]"); +} + +TEST(TwistDelayStepWarningMessage, SmokeTest) +{ + EXPECT_STREQ( + twistDelayStepWarningMessage(10.0, 3, 2.0).c_str(), + "Twist delay exceeds the compensation limit, ignored. " + "delay: 10.000[s], limit = extend_state_step * ekf_dt : 6.000[s]"); +} + TEST(PoseDelayTimeWarningMessage, SmokeTest) { EXPECT_STREQ( From 1e90380a3570d02e16d40287e0adf51a27cf34d0 Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Wed, 8 Feb 2023 14:22:02 +0900 Subject: [PATCH 50/72] refactor(ekf_localizer): add a function to erase a slash from the beginning of a string (#2702) * Add a function to remove the begin slash Signed-off-by: IshitaTakeshi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ekf_localizer/CMakeLists.txt | 1 + .../include/ekf_localizer/string.hpp | 29 +++++++++++++++++++ .../include/ekf_localizer/warning.hpp | 4 +-- .../ekf_localizer/src/ekf_localizer.cpp | 10 +++---- .../ekf_localizer/test/test_string.cpp | 26 +++++++++++++++++ 5 files changed, 62 insertions(+), 8 deletions(-) create mode 100644 localization/ekf_localizer/include/ekf_localizer/string.hpp create mode 100644 localization/ekf_localizer/test/test_string.cpp diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 75d2c88bad060..6ae8cefc9088c 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -49,6 +49,7 @@ if(BUILD_TESTING) test/test_measurement.cpp test/test_numeric.cpp test/test_state_transition.cpp + test/test_string.cpp test/test_warning_message.cpp) foreach(filepath ${TEST_FILES}) diff --git a/localization/ekf_localizer/include/ekf_localizer/string.hpp b/localization/ekf_localizer/include/ekf_localizer/string.hpp new file mode 100644 index 0000000000000..a4cd1f320367c --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/string.hpp @@ -0,0 +1,29 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__STRING_HPP_ +#define EKF_LOCALIZER__STRING_HPP_ + +#include + +inline std::string eraseLeadingSlash(const std::string & s) +{ + std::string a = s; + if (a.front() == '/') { + a.erase(0, 1); + } + return a; +} + +#endif // EKF_LOCALIZER__STRING_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/warning.hpp b/localization/ekf_localizer/include/ekf_localizer/warning.hpp index dcaae3c87b3b5..a3c8800242e2b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning.hpp @@ -26,14 +26,14 @@ class Warning void warn(const std::string & message) const { - RCLCPP_WARN(node_->get_logger(), message.c_str()); + RCLCPP_WARN(node_->get_logger(), "%s", message.c_str()); } void warnThrottle(const std::string & message, const int duration_milliseconds) const { RCLCPP_WARN_THROTTLE( node_->get_logger(), *(node_->get_clock()), - std::chrono::milliseconds(duration_milliseconds).count(), message.c_str()); + std::chrono::milliseconds(duration_milliseconds).count(), "%s", message.c_str()); } private: diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5a62e9c997836..a30ccef5a39bb 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -21,6 +21,7 @@ #include "ekf_localizer/numeric.hpp" #include "ekf_localizer/state_index.hpp" #include "ekf_localizer/state_transition.hpp" +#include "ekf_localizer/string.hpp" #include "ekf_localizer/warning.hpp" #include "ekf_localizer/warning_message.hpp" @@ -273,12 +274,9 @@ bool EKFLocalizer::getTransformFromTF( tf2::BufferCore tf_buffer; tf2_ros::TransformListener tf_listener(tf_buffer); rclcpp::sleep_for(std::chrono::milliseconds(100)); - if (parent_frame.front() == '/') { - parent_frame.erase(0, 1); - } - if (child_frame.front() == '/') { - child_frame.erase(0, 1); - } + + parent_frame = eraseLeadingSlash(parent_frame); + child_frame = eraseLeadingSlash(child_frame); for (int i = 0; i < 50; ++i) { try { diff --git a/localization/ekf_localizer/test/test_string.cpp b/localization/ekf_localizer/test/test_string.cpp new file mode 100644 index 0000000000000..7b35a56d889e9 --- /dev/null +++ b/localization/ekf_localizer/test/test_string.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ekf_localizer/string.hpp" + +#include + +TEST(EraseLeadingSlash, SmokeTest) +{ + EXPECT_EQ(eraseLeadingSlash("/topic"), "topic"); + EXPECT_EQ(eraseLeadingSlash("topic"), "topic"); // do nothing + + EXPECT_EQ(eraseLeadingSlash(""), ""); + EXPECT_EQ(eraseLeadingSlash("/"), ""); +} From 14979d0a6ae0706dcc5fdb4815369585a7c51d9b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 8 Feb 2023 18:39:35 +0900 Subject: [PATCH 51/72] fix(behavior_path_planner): fix drivable area for long backawrd avoidance path (#2833) * fix(behavior_path_planner): fix drivable area for long backawrd avoidance path Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../avoidance/avoidance_utils.cpp | 32 +++++++++++-------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index 84edfd8b241bb..76a60b35e1d11 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -643,24 +643,28 @@ std::vector updateBoundary( const auto closest_end_point = motion_utils::calcLongitudinalOffsetPoint(original_bound, end_segment_idx, end_offset); - std::vector updated_bound; - const double min_dist = 1e-3; - // copy original points until front point - std::copy( - original_bound.begin(), original_bound.begin() + start_segment_idx + 1, - std::back_inserter(updated_bound)); - // insert closest front point - if ( - closest_front_point && - tier4_autoware_utils::calcDistance2d(*closest_front_point, updated_bound.back()) > min_dist) { - updated_bound.push_back(*closest_front_point); + std::vector updated_bound; + if (0 < front_offset) { + // copy original points until front point + std::copy( + original_bound.begin(), original_bound.begin() + start_segment_idx + 1, + std::back_inserter(updated_bound)); + + // insert closest front point + if ( + closest_front_point && + tier4_autoware_utils::calcDistance2d(*closest_front_point, updated_bound.back()) > min_dist) { + updated_bound.push_back(*closest_front_point); + } } // insert sorted points for (const auto & sorted_point : sorted_points) { - if (tier4_autoware_utils::calcDistance2d(sorted_point.point, updated_bound.back()) > min_dist) { + if ( + updated_bound.empty() || + tier4_autoware_utils::calcDistance2d(sorted_point.point, updated_bound.back()) > min_dist) { updated_bound.push_back(sorted_point.point); } } @@ -668,13 +672,15 @@ std::vector updateBoundary( // insert closest end point if ( closest_end_point && - tier4_autoware_utils::calcDistance2d(*closest_end_point, updated_bound.back()) > min_dist) { + (updated_bound.empty() || + tier4_autoware_utils::calcDistance2d(*closest_end_point, updated_bound.back()) > min_dist)) { updated_bound.push_back(*closest_end_point); } // copy original points until the end of the original bound for (size_t i = end_segment_idx + 1; i < original_bound.size(); ++i) { if ( + updated_bound.empty() || tier4_autoware_utils::calcDistance2d(original_bound.at(i), updated_bound.back()) > min_dist) { updated_bound.push_back(original_bound.at(i)); } From ff9d064492f44ef51e048a7f41341eed344e0139 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 9 Feb 2023 10:19:00 +0900 Subject: [PATCH 52/72] feat(behavior_path_planner): add option to disable path update (#2831) Signed-off-by: Takayuki Murooka --- .../scene_module/avoidance/avoidance_module_data.hpp | 3 +++ .../src/behavior_path_planner_node.cpp | 1 + .../src/scene_module/avoidance/avoidance_module.cpp | 8 ++++++-- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 781c59bcc3901..80be249ee1209 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -76,6 +76,9 @@ struct AvoidanceParameters // enable yield maneuver. bool enable_yield_maneuver{false}; + // disable path update + bool disable_path_update{false}; + // constrains bool use_constraints_for_decel{false}; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 322203545183d..29b522cd3b919 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -324,6 +324,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() declare_parameter(ns + "enable_update_path_when_object_is_gone"); p.enable_safety_check = declare_parameter(ns + "enable_safety_check"); p.enable_yield_maneuver = declare_parameter(ns + "enable_yield_maneuver"); + p.disable_path_update = declare_parameter(ns + "disable_path_update"); p.publish_debug_marker = declare_parameter(ns + "publish_debug_marker"); p.print_debug_info = declare_parameter(ns + "print_debug_info"); } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 93c20fd25b1b1..4e9c0edce57b5 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2863,7 +2863,9 @@ BehaviorModuleOutput AvoidanceModule::plan() } else { RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); } - addShiftLineIfApproved(data.safe_new_sl); + if (!parameters_->disable_path_update) { + addShiftLineIfApproved(data.safe_new_sl); + } } else if (isWaitingApproval()) { clearWaitingApproval(); removeCandidateRTCStatus(); @@ -2894,7 +2896,9 @@ BehaviorModuleOutput AvoidanceModule::plan() } avoidance_data_.state = updateEgoState(data); - updateEgoBehavior(data, avoidance_path); + if (!parameters_->disable_path_update) { + updateEgoBehavior(data, avoidance_path); + } if (parameters_->publish_debug_marker) { setDebugData(avoidance_data_, path_shifter_, debug_data_); From a0525560f04ce429bb13aa17d326e8ae8eae9510 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 9 Feb 2023 14:07:25 +0900 Subject: [PATCH 53/72] fix(lane_change): get sorted lane ids (#2847) * fix(lane_change): get sorted lane ids Signed-off-by: Muhammad Zulfaqar Azmi * slightly change the find Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/util.cpp | 113 ++++++++++++++---- 1 file changed, 88 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 1059a1187684c..388537a01eb3c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -44,6 +44,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::util::calcObjectPolygon; using behavior_path_planner::util::getHighestProbLabel; using geometry_msgs::msg::Pose; +using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; @@ -112,6 +113,66 @@ void filterObjectIndices( } } } + +std::vector> getSortedLaneIds( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const double rough_shift_length) +{ + std::vector> sorted_lane_ids{}; + sorted_lane_ids.reserve(target_lanes.size()); + const auto get_sorted_lane_ids = [&](const lanelet::ConstLanelet & target_lane) { + const auto routing_graph_ptr = route_handler.getRoutingGraphPtr(); + lanelet::ConstLanelet lane; + if (rough_shift_length < 0.0) { + // lane change to the left, so I wan to take the lane right to target + const auto has_target_right = routing_graph_ptr->right(target_lane); + if (has_target_right) { + lane = *has_target_right; + } + } else if (rough_shift_length > 0.0) { + const auto has_target_left = routing_graph_ptr->left(target_lane); + if (has_target_left) { + lane = *has_target_left; + } + } else { + lane = target_lane; + } + + const auto find_same_id = std::find_if( + current_lanes.cbegin(), current_lanes.cend(), + [&lane](const lanelet::ConstLanelet & orig) { return orig.id() == lane.id(); }); + + if (find_same_id == current_lanes.cend()) { + return std::vector{target_lane.id()}; + } + + if (target_lane.id() > find_same_id->id()) { + return std::vector{find_same_id->id(), target_lane.id()}; + } + + return std::vector{target_lane.id(), find_same_id->id()}; + }; + + std::transform( + target_lanes.cbegin(), target_lanes.cend(), std::back_inserter(sorted_lane_ids), + get_sorted_lane_ids); + + return sorted_lane_ids; +} + +std::vector replaceWithSortedIds( + const std::vector & original_lane_ids, + const std::vector> & sorted_lane_ids) +{ + for (const auto original_id : original_lane_ids) { + for (const auto & sorted_id : sorted_lane_ids) { + if (std::find(sorted_id.cbegin(), sorted_id.cend(), original_id) != sorted_id.cend()) { + return sorted_id; + } + } + } + return original_lane_ids; +} } // namespace namespace behavior_path_planner::lane_change_utils @@ -181,7 +242,8 @@ std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const double acceleration, const LaneChangePhaseInfo distance, const LaneChangePhaseInfo speed, + const std::vector> & sorted_lane_ids, const double acceleration, + const LaneChangePhaseInfo distance, const LaneChangePhaseInfo speed, const LaneChangeParameters & lane_change_param) { PathShifter path_shifter; @@ -236,36 +298,33 @@ std::optional constructCandidatePath( const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose; const auto lanechange_end_idx = motion_utils::findNearestIndex(shifted_path.path.points, lane_changing_end_pose); - const auto insertLaneIDs = [](auto & target, const auto src) { - target.lane_ids.insert(target.lane_ids.end(), src.lane_ids.begin(), src.lane_ids.end()); - }; - if (lanechange_end_idx) { - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *lanechange_end_idx) { - insertLaneIDs(point, lane_changing_start_point); - insertLaneIDs(point, lane_changing_end_point); - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - lane_changing_start_point.point.longitudinal_velocity_mps); - continue; - } - point.point.longitudinal_velocity_mps = - std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_speed)); - const auto nearest_idx = - motion_utils::findNearestIndex(lane_changing_segment.points, point.point.pose); - point.lane_ids = lane_changing_segment.points.at(*nearest_idx).lane_ids; - } - candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); - candidate_path.shifted_path = shifted_path; - } else { + if (!lanechange_end_idx) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), "lane change end idx not found on target path."); return std::nullopt; } + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *lanechange_end_idx) { + point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + lane_changing_start_point.point.longitudinal_velocity_mps); + continue; + } + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_speed)); + const auto nearest_idx = + motion_utils::findNearestIndex(lane_changing_segment.points, point.point.pose); + point.lane_ids = lane_changing_segment.points.at(*nearest_idx).lane_ids; + } + + candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); + candidate_path.shifted_path = shifted_path; + // check candidate path is in lanelet if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { return std::nullopt; @@ -322,6 +381,9 @@ LaneChangePaths getLaneChangePaths( const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); + const auto sorted_lane_ids = getSortedLaneIds( + route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance); + for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { const double prepare_speed = getExpectedVelocityWhenDecelerate( @@ -385,7 +447,8 @@ LaneChangePaths getLaneChangePaths( const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed}; const auto candidate_path = constructCandidatePath( prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, - shift_line, original_lanelets, target_lanelets, acceleration, lc_dist, lc_speed, parameter); + shift_line, original_lanelets, target_lanelets, sorted_lane_ids, acceleration, lc_dist, + lc_speed, parameter); if (!candidate_path) { continue; From 6a8f2a60f9e11d6a5d2152634d0dc544dcb5a21b Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 9 Feb 2023 16:52:25 +0900 Subject: [PATCH 54/72] fix(lane_change): safe intervehicle distance in collision check (#2807) * fix(lane_change): safe intervehicle distance in collision check Signed-off-by: Muhammad Zulfaqar * fix spellcheck Signed-off-by: Muhammad Zulfaqar --------- Signed-off-by: Muhammad Zulfaqar --- .../behavior_path_planner/src/utilities.cpp | 38 +++++++++++++++---- 1 file changed, 31 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 229fa2977b4eb..1753f3bebb402 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2329,19 +2329,43 @@ bool hasEnoughDistance( const auto is_obj_in_front = isObjectFront(front_vehicle_pose); debug.is_front = is_obj_in_front; - const auto front_vehicle_velocity = - (is_obj_in_front) ? object_current_twist.linear : ego_current_twist.linear; + const auto [front_vehicle_velocity, rear_vehicle_velocity] = std::invoke([&]() { + debug.object_twist.linear = object_current_twist.linear; + if (is_obj_in_front) { + return std::make_pair( + util::l2Norm(object_current_twist.linear), util::l2Norm(ego_current_twist.linear)); + } + return std::make_pair( + util::l2Norm(ego_current_twist.linear), util::l2Norm(object_current_twist.linear)); + }); + + const auto is_unsafe_dist_between_vehicle = std::invoke([&]() { + // ignore this for parked vehicle. + if (l2Norm(object_current_twist.linear) < 0.1) { + return false; + } + + // the value guarantee distance between vehicles are always more than dist + const auto max_vel = std::max(front_vehicle_velocity, rear_vehicle_velocity); + constexpr auto scale = 0.8; + const auto dist = scale * std::abs(max_vel) + param.longitudinal_distance_min_threshold; - const auto rear_vehicle_velocity = - (is_obj_in_front) ? ego_current_twist.linear : object_current_twist.linear; - debug.object_twist.linear = (is_obj_in_front) ? front_vehicle_velocity : rear_vehicle_velocity; + // return value rounded to the nearest two floating point + return std::abs(front_vehicle_pose.position.x) < dist; + }); + + if (is_unsafe_dist_between_vehicle) { + return false; + } const auto front_vehicle_stop_threshold = frontVehicleStopDistance( - util::l2Norm(front_vehicle_velocity), front_decel, std::fabs(front_vehicle_pose.position.x)); + front_vehicle_velocity, front_decel, std::abs(front_vehicle_pose.position.x)); + // longitudinal_distance_min_threshold here guarantee future stopping distance must be more than + // longitudinal_distance_min_threshold const auto rear_vehicle_stop_threshold = std::max( rearVehicleStopDistance( - util::l2Norm(rear_vehicle_velocity), rear_decel, param.rear_vehicle_reaction_time, + rear_vehicle_velocity, rear_decel, param.rear_vehicle_reaction_time, param.rear_vehicle_safety_time_margin), param.longitudinal_distance_min_threshold); From fe32cd5f02792186fd58c4aa9dfe582cfe3cded0 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 9 Feb 2023 16:53:01 +0900 Subject: [PATCH 55/72] fix(lane_change): checks only shifted path for isInLanelet check (#2853) Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/util.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 388537a01eb3c..fa45880f4c0ee 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -322,14 +322,14 @@ std::optional constructCandidatePath( point.lane_ids = lane_changing_segment.points.at(*nearest_idx).lane_ids; } - candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); - candidate_path.shifted_path = shifted_path; - - // check candidate path is in lanelet - if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { + if (!isPathInLanelets(shifted_path.path, original_lanelets, target_lanelets)) { return std::nullopt; } + // check candidate path is in lanelet + candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); + candidate_path.shifted_path = shifted_path; + return std::optional{candidate_path}; } From 9c632a5001b5b8b7747540ac499113cceffae063 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 10 Feb 2023 10:08:34 +0900 Subject: [PATCH 56/72] feat(behavior_path_planner): fix z of extracted drivable area (#2856) Signed-off-by: Takayuki Murooka --- .../src/scene_module/avoidance/avoidance_utils.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index 76a60b35e1d11..6af7a4aead4f3 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -704,11 +704,14 @@ void generateDrivableArea( const auto & obj_poly = object.envelope_poly; // get edge points of the object + const size_t nearest_path_idx = motion_utils::findNearestIndex( + path.points, obj_pose.position); // to get z for object polygon std::vector edge_points; for (size_t i = 0; i < obj_poly.outer().size() - 1; ++i) { // NOTE: There is a duplicated points edge_points.push_back(tier4_autoware_utils::createPoint( - obj_poly.outer().at(i).x(), obj_poly.outer().at(i).y(), 0.0)); + obj_poly.outer().at(i).x(), obj_poly.outer().at(i).y(), + path.points.at(nearest_path_idx).point.pose.position.z)); } // get a boundary that we have to change From a7f20aa7d3f7ab4ffed34a0fe88f52ac5c731966 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Fri, 10 Feb 2023 11:52:57 +0900 Subject: [PATCH 57/72] feat(operation_mode_transition_manager): add guard to invalid parameter (#2388) Signed-off-by: tomoya.kimura --- .../operation_mode_transition_manager/src/node.cpp | 13 ++++++++++++- .../operation_mode_transition_manager/src/state.cpp | 2 +- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp index d620e68ababe5..c54f9c1b76f43 100644 --- a/control/operation_mode_transition_manager/src/node.cpp +++ b/control/operation_mode_transition_manager/src/node.cpp @@ -51,12 +51,23 @@ OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::Nod } // initialize state - transition_timeout_ = declare_parameter("transition_timeout"); current_mode_ = OperationMode::STOP; transition_ = nullptr; gate_operation_mode_.mode = OperationModeState::UNKNOWN; gate_operation_mode_.is_in_transition = false; control_mode_report_.mode = ControlModeReport::NO_COMMAND; + transition_timeout_ = declare_parameter("transition_timeout"); + { + // check `transition_timeout` value + const auto stable_duration = declare_parameter("stable_check.duration"); + const double TIMEOUT_MARGIN = 0.5; + if (transition_timeout_ < stable_duration + TIMEOUT_MARGIN) { + transition_timeout_ = stable_duration + TIMEOUT_MARGIN; + RCLCPP_WARN( + get_logger(), "`transition_timeout` must be somewhat larger than `stable_check.duration`"); + RCLCPP_WARN_STREAM(get_logger(), "transition_timeout is set to " << transition_timeout_); + } + } // modes modes_[OperationMode::STOP] = std::make_unique(); diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index c823c11a4f4ba..f8434bbdb081f 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -71,7 +71,7 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) // params for mode change completed { auto & p = stable_check_param_; - p.duration = node->declare_parameter("stable_check.duration"); + p.duration = node->get_parameter("stable_check.duration").as_double(); p.dist_threshold = node->declare_parameter("stable_check.dist_threshold"); p.speed_upper_threshold = node->declare_parameter("stable_check.speed_upper_threshold"); p.speed_lower_threshold = node->declare_parameter("stable_check.speed_lower_threshold"); From 95be33edec9d86b33c4425c9be8eb2e07703510a Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 10 Feb 2023 14:12:32 +0900 Subject: [PATCH 58/72] feat(simple_planning_sim): publish sensing interface imu data (#2843) * feat(simple_planning_sim): publish sensing interface imu data Signed-off-by: Takamasa Horibe * fix covariance index Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../simple_planning_simulator_core.hpp | 8 ++++++ .../simple_planning_simulator.launch.py | 1 + .../simple_planning_simulator/package.xml | 1 + .../simple_planning_simulator_core.cpp | 25 +++++++++++++++++++ 4 files changed, 35 insertions(+) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index a270be702fb4c..742f87411b7d8 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -44,6 +44,7 @@ #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/imu.hpp" #include "tier4_external_api_msgs/srv/initialize_pose.hpp" #include @@ -82,6 +83,7 @@ using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Twist; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::Odometry; +using sensor_msgs::msg::Imu; using tier4_external_api_msgs::srv::InitializePose; class DeltaTime @@ -126,6 +128,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_odom_; rclcpp::Publisher::SharedPtr pub_steer_; rclcpp::Publisher::SharedPtr pub_acc_; + rclcpp::Publisher::SharedPtr pub_imu_; rclcpp::Publisher::SharedPtr pub_control_mode_report_; rclcpp::Publisher::SharedPtr pub_gear_report_; rclcpp::Publisher::SharedPtr pub_turn_indicators_report_; @@ -328,6 +331,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void publish_acceleration(); + /** + * @brief publish imu + */ + void publish_imu(); + /** * @brief publish control_mode report */ diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 43a0fe25888c7..321e8c9311f79 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -66,6 +66,7 @@ def launch_setup(context, *args, **kwargs): ("output/twist", "/vehicle/status/velocity_status"), ("output/odometry", "/localization/kinematic_state"), ("output/acceleration", "/localization/acceleration"), + ("output/imu", "/sensing/imu/imu_data"), ("output/steering", "/vehicle/status/steering_status"), ("output/gear_report", "/vehicle/status/gear_status"), ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"), diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 1974020d802ab..ca4766f875dbe 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -20,6 +20,7 @@ nav_msgs rclcpp rclcpp_components + sensor_msgs tf2 tf2_geometry_msgs tf2_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 121d9511478a7..cfc58cdaeed63 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -130,6 +130,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt pub_odom_ = create_publisher("output/odometry", QoS{1}); pub_steer_ = create_publisher("output/steering", QoS{1}); pub_acc_ = create_publisher("output/acceleration", QoS{1}); + pub_imu_ = create_publisher("output/imu", QoS{1}); pub_tf_ = create_publisher("/tf", QoS{1}); /* set param callback */ @@ -298,6 +299,7 @@ void SimplePlanningSimulator::on_timer() publish_velocity(current_velocity_); publish_steering(current_steer_); publish_acceleration(); + publish_imu(); publish_control_mode_report(); publish_gear_report(); @@ -563,6 +565,29 @@ void SimplePlanningSimulator::publish_acceleration() pub_acc_->publish(msg); } +void SimplePlanningSimulator::publish_imu() +{ + using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + + sensor_msgs::msg::Imu imu; + imu.header.frame_id = "base_link"; + imu.header.stamp = now(); + imu.linear_acceleration.x = vehicle_model_ptr_->getAx(); + constexpr auto COV = 0.001; + imu.linear_acceleration_covariance.at(COV_IDX::X_X) = COV; + imu.linear_acceleration_covariance.at(COV_IDX::Y_Y) = COV; + imu.linear_acceleration_covariance.at(COV_IDX::Z_Z) = COV; + imu.angular_velocity = current_odometry_.twist.twist.angular; + imu.angular_velocity_covariance.at(COV_IDX::X_X) = COV; + imu.angular_velocity_covariance.at(COV_IDX::Y_Y) = COV; + imu.angular_velocity_covariance.at(COV_IDX::Z_Z) = COV; + imu.orientation = current_odometry_.pose.pose.orientation; + imu.orientation_covariance.at(COV_IDX::X_X) = COV; + imu.orientation_covariance.at(COV_IDX::Y_Y) = COV; + imu.orientation_covariance.at(COV_IDX::Z_Z) = COV; + pub_imu_->publish(imu); +} + void SimplePlanningSimulator::publish_control_mode_report() { current_control_mode_.stamp = get_clock()->now(); From d30616f4fac9cd1ee2b10d6e9fb5fce4baf074e8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 10 Feb 2023 17:58:04 +0900 Subject: [PATCH 59/72] fix(behavior_path_planner): fix drivable area expansion method outside intersection (#2861) Signed-off-by: Takayuki Murooka --- .../src/scene_module/avoidance/avoidance_module.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 4e9c0edce57b5..e3222f5eed2ad 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2573,12 +2573,8 @@ void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const // 2. when there are multiple turning lanes whose previous lanelet is the same in // intersection - const lanelet::ConstLanelets next_lanes_from_intersection = std::invoke( + const lanelet::ConstLanelets next_lanes = std::invoke( [&route_handler](const lanelet::ConstLanelet & lane) { - if (!lane.hasAttribute("turn_direction")) { - return lanelet::ConstLanelets{}; - } - // get previous lane, and return false if previous lane does not exist lanelet::ConstLanelets prev_lanes; if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { @@ -2598,7 +2594,7 @@ void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const // 2.1 look for neighbour lane, where end line of the lane is connected to end line of the // original lane - for (const auto & next_lane : next_lanes_from_intersection) { + for (const auto & next_lane : next_lanes) { if (current_lane.id() == next_lane.id()) { continue; } From 5907db6db2810045c44aaa2f0bc58e68b8d7bf55 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Fri, 10 Feb 2023 18:03:31 +0900 Subject: [PATCH 60/72] fix(behavior_path_planner): fix overlapping checker (#2855) * fix(behavior_path_planner): fix overlapping checker Signed-off-by: yutaka * fix format Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../behavior_path_planner/src/utilities.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 1753f3bebb402..f5f2987f63206 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1088,9 +1088,9 @@ void generateDrivableArea( }; const auto has_overlap = - [&](const lanelet::ConstLanelet & lane, const lanelet::Id & ignore_lane_id = lanelet::InvalId) { + [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { for (const auto & transformed_lane : transformed_lanes) { - if (transformed_lane.id() == ignore_lane_id) { + if (checkHasSameLane(ignore_lanelets, transformed_lane)) { continue; } if (boost::geometry::intersects( @@ -1114,6 +1114,16 @@ void generateDrivableArea( checkHasSameLane(transformed_lanes, goal_lanelet)) { const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); + const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); + const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); + lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; + if (goal_left_lanelet) { + goal_lanelets.push_back(*goal_left_lanelet); + } + if (goal_right_lanelet) { + goal_lanelets.push_back(*goal_right_lanelet); + } + for (const auto & lane : lanes_after_goal) { // If lane is already in the transformed lanes, ignore it if (checkHasSameLane(transformed_lanes, lane)) { @@ -1121,9 +1131,8 @@ void generateDrivableArea( } // Check if overlapped const bool is_overlapped = - (checkHasSameLane(next_lanes_after_goal, lane) - ? has_overlap(lane, route_handler->getGoalLaneId()) - : has_overlap(lane)); + (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) + : has_overlap(lane)); if (is_overlapped) { continue; } From d034c09031ed4b30e2611588ed57c63ae87dfecb Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 10 Feb 2023 18:56:58 +0900 Subject: [PATCH 61/72] fix(behavior_path_planner): fix geometric parallel parking guard (#2857) Signed-off-by: kosuke55 --- .../src/scene_module/utils/geometric_parallel_parking.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 4a681445594f4..913139774136f 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -138,8 +138,8 @@ std::vector GeometricParallelParking::generatePullOverPaths( // check the continuity of straight path and arc path const Pose & road_path_last_pose = straight_path.points.back().point.pose; const Pose & arc_path_first_pose = arc_paths.front().points.front().point.pose; - const double yaw_diff = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(road_path_last_pose.orientation), tf2::getYaw(arc_path_first_pose.orientation)); + const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(road_path_last_pose.orientation) - tf2::getYaw(arc_path_first_pose.orientation))); const double distance = calcDistance2d(road_path_last_pose, arc_path_first_pose); if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { return std::vector{}; @@ -277,8 +277,8 @@ bool GeometricParallelParking::planPullOut( // check the continuity of straight path and arc path const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose; - const double yaw_diff = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(road_path_first_pose.orientation), tf2::getYaw(arc_path_last_pose.orientation)); + const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(road_path_first_pose.orientation) - tf2::getYaw(arc_path_last_pose.orientation))); const double distance = calcDistance2d(road_path_first_pose, arc_path_last_pose); if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { continue; From efd61ea54297e07158683ab2e0f163f86cc549f1 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 10 Feb 2023 21:15:45 +0900 Subject: [PATCH 62/72] fix(behavior_path_planner): fix the reason of behind threshold (#2851) Signed-off-by: Takayuki Murooka --- .../src/scene_module/avoidance/avoidance_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index e3222f5eed2ad..effaca9501bd0 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -332,7 +332,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // object is behind ego or too far. if (object_data.longitudinal < -parameters_->object_check_backward_distance) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); - object_data.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; + object_data.reason = AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD; data.other_objects.push_back(object_data); continue; } From 93fbdb9ba11fb8650520ce8cdefbd3bb0a03eabe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Sat, 11 Feb 2023 12:19:22 +0300 Subject: [PATCH 63/72] feat(image_projection_based_fusion): add trust distance to fusion (#2844) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(image_projection_based_fusion): add thrust distance to fusion Signed-off-by: Kaan Çolak * feat(image_projection_based_fusion): fix typo Signed-off-by: Kaan Çolak --------- Signed-off-by: Kaan Çolak --- .../roi_cluster_fusion/node.hpp | 2 ++ .../launch/roi_cluster_fusion.launch.xml | 2 ++ .../src/roi_cluster_fusion/node.cpp | 12 ++++++++++++ 3 files changed, 16 insertions(+) diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 303d2acd9801f..5d6aba9b7d51d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -44,7 +44,9 @@ class RoiClusterFusionNode bool use_cluster_semantic_type_{false}; float iou_threshold_{0.0f}; bool remove_unknown_; + float trust_distance_; + bool filter_by_distance(const DetectedObjectWithFeature & obj); bool out_of_scope(const DetectedObjectWithFeature & obj); // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 0f4327bb35605..2848a16c33af1 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -20,6 +20,7 @@ + @@ -75,6 +76,7 @@ + diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index fc3e714e8c91f..717f21da21fc3 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -40,6 +40,7 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); iou_threshold_ = declare_parameter("iou_threshold", 0.1); remove_unknown_ = declare_parameter("remove_unknown", false); + trust_distance_ = declare_parameter("trust_distance", 100.0); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -103,6 +104,10 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } + if (filter_by_distance(input_cluster_msg.feature_objects.at(i))) { + continue; + } + // filter point out of scope if (debugger_ && out_of_scope(input_cluster_msg.feature_objects.at(i))) { continue; @@ -225,6 +230,13 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj) return is_out; } +bool RoiClusterFusionNode::filter_by_distance(const DetectedObjectWithFeature & obj) +{ + const auto & position = obj.object.kinematics.pose_with_covariance.pose.position; + const auto square_distance = position.x * position.x + position.y + position.y; + return square_distance > trust_distance_ * trust_distance_; +} + } // namespace image_projection_based_fusion #include From a8caf21123b28b3127c1f9ecc113f2654d47ce1b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 11 Feb 2023 21:53:16 +0900 Subject: [PATCH 64/72] refactor(tier4_planning_rviz_plugin): create abstract class for footprint (#2870) * refactor(tier4_planning_rviz_plugin): create abstract class for footprint Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../tier4_planning_rviz_plugin/CMakeLists.txt | 10 +- .../include/path_footprint/display.hpp | 87 ++--- .../include/path_footprint/display_base.hpp | 315 ++++++++++++++++++ .../path_with_lane_id_footprint/display.hpp | 100 ------ .../include/trajectory_footprint/display.hpp | 97 ------ .../src/path_footprint/display.cpp | 218 +++--------- .../path_with_lane_id_footprint/display.cpp | 264 --------------- .../src/trajectory_footprint/display.cpp | 275 --------------- 8 files changed, 406 insertions(+), 960 deletions(-) create mode 100644 common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp delete mode 100644 common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp delete mode 100644 common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp delete mode 100644 common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp delete mode 100644 common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp diff --git a/common/tier4_planning_rviz_plugin/CMakeLists.txt b/common/tier4_planning_rviz_plugin/CMakeLists.txt index 967e010558eb2..58240fa964d04 100644 --- a/common/tier4_planning_rviz_plugin/CMakeLists.txt +++ b/common/tier4_planning_rviz_plugin/CMakeLists.txt @@ -21,18 +21,16 @@ ament_auto_add_library(tier4_planning_rviz_plugin SHARED src/drivable_area/display.cpp include/path/display.hpp src/path/display.cpp - include/path_footprint/display.hpp - src/path_footprint/display.cpp include/path_with_lane_id/display.hpp src/path_with_lane_id/display.cpp - include/path_with_lane_id_footprint/display.hpp - src/path_with_lane_id_footprint/display.cpp + # footprint + include/path_footprint/display_base.hpp + include/path_footprint/display.hpp + src/path_footprint/display.cpp include/pose_with_uuid_stamped/display.hpp src/pose_with_uuid_stamped/display.cpp include/trajectory/display.hpp src/trajectory/display.cpp - include/trajectory_footprint/display.hpp - src/trajectory_footprint/display.cpp include/mission_checkpoint/mission_checkpoint.hpp src/mission_checkpoint/mission_checkpoint.cpp src/tools/jsk_overlay_utils.cpp diff --git a/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp index 21442120b0062..1db170dd0cd46 100644 --- a/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp @@ -15,76 +15,53 @@ #ifndef PATH_FOOTPRINT__DISPLAY_HPP_ #define PATH_FOOTPRINT__DISPLAY_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include - -#include -#include -#include -#include -#include +#include +#include #include +#include +#include namespace rviz_plugins { -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; - -class AutowarePathFootprintDisplay -: public rviz_common::MessageFilterDisplay +class AutowarePathWithLaneIdFootprintDisplay +: public AutowarePathFootBaseprintDisplay { Q_OBJECT public: - AutowarePathFootprintDisplay(); - virtual ~AutowarePathFootprintDisplay(); - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - void updateVehicleInfo(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) override; - Ogre::ManualObject * path_footprint_manual_object_; - rviz_common::properties::BoolProperty * property_path_footprint_view_; - rviz_common::properties::ColorProperty * property_path_footprint_color_; - rviz_common::properties::FloatProperty * property_path_footprint_alpha_; - rviz_common::properties::FloatProperty * property_vehicle_length_; - rviz_common::properties::FloatProperty * property_vehicle_width_; - rviz_common::properties::FloatProperty * property_rear_overhang_; - - struct VehicleFootprintInfo - { - VehicleFootprintInfo(const float l, const float w, const float r) - : length(l), width(w), rear_overhang(r) - { - } - float length, width, rear_overhang; - }; - - std::shared_ptr vehicle_info_; - std::shared_ptr vehicle_footprint_info_; + AutowarePathWithLaneIdFootprintDisplay(); private: - autoware_auto_planning_msgs::msg::Path::ConstSharedPtr last_msg_ptr_; - bool validateFloats(const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr); + void resetDetail() override; + void preprocessMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; + void processMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const size_t p_idx) override; + + rviz_common::properties::BoolProperty property_lane_id_view_; + rviz_common::properties::FloatProperty property_lane_id_scale_; + + using LaneIdObject = + std::pair, std::unique_ptr>; + std::vector lane_id_obj_ptrs_; +}; +class AutowarePathFootprintDisplay +: public AutowarePathFootBaseprintDisplay +{ + Q_OBJECT }; +class AutowareTrajectoryFootprintDisplay +: public AutowarePathFootBaseprintDisplay +{ + Q_OBJECT +}; } // namespace rviz_plugins #endif // PATH_FOOTPRINT__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp new file mode 100644 index 0000000000000..1b83b7c3258f3 --- /dev/null +++ b/common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp @@ -0,0 +1,315 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_FOOTPRINT__DISPLAY_BASE_HPP_ +#define PATH_FOOTPRINT__DISPLAY_BASE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace rviz_plugins +{ +using vehicle_info_util::VehicleInfo; +using vehicle_info_util::VehicleInfoUtil; + +template +class AutowarePathFootBaseprintDisplay : public rviz_common::MessageFilterDisplay +{ +public: + AutowarePathFootBaseprintDisplay() + : // footprint + property_footprint_view_{"View Footprint", true, "", this}, + property_footprint_alpha_{"Alpha", 1.0, "", &property_footprint_view_}, + property_footprint_color_{"Color", QColor(230, 230, 50), "", &property_footprint_view_}, + property_vehicle_length_{"Vehicle Length", 4.77, "", &property_footprint_view_}, + property_vehicle_width_{"Vehicle Width", 1.83, "", &property_footprint_view_}, + property_rear_overhang_{"Rear Overhang", 1.03, "", &property_footprint_view_}, + property_offset_{"Offset from BaseLink", 0.0, "", &property_footprint_view_}, + // point + property_point_view_{"View Point", false, "", this}, + property_point_alpha_{"Alpha", 1.0, "", &property_point_view_}, + property_point_color_{"Color", QColor(0, 60, 255), "", &property_point_view_}, + property_point_radius_{"Radius", 0.1, "", &property_point_view_}, + property_point_offset_{"Offset", 0.0, "", &property_point_view_} + { + // initialize footprint + property_footprint_alpha_.setMin(0.0); + property_footprint_alpha_.setMax(1.0); + property_vehicle_length_.setMin(0.0); + property_vehicle_width_.setMin(0.0); + property_rear_overhang_.setMin(0.0); + // initialize point + property_point_alpha_.setMin(0.0); + property_point_alpha_.setMax(1.0); + + updateVehicleInfo(); + } + + virtual ~AutowarePathFootBaseprintDisplay() + { + if (this->initialized()) { + this->scene_manager_->destroyManualObject(footprint_manual_object_); + this->scene_manager_->destroyManualObject(point_manual_object_); + } + } + + void onInitialize() override + { + rviz_common::MessageFilterDisplay::MFDClass::onInitialize(); + + footprint_manual_object_ = this->scene_manager_->createManualObject(); + footprint_manual_object_->setDynamic(true); + this->scene_node_->attachObject(footprint_manual_object_); + + point_manual_object_ = this->scene_manager_->createManualObject(); + point_manual_object_->setDynamic(true); + this->scene_node_->attachObject(point_manual_object_); + } + + void reset() override + { + rviz_common::MessageFilterDisplay::MFDClass::reset(); + footprint_manual_object_->clear(); + point_manual_object_->clear(); + + resetDetail(); + } + +protected: + virtual void resetDetail() {} + virtual void preprocessMessageDetail([[maybe_unused]] const typename T::ConstSharedPtr msg_ptr) {} + virtual void processMessageDetail( + [[maybe_unused]] const typename T::ConstSharedPtr msg_ptr, [[maybe_unused]] const size_t p_idx) + { + } + + Ogre::ManualObject * footprint_manual_object_; + rviz_common::properties::BoolProperty property_footprint_view_; + rviz_common::properties::FloatProperty property_footprint_alpha_; + rviz_common::properties::ColorProperty property_footprint_color_; + rviz_common::properties::FloatProperty property_vehicle_length_; + rviz_common::properties::FloatProperty property_vehicle_width_; + rviz_common::properties::FloatProperty property_rear_overhang_; + rviz_common::properties::FloatProperty property_offset_; + + Ogre::ManualObject * point_manual_object_; + rviz_common::properties::BoolProperty property_point_view_; + rviz_common::properties::FloatProperty property_point_alpha_; + rviz_common::properties::ColorProperty property_point_color_; + rviz_common::properties::FloatProperty property_point_radius_; + rviz_common::properties::FloatProperty property_point_offset_; + + void updateVisualization() + { + if (last_msg_ptr_ != nullptr) { + processMessage(last_msg_ptr_); + } + } + +private: + struct VehicleFootprintInfo + { + VehicleFootprintInfo(const float l, const float w, const float r) + : length(l), width(w), rear_overhang(r) + { + } + float length, width, rear_overhang; + }; + + std::shared_ptr vehicle_info_; + std::shared_ptr vehicle_footprint_info_; + + void updateVehicleInfo() + { + if (vehicle_info_) { + vehicle_footprint_info_ = std::make_shared( + vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, + vehicle_info_->rear_overhang_m); + } else { + const float length{property_vehicle_length_.getFloat()}; + const float width{property_vehicle_width_.getFloat()}; + const float rear_overhang{property_rear_overhang_.getFloat()}; + + vehicle_footprint_info_ = + std::make_shared(length, width, rear_overhang); + } + } + + typename T::ConstSharedPtr last_msg_ptr_; + bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) + { + for (auto && point : msg_ptr->points) { + if (!rviz_common::validateFloats(tier4_autoware_utils::getPose(point))) { + return false; + } + } + return true; + } + + void processMessage(const typename T::ConstSharedPtr msg_ptr) override + { + if (!validateFloats(msg_ptr)) { + this->setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + // This doesn't work in the constructor. + if (!vehicle_info_) { + try { + vehicle_info_ = std::make_shared( + VehicleInfoUtil(*this->rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + updateVehicleInfo(); + } catch (const std::exception & e) { + RCLCPP_WARN_THROTTLE( + this->rviz_ros_node_.lock()->get_raw_node()->get_logger(), + *this->rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, + "Failed to get vehicle_info: %s", e.what()); + } + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!this->context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { + RCLCPP_DEBUG( + this->rviz_ros_node_.lock()->get_raw_node()->get_logger(), + "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), + qPrintable(this->fixed_frame_)); + } + + this->scene_node_->setPosition(position); + this->scene_node_->setOrientation(orientation); + + footprint_manual_object_->clear(); + point_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + if (!msg_ptr->points.empty()) { + footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); + footprint_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); + + point_manual_object_->estimateVertexCount(msg_ptr->points.size() * 3 * 8); + point_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); + + const float offset_from_baselink = property_offset_.getFloat(); + + preprocessMessageDetail(msg_ptr); + + for (size_t p_idx = 0; p_idx < msg_ptr->points.size(); p_idx++) { + const auto & point = msg_ptr->points.at(p_idx); + const auto & pose = tier4_autoware_utils::getPose(point); + // footprint + if (property_footprint_view_.getBool()) { + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(property_footprint_color_.getColor()); + color.a = property_footprint_alpha_.getFloat(); + + const auto info = vehicle_footprint_info_; + const float top = info->length - info->rear_overhang - offset_from_baselink; + const float bottom = -info->rear_overhang + offset_from_baselink; + const float left = -info->width / 2.0; + const float right = info->width / 2.0; + + const std::array lon_offset_vec{top, top, bottom, bottom}; + const std::array lat_offset_vec{left, right, right, left}; + + for (int f_idx = 0; f_idx < 4; ++f_idx) { + const Eigen::Quaternionf quat( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + + { + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; + const auto offset_to_edge = quat * offset_vec; + footprint_manual_object_->position( + pose.position.x + offset_to_edge.x(), pose.position.y + offset_to_edge.y(), + pose.position.z); + footprint_manual_object_->colour(color); + } + { + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; + const auto offset_to_edge = quat * offset_vec; + footprint_manual_object_->position( + pose.position.x + offset_to_edge.x(), pose.position.y + offset_to_edge.y(), + pose.position.z); + footprint_manual_object_->colour(color); + } + } + } + + // point + if (property_point_view_.getBool()) { + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(property_point_color_.getColor()); + color.a = property_point_alpha_.getFloat(); + + const double offset = property_point_offset_.getFloat(); + const double yaw = tf2::getYaw(pose.orientation); + const double base_x = pose.position.x + offset * std::cos(yaw); + const double base_y = pose.position.y + offset * std::sin(yaw); + const double base_z = pose.position.z; + + const double radius = property_point_radius_.getFloat(); + for (size_t s_idx = 0; s_idx < 8; ++s_idx) { + const double current_angle = static_cast(s_idx) / 8.0 * 2.0 * M_PI; + const double next_angle = static_cast(s_idx + 1) / 8.0 * 2.0 * M_PI; + point_manual_object_->position( + base_x + radius * std::cos(current_angle), base_y + radius * std::sin(current_angle), + base_z); + point_manual_object_->colour(color); + + point_manual_object_->position( + base_x + radius * std::cos(next_angle), base_y + radius * std::sin(next_angle), + base_z); + point_manual_object_->colour(color); + + point_manual_object_->position(base_x, base_y, base_z); + point_manual_object_->colour(color); + } + } + + processMessageDetail(msg_ptr, p_idx); + } + + footprint_manual_object_->end(); + point_manual_object_->end(); + } + last_msg_ptr_ = msg_ptr; + } +}; +} // namespace rviz_plugins + +#endif // PATH_FOOTPRINT__DISPLAY_BASE_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp deleted file mode 100644 index 4cbcb9a47daa2..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp +++ /dev/null @@ -1,100 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PATH_WITH_LANE_ID_FOOTPRINT__DISPLAY_HPP_ -#define PATH_WITH_LANE_ID_FOOTPRINT__DISPLAY_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace rviz_plugins -{ -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; - -class AutowarePathWithLaneIdFootprintDisplay -: public rviz_common::MessageFilterDisplay -{ - Q_OBJECT - -public: - AutowarePathWithLaneIdFootprintDisplay(); - virtual ~AutowarePathWithLaneIdFootprintDisplay(); - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - void updateVehicleInfo(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; - Ogre::ManualObject * path_footprint_manual_object_; - rviz_common::properties::BoolProperty * property_path_footprint_view_; - rviz_common::properties::ColorProperty * property_path_footprint_color_; - rviz_common::properties::FloatProperty * property_path_footprint_alpha_; - rviz_common::properties::FloatProperty * property_vehicle_length_; - rviz_common::properties::FloatProperty * property_vehicle_width_; - rviz_common::properties::FloatProperty * property_rear_overhang_; - rviz_common::properties::BoolProperty * property_lane_id_view_; - rviz_common::properties::FloatProperty * property_lane_id_scale_; - struct VehicleFootprintInfo - { - VehicleFootprintInfo(const float l, const float w, const float r) - : length(l), width(w), rear_overhang(r) - { - } - float length, width, rear_overhang; - }; - std::shared_ptr vehicle_info_; - std::shared_ptr vehicle_footprint_info_; - - using LaneIdObject = - std::pair, std::unique_ptr>; - std::vector lane_id_obj_ptrs_; - -private: - autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr last_msg_ptr_; - bool validateFloats( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr); - - void allocateLaneIdObjects(const std::size_t size); -}; - -} // namespace rviz_plugins - -#endif // PATH_WITH_LANE_ID_FOOTPRINT__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp deleted file mode 100644 index d3964f9e7ce07..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TRAJECTORY_FOOTPRINT__DISPLAY_HPP_ -#define TRAJECTORY_FOOTPRINT__DISPLAY_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include - -namespace rviz_plugins -{ -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; - -class AutowareTrajectoryFootprintDisplay -: public rviz_common::MessageFilterDisplay -{ - Q_OBJECT - -public: - AutowareTrajectoryFootprintDisplay(); - virtual ~AutowareTrajectoryFootprintDisplay(); - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - void updateVehicleInfo(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) override; - Ogre::ManualObject * trajectory_footprint_manual_object_; - rviz_common::properties::BoolProperty * property_trajectory_footprint_view_; - rviz_common::properties::ColorProperty * property_trajectory_footprint_color_; - rviz_common::properties::FloatProperty * property_trajectory_footprint_alpha_; - rviz_common::properties::FloatProperty * property_vehicle_length_; - rviz_common::properties::FloatProperty * property_vehicle_width_; - rviz_common::properties::FloatProperty * property_rear_overhang_; - rviz_common::properties::FloatProperty * property_offset_; - - Ogre::ManualObject * trajectory_point_manual_object_; - rviz_common::properties::BoolProperty * property_trajectory_point_view_; - rviz_common::properties::ColorProperty * property_trajectory_point_color_; - rviz_common::properties::FloatProperty * property_trajectory_point_alpha_; - rviz_common::properties::FloatProperty * property_trajectory_point_radius_; - rviz_common::properties::FloatProperty * property_trajectory_point_offset_; - - struct VehicleFootprintInfo - { - VehicleFootprintInfo(const float l, const float w, const float r) - : length(l), width(w), rear_overhang(r) - { - } - float length, width, rear_overhang; - }; - std::shared_ptr vehicle_info_; - std::shared_ptr vehicle_footprint_info_; - -private: - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr last_msg_ptr_; - bool validateFloats(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr); -}; - -} // namespace rviz_plugins - -#endif // TRAJECTORY_FOOTPRINT__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp index 5e58d6da03628..1124cde14da15 100644 --- a/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. +// Copyright 2023 TIER IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,188 +18,80 @@ #include #include #include +#include namespace rviz_plugins { -AutowarePathFootprintDisplay::AutowarePathFootprintDisplay() +AutowarePathWithLaneIdFootprintDisplay::AutowarePathWithLaneIdFootprintDisplay() +: property_lane_id_view_{"View LaneId", true, "", this}, + property_lane_id_scale_{"Scale", 0.1, "", &property_lane_id_view_} { - property_path_footprint_view_ = new rviz_common::properties::BoolProperty( - "View Path Footprint", true, "", this, SLOT(updateVisualization()), this); - property_path_footprint_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_path_footprint_view_, SLOT(updateVisualization()), this); - property_path_footprint_alpha_->setMin(0.0); - property_path_footprint_alpha_->setMax(1.0); - property_path_footprint_color_ = new rviz_common::properties::ColorProperty( - "Color", QColor(230, 230, 50), "", property_path_footprint_view_, SLOT(updateVisualization()), - this); - property_vehicle_length_ = new rviz_common::properties::FloatProperty( - "Vehicle Length", 4.77, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_vehicle_width_ = new rviz_common::properties::FloatProperty( - "Vehicle Width", 1.83, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_rear_overhang_ = new rviz_common::properties::FloatProperty( - "Rear Overhang", 1.03, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_vehicle_length_->setMin(0.0); - property_vehicle_width_->setMin(0.0); - property_rear_overhang_->setMin(0.0); - - updateVehicleInfo(); } -AutowarePathFootprintDisplay::~AutowarePathFootprintDisplay() +void AutowarePathWithLaneIdFootprintDisplay::resetDetail() { - if (initialized()) { - scene_manager_->destroyManualObject(path_footprint_manual_object_); + for (const auto & e : lane_id_obj_ptrs_) { + scene_node_->removeChild(e.first.get()); } + lane_id_obj_ptrs_.clear(); + lane_id_obj_ptrs_.shrink_to_fit(); } -void AutowarePathFootprintDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - path_footprint_manual_object_ = scene_manager_->createManualObject(); - path_footprint_manual_object_->setDynamic(true); - scene_node_->attachObject(path_footprint_manual_object_); -} - -void AutowarePathFootprintDisplay::reset() +void AutowarePathWithLaneIdFootprintDisplay::preprocessMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) { - MFDClass::reset(); - path_footprint_manual_object_->clear(); -} - -bool AutowarePathFootprintDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr) -{ - for (auto && path_point : msg_ptr->points) { - if (!rviz_common::validateFloats(path_point.pose)) { - return false; + const size_t size = msg_ptr->points.size(); + if (size > lane_id_obj_ptrs_.size()) { + for (std::size_t i = lane_id_obj_ptrs_.size(); i < size; i++) { + std::unique_ptr node_ptr; + node_ptr.reset(scene_node_->createChildSceneNode()); + auto text_ptr = + std::make_unique("not initialized", "Liberation Sans", 0.1); + text_ptr->setVisible(false); + text_ptr->setTextAlignment( + rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); + node_ptr->attachObject(text_ptr.get()); + lane_id_obj_ptrs_.push_back(std::make_pair(std::move(node_ptr), std::move(text_ptr))); } - } - return true; -} - -void AutowarePathFootprintDisplay::processMessage( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - // This doesn't work in the constructor. - if (!vehicle_info_) { - try { - vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); - updateVehicleInfo(); - } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", - e.what()); - } - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - path_footprint_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - path_footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); - path_footprint_manual_object_->begin( - "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - /* - * Footprint - */ - if (property_path_footprint_view_->getBool()) { - Ogre::ColourValue color; - color = rviz_common::properties::qtToOgre(property_path_footprint_color_->getColor()); - color.a = property_path_footprint_alpha_->getFloat(); - - const auto info = vehicle_footprint_info_; - const float top = info->length - info->rear_overhang; - const float bottom = -info->rear_overhang; - const float left = -info->width / 2.0; - const float right = info->width / 2.0; - - const std::array lon_offset_vec{top, top, bottom, bottom}; - const std::array lat_offset_vec{left, right, right, left}; - - for (int f_idx = 0; f_idx < 4; ++f_idx) { - const Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; - const auto offset_to_edge = quat * offset_vec; - path_footprint_manual_object_->position( - path_point.pose.position.x + offset_to_edge.x(), - path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); - path_footprint_manual_object_->colour(color); - } - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; - const auto offset_to_edge = quat * offset_vec; - path_footprint_manual_object_->position( - path_point.pose.position.x + offset_to_edge.x(), - path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); - path_footprint_manual_object_->colour(color); - } - } - } + } else { + for (std::size_t i = lane_id_obj_ptrs_.size() - 1; i >= size; i--) { + scene_node_->removeChild(lane_id_obj_ptrs_.at(i).first.get()); } - - path_footprint_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowarePathFootprintDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); + lane_id_obj_ptrs_.resize(size); } } -void AutowarePathFootprintDisplay::updateVehicleInfo() +void AutowarePathWithLaneIdFootprintDisplay::processMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const size_t p_idx) { - if (vehicle_info_) { - vehicle_footprint_info_ = std::make_shared( - vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, - vehicle_info_->rear_overhang_m); + const auto & point = msg_ptr->points.at(p_idx); + + // LaneId + if (property_lane_id_view_.getBool()) { + Ogre::Vector3 position; + position.x = point.point.pose.position.x; + position.y = point.point.pose.position.y; + position.z = point.point.pose.position.z; + auto & node_ptr = lane_id_obj_ptrs_.at(p_idx).first; + node_ptr->setPosition(position); + + const auto & text_ptr = lane_id_obj_ptrs_.at(p_idx).second; + std::string lane_ids_str = ""; + for (const auto & e : point.lane_ids) { + lane_ids_str += std::to_string(e) + ", "; + } + text_ptr->setCaption(lane_ids_str); + text_ptr->setCharacterHeight(property_lane_id_scale_.getFloat()); + text_ptr->setVisible(true); } else { - const float length{property_vehicle_length_->getFloat()}; - const float width{property_vehicle_width_->getFloat()}; - const float rear_overhang{property_rear_overhang_->getFloat()}; - - vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); + const auto & text_ptr = lane_id_obj_ptrs_.at(p_idx).second; + text_ptr->setVisible(false); } } - } // namespace rviz_plugins -#include +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdFootprintDisplay, rviz_common::Display) PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathFootprintDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryFootprintDisplay, rviz_common::Display) diff --git a/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp deleted file mode 100644 index c1eb332b4332d..0000000000000 --- a/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp +++ /dev/null @@ -1,264 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#define EIGEN_MPL2_ONLY -#include -#include -#include - -namespace rviz_plugins -{ -AutowarePathWithLaneIdFootprintDisplay::AutowarePathWithLaneIdFootprintDisplay() -{ - property_path_footprint_view_ = new rviz_common::properties::BoolProperty( - "View PathWithLaneId Footprint", true, "", this, SLOT(updateVisualization()), this); - property_path_footprint_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_path_footprint_view_, SLOT(updateVisualization()), this); - property_path_footprint_alpha_->setMin(0.0); - property_path_footprint_alpha_->setMax(1.0); - property_path_footprint_color_ = new rviz_common::properties::ColorProperty( - "Color", QColor(230, 230, 50), "", property_path_footprint_view_, SLOT(updateVisualization()), - this); - property_vehicle_length_ = new rviz_common::properties::FloatProperty( - "Vehicle Length", 4.77, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_vehicle_width_ = new rviz_common::properties::FloatProperty( - "Vehicle Width", 1.83, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_rear_overhang_ = new rviz_common::properties::FloatProperty( - "Rear Overhang", 1.03, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_vehicle_length_->setMin(0.0); - property_vehicle_width_->setMin(0.0); - property_rear_overhang_->setMin(0.0); - - property_lane_id_view_ = new rviz_common::properties::BoolProperty( - "View LaneId", true, "", this, SLOT(updateVisualization()), this); - property_lane_id_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.1, "", property_lane_id_view_, SLOT(updateVisualization()), this); - - updateVehicleInfo(); -} - -AutowarePathWithLaneIdFootprintDisplay::~AutowarePathWithLaneIdFootprintDisplay() -{ - if (initialized()) { - scene_manager_->destroyManualObject(path_footprint_manual_object_); - } -} - -void AutowarePathWithLaneIdFootprintDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - path_footprint_manual_object_ = scene_manager_->createManualObject(); - path_footprint_manual_object_->setDynamic(true); - scene_node_->attachObject(path_footprint_manual_object_); -} - -void AutowarePathWithLaneIdFootprintDisplay::reset() -{ - MFDClass::reset(); - path_footprint_manual_object_->clear(); - - for (const auto & e : lane_id_obj_ptrs_) { - scene_node_->removeChild(e.first.get()); - } - lane_id_obj_ptrs_.clear(); - lane_id_obj_ptrs_.shrink_to_fit(); -} - -bool AutowarePathWithLaneIdFootprintDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr) -{ - for (auto && path_point : msg_ptr->points) { - if (!rviz_common::validateFloats(path_point.point.pose)) { - return false; - } - } - return true; -} - -void AutowarePathWithLaneIdFootprintDisplay::allocateLaneIdObjects(const std::size_t size) -{ - if (size > lane_id_obj_ptrs_.size()) { - for (std::size_t i = lane_id_obj_ptrs_.size(); i < size; i++) { - std::unique_ptr node_ptr; - node_ptr.reset(scene_node_->createChildSceneNode()); - auto text_ptr = - std::make_unique("not initialized", "Liberation Sans", 0.1); - text_ptr->setVisible(false); - text_ptr->setTextAlignment( - rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); - node_ptr->attachObject(text_ptr.get()); - lane_id_obj_ptrs_.push_back(std::make_pair(std::move(node_ptr), std::move(text_ptr))); - } - } else { - for (std::size_t i = lane_id_obj_ptrs_.size() - 1; i >= size; i--) { - scene_node_->removeChild(lane_id_obj_ptrs_.at(i).first.get()); - } - lane_id_obj_ptrs_.resize(size); - } -} - -void AutowarePathWithLaneIdFootprintDisplay::processMessage( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - // This doesn't work in the constructor. - if (!vehicle_info_) { - try { - vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); - updateVehicleInfo(); - } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", - e.what()); - } - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - path_footprint_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - path_footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); - path_footprint_manual_object_->begin( - "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); - - allocateLaneIdObjects(msg_ptr->points.size()); - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - /* - * Footprint - */ - if (property_path_footprint_view_->getBool()) { - Ogre::ColourValue color; - color = rviz_common::properties::qtToOgre(property_path_footprint_color_->getColor()); - color.a = property_path_footprint_alpha_->getFloat(); - - const auto info = vehicle_footprint_info_; - const float top = info->length - info->rear_overhang; - const float bottom = -info->rear_overhang; - const float left = -info->width / 2.0; - const float right = info->width / 2.0; - - const std::array lon_offset_vec{top, top, bottom, bottom}; - const std::array lat_offset_vec{left, right, right, left}; - - for (int f_idx = 0; f_idx < 4; ++f_idx) { - const Eigen::Quaternionf quat( - path_point.point.pose.orientation.w, path_point.point.pose.orientation.x, - path_point.point.pose.orientation.y, path_point.point.pose.orientation.z); - - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; - const auto offset_to_edge = quat * offset_vec; - path_footprint_manual_object_->position( - path_point.point.pose.position.x + offset_to_edge.x(), - path_point.point.pose.position.y + offset_to_edge.y(), - path_point.point.pose.position.z); - path_footprint_manual_object_->colour(color); - } - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; - const auto offset_to_edge = quat * offset_vec; - path_footprint_manual_object_->position( - path_point.point.pose.position.x + offset_to_edge.x(), - path_point.point.pose.position.y + offset_to_edge.y(), - path_point.point.pose.position.z); - path_footprint_manual_object_->colour(color); - } - } - } - - // LaneId - if (property_lane_id_view_->getBool()) { - Ogre::Vector3 position; - position.x = path_point.point.pose.position.x; - position.y = path_point.point.pose.position.y; - position.z = path_point.point.pose.position.z; - auto & node_ptr = lane_id_obj_ptrs_.at(point_idx).first; - node_ptr->setPosition(position); - - const auto & text_ptr = lane_id_obj_ptrs_.at(point_idx).second; - std::string lane_ids_str = ""; - for (const auto & e : path_point.lane_ids) { - lane_ids_str += std::to_string(e) + ", "; - } - text_ptr->setCaption(lane_ids_str); - text_ptr->setCharacterHeight(property_lane_id_scale_->getFloat()); - text_ptr->setVisible(true); - } else { - const auto & text_ptr = lane_id_obj_ptrs_.at(point_idx).second; - text_ptr->setVisible(false); - } - } - - path_footprint_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowarePathWithLaneIdFootprintDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } -} - -void AutowarePathWithLaneIdFootprintDisplay::updateVehicleInfo() -{ - if (vehicle_info_) { - vehicle_footprint_info_ = std::make_shared( - vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, - vehicle_info_->rear_overhang_m); - } else { - const float length{property_vehicle_length_->getFloat()}; - const float width{property_vehicle_width_->getFloat()}; - const float rear_overhang{property_rear_overhang_->getFloat()}; - - vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); - } -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdFootprintDisplay, rviz_common::Display) diff --git a/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp deleted file mode 100644 index b9c32aea58be3..0000000000000 --- a/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp +++ /dev/null @@ -1,275 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#define EIGEN_MPL2_ONLY -#include -#include -#include - -#include - -namespace rviz_plugins -{ -AutowareTrajectoryFootprintDisplay::AutowareTrajectoryFootprintDisplay() -{ - // trajectory footprint - property_trajectory_footprint_view_ = new rviz_common::properties::BoolProperty( - "View Trajectory Footprint", true, "", this, SLOT(updateVisualization()), this); - property_trajectory_footprint_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_trajectory_footprint_view_, SLOT(updateVisualization()), this); - property_trajectory_footprint_alpha_->setMin(0.0); - property_trajectory_footprint_alpha_->setMax(1.0); - property_trajectory_footprint_color_ = new rviz_common::properties::ColorProperty( - "Color", QColor(230, 230, 50), "", property_trajectory_footprint_view_, - SLOT(updateVisualization()), this); - property_vehicle_length_ = new rviz_common::properties::FloatProperty( - "Vehicle Length", 4.77, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), - this); - property_vehicle_width_ = new rviz_common::properties::FloatProperty( - "Vehicle Width", 1.83, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), - this); - property_rear_overhang_ = new rviz_common::properties::FloatProperty( - "Rear Overhang", 1.03, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), - this); - property_offset_ = new rviz_common::properties::FloatProperty( - "Offset from BaseLink", 0.0, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), - this); - property_vehicle_length_->setMin(0.0); - property_vehicle_width_->setMin(0.0); - property_rear_overhang_->setMin(0.0); - - // trajectory point - property_trajectory_point_view_ = new rviz_common::properties::BoolProperty( - "View Trajectory Point", false, "", this, SLOT(updateVisualization()), this); - property_trajectory_point_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_trajectory_point_view_, SLOT(updateVisualization()), this); - property_trajectory_point_alpha_->setMin(0.0); - property_trajectory_point_alpha_->setMax(1.0); - property_trajectory_point_color_ = new rviz_common::properties::ColorProperty( - "Color", QColor(0, 60, 255), "", property_trajectory_point_view_, SLOT(updateVisualization()), - this); - property_trajectory_point_radius_ = new rviz_common::properties::FloatProperty( - "Radius", 0.1, "", property_trajectory_point_view_, SLOT(updateVisualization()), this); - property_trajectory_point_offset_ = new rviz_common::properties::FloatProperty( - "Offset", 0.0, "", property_trajectory_point_view_, SLOT(updateVisualization()), this); - - updateVehicleInfo(); -} - -AutowareTrajectoryFootprintDisplay::~AutowareTrajectoryFootprintDisplay() -{ - if (initialized()) { - scene_manager_->destroyManualObject(trajectory_footprint_manual_object_); - scene_manager_->destroyManualObject(trajectory_point_manual_object_); - } -} - -void AutowareTrajectoryFootprintDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - trajectory_footprint_manual_object_ = scene_manager_->createManualObject(); - trajectory_footprint_manual_object_->setDynamic(true); - scene_node_->attachObject(trajectory_footprint_manual_object_); - - trajectory_point_manual_object_ = scene_manager_->createManualObject(); - trajectory_point_manual_object_->setDynamic(true); - scene_node_->attachObject(trajectory_point_manual_object_); -} - -void AutowareTrajectoryFootprintDisplay::reset() -{ - MFDClass::reset(); - trajectory_footprint_manual_object_->clear(); - trajectory_point_manual_object_->clear(); -} - -bool AutowareTrajectoryFootprintDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr) -{ - for (auto && trajectory_point : msg_ptr->points) { - if (!rviz_common::validateFloats(trajectory_point.pose)) { - return false; - } - } - return true; -} - -void AutowareTrajectoryFootprintDisplay::processMessage( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - // This doesn't work in the constructor. - if (!vehicle_info_) { - try { - vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); - updateVehicleInfo(); - } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", - e.what()); - } - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - trajectory_footprint_manual_object_->clear(); - trajectory_point_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - trajectory_footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); - trajectory_footprint_manual_object_->begin( - "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); - - trajectory_point_manual_object_->estimateVertexCount(msg_ptr->points.size() * 3 * 8); - trajectory_point_manual_object_->begin( - "BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); - - const float offset_from_baselink = property_offset_->getFloat(); - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - /* - * Footprint - */ - if (property_trajectory_footprint_view_->getBool()) { - Ogre::ColourValue color; - color = rviz_common::properties::qtToOgre(property_trajectory_footprint_color_->getColor()); - color.a = property_trajectory_footprint_alpha_->getFloat(); - - const auto info = vehicle_footprint_info_; - const float top = info->length - info->rear_overhang - offset_from_baselink; - const float bottom = -info->rear_overhang + offset_from_baselink; - const float left = -info->width / 2.0; - const float right = info->width / 2.0; - - const std::array lon_offset_vec{top, top, bottom, bottom}; - const std::array lat_offset_vec{left, right, right, left}; - - for (int f_idx = 0; f_idx < 4; ++f_idx) { - const Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; - const auto offset_to_edge = quat * offset_vec; - trajectory_footprint_manual_object_->position( - path_point.pose.position.x + offset_to_edge.x(), - path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); - trajectory_footprint_manual_object_->colour(color); - } - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; - const auto offset_to_edge = quat * offset_vec; - trajectory_footprint_manual_object_->position( - path_point.pose.position.x + offset_to_edge.x(), - path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); - trajectory_footprint_manual_object_->colour(color); - } - } - } - - /* - * Point - */ - if (property_trajectory_point_view_->getBool()) { - Ogre::ColourValue color; - color = rviz_common::properties::qtToOgre(property_trajectory_point_color_->getColor()); - color.a = property_trajectory_point_alpha_->getFloat(); - - const double offset = property_trajectory_point_offset_->getFloat(); - const double yaw = tf2::getYaw(path_point.pose.orientation); - const double base_x = path_point.pose.position.x + offset * std::cos(yaw); - const double base_y = path_point.pose.position.y + offset * std::sin(yaw); - const double base_z = path_point.pose.position.z; - - const double radius = property_trajectory_point_radius_->getFloat(); - for (size_t s_idx = 0; s_idx < 8; ++s_idx) { - const double current_angle = static_cast(s_idx) / 8.0 * 2.0 * M_PI; - const double next_angle = static_cast(s_idx + 1) / 8.0 * 2.0 * M_PI; - trajectory_point_manual_object_->position( - base_x + radius * std::cos(current_angle), base_y + radius * std::sin(current_angle), - base_z); - trajectory_point_manual_object_->colour(color); - - trajectory_point_manual_object_->position( - base_x + radius * std::cos(next_angle), base_y + radius * std::sin(next_angle), base_z); - trajectory_point_manual_object_->colour(color); - - trajectory_point_manual_object_->position(base_x, base_y, base_z); - trajectory_point_manual_object_->colour(color); - } - } - } - - trajectory_footprint_manual_object_->end(); - trajectory_point_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowareTrajectoryFootprintDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } -} - -void AutowareTrajectoryFootprintDisplay::updateVehicleInfo() -{ - if (vehicle_info_) { - vehicle_footprint_info_ = std::make_shared( - vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, - vehicle_info_->rear_overhang_m); - } else { - const float length{property_vehicle_length_->getFloat()}; - const float width{property_vehicle_width_->getFloat()}; - const float rear_overhang{property_rear_overhang_->getFloat()}; - - vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); - } -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryFootprintDisplay, rviz_common::Display) From fb2bb6dc68ed5c072a5e540cbd3ae737c8dc3da4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 11 Feb 2023 23:15:56 +0900 Subject: [PATCH 65/72] refactor(tier4_planning_rviz_plugin): clean up the code of path (#2871) * refactor(tier4_planning_rviz_plugin): clean up the code of path Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../tier4_planning_rviz_plugin/CMakeLists.txt | 6 +- .../include/path/display.hpp | 71 +--- .../include/path/display_base.hpp | 354 ++++++++++++++++++ .../include/path_with_lane_id/display.hpp | 91 ----- .../include/trajectory/display.hpp | 90 ----- .../src/path/display.cpp | 247 +----------- .../src/path_with_lane_id/display.cpp | 312 --------------- .../src/trajectory/display.cpp | 318 ---------------- 8 files changed, 374 insertions(+), 1115 deletions(-) create mode 100644 common/tier4_planning_rviz_plugin/include/path/display_base.hpp delete mode 100644 common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp delete mode 100644 common/tier4_planning_rviz_plugin/include/trajectory/display.hpp delete mode 100644 common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp delete mode 100644 common/tier4_planning_rviz_plugin/src/trajectory/display.cpp diff --git a/common/tier4_planning_rviz_plugin/CMakeLists.txt b/common/tier4_planning_rviz_plugin/CMakeLists.txt index 58240fa964d04..7dc662e52f29f 100644 --- a/common/tier4_planning_rviz_plugin/CMakeLists.txt +++ b/common/tier4_planning_rviz_plugin/CMakeLists.txt @@ -19,18 +19,16 @@ include_directories( ament_auto_add_library(tier4_planning_rviz_plugin SHARED include/drivable_area/display.hpp src/drivable_area/display.cpp + # path point + include/path/display_base.hpp include/path/display.hpp src/path/display.cpp - include/path_with_lane_id/display.hpp - src/path_with_lane_id/display.cpp # footprint include/path_footprint/display_base.hpp include/path_footprint/display.hpp src/path_footprint/display.cpp include/pose_with_uuid_stamped/display.hpp src/pose_with_uuid_stamped/display.cpp - include/trajectory/display.hpp - src/trajectory/display.cpp include/mission_checkpoint/mission_checkpoint.hpp src/mission_checkpoint/mission_checkpoint.cpp src/tools/jsk_overlay_utils.cpp diff --git a/common/tier4_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/path/display.hpp index 0029d13369a3e..01ce426da1985 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,71 +15,30 @@ #ifndef PATH__DISPLAY_HPP_ #define PATH__DISPLAY_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include #include - -#include -#include -#include -#include -#include - -#include -#include +#include +#include namespace rviz_plugins { -class AutowarePathDisplay -: public rviz_common::MessageFilterDisplay +class AutowarePathWithLaneIdDisplay +: public AutowarePathBaseDisplay { Q_OBJECT +}; -public: - AutowarePathDisplay(); - ~AutowarePathDisplay() override; - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) override; - static std::unique_ptr setColorDependsOnVelocity( - const double vel_max, const double cmd_vel); - static std::unique_ptr gradation( - const QColor & color_min, const QColor & color_max, const double ratio); - Ogre::ManualObject * path_manual_object_{nullptr}; - Ogre::ManualObject * velocity_manual_object_{nullptr}; - rviz_common::properties::BoolProperty * property_path_view_; - rviz_common::properties::BoolProperty * property_velocity_view_; - rviz_common::properties::FloatProperty * property_path_width_; - rviz_common::properties::ColorProperty * property_path_color_; - rviz_common::properties::ColorProperty * property_velocity_color_; - rviz_common::properties::FloatProperty * property_path_alpha_; - rviz_common::properties::FloatProperty * property_velocity_alpha_; - rviz_common::properties::FloatProperty * property_velocity_scale_; - rviz_common::properties::BoolProperty * property_path_color_view_; - rviz_common::properties::BoolProperty * property_velocity_color_view_; - rviz_common::properties::FloatProperty * property_vel_max_; - -private: // NOLINT for Qt - autoware_auto_planning_msgs::msg::Path::ConstSharedPtr last_msg_ptr_; - static bool validateFloats( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr); +class AutowarePathDisplay : public AutowarePathBaseDisplay +{ + Q_OBJECT }; +class AutowareTrajectoryDisplay +: public AutowarePathBaseDisplay +{ + Q_OBJECT +}; } // namespace rviz_plugins #endif // PATH__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp new file mode 100644 index 0000000000000..567d6a0f1187b --- /dev/null +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -0,0 +1,354 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH__DISPLAY_BASE_HPP_ +#define PATH__DISPLAY_BASE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include + +namespace +{ +std::unique_ptr gradation( + const QColor & color_min, const QColor & color_max, const double ratio) +{ + std::unique_ptr color_ptr(new Ogre::ColourValue); + color_ptr->g = + static_cast(color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio)); + color_ptr->r = static_cast(color_max.redF() * ratio + color_min.redF() * (1.0 - ratio)); + color_ptr->b = static_cast(color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio)); + + return color_ptr; +} + +std::unique_ptr setColorDependsOnVelocity( + const double vel_max, const double cmd_vel) +{ + const double cmd_vel_abs = std::fabs(cmd_vel); + const double vel_min = 0.0; + + std::unique_ptr color_ptr(new Ogre::ColourValue()); + if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { + double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); + color_ptr = gradation(Qt::red, Qt::yellow, ratio); + } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { + double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); + color_ptr = gradation(Qt::yellow, Qt::green, ratio); + } else if (vel_max < cmd_vel_abs) { + *color_ptr = Ogre::ColourValue::Green; + } else { + *color_ptr = Ogre::ColourValue::Red; + } + + return color_ptr; +} + +template +bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) +{ + for (auto && path_point : msg_ptr->points) { + if ( + !rviz_common::validateFloats(tier4_autoware_utils::getPose(path_point)) && + !rviz_common::validateFloats(tier4_autoware_utils::getLongitudinalVelocity(path_point))) { + return false; + } + } + return true; +} +} // namespace + +namespace rviz_plugins +{ +template +class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay +{ +public: + AutowarePathBaseDisplay() + : // path + property_path_view_{"View Path", true, "", this}, + property_path_width_{"Width", 2.0, "", &property_path_view_}, + property_path_alpha_{"Alpha", 1.0, "", &property_path_view_}, + property_path_color_view_{"Constant Color", false, "", &property_path_view_}, + property_path_color_{"Color", Qt::black, "", &property_path_view_}, + property_vel_max_{"Color Border Vel Max", 3.0, "[m/s]", this}, + // velocity + property_velocity_view_{"View Velocity", true, "", this}, + property_velocity_alpha_{"Alpha", 1.0, "", &property_velocity_view_}, + property_velocity_scale_{"Scale", 0.3, "", &property_velocity_view_}, + property_velocity_color_view_{"Constant Color", false, "", &property_velocity_view_}, + property_velocity_color_{"Color", Qt::black, "", &property_velocity_view_}, + // velocity text + property_velocity_text_view_{"View Text Velocity", false, "", this}, + property_velocity_text_scale_{"Scale", 0.3, "", &property_velocity_text_view_} + { + // path + property_path_width_.setMin(0.0); + property_path_alpha_.setMin(0.0); + property_path_alpha_.setMax(1.0); + property_vel_max_.setMin(0.0); + // velocity + property_velocity_alpha_.setMin(0.0); + property_velocity_alpha_.setMax(1.0); + // velocity text + property_velocity_scale_.setMin(0.1); + property_velocity_scale_.setMax(10.0); + } + + ~AutowarePathBaseDisplay() override + { + if (this->initialized()) { + this->scene_manager_->destroyManualObject(path_manual_object_); + this->scene_manager_->destroyManualObject(velocity_manual_object_); + for (size_t i = 0; i < velocity_text_nodes_.size(); i++) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->removeAndDestroyAllChildren(); + node->detachAllObjects(); + this->scene_manager_->destroySceneNode(node); + } + } + } + void onInitialize() override + { + rviz_common::MessageFilterDisplay::MFDClass::onInitialize(); + + path_manual_object_ = this->scene_manager_->createManualObject(); + velocity_manual_object_ = this->scene_manager_->createManualObject(); + path_manual_object_->setDynamic(true); + velocity_manual_object_->setDynamic(true); + this->scene_node_->attachObject(path_manual_object_); + this->scene_node_->attachObject(velocity_manual_object_); + } + void reset() override + { + rviz_common::MessageFilterDisplay::MFDClass::reset(); + path_manual_object_->clear(); + velocity_manual_object_->clear(); + for (size_t i = 0; i < velocity_texts_.size(); i++) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + this->scene_manager_->destroySceneNode(node); + } + velocity_text_nodes_.clear(); + velocity_texts_.clear(); + } + +protected: + void processMessage(const typename T::ConstSharedPtr msg_ptr) override + { + if (!validateFloats(msg_ptr)) { + this->setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!this->context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { + RCLCPP_DEBUG( + rclcpp::get_logger("AutowarePathBaseDisplay"), + "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), + qPrintable(this->fixed_frame_)); + } + + this->scene_node_->setPosition(position); + this->scene_node_->setOrientation(orientation); + + path_manual_object_->clear(); + velocity_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + if (!msg_ptr->points.empty()) { + path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); + velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); + path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); + // path_manual_object_->begin("BaseWhiteNoLighting", + // Ogre::RenderOperation::OT_TRIANGLE_STRIP); + velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); + + if (msg_ptr->points.size() > velocity_texts_.size()) { + for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { + Ogre::SceneNode * node = this->scene_node_->createChildSceneNode(); + rviz_rendering::MovableText * text = + new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); + text->setVisible(false); + text->setTextAlignment( + rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); + node->attachObject(text); + velocity_texts_.push_back(text); + velocity_text_nodes_.push_back(node); + } + } else if (msg_ptr->points.size() < velocity_texts_.size()) { + for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + this->scene_manager_->destroySceneNode(node); + } + velocity_texts_.resize(msg_ptr->points.size()); + velocity_text_nodes_.resize(msg_ptr->points.size()); + } + + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { + const auto & path_point = msg_ptr->points.at(point_idx); + const auto & pose = tier4_autoware_utils::getPose(path_point); + const auto & velocity = tier4_autoware_utils::getLongitudinalVelocity(path_point); + + // path + if (property_path_view_.getBool()) { + Ogre::ColourValue color; + if (property_path_color_view_.getBool()) { + color = rviz_common::properties::qtToOgre(property_path_color_.getColor()); + } else { + // color change depending on velocity + std::unique_ptr dynamic_color_ptr = + setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); + color = *dynamic_color_ptr; + } + color.a = property_path_alpha_.getFloat(); + Eigen::Vector3f vec_in; + Eigen::Vector3f vec_out; + Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); + { + vec_in << 0, (property_path_width_.getFloat() / 2.0), 0; + Eigen::Quaternionf quat( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + if (!isDrivingForward(msg_ptr->points, point_idx)) { + quat *= quat_yaw_reverse; + } + vec_out = quat * vec_in; + path_manual_object_->position( + static_cast(pose.position.x) + vec_out.x(), + static_cast(pose.position.y) + vec_out.y(), + static_cast(pose.position.z) + vec_out.z()); + path_manual_object_->colour(color); + } + { + vec_in << 0, -(property_path_width_.getFloat() / 2.0), 0; + Eigen::Quaternionf quat( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + if (!isDrivingForward(msg_ptr->points, point_idx)) { + quat *= quat_yaw_reverse; + } + vec_out = quat * vec_in; + path_manual_object_->position( + static_cast(pose.position.x) + vec_out.x(), + static_cast(pose.position.y) + vec_out.y(), + static_cast(pose.position.z) + vec_out.z()); + path_manual_object_->colour(color); + } + } + + // velocity + if (property_velocity_view_.getBool()) { + Ogre::ColourValue color; + if (property_velocity_color_view_.getBool()) { + color = rviz_common::properties::qtToOgre(property_velocity_color_.getColor()); + } else { + /* color change depending on velocity */ + std::unique_ptr dynamic_color_ptr = + setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); + color = *dynamic_color_ptr; + } + color.a = property_velocity_alpha_.getFloat(); + + velocity_manual_object_->position( + pose.position.x, pose.position.y, + static_cast(pose.position.z) + velocity * property_velocity_scale_.getFloat()); + velocity_manual_object_->colour(color); + } + + // velocity text + if (property_velocity_text_view_.getBool()) { + Ogre::Vector3 position; + position.x = pose.position.x; + position.y = pose.position.y; + position.z = pose.position.z; + Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); + node->setPosition(position); + + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + const double vel = velocity; + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << vel; + text->setCaption(ss.str()); + text->setCharacterHeight(property_velocity_text_scale_.getFloat()); + text->setVisible(true); + } else { + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + text->setVisible(false); + } + } + + path_manual_object_->end(); + velocity_manual_object_->end(); + } + last_msg_ptr_ = msg_ptr; + } + + Ogre::ManualObject * path_manual_object_{nullptr}; + Ogre::ManualObject * velocity_manual_object_{nullptr}; + std::vector velocity_texts_; + std::vector velocity_text_nodes_; + + rviz_common::properties::BoolProperty property_path_view_; + rviz_common::properties::FloatProperty property_path_width_; + rviz_common::properties::FloatProperty property_path_alpha_; + rviz_common::properties::BoolProperty property_path_color_view_; + rviz_common::properties::ColorProperty property_path_color_; + rviz_common::properties::FloatProperty property_vel_max_; + rviz_common::properties::BoolProperty property_velocity_view_; + rviz_common::properties::FloatProperty property_velocity_alpha_; + rviz_common::properties::FloatProperty property_velocity_scale_; + rviz_common::properties::BoolProperty property_velocity_color_view_; + rviz_common::properties::ColorProperty property_velocity_color_; + rviz_common::properties::BoolProperty property_velocity_text_view_; + rviz_common::properties::FloatProperty property_velocity_text_scale_; + +private: + typename T::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // PATH__DISPLAY_BASE_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp b/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp deleted file mode 100644 index df01ffc73c466..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp +++ /dev/null @@ -1,91 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PATH_WITH_LANE_ID__DISPLAY_HPP_ -#define PATH_WITH_LANE_ID__DISPLAY_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace rviz_plugins -{ -class AutowarePathWithLaneIdDisplay -: public rviz_common::MessageFilterDisplay -{ - Q_OBJECT - -public: - AutowarePathWithLaneIdDisplay(); - virtual ~AutowarePathWithLaneIdDisplay(); - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; - std::unique_ptr setColorDependsOnVelocity( - const double vel_max, const double cmd_vel); - std::unique_ptr gradation( - const QColor & color_min, const QColor & color_max, const double ratio); - Ogre::ManualObject * path_manual_object_; - Ogre::ManualObject * velocity_manual_object_; - std::vector velocity_texts_; - std::vector velocity_text_nodes_; - rviz_common::properties::BoolProperty * property_path_view_; - rviz_common::properties::BoolProperty * property_velocity_view_; - rviz_common::properties::FloatProperty * property_path_width_; - rviz_common::properties::ColorProperty * property_path_color_; - rviz_common::properties::ColorProperty * property_velocity_color_; - rviz_common::properties::FloatProperty * property_path_alpha_; - rviz_common::properties::FloatProperty * property_velocity_alpha_; - rviz_common::properties::FloatProperty * property_velocity_scale_; - rviz_common::properties::BoolProperty * property_velocity_text_view_; - rviz_common::properties::FloatProperty * property_velocity_text_scale_; - rviz_common::properties::BoolProperty * property_path_color_view_; - rviz_common::properties::BoolProperty * property_velocity_color_view_; - rviz_common::properties::FloatProperty * property_vel_max_; - -private: - autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr last_msg_ptr_; - bool validateFloats( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr); -}; - -} // namespace rviz_plugins - -#endif // PATH_WITH_LANE_ID__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/trajectory/display.hpp b/common/tier4_planning_rviz_plugin/include/trajectory/display.hpp deleted file mode 100644 index 428774e1d3b90..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/trajectory/display.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TRAJECTORY__DISPLAY_HPP_ -#define TRAJECTORY__DISPLAY_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace rviz_plugins -{ -class AutowareTrajectoryDisplay -: public rviz_common::MessageFilterDisplay -{ - Q_OBJECT - -public: - AutowareTrajectoryDisplay(); - virtual ~AutowareTrajectoryDisplay(); - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) override; - std::unique_ptr setColorDependsOnVelocity( - const double vel_max, const double cmd_vel); - std::unique_ptr gradation( - const QColor & color_min, const QColor & color_max, const double ratio); - Ogre::ManualObject * path_manual_object_; - Ogre::ManualObject * velocity_manual_object_; - std::vector velocity_texts_; - std::vector velocity_text_nodes_; - rviz_common::properties::BoolProperty * property_path_view_; - rviz_common::properties::BoolProperty * property_velocity_view_; - rviz_common::properties::FloatProperty * property_path_width_; - rviz_common::properties::ColorProperty * property_path_color_; - rviz_common::properties::ColorProperty * property_velocity_color_; - rviz_common::properties::FloatProperty * property_velocity_scale_; - rviz_common::properties::BoolProperty * property_velocity_text_view_; - rviz_common::properties::FloatProperty * property_velocity_text_scale_; - rviz_common::properties::FloatProperty * property_path_alpha_; - rviz_common::properties::FloatProperty * property_velocity_alpha_; - rviz_common::properties::BoolProperty * property_path_color_view_; - rviz_common::properties::BoolProperty * property_velocity_color_view_; - rviz_common::properties::FloatProperty * property_vel_max_; - -private: - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr last_msg_ptr_; - bool validateFloats(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr); -}; - -} // namespace rviz_plugins - -#endif // TRAJECTORY__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 02d6623cc5b6e..b4e61616e18e3 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,248 +13,7 @@ // limitations under the License. #include -#include - -#include -#define EIGEN_MPL2_ONLY -#include -#include - -namespace rviz_plugins -{ -std::unique_ptr AutowarePathDisplay::gradation( - const QColor & color_min, const QColor & color_max, const double ratio) -{ - std::unique_ptr color_ptr(new Ogre::ColourValue); - color_ptr->g = - static_cast(color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio)); - color_ptr->r = static_cast(color_max.redF() * ratio + color_min.redF() * (1.0 - ratio)); - color_ptr->b = static_cast(color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio)); - - return color_ptr; -} - -std::unique_ptr AutowarePathDisplay::setColorDependsOnVelocity( - const double vel_max, const double cmd_vel) -{ - const double cmd_vel_abs = std::fabs(cmd_vel); - const double vel_min = 0.0; - - std::unique_ptr color_ptr(new Ogre::ColourValue()); - if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { - double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); - color_ptr = gradation(Qt::red, Qt::yellow, ratio); - } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { - double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); - color_ptr = gradation(Qt::yellow, Qt::green, ratio); - } else if (vel_max < cmd_vel_abs) { - *color_ptr = Ogre::ColourValue::Green; - } else { - *color_ptr = Ogre::ColourValue::Red; - } - - return color_ptr; -} - -AutowarePathDisplay::AutowarePathDisplay() -{ - property_path_view_ = new rviz_common::properties::BoolProperty( - "View Path", true, "", this, SLOT(updateVisualization())); - property_path_width_ = new rviz_common::properties::FloatProperty( - "Width", 2.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_width_->setMin(0.0); - property_path_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_alpha_->setMin(0.0); - property_path_alpha_->setMax(1.0); - property_path_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_path_view_, SLOT(updateVisualization()), this); - - property_velocity_view_ = new rviz_common::properties::BoolProperty( - "View Velocity", true, "", this, SLOT(updateVisualization()), this); - property_velocity_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_alpha_->setMin(0.0); - property_velocity_alpha_->setMax(1.0); - property_velocity_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.3, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_scale_->setMin(0.1); - property_velocity_scale_->setMax(10.0); - property_velocity_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this); - - property_vel_max_ = new rviz_common::properties::FloatProperty( - "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); - property_vel_max_->setMin(0.0); -} - -AutowarePathDisplay::~AutowarePathDisplay() -{ - if (initialized()) { - scene_manager_->destroyManualObject(path_manual_object_); - scene_manager_->destroyManualObject(velocity_manual_object_); - } -} - -void AutowarePathDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - path_manual_object_ = scene_manager_->createManualObject(); - velocity_manual_object_ = scene_manager_->createManualObject(); - path_manual_object_->setDynamic(true); - velocity_manual_object_->setDynamic(true); - scene_node_->attachObject(path_manual_object_); - scene_node_->attachObject(velocity_manual_object_); -} - -void AutowarePathDisplay::reset() -{ - MFDClass::reset(); - path_manual_object_->clear(); - velocity_manual_object_->clear(); -} - -bool AutowarePathDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr) -{ - for (auto && path_point : msg_ptr->points) { - if ( - !rviz_common::validateFloats(path_point.pose) && - !rviz_common::validateFloats(path_point.longitudinal_velocity_mps)) { - return false; - } - } - return true; -} - -void AutowarePathDisplay::processMessage( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rclcpp::get_logger("AutowarePathDisplay"), "Error transforming from frame '%s' to frame '%s'", - msg_ptr->header.frame_id.c_str(), qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - path_manual_object_->clear(); - velocity_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); - velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); - path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - // path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - /* - * Path - */ - if (property_path_view_->getBool()) { - Ogre::ColourValue color; - if (property_path_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_path_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_path_alpha_->getFloat(); - Eigen::Vector3f vec_in; - Eigen::Vector3f vec_out; - Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); - { - vec_in << 0, (property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - static_cast(path_point.pose.position.x) + vec_out.x(), - static_cast(path_point.pose.position.y) + vec_out.y(), - static_cast(path_point.pose.position.z) + vec_out.z()); - path_manual_object_->colour(color); - } - { - vec_in << 0, -(property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - static_cast(path_point.pose.position.x) + vec_out.x(), - static_cast(path_point.pose.position.y) + vec_out.y(), - static_cast(path_point.pose.position.z) + vec_out.z()); - path_manual_object_->colour(color); - } - } - /* - * Velocity - */ - if (property_velocity_view_->getBool()) { - Ogre::ColourValue color; - if (property_velocity_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_velocity_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_velocity_alpha_->getFloat(); - - velocity_manual_object_->position( - path_point.pose.position.x, path_point.pose.position.y, - static_cast(path_point.pose.position.z) + - path_point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); - velocity_manual_object_->colour(color); - } - } - - path_manual_object_->end(); - velocity_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowarePathDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } -} - -} // namespace rviz_plugins - #include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdDisplay, rviz_common::Display) PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryDisplay, rviz_common::Display) diff --git a/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp b/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp deleted file mode 100644 index 64400197fbbb7..0000000000000 --- a/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#define EIGEN_MPL2_ONLY -#include -#include - -namespace rviz_plugins -{ -std::unique_ptr AutowarePathWithLaneIdDisplay::gradation( - const QColor & color_min, const QColor & color_max, const double ratio) -{ - std::unique_ptr color_ptr(new Ogre::ColourValue); - color_ptr->g = color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio); - color_ptr->r = color_max.redF() * ratio + color_min.redF() * (1.0 - ratio); - color_ptr->b = color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio); - - return color_ptr; -} - -std::unique_ptr AutowarePathWithLaneIdDisplay::setColorDependsOnVelocity( - const double vel_max, const double cmd_vel) -{ - const double cmd_vel_abs = std::fabs(cmd_vel); - const double vel_min = 0.0; - - std::unique_ptr color_ptr(new Ogre::ColourValue()); - if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { - double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); - color_ptr = gradation(Qt::red, Qt::yellow, ratio); - } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { - double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); - color_ptr = gradation(Qt::yellow, Qt::green, ratio); - } else if (vel_max < cmd_vel_abs) { - *color_ptr = Ogre::ColourValue::Green; - } else { - *color_ptr = Ogre::ColourValue::Red; - } - - return color_ptr; -} - -AutowarePathWithLaneIdDisplay::AutowarePathWithLaneIdDisplay() -{ - property_path_view_ = new rviz_common::properties::BoolProperty( - "View Path", true, "", this, SLOT(updateVisualization())); - property_path_width_ = new rviz_common::properties::FloatProperty( - "Width", 2.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_width_->setMin(0.0); - property_path_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_alpha_->setMin(0.0); - property_path_alpha_->setMax(1.0); - property_path_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_path_view_, SLOT(updateVisualization()), this); - - property_velocity_view_ = new rviz_common::properties::BoolProperty( - "View Velocity", true, "", this, SLOT(updateVisualization()), this); - property_velocity_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_alpha_->setMin(0.0); - property_velocity_alpha_->setMax(1.0); - property_velocity_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.3, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_scale_->setMin(0.1); - property_velocity_scale_->setMax(10.0); - property_velocity_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_text_view_ = new rviz_common::properties::BoolProperty( - "View Text Velocity", false, "", this, SLOT(updateVisualization()), this); - property_velocity_text_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.3, "", property_velocity_text_view_, SLOT(updateVisualization()), this); - property_vel_max_ = new rviz_common::properties::FloatProperty( - "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); - property_vel_max_->setMin(0.0); -} - -AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay() -{ - if (initialized()) { - scene_manager_->destroyManualObject(path_manual_object_); - scene_manager_->destroyManualObject(velocity_manual_object_); - for (size_t i = 0; i < velocity_text_nodes_.size(); i++) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->removeAndDestroyAllChildren(); - node->detachAllObjects(); - scene_manager_->destroySceneNode(node); - } - } -} - -void AutowarePathWithLaneIdDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - path_manual_object_ = scene_manager_->createManualObject(); - velocity_manual_object_ = scene_manager_->createManualObject(); - path_manual_object_->setDynamic(true); - velocity_manual_object_->setDynamic(true); - scene_node_->attachObject(path_manual_object_); - scene_node_->attachObject(velocity_manual_object_); -} - -void AutowarePathWithLaneIdDisplay::reset() -{ - MFDClass::reset(); - path_manual_object_->clear(); - velocity_manual_object_->clear(); -} - -bool AutowarePathWithLaneIdDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr) -{ - for (auto && e : msg_ptr->points) { - if ( - !rviz_common::validateFloats(e.point.pose) && - !rviz_common::validateFloats(e.point.longitudinal_velocity_mps)) { - return false; - } - } - return true; -} - -void AutowarePathWithLaneIdDisplay::processMessage( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rclcpp::get_logger("AutowarePathWithLaneIdDisplay"), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - path_manual_object_->clear(); - velocity_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); - velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); - path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - // path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); - - if (msg_ptr->points.size() > velocity_texts_.size()) { - for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { - Ogre::SceneNode * node = scene_node_->createChildSceneNode(); - rviz_rendering::MovableText * text = - new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); - text->setVisible(false); - text->setTextAlignment( - rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); - node->attachObject(text); - velocity_texts_.push_back(text); - velocity_text_nodes_.push_back(node); - } - } else if (msg_ptr->points.size() < velocity_texts_.size()) { - for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->detachAllObjects(); - node->removeAndDestroyAllChildren(); - scene_manager_->destroySceneNode(node); - } - velocity_texts_.resize(msg_ptr->points.size()); - velocity_text_nodes_.resize(msg_ptr->points.size()); - } - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & e = msg_ptr->points.at(point_idx); - /* - * Path - */ - if (property_path_view_->getBool()) { - Ogre::ColourValue color; - if (property_path_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_path_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), e.point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_path_alpha_->getFloat(); - Eigen::Vector3f vec_in, vec_out; - Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); - { - vec_in << 0, (property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - e.point.pose.orientation.w, e.point.pose.orientation.x, e.point.pose.orientation.y, - e.point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - e.point.pose.position.x + vec_out.x(), e.point.pose.position.y + vec_out.y(), - e.point.pose.position.z + vec_out.z()); - path_manual_object_->colour(color); - } - { - vec_in << 0, -(property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - e.point.pose.orientation.w, e.point.pose.orientation.x, e.point.pose.orientation.y, - e.point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - e.point.pose.position.x + vec_out.x(), e.point.pose.position.y + vec_out.y(), - e.point.pose.position.z + vec_out.z()); - path_manual_object_->colour(color); - } - } - /* - * Velocity - */ - if (property_velocity_view_->getBool()) { - Ogre::ColourValue color; - if (property_velocity_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_velocity_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), e.point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_velocity_alpha_->getFloat(); - - velocity_manual_object_->position( - e.point.pose.position.x, e.point.pose.position.y, - e.point.pose.position.z + - e.point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); - velocity_manual_object_->colour(color); - } - - /* - * Velocity Text - */ - if (property_velocity_text_view_->getBool()) { - Ogre::Vector3 position; - position.x = e.point.pose.position.x; - position.y = e.point.pose.position.y; - position.z = e.point.pose.position.z; - Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); - node->setPosition(position); - - rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); - const double vel = e.point.longitudinal_velocity_mps; - std::stringstream ss; - ss << std::fixed << std::setprecision(2) << vel; - text->setCaption(ss.str()); - text->setCharacterHeight(property_velocity_text_scale_->getFloat()); - text->setVisible(true); - } else { - rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); - text->setVisible(false); - } - } - - path_manual_object_->end(); - velocity_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowarePathWithLaneIdDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } -} - -} // namespace rviz_plugins - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdDisplay, rviz_common::Display) diff --git a/common/tier4_planning_rviz_plugin/src/trajectory/display.cpp b/common/tier4_planning_rviz_plugin/src/trajectory/display.cpp deleted file mode 100644 index 558063ca6a032..0000000000000 --- a/common/tier4_planning_rviz_plugin/src/trajectory/display.cpp +++ /dev/null @@ -1,318 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#define EIGEN_MPL2_ONLY -#include -#include - -namespace rviz_plugins -{ -std::unique_ptr AutowareTrajectoryDisplay::gradation( - const QColor & color_min, const QColor & color_max, const double ratio) -{ - std::unique_ptr color_ptr(new Ogre::ColourValue); - color_ptr->g = color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio); - color_ptr->r = color_max.redF() * ratio + color_min.redF() * (1.0 - ratio); - color_ptr->b = color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio); - - return color_ptr; -} - -std::unique_ptr AutowareTrajectoryDisplay::setColorDependsOnVelocity( - const double vel_max, const double cmd_vel) -{ - const double cmd_vel_abs = std::fabs(cmd_vel); - const double vel_min = 0.0; - - std::unique_ptr color_ptr(new Ogre::ColourValue()); - if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { - double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); - color_ptr = gradation(Qt::red, Qt::yellow, ratio); - } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { - double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); - color_ptr = gradation(Qt::yellow, Qt::green, ratio); - } else if (vel_max < cmd_vel_abs) { - *color_ptr = Ogre::ColourValue::Green; - } else { - *color_ptr = Ogre::ColourValue::Red; - } - - return color_ptr; -} - -AutowareTrajectoryDisplay::AutowareTrajectoryDisplay() -{ - property_path_view_ = new rviz_common::properties::BoolProperty( - "View Path", true, "", this, SLOT(updateVisualization())); - property_path_width_ = new rviz_common::properties::FloatProperty( - "Width", 2.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_width_->setMin(0.0); - property_path_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_alpha_->setMin(0.0); - property_path_alpha_->setMax(1.0); - property_path_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_path_view_, SLOT(updateVisualization()), this); - - property_velocity_view_ = new rviz_common::properties::BoolProperty( - "View Velocity", true, "", this, SLOT(updateVisualization()), this); - property_velocity_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_alpha_->setMin(0.0); - property_velocity_alpha_->setMax(1.0); - property_velocity_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.3, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_scale_->setMin(0.1); - property_velocity_scale_->setMax(10.0); - property_velocity_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_text_view_ = new rviz_common::properties::BoolProperty( - "View Text Velocity", false, "", this, SLOT(updateVisualization()), this); - property_velocity_text_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.3, "", property_velocity_text_view_, SLOT(updateVisualization()), this); - property_vel_max_ = new rviz_common::properties::FloatProperty( - "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); - property_vel_max_->setMin(0.0); -} - -AutowareTrajectoryDisplay::~AutowareTrajectoryDisplay() -{ - if (initialized()) { - scene_manager_->destroyManualObject(path_manual_object_); - scene_manager_->destroyManualObject(velocity_manual_object_); - for (size_t i = 0; i < velocity_text_nodes_.size(); i++) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->removeAndDestroyAllChildren(); - node->detachAllObjects(); - scene_manager_->destroySceneNode(node); - } - } -} - -void AutowareTrajectoryDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - path_manual_object_ = scene_manager_->createManualObject(); - velocity_manual_object_ = scene_manager_->createManualObject(); - path_manual_object_->setDynamic(true); - velocity_manual_object_->setDynamic(true); - scene_node_->attachObject(path_manual_object_); - scene_node_->attachObject(velocity_manual_object_); -} - -void AutowareTrajectoryDisplay::reset() -{ - MFDClass::reset(); - path_manual_object_->clear(); - velocity_manual_object_->clear(); - for (size_t i = 0; i < velocity_texts_.size(); i++) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->detachAllObjects(); - node->removeAndDestroyAllChildren(); - scene_manager_->destroySceneNode(node); - } - velocity_text_nodes_.clear(); - velocity_texts_.clear(); -} - -bool AutowareTrajectoryDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr) -{ - for (auto && trajectory_point : msg_ptr->points) { - if ( - !rviz_common::validateFloats(trajectory_point.pose) && - !rviz_common::validateFloats(trajectory_point.longitudinal_velocity_mps)) { - return false; - } - } - return true; -} - -void AutowareTrajectoryDisplay::processMessage( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rclcpp::get_logger("AutowareTrajectoryDisplay"), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - path_manual_object_->clear(); - velocity_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); - velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); - path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); - - if (msg_ptr->points.size() > velocity_texts_.size()) { - for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { - Ogre::SceneNode * node = scene_node_->createChildSceneNode(); - rviz_rendering::MovableText * text = - new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); - text->setVisible(false); - text->setTextAlignment( - rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); - node->attachObject(text); - velocity_texts_.push_back(text); - velocity_text_nodes_.push_back(node); - } - } else if (msg_ptr->points.size() < velocity_texts_.size()) { - for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->detachAllObjects(); - node->removeAndDestroyAllChildren(); - scene_manager_->destroySceneNode(node); - } - velocity_texts_.resize(msg_ptr->points.size()); - velocity_text_nodes_.resize(msg_ptr->points.size()); - } - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - /* - * Path - */ - if (property_path_view_->getBool()) { - Ogre::ColourValue color; - if (property_path_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_path_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_path_alpha_->getFloat(); - Eigen::Vector3f vec_in, vec_out; - Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); - { - vec_in << 0, (property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - path_point.pose.position.x + vec_out.x(), path_point.pose.position.y + vec_out.y(), - path_point.pose.position.z + vec_out.z()); - path_manual_object_->colour(color); - } - { - vec_in << 0, -(property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - path_point.pose.position.x + vec_out.x(), path_point.pose.position.y + vec_out.y(), - path_point.pose.position.z + vec_out.z()); - path_manual_object_->colour(color); - } - } - /* - * Velocity - */ - if (property_velocity_view_->getBool()) { - Ogre::ColourValue color; - if (property_velocity_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_velocity_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_velocity_alpha_->getFloat(); - - velocity_manual_object_->position( - path_point.pose.position.x, path_point.pose.position.y, - path_point.pose.position.z + - path_point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); - velocity_manual_object_->colour(color); - } - /* - * Velocity Text - */ - if (property_velocity_text_view_->getBool()) { - Ogre::Vector3 position; - position.x = path_point.pose.position.x; - position.y = path_point.pose.position.y; - position.z = path_point.pose.position.z; - Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); - node->setPosition(position); - - rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); - const double vel = path_point.longitudinal_velocity_mps; - std::stringstream ss; - ss << std::fixed << std::setprecision(2) << vel; - text->setCaption(ss.str()); - text->setCharacterHeight(property_velocity_text_scale_->getFloat()); - text->setVisible(true); - } else { - rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); - text->setVisible(false); - } - } - - path_manual_object_->end(); - velocity_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowareTrajectoryDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryDisplay, rviz_common::Display) From 00b97e4a9fe3bead289fe6ea8a8ba47918dcb099 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Sun, 12 Feb 2023 16:34:55 +0300 Subject: [PATCH 66/72] feat(tier4_perception_launch): update cam/lidar detection architecture (#2845) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(tier4_perception_launch): update cam/lidar detection architecture Signed-off-by: Kaan Çolak * style(pre-commit): autofix --------- Signed-off-by: Kaan Çolak Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- ...ra_lidar_fusion_based_detection.launch.xml | 22 +++---------------- 1 file changed, 3 insertions(+), 19 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 95485a2706237..cf38c44cf562e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -113,6 +113,7 @@ + @@ -129,15 +130,6 @@ - - - - - - - - - @@ -241,25 +233,17 @@ - + - - - - - - - - - + From b850b25e38019917ab87e4474d47ccf8cafc133c Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Mon, 13 Feb 2023 10:25:56 +0900 Subject: [PATCH 67/72] perf(detection_area): point and polygon inclusion check by minimum enclosing circle (#2846) * perf(detection_area): point and polygon inclusion check by minimum enclosing circle Signed-off-by: veqcc * style(pre-commit): autofix * Update planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * style(pre-commit): autofix * Update planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp * Update planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * Update planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * Update planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp * Update planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp * style(pre-commit): autofix * style(pre-commit): autofix --------- Signed-off-by: veqcc Co-authored-by: veqcc Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> --- .../src/scene_module/detection_area/scene.cpp | 81 ++++++++++++++++++- 1 file changed, 77 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index faa5bbcc06ea1..3de7279aeb27c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -203,6 +203,73 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } +// calc smallest enclosing circle with average O(N) algorithm +// reference: +// https://erickimphotography.com/blog/wp-content/uploads/2018/09/Computational-Geometry-Algorithms-and-Applications-3rd-Ed.pdf +std::pair calcSmallestEnclosingCircle( + const lanelet::ConstPolygon2d & poly) +{ + // The `eps` is used to avoid precision bugs in circle inclusion checkings. + // If the value of `eps` is too small, this function doesn't work well. More than 1e-10 is + // recommended. + const double eps = 1e-5; + lanelet::BasicPoint2d center(0.0, 0.0); + double radius_squared = 0.0; + + auto cross = [](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> double { + return p1.x() * p2.y() - p1.y() * p2.x(); + }; + + auto make_circle_3 = [&]( + const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, + const lanelet::BasicPoint2d & p3) -> void { + // reference for circumcenter vector https://en.wikipedia.org/wiki/Circumscribed_circle + const double A = (p2 - p3).squaredNorm(); + const double B = (p3 - p1).squaredNorm(); + const double C = (p1 - p2).squaredNorm(); + const double S = cross(p2 - p1, p3 - p1); + if (std::abs(S) < eps) return; + center = (A * (B + C - A) * p1 + B * (C + A - B) * p2 + C * (A + B - C) * p3) / (4 * S * S); + radius_squared = (center - p1).squaredNorm() + eps; + }; + + auto make_circle_2 = + [&](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> void { + center = (p1 + p2) * 0.5; + radius_squared = (center - p1).squaredNorm() + eps; + }; + + auto in_circle = [&](const lanelet::BasicPoint2d & p) -> bool { + return (center - p).squaredNorm() <= radius_squared; + }; + + // mini disc + for (size_t i = 1; i < poly.size(); i++) { + const auto p1 = poly[i].basicPoint2d(); + if (in_circle(p1)) continue; + + // mini disc with point + const auto p0 = poly[0].basicPoint2d(); + make_circle_2(p0, p1); + for (size_t j = 0; j < i; j++) { + const auto p2 = poly[j].basicPoint2d(); + if (in_circle(p2)) continue; + + // mini disc with two points + make_circle_2(p1, p2); + for (size_t k = 0; k < j; k++) { + const auto p3 = poly[k].basicPoint2d(); + if (in_circle(p3)) continue; + + // mini disc with tree points + make_circle_3(p1, p2, p3); + } + } + } + + return std::make_pair(center, radius_squared); +} + std::vector DetectionAreaModule::getObstaclePoints() const { std::vector obstacle_points; @@ -211,11 +278,17 @@ std::vector DetectionAreaModule::getObstaclePoints() const auto & points = *(planner_data_->no_ground_pointcloud); for (const auto & detection_area : detection_areas) { + const auto poly = lanelet::utils::to2D(detection_area); + const auto circle = calcSmallestEnclosingCircle(poly); for (const auto p : points) { - if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(detection_area).basicPolygon())) { - obstacle_points.push_back(planning_utils::toRosPoint(p)); - // get all obstacle point becomes high computation cost so skip if any point is found - break; + const double squared_dist = (circle.first.x() - p.x) * (circle.first.x() - p.x) + + (circle.first.y() - p.y) * (circle.first.y() - p.y); + if (squared_dist <= circle.second) { + if (bg::within(Point2d{p.x, p.y}, poly.basicPolygon())) { + obstacle_points.push_back(planning_utils::toRosPoint(p)); + // get all obstacle point becomes high computation cost so skip if any point is found + break; + } } } } From 58ee5e401383bd4bc1b2cc9839b45739bcd40309 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 13 Feb 2023 12:36:48 +0900 Subject: [PATCH 68/72] fix(behavior_path_planner): fix lane change path validation logic (#2873) Signed-off-by: Fumiya Watanabe --- .../src/scene_module/lane_change/util.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index fa45880f4c0ee..98d7d4d837c85 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -541,7 +541,9 @@ bool hasEnoughDistance( return true; } - if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, target_lanes)) { + if ( + lane_change_total_distance + lane_change_required_distance > + util::getDistanceToEndOfLane(current_pose, target_lanes)) { return false; } From b0b0b7af70d2b73edae5923c51a2d9e30f847b2b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 13 Feb 2023 14:57:10 +0900 Subject: [PATCH 69/72] fix(behavior_velocity_planner): continue collision checking after pass judge (#2859) * fix(behavior_velocity_planner): check if over pass judge line after collision checking Signed-off-by: Mamoru Sobue * fix(behavior_velocity_planner): revert part of #2719 Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 1 + .../intersection/scene_intersection.hpp | 1 + .../src/scene_module/intersection/manager.cpp | 1 + .../intersection/scene_intersection.cpp | 16 ++++++++++------ 4 files changed, 13 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 9ed153bb1c3d4..c46861d32a384 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -3,6 +3,7 @@ intersection: state_transit_margin_time: 1.0 stop_line_margin: 3.0 + keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 80280a0946c0a..bd3f3c10cf0fa 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -68,6 +68,7 @@ class IntersectionModule : public SceneModuleInterface { double state_transit_margin_time; double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary + double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle check diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 70813af895d82..bc9664c2a2535 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -46,6 +46,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0); ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); + ip.keep_detection_vel_thr = node.declare_parameter(ns + ".keep_detection_vel_thr", 0.833); ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0); ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) + vehicle_info.max_longitudinal_offset_m; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 83099c46a6336..1dad05f96fb22 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -168,7 +168,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); /* if ego is over the pass judge line before collision is detected, keep going */ - if (is_over_pass_judge_line && is_go_out_ && !external_stop) { + const double current_velocity = planner_data_->current_velocity->twist.linear.x; + if ( + is_over_pass_judge_line && is_go_out_ && + current_velocity > planner_param_.keep_detection_vel_thr) { RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); @@ -178,6 +181,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } } + /* collision checking */ bool is_entry_prohibited = false; @@ -229,6 +233,11 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * : std::nullopt; } + state_machine_.setStateWithMarginTime( + is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("state_machine"), *clock_); + setSafe(state_machine_.getState() == StateMachine::State::GO); + if (!stop_line_idx.has_value()) { RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed"); RCLCPP_DEBUG(logger_, "===== plan end ====="); @@ -237,11 +246,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return false; } - state_machine_.setStateWithMarginTime( - is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, - logger_.get_child("state_machine"), *clock_); - - setSafe(state_machine_.getState() == StateMachine::State::GO); setDistance(motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, path->points.at(stop_line_idx.value()).point.pose.position)); From a18de91f8739e363b2c0ef3c7bd2c88146268df0 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 13 Feb 2023 15:04:48 +0900 Subject: [PATCH 70/72] fix(behavior_velocity_planner): keep intersection module for lanes with same parent and same direction (#2874) Signed-off-by: Mamoru Sobue --- .../scene_module/intersection/manager.hpp | 15 +++- .../src/scene_module/intersection/manager.cpp | 85 +++++++++++-------- 2 files changed, 63 insertions(+), 37 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp index 735f807949492..01c21d2197202 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -25,6 +25,7 @@ #include #include +#include namespace behavior_velocity_planner { @@ -43,9 +44,12 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - bool hasSameParentLaneletWith(const lanelet::ConstLanelet & lane, const size_t module_id) const; + bool hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction_lane) const; - bool hasSameParentLaneletWithRegistered(const lanelet::ConstLanelet & lane) const; + bool hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane, const std::string & turn_direction) const; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -63,7 +67,12 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - bool hasSameParentLaneletWith(const lanelet::ConstLanelet & lane, const size_t module_id) const; + bool hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction_lane) const; + + bool hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane, const std::string & turn_direction) const; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index bc9664c2a2535..f1289afd25b06 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -98,10 +98,6 @@ void IntersectionModuleManager::launchNewModules( const auto lane_id = ll.id(); const auto module_id = lane_id; - if (hasSameParentLaneletWithRegistered(ll)) { - continue; - } - // Is intersection? const std::string turn_direction = ll.attributeOr("turn_direction", "else"); const auto is_intersection = @@ -109,6 +105,11 @@ void IntersectionModuleManager::launchNewModules( if (!is_intersection) { continue; } + + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll, turn_direction)) { + continue; + } + registerModule(std::make_shared( module_id, lane_id, planner_data_, intersection_param_, logger_.get_child("intersection_module"), clock_)); @@ -143,29 +144,31 @@ void MergeFromPrivateModuleManager::launchNewModules( // Is merging from private road? // In case the goal is in private road, check if this lanelet is conflicting with urban lanelet const std::string lane_location = ll.attributeOr("location", "else"); - if (lane_location == "private") { - if (i + 1 < lanelets.size()) { - const auto next_lane = lanelets.at(i + 1); - const std::string next_lane_location = next_lane.attributeOr("location", "else"); - if (next_lane_location != "private") { + if (lane_location != "private") { + continue; + } + + if (i + 1 < lanelets.size()) { + const auto next_lane = lanelets.at(i + 1); + const std::string next_lane_location = next_lane.attributeOr("location", "else"); + if (next_lane_location != "private") { + registerModule(std::make_shared( + module_id, lane_id, planner_data_, merge_from_private_area_param_, + logger_.get_child("merge_from_private_road_module"), clock_)); + continue; + } + } else { + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, ll); + for (auto && conflicting_lanelet : conflicting_lanelets) { + const std::string conflicting_attr = conflicting_lanelet.attributeOr("location", "else"); + if (conflicting_attr == "urban") { registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, logger_.get_child("merge_from_private_road_module"), clock_)); continue; } - } else { - const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); - const auto conflicting_lanelets = - lanelet::utils::getConflictingLanelets(routing_graph_ptr, ll); - for (auto && conflicting_lanelet : conflicting_lanelets) { - const std::string conflicting_attr = conflicting_lanelet.attributeOr("location", "else"); - if (conflicting_attr == "urban") { - registerModule(std::make_shared( - module_id, lane_id, planner_data_, merge_from_private_area_param_, - logger_.get_child("merge_from_private_road_module"), clock_)); - continue; - } - } } } } @@ -186,7 +189,8 @@ IntersectionModuleManager::getModuleExpiredFunction( if (!is_intersection) { continue; } - if (hasSameParentLaneletWith(lane, scene_module->getModuleId())) { + if (hasSameParentLaneletAndTurnDirectionWith( + lane, scene_module->getModuleId(), turn_direction)) { return false; } } @@ -209,7 +213,8 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( if (!is_intersection) { continue; } - if (hasSameParentLaneletWith(lane, scene_module->getModuleId())) { + if (hasSameParentLaneletAndTurnDirectionWith( + lane, scene_module->getModuleId(), turn_direction)) { return false; } } @@ -217,14 +222,18 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( }; } -bool IntersectionModuleManager::hasSameParentLaneletWith( - const lanelet::ConstLanelet & lane, const size_t module_id) const +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction_lane) const { - lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); - const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(module_id); + const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else"); + if (turn_direction_registered != turn_direction_lane) { + return false; + } lanelet::ConstLanelets registered_parents = planner_data_->route_handler_->getPreviousLanelets(registered_lane); + lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); for (const auto & ll : registered_parents) { auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); neighbor_lanes.push_back(ll); @@ -235,13 +244,17 @@ bool IntersectionModuleManager::hasSameParentLaneletWith( return false; } -bool IntersectionModuleManager::hasSameParentLaneletWithRegistered( - const lanelet::ConstLanelet & lane) const +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane, const std::string & turn_direction) const { lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); for (const auto & id : registered_module_id_set_) { const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id); + const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else"); + if (turn_direction_registered != turn_direction) { + continue; + } lanelet::ConstLanelets registered_parents = planner_data_->route_handler_->getPreviousLanelets(registered_lane); for (const auto & ll : registered_parents) { @@ -255,14 +268,18 @@ bool IntersectionModuleManager::hasSameParentLaneletWithRegistered( return false; } -bool MergeFromPrivateModuleManager::hasSameParentLaneletWith( - const lanelet::ConstLanelet & lane, const size_t module_id) const +bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction) const { - lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); - const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(module_id); + const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else"); + if (turn_direction_registered != turn_direction) { + return false; + } lanelet::ConstLanelets registered_parents = planner_data_->route_handler_->getPreviousLanelets(registered_lane); + lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); for (const auto & ll : registered_parents) { auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); neighbor_lanes.push_back(ll); From 501309b2c75f66b7226648c4ccf300aada805fe8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 13 Feb 2023 20:08:02 +0900 Subject: [PATCH 71/72] fix(behavior_path_planner): fix drivable area expansion method outside intersection (#2862) * fix(behavior_path_planner): deal with edge case of drivable area expansion Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../avoidance/avoidance_module.cpp | 166 ++++++++++++------ 1 file changed, 112 insertions(+), 54 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index effaca9501bd0..a364fff2ca269 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -70,6 +70,21 @@ AvoidLine getNonStraightShiftLine(const AvoidLineArray & shift_lines) return {}; } +bool isEndPointsConnected( + const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) +{ + const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint(); + const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint(); + + constexpr double epsilon = 1e-5; + return (right_back_point_2d - left_back_point_2d).norm() < epsilon; +} + +template +void pushUniqueVector(T & base_vector, const T & additional_vector) +{ + base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); +} } // namespace AvoidanceModule::AvoidanceModule( @@ -2546,34 +2561,33 @@ void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const continue; } - // get left side lane - const lanelet::ConstLanelets all_left_lanelets = - route_handler->getAllLeftSharedLinestringLanelets(current_lane, enable_opposite, true); - if (!all_left_lanelets.empty()) { - current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet - - for (int i = all_left_lanelets.size() - 2; i >= 0; --i) { - current_drivable_lanes.middle_lanes.push_back(all_left_lanelets.at(i)); + // 1. get left/right side lanes + const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_left_lanelets = + route_handler->getAllLeftSharedLinestringLanelets(target_lane, enable_opposite, true); + if (!all_left_lanelets.empty()) { + current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet + pushUniqueVector( + current_drivable_lanes.middle_lanes, + lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1)); } - } - - // get right side lane - const lanelet::ConstLanelets all_right_lanelets = - route_handler->getAllRightSharedLinestringLanelets(current_lane, enable_opposite, true); - if (!all_right_lanelets.empty()) { - current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet - if (current_drivable_lanes.left_lane.id() != current_lane.id()) { - current_drivable_lanes.middle_lanes.push_back(current_lane); + }; + const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_right_lanelets = + route_handler->getAllRightSharedLinestringLanelets(target_lane, enable_opposite, true); + if (!all_right_lanelets.empty()) { + current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet + pushUniqueVector( + current_drivable_lanes.middle_lanes, + lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1)); } + }; - for (size_t i = 0; i < all_right_lanelets.size() - 1; ++i) { - current_drivable_lanes.middle_lanes.push_back(all_right_lanelets.at(i)); - } - } + update_left_lanelets(current_lane); + update_right_lanelets(current_lane); - // 2. when there are multiple turning lanes whose previous lanelet is the same in - // intersection - const lanelet::ConstLanelets next_lanes = std::invoke( + // 2.1 when there are multiple lanes whose previous lanelet is the same + const auto get_next_lanes_from_same_previous_lane = [&route_handler](const lanelet::ConstLanelet & lane) { // get previous lane, and return false if previous lane does not exist lanelet::ConstLanelets prev_lanes; @@ -2584,42 +2598,86 @@ void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const lanelet::ConstLanelets next_lanes; for (const auto & prev_lane : prev_lanes) { const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane); - next_lanes.reserve(next_lanes.size() + next_lanes_from_prev.size()); - next_lanes.insert( - next_lanes.end(), next_lanes_from_prev.begin(), next_lanes_from_prev.end()); + pushUniqueVector(next_lanes, next_lanes_from_prev); } return next_lanes; - }, - current_lane); + }; - // 2.1 look for neighbour lane, where end line of the lane is connected to end line of the - // original lane - for (const auto & next_lane : next_lanes) { - if (current_lane.id() == next_lane.id()) { - continue; - } - constexpr double epsilon = 1e-5; - const auto & next_left_back_point_2d = next_lane.leftBound2d().back().basicPoint(); - const auto & next_right_back_point_2d = next_lane.rightBound2d().back().basicPoint(); - const auto & orig_left_back_point_2d = current_lane.leftBound2d().back().basicPoint(); - const auto & orig_right_back_point_2d = current_lane.rightBound2d().back().basicPoint(); - - if ((next_right_back_point_2d - orig_left_back_point_2d).norm() < epsilon) { - current_drivable_lanes.left_lane = next_lane; - if ( - current_drivable_lanes.right_lane.id() != current_lane.id() && - !has_same_lane(current_drivable_lanes.middle_lanes, current_lane)) { - current_drivable_lanes.middle_lanes.push_back(current_lane); + const auto next_lanes_for_right = + get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane); + const auto next_lanes_for_left = + get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane); + + // 2.2 look for neighbour lane recursively, where end line of the lane is connected to end line + // of the original lane + const auto update_drivable_lanes = + [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { + for (const auto & next_lane : next_lanes) { + const auto & edge_lane = + is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane; + if (next_lane.id() == edge_lane.id()) { + continue; + } + + const auto & left_lane = is_left ? next_lane : edge_lane; + const auto & right_lane = is_left ? edge_lane : next_lane; + if (!isEndPointsConnected(left_lane, right_lane)) { + continue; + } + + if (is_left) { + current_drivable_lanes.left_lane = next_lane; + } else { + current_drivable_lanes.right_lane = next_lane; + } + + if (!has_same_lane(current_drivable_lanes.middle_lanes, edge_lane)) { + if (is_left) { + if (current_drivable_lanes.right_lane.id() != edge_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(edge_lane); + } + } else { + if (current_drivable_lanes.left_lane.id() != edge_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(edge_lane); + } + } + } + + return true; } - } else if ( - (next_left_back_point_2d - orig_right_back_point_2d).norm() < epsilon && - !has_same_lane(current_drivable_lanes.middle_lanes, current_lane)) { - current_drivable_lanes.right_lane = next_lane; - if (current_drivable_lanes.left_lane.id() != current_lane.id()) { - current_drivable_lanes.middle_lanes.push_back(current_lane); + return false; + }; + + const auto expand_drivable_area_recursively = + [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { + // NOTE: set max search num to avoid infinity loop for drivable area expansion + constexpr size_t max_recursive_search_num = 3; + for (size_t i = 0; i < max_recursive_search_num; ++i) { + const bool is_update_kept = update_drivable_lanes(next_lanes, is_left); + if (!is_update_kept) { + break; + } + if (i == max_recursive_search_num - 1) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "Drivable area expansion reaches max iteration."); + } } - } + }; + expand_drivable_area_recursively(next_lanes_for_right, false); + expand_drivable_area_recursively(next_lanes_for_left, true); + + // 3. update again for new left/right lanes + update_left_lanelets(current_drivable_lanes.left_lane); + update_right_lanelets(current_drivable_lanes.right_lane); + + // 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes. + if ( + current_drivable_lanes.left_lane.id() != current_lane.id() && + current_drivable_lanes.right_lane.id() != current_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(current_lane); } + drivable_lanes.push_back(current_drivable_lanes); } From f63d7d4a3539433cc620d08533a30e7edd14dce1 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 13 Feb 2023 12:23:42 +0100 Subject: [PATCH 72/72] build(lidar_centerpoint): use APT version of python3-open3d as dependency (#2869) Signed-off-by: Esteve Fernandez --- perception/lidar_centerpoint/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 602b81ae5b946..970f69921c6e2 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -16,7 +16,7 @@ autoware_auto_perception_msgs pcl_ros perception_utils - python3-open3d-pip + python3-open3d rclcpp rclcpp_components tf2_eigen