From 848f2b060725d60b390eb60e9a6b25a8003961c0 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Date: Mon, 24 Mar 2025 17:56:35 +0100 Subject: [PATCH 01/70] Pre-Commit (#4915) * Add pre-commit Signed-off-by: Nils-ChristianIseke * Add codespell workflow Signed-off-by: Nils-ChristianIseke * Codespell write_changes=false. As otherwise CI does not fail. Signed-off-by: Nils-ChristianIseke * Configure isort Signed-off-by: Nils-ChristianIseke * add precommit Signed-off-by: Nils-ChristianIseke * Introducing some issues. Signed-off-by: Nils-ChristianIseke * Revert "Introducing some issues." This reverts commit 5377b656361ef220dce6d6af36013060798f06de. Signed-off-by: Nils-ChristianIseke * Removing pre-commit workflow. Signed-off-by: Nils-ChristianIseke * Fix formatting error Signed-off-by: Nils-ChristianIseke * Merge remote-tracking branch 'origin/main' into precommit Signed-off-by: Nils-ChristianIseke * Change v31 to v32 Signed-off-by: Nils-ChristianIseke * Revert "Merge remote-tracking branch 'origin/main' into precommit" This reverts commit 8a7ca3983a0b93ef5a94e9517e9598de96be2fe6. Signed-off-by: Nils-ChristianIseke * Rm submodule Signed-off-by: Nils-ChristianIseke * pre-commit run --all after merge Signed-off-by: Nils-ChristianIseke --------- Signed-off-by: Nils-ChristianIseke --- .../srv/assets/foxglove/nav2_layout.json | 2 +- .../caddy/srv/nav2/github-markdown.css | 2 +- .github/mergify.yml | 4 +- .github/workflows/codespell.yml | 14 +++ .gitignore | 1 - .pre-commit-config.yaml | 97 +++++++++++++++++++ Dockerfile | 2 +- README.md | 78 +++++++-------- doc/development/codespaces.md | 2 +- doc/requirements/_template_requirement.md | 2 +- doc/requirements/requirements.md | 16 +-- doc/use_cases/README.md | 1 - doc/use_cases/_template_use_case.md | 4 +- doc/use_cases/collision_avoidance_use_case.md | 2 - doc/use_cases/indoor_localization_use_case.md | 2 - doc/use_cases/indoor_navigation_use_case.md | 4 +- doc/use_cases/keep_out_zones_use_case.md | 4 +- .../multi-story-building_use_case.md | 6 +- .../outdoor_localization_use_case.md | 2 - doc/use_cases/outdoor_navigation_use_case.md | 4 +- nav2_behavior_tree/README.md | 2 +- nav2_behavior_tree/plugins_list.hpp.in | 2 +- .../plugins/action/test_spin_cancel_node.cpp | 4 +- nav2_behaviors/README.md | 2 +- nav2_bringup/launch/bringup_launch.py | 12 +-- .../cloned_multi_tb3_simulation_launch.py | 16 +-- nav2_bringup/launch/localization_launch.py | 12 +-- nav2_bringup/launch/navigation_launch.py | 4 +- nav2_bringup/launch/rviz_launch.py | 1 - nav2_bringup/launch/slam_launch.py | 1 - .../launch/tb3_loopback_simulation.launch.py | 8 +- nav2_bringup/launch/tb3_simulation_launch.py | 11 +-- .../launch/tb4_loopback_simulation.launch.py | 8 +- nav2_bringup/launch/tb4_simulation_launch.py | 12 +-- .../unique_multi_tb3_simulation_launch.py | 14 +-- nav2_collision_monitor/README.md | 2 +- .../launch/collision_detector_node.launch.py | 8 +- .../launch/collision_monitor_node.launch.py | 8 +- .../params/collision_monitor_params.yaml | 2 +- nav2_common/cmake/nav2_package.cmake | 4 +- .../nav2_common/launch/has_node_params.py | 8 +- .../nav2_common/launch/replace_string.py | 6 +- .../nav2_common/launch/rewritten_yaml.py | 5 +- nav2_controller/README.md | 2 +- nav2_costmap_2d/README.md | 4 +- nav2_costmap_2d/costmap_plugins.xml | 1 - .../test/integration/CMakeLists.txt | 2 +- .../test/integration/costmap_tests_launch.py | 6 +- .../test/integration/inflation_tests.cpp | 2 +- .../test/integration/obstacle_tests.cpp | 2 +- nav2_costmap_2d/test/module_tests.cpp | 4 +- .../test/regression/costmap_bresenham_2d.cpp | 2 +- nav2_docking/README.md | 15 ++- .../test/test_docking_server.py | 2 +- .../dwb_core/src/dwb_local_planner.cpp | 2 +- nav2_graceful_controller/README.md | 40 ++++---- nav2_lifecycle_manager/README.md | 2 +- .../test/launch_bond_test.py | 3 +- .../test/launch_lifecycle_test.py | 3 +- .../launch/loopback_simulation.launch.py | 1 - .../nav2_loopback_sim/loopback_simulator.py | 13 +-- nav2_loopback_sim/nav2_loopback_sim/utils.py | 1 - nav2_loopback_sim/setup.py | 1 - nav2_map_server/README.md | 3 +- .../cmake_modules/FindGRAPHICSMAGICKCPP.cmake | 2 +- .../test/component/test_map_saver_launch.py | 6 +- .../test/component/test_map_server_launch.py | 6 +- nav2_mppi_controller/README.md | 18 ++-- nav2_mppi_controller/test/critics_tests.cpp | 2 +- .../test/optimizer_unit_tests.cpp | 2 +- nav2_msgs/srv/GetCosts.srv | 2 +- nav2_msgs/srv/IsPathValid.srv | 2 +- nav2_msgs/srv/SetInitialPose.srv | 1 - nav2_navfn_planner/README.md | 2 +- nav2_planner/README.md | 2 +- .../README.md | 64 ++++++------ ...nav2_regulated_pure_pursuit_controller.xml | 1 - nav2_rotation_shim_controller/README.md | 22 ++--- nav2_simple_commander/README.md | 2 +- .../launch/assisted_teleop_example_launch.py | 11 +-- .../launch/follow_path_example_launch.py | 11 +-- .../launch/inspection_demo_launch.py | 11 +-- .../nav_through_poses_example_launch.py | 11 +-- .../launch/nav_to_pose_example_launch.py | 11 +-- .../launch/picking_demo_launch.py | 11 +-- .../launch/recoveries_example_launch.py | 11 +-- .../launch/security_demo_launch.py | 11 +-- .../waypoint_follower_example_launch.py | 11 +-- .../nav2_simple_commander/demo_picking.py | 14 ++- .../nav2_simple_commander/demo_recoveries.py | 3 +- .../nav2_simple_commander/demo_security.py | 2 - .../example_follow_path.py | 1 - .../nav2_simple_commander/robot_navigator.py | 27 ++---- nav2_simple_commander/pytest.ini | 2 +- nav2_simple_commander/setup.py | 1 - nav2_smac_planner/README.md | 2 +- .../nav2_smac_planner/thirdparty/robin_hood.h | 2 +- .../lattice_primitives/README.md | 8 +- .../lattice_primitives/config.json | 2 +- .../generate_motion_primitives.py | 2 - .../lattice_primitives/lattice_generator.py | 4 - .../lattice_primitives/requirements.txt | 2 +- .../0.5m_turning_radius/ackermann/output.json | 2 +- .../0.5m_turning_radius/diff/output.json | 2 +- .../0.5m_turning_radius/omni/output.json | 2 +- .../1m_turning_radius/ackermann/output.json | 2 +- .../1m_turning_radius/diff/output.json | 2 +- .../1m_turning_radius/omni/output.json | 2 +- .../tests/trajectory_visualizer.py | 1 - .../lattice_primitives/trajectory.py | 1 - .../trajectory_generator.py | 1 - nav2_smac_planner/src/smoother.cpp | 2 +- nav2_smac_planner/test/test_smac_lattice.cpp | 2 +- nav2_smoother/README.md | 2 +- nav2_smoother/src/savitzky_golay_smoother.cpp | 2 +- nav2_smoother/src/simple_smoother.cpp | 2 +- nav2_system_tests/maps/map_circular.yaml | 1 - nav2_system_tests/models/cardboard_box.sdf | 2 +- nav2_system_tests/scripts/ctest_loop.bash | 1 - .../test_assisted_teleop_behavior_launch.py | 13 +-- .../src/behaviors/backup/backup_tester.py | 5 +- .../backup/test_backup_behavior.launch.py | 12 +-- .../drive_on_heading/drive_tester.py | 5 +- .../test_drive_on_heading_behavior.launch.py | 12 +-- .../src/behaviors/spin/spin_tester.py | 5 +- .../spin/test_spin_behavior.launch.py | 12 +-- .../wait/test_wait_behavior_launch.py | 13 +-- .../costmap_filters/test_keepout_launch.py | 13 +-- .../src/costmap_filters/test_speed_launch.py | 13 +-- .../src/costmap_filters/tester_node.py | 11 +-- .../src/error_codes/test_error_codes.py | 7 +- .../error_codes/test_error_codes_launch.py | 6 +- .../src/gps_navigation/CMakeLists.txt | 1 - .../dual_ekf_navsat_params.yaml | 2 +- .../gps_navigation/nav2_no_map_params.yaml | 4 +- .../src/gps_navigation/test_case_py.launch.py | 13 +-- .../src/gps_navigation/tester.py | 2 - nav2_system_tests/src/localization/README.md | 2 +- .../localization/test_localization_launch.py | 6 +- nav2_system_tests/src/planning/README.md | 2 +- .../planning/test_planner_costmaps_launch.py | 4 +- .../planning/test_planner_random_launch.py | 4 +- .../src/system/nav2_system_params.yaml | 1 - ...nav_through_poses_tester_error_msg_node.py | 11 +-- .../system/nav_through_poses_tester_node.py | 10 +- .../src/system/nav_to_pose_tester_node.py | 10 +- .../src/system/test_multi_robot_launch.py | 10 +- .../src/system/test_system_launch.py | 13 +-- .../test_system_with_obstacle_launch.py | 13 +-- .../src/system/test_wrong_init_pose_launch.py | 12 +-- .../test_system_failure_launch.py | 13 +-- .../src/system_failure/tester_node.py | 9 +- nav2_system_tests/src/updown/README.md | 2 +- .../src/updown/test_updown_launch.py | 4 +- .../src/updown/test_updown_reliability | 2 +- .../src/waypoint_follower/test_case_launch.py | 13 +-- .../src/waypoint_follower/tester.py | 4 +- nav2_theta_star_planner/README.md | 4 +- nav2_util/README.md | 6 +- nav2_util/test/test_actions.cpp | 2 +- nav2_velocity_smoother/README.md | 4 +- nav2_voxel_grid/README.md | 6 +- .../test/voxel_grid_bresenham_3d.cpp | 2 +- nav2_waypoint_follower/README.md | 6 +- .../plugins/input_at_waypoint.hpp | 2 +- .../plugins/wait_at_waypoint.hpp | 2 +- nav2_waypoint_follower/plugins.xml | 6 +- tools/.codespell_ignore_words | 15 +++ tools/pyproject.toml | 14 +++ tools/skip_keys.txt | 2 +- tools/smoother_benchmarking/README.md | 12 +-- tools/smoother_benchmarking/metrics.py | 1 - tools/underlay.repos | 5 +- tools/update_readme_table.py | 17 +++- 174 files changed, 521 insertions(+), 725 deletions(-) create mode 100644 .github/workflows/codespell.yml create mode 100644 .pre-commit-config.yaml create mode 100644 tools/.codespell_ignore_words create mode 100644 tools/pyproject.toml diff --git a/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json b/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json index f92821fac49..be9d963a25d 100644 --- a/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json +++ b/.devcontainer/caddy/srv/assets/foxglove/nav2_layout.json @@ -460,4 +460,4 @@ "direction": "row", "splitPercentage": 74.87855655794587 } - } \ No newline at end of file + } diff --git a/.devcontainer/caddy/srv/nav2/github-markdown.css b/.devcontainer/caddy/srv/nav2/github-markdown.css index 049cae6b291..7cb35e4a33e 100644 --- a/.devcontainer/caddy/srv/nav2/github-markdown.css +++ b/.devcontainer/caddy/srv/nav2/github-markdown.css @@ -1099,4 +1099,4 @@ .markdown-body ::-webkit-calendar-picker-indicator { filter: invert(50%); - } \ No newline at end of file + } diff --git a/.github/mergify.yml b/.github/mergify.yml index 332495c67f9..9abbb04672f 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -7,7 +7,7 @@ pull_request_rules: backport: branches: - jazzy - + - name: backport to iron at reviewers discretion conditions: - base=main @@ -16,7 +16,7 @@ pull_request_rules: backport: branches: - iron - + - name: backport to humble at reviewers discretion conditions: - base=main diff --git a/.github/workflows/codespell.yml b/.github/workflows/codespell.yml new file mode 100644 index 00000000000..61bb416e3b0 --- /dev/null +++ b/.github/workflows/codespell.yml @@ -0,0 +1,14 @@ +name: Codespell +on: + pull_request: + +jobs: + codespell: + name: Run codespell + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: Install codespell + run: pip install codespell + - name: Run codespell + run: codespell --toml ./tools/pyproject.toml diff --git a/.gitignore b/.gitignore index 1247ce1b159..203501ea402 100644 --- a/.gitignore +++ b/.gitignore @@ -69,4 +69,3 @@ Session.vim # Vim Temporary .netrwhist - diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000000..95cb7e12bac --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,97 @@ + +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit +exclude: ".pgm$|.svg$" +repos: +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v5.0.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + args: ["--allow-multiple-documents"] + - id: debug-statements + - id: end-of-file-fixer + - id: forbid-submodules + - id: mixed-line-ending + - id: trailing-whitespace + exclude_types: [rst] + - id: fix-byte-order-marker +- repo: https://github.com/pycqa/isort + rev: 6.0.1 + hooks: + - id: isort + args: ["tools/pyproject.toml"] + name: isort (python) + +- repo: https://github.com/codespell-project/codespell + rev: v2.4.1 + hooks: + - id: codespell + additional_dependencies: + - tomli + args: + [--toml=./tools/pyproject.toml] +- repo: https://github.com/python-jsonschema/check-jsonschema + rev: 0.31.1 + hooks: + - id: check-github-workflows + args: ["--verbose"] + - id: check-github-actions + args: ["--verbose"] + - id: check-dependabot + args: ["--verbose"] +- repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check CMake code style using cmakelint. + language: system + types: [cmake] + entry: ament_lint_cmake + - id: ament_cpplint + name: ament_cpplint + description: Code style checking using cpplint. + language: system + types: [c++] + entry: ament_cpplint + - id: ament_uncrustify + name: ament_uncrustify + description: Code style checking using uncrustify. + language: system + types: [c++] + args: ["--reformat"] + entry: ament_uncrustify + - id: ament_xmllint + name: ament_xmllint + description: Check XML markup using xmllint. + language: system + types: [xml] + entry: ament_xmllint + - id: ament_flake8 + name: ament_flake8 + description: Check Python code style using flake8. + language: system + types: [python] + entry: ament_flake8 + - id: ament_pep257 + name: ament_pep257 + description: Check Python code style using pep257. + language: system + types: [python] + entry: ament_pep257 diff --git a/Dockerfile b/Dockerfile index 1f959ff1f7f..fb3555eb8fb 100644 --- a/Dockerfile +++ b/Dockerfile @@ -168,7 +168,7 @@ RUN mkdir -p $ROOT_SRV # install demo dependencies RUN apt-get update && apt-get install -y \ - ros-$ROS_DISTRO-rviz2 + ros-$ROS_DISTRO-rviz2 # install gzweb dependacies RUN apt-get install -y --no-install-recommends \ diff --git a/README.md b/README.md index dfadfc6a0cc..424348f2db1 100644 --- a/README.md +++ b/README.md @@ -26,7 +26,7 @@ Please visit our [documentation site](https://docs.nav2.org/). [Please visit our ## Our Sponsors -Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. +Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.

@@ -46,7 +46,7 @@ If you use the navigation framework, an algorithm from this repository, or ideas please cite this work in your papers! - S. Macenski, F. Martín, R. White, J. Clavero. [**The Marathon 2: A Navigation System**](https://arxiv.org/abs/2003.00368). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020. - + ```bibtex @InProceedings{macenski2020marathon2, title = {The Marathon 2: A Navigation System}, @@ -64,7 +64,7 @@ If you use **any** of the algorithms in Nav2 or the analysis of the algorithms i ```bibtex @article{macenski2023survey, - title={From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2}, + title={From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2}, author={S. Macenski, T. Moore, DV Lu, A. Merzlyakov, M. Ferguson}, year={2023}, journal = {Robotics and Autonomous Systems} @@ -77,7 +77,7 @@ If you use the Smac Planner (Hybrid A*, State Lattice, 2D), please cite this wor ```bibtex @article{macenski2024smac, - title={Open-Source, Cost-Aware Kinematically Feasible Planning for Mobile and Surface Robotics}, + title={Open-Source, Cost-Aware Kinematically Feasible Planning for Mobile and Surface Robotics}, author={Steve Macenski and Matthew Booker and Josh Wallace}, year={2024}, journal = {Arxiv} @@ -90,7 +90,7 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this ```bibtex @article{macenski2023regulated, - title={Regulated Pure Pursuit for Robot Path Tracking}, + title={Regulated Pure Pursuit for Robot Path Tracking}, author={Steve Macenski and Shrijit Singh and Francisco Martin and Jonatan Gines}, year={2023}, journal = {Autonomous Robots} @@ -113,38 +113,38 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this ## Build Status -| Package | humble Source | humble Debian | iron Source | iron Debian | jazzy Source | jazzy Debian | +| Package | humble Source | humble Debian | iron Source | iron Debian | jazzy Source | jazzy Debian | | :---: | :---: | :---: | :---: | :---: | :---: | :---: | -| navigation2 | [![Build Status](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/) | -| nav2_amcl | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/) | -| nav2_behavior_tree | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/) | -| nav2_behaviors | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/) | -| nav2_bringup | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/) | -| nav2_bt_navigator | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/) | -| nav2_collision_monitor | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/) | -| nav2_common | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/) | -| nav2_constrained_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/) | -| nav2_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/) | -| nav2_core | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/) | -| nav2_costmap_2d | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/) | -| nav2_docking | [![Build Status](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/) | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/) | -| nav2_dwb_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/) | -| nav2_graceful_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/) | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/) | -| nav2_lifecycle_manager | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/) | -| nav2_map_server | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/) | -| nav2_mppi_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/) | -| nav2_msgs | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/) | -| nav2_navfn_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/) | -| nav2_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/) | -| nav2_regulated_pure_pursuit | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/) | -| nav2_rotation_shim_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/) | -| nav2_rviz_plugins | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/) | -| nav2_simple_commander | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/) | -| nav2_smac_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/) | -| nav2_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/) | -| nav2_system_tests | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/) | -| nav2_theta_star_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/) | -| nav2_util | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/) | -| nav2_velocity_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/) | -| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/) | -| nav2_waypoint_follower | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/) | +| navigation2 | [![Build Status](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__navigation2__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__navigation2__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__navigation2__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__navigation2__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__navigation2__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__navigation2__ubuntu_noble_amd64__binary/) | +| nav2_amcl | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_amcl__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_amcl__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_amcl__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_amcl__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/) | +| nav2_behavior_tree | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_behavior_tree__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_behavior_tree__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_behavior_tree__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behavior_tree__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/) | +| nav2_behaviors | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_behaviors__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_behaviors__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_behaviors__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_behaviors__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/) | +| nav2_bringup | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_bringup__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_bringup__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_bringup__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bringup__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/) | +| nav2_bt_navigator | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_bt_navigator__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_bt_navigator__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_bt_navigator__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_bt_navigator__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/) | +| nav2_collision_monitor | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_collision_monitor__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_collision_monitor__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_collision_monitor__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_collision_monitor__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/) | +| nav2_common | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_common__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_common__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_common__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_common__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_common__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_common__ubuntu_noble_amd64__binary/) | +| nav2_constrained_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_constrained_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_constrained_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_constrained_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/) | +| nav2_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_controller__ubuntu_noble_amd64__binary/) | +| nav2_core | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_core__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_core__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_core__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_core__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_core__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_core__ubuntu_noble_amd64__binary/) | +| nav2_costmap_2d | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_costmap_2d__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_costmap_2d__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_costmap_2d__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_costmap_2d__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/) | +| nav2_docking | [![Build Status](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__opennav_docking__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__opennav_docking__ubuntu_jammy_amd64__binary/) | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__opennav_docking__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__opennav_docking__ubuntu_noble_amd64__binary/) | +| nav2_dwb_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_dwb_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_dwb_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_dwb_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_dwb_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/) | +| nav2_graceful_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_graceful_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_graceful_controller__ubuntu_jammy_amd64__binary/) | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_graceful_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/) | +| nav2_lifecycle_manager | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_lifecycle_manager__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_lifecycle_manager__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_lifecycle_manager__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/) | +| nav2_map_server | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_map_server__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_map_server__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_map_server__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_map_server__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/) | +| nav2_mppi_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_mppi_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_mppi_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_mppi_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_mppi_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/) | +| nav2_msgs | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_msgs__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_msgs__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_msgs__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_msgs__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/) | +| nav2_navfn_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_navfn_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_navfn_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_navfn_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_navfn_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/) | +| nav2_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_planner__ubuntu_noble_amd64__binary/) | +| nav2_regulated_pure_pursuit | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_regulated_pure_pursuit_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_regulated_pure_pursuit_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/) | +| nav2_rotation_shim_controller | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_rotation_shim_controller__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_rotation_shim_controller__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/) | +| nav2_rviz_plugins | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_rviz_plugins__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_rviz_plugins__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_rviz_plugins__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/) | +| nav2_simple_commander | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_simple_commander__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_simple_commander__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_simple_commander__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_simple_commander__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/) | +| nav2_smac_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_smac_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_smac_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_smac_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smac_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/) | +| nav2_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/) | +| nav2_system_tests | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_system_tests__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_system_tests__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_system_tests__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_system_tests__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/) | +| nav2_theta_star_planner | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_theta_star_planner__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_theta_star_planner__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_theta_star_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/) | +| nav2_util | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_util__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_util__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_util__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_util__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_util__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_util__ubuntu_noble_amd64__binary/) | +| nav2_velocity_smoother | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_velocity_smoother__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_velocity_smoother__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_velocity_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/) | +| nav2_voxel_grid | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_voxel_grid__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_voxel_grid__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_voxel_grid__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_voxel_grid__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/) | +| nav2_waypoint_follower | [![Build Status](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Hsrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Isrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/badge/icon)](https://build.ros2.org/job/Isrc_uj__nav2_waypoint_follower__ubuntu_jammy__source/) | [![Build Status](https://build.ros2.org/job/Ibin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Ibin_uj64__nav2_waypoint_follower__ubuntu_jammy_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_waypoint_follower__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/) | diff --git a/doc/development/codespaces.md b/doc/development/codespaces.md index 841db7532db..7d7616ace7b 100644 --- a/doc/development/codespaces.md +++ b/doc/development/codespaces.md @@ -20,4 +20,4 @@ TODO: gazebo example with gzweb # References -TODO: links to more info \ No newline at end of file +TODO: links to more info diff --git a/doc/requirements/_template_requirement.md b/doc/requirements/_template_requirement.md index 6b751e13921..aba4eeaf8e9 100644 --- a/doc/requirements/_template_requirement.md +++ b/doc/requirements/_template_requirement.md @@ -3,7 +3,7 @@ The \ should be able to \ \ ## More details - Why is this needed? -- What is the expected user interaction? +- What is the expected user interaction? - What use case does this map to? - Are there any non-functional requirements (build system, tools, performance, etc) diff --git a/doc/requirements/requirements.md b/doc/requirements/requirements.md index 1ca7171c6a0..41e13483973 100644 --- a/doc/requirements/requirements.md +++ b/doc/requirements/requirements.md @@ -156,7 +156,7 @@ Id | Handle | Priority | Description | Notes TP001 | Target Platforms.Operating Systems.Ubuntu | 1 | The Navigation System MUST support Ubuntu Desktop 16.04 and Ubuntu Desktop 18.04 TP002 | Target Platforms.Operating Systems.MacOS | 1 | The Navigation System MUST support MacOS 10.13 (High Sierra) and MacOS 10.14 (Mohave) TP003 | Target Platforms.Operating Systems.Windows | 1 | The Navigation System MUST support Windows 10 Professional -TP004 | Target Platforms.Operating Systems.Clear Linux | 1 | The Navigation System SHOULD support the Intel's Clear Linux distribution | Clear Linux uses a continuous deployment model. +TP004 | Target Platforms.Operating Systems.Clear Linux | 1 | The Navigation System SHOULD support the Intel's Clear Linux distribution | Clear Linux uses a continuous deployment model. TP005 | Target Platforms.CPU.Word Size | 1 | The Navigation System SHALL support 64-bit processors | Don't assume a specific pointer size TP006 | Target Platforms.Minimum Platform | 1 | *TODO: Should we specify a minimum target platform? Or, should this be expressed as minimum platform requirements?* @@ -178,7 +178,7 @@ MP005 | Mission Planning.Navigation Commands.Enqueue | 2 | The Mission Plan SHOU MP006 | Mission Planning.Navigation Commands.Follow | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to be able to follow another specified robot. | This one doesn't have a completion state (reaching the goal), unless it specifies additional information such as "follow until destination reached." MP007 | Mission Planning.Navigation Commands.Maintain Pose | 1 | The Mission Plan SHOULD be able to convey the information required for a robot to maintain its current pose. | Could be indefinite or time-based. MP008 | Mission Planning.Navigation Commands.Park | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to park itself. | The implementation of the parking command could interact with the robot to cause it, for example, to shut down or enter a low-power state. -MP009 | Mission Planning.Navigation Commands.Dock to Charger | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to dock to a specific charging station. +MP009 | Mission Planning.Navigation Commands.Dock to Charger | 2 | The Mission Plan SHOULD be able to convey the information required for a robot to dock to a specific charging station. MP010 | Mission Planning.Policy | 1 | The Mission Plan SHOULD be able to express information about how and when the navigation commands are to be carried out. | Time and safety constraints. MP011 | Mission Planning.Policy.Time.Initiation | 1 | The Mission Plan SHOULD be able to convey when a mission should begin. MP012 | Mission Planning.Policy.Time.Completion | 1 | The Mission Plan SHOULD be able to convey by when a mission should end. @@ -216,7 +216,7 @@ PLN002 | Planning.Inputs.Navigation Command | 1 | The Planning Module SHALL rece PLN003 | Planning.Inputs.Policy | 1 | The Planning Module SHALL receive policy information associated with the Navigation Command to execute. | This could be global policy and/or per-command policy. Policy could contain, for example, a list of conventions for the robot to follow (navigate on the right side of a path, for example). PLN004 | Planning.Inputs.Mapping.Maps | 1 | The Planning Module MUST have access to one or more maps available that describe the robot's environment. PLN005 | Planning.Inputs.Perception.Sensory Input | 1 | The Planning Module MUST have access to data from the Perception Subsystem. -PLN006 | Planning.Inputs.Prediction.Predicted Trajectories | 1 | The Planning Module MAY have access to predicted trajectories of objects detected by the Perception Subsystem. | In simple planners, there is no prediction of moving objects, but in more complex planners, this may be considered. +PLN006 | Planning.Inputs.Prediction.Predicted Trajectories | 1 | The Planning Module MAY have access to predicted trajectories of objects detected by the Perception Subsystem. | In simple planners, there is no prediction of moving objects, but in more complex planners, this may be considered. PLN007 | Planning.Inputs.Localization.Current Pose | 1 | The Planning Module MUST have access to the robot's current pose. | The pose could be be provided manually or automatically determined (outside of this module). PLN008 | Planning.Outputs.Path | 1 | The Planning Module SHOULD output the Path for the robot to follow to execute the input Navigation Command and MUST respect any associated policy. PLN009 | Planning.Feedback.Inputs | 1 | The Planning Module MAY receive error input from the downstream Execution Module. | So that it can attempt to recover from execution failures. @@ -303,7 +303,7 @@ PER002 | Perception.Latency | 1 | *TODO* ### 2.4.3 Prediction -The Prediction Subsystem uses input from the Perception Subsystem and predicts the trajectories of the detected objects over time. +The Prediction Subsystem uses input from the Perception Subsystem and predicts the trajectories of the detected objects over time. Id | Handle | Priority | Description | Notes -- | ------ | -------- | ----------- | ----- @@ -325,8 +325,8 @@ LOC002 | Localization.Robot Pose.Accuracy | 1 | The Localization Module MUST pro * What are the scalability for the ROS2 Navigation System? * Any other important design goals to call out? * Should we specify a minimum target platform? Or, should this be expressed as minimum platform requirements? -* What is the right latency value for detecting a collision? -* Should we add any safety-related functionality at the robot interface level? -* Do safety zones need unique names? +* What is the right latency value for detecting a collision? +* Should we add any safety-related functionality at the robot interface level? +* Do safety zones need unique names? * What is the target latency for the perception subsystem? -* How far into the future should the object prediction work? +* How far into the future should the object prediction work? diff --git a/doc/use_cases/README.md b/doc/use_cases/README.md index 47d6ee56885..4b7ec8d64db 100644 --- a/doc/use_cases/README.md +++ b/doc/use_cases/README.md @@ -13,4 +13,3 @@ The Nav2 system is targeting the following use cases: ## Stretch Target 3D Navigation - Drones - diff --git a/doc/use_cases/_template_use_case.md b/doc/use_cases/_template_use_case.md index a7c3fcb3dd3..4c583a94b74 100644 --- a/doc/use_cases/_template_use_case.md +++ b/doc/use_cases/_template_use_case.md @@ -3,7 +3,7 @@ As a \ I want the robot to \ so ## More details - Why is this needed? -- What is the expected user interaction? +- What is the expected user interaction? - Are there any non-functional requirements? (build system, tools, performance, etc) # Example: @@ -15,7 +15,7 @@ As a robot user, I want the robot to navigate without colliding into people or o - Why is this needed? - I want this so that I know the robot won't damage itself, damage property or hurt anyone - Example: a logistics robot in a warehouse must avoid shelves, people, forklifts, and other robots - - What is the expected user interaction? + - What is the expected user interaction? - I shouldn't have to interact with the robot to prevent it from crashing into people or things - Are there any non-functional requirements? (build system, tools, performance, etc) - The performance needs to be fast enough to avoid moving objects such as people walking or other moving robots diff --git a/doc/use_cases/collision_avoidance_use_case.md b/doc/use_cases/collision_avoidance_use_case.md index 3af0998c798..193067e01c1 100644 --- a/doc/use_cases/collision_avoidance_use_case.md +++ b/doc/use_cases/collision_avoidance_use_case.md @@ -10,5 +10,3 @@ As a Robot user I want to my robot to avoid colliding with people or objects so - The user should be able to walk in front of a robot and it should avoid crashing into that person - Are there any non-functional requirements? (build system, tools, performance, etc) - - diff --git a/doc/use_cases/indoor_localization_use_case.md b/doc/use_cases/indoor_localization_use_case.md index cee2149d89f..a0c9ab8dcdc 100644 --- a/doc/use_cases/indoor_localization_use_case.md +++ b/doc/use_cases/indoor_localization_use_case.md @@ -11,5 +11,3 @@ As a Robot user I want my robot to know its location on a given map of an indoor - The robot should be able to deduce it's own position on a map autonomously - Are there any non-functional requirements? (build system, tools, performance, etc) - - diff --git a/doc/use_cases/indoor_navigation_use_case.md b/doc/use_cases/indoor_navigation_use_case.md index 81a0824485f..5e6f954fb23 100644 --- a/doc/use_cases/indoor_navigation_use_case.md +++ b/doc/use_cases/indoor_navigation_use_case.md @@ -7,8 +7,6 @@ As a Robot user I want my robot to autonomously navigate to a given location on - Example: a courier robot in a logistics warehouse - What is the expected user interaction? - - The user should be able to specify a map to use and a location on that map for the robot to move to. + - The user should be able to specify a map to use and a location on that map for the robot to move to. - Are there any non-functional requirements? (build system, tools, performance, etc) - - diff --git a/doc/use_cases/keep_out_zones_use_case.md b/doc/use_cases/keep_out_zones_use_case.md index 448d38267f9..967916e2ce2 100644 --- a/doc/use_cases/keep_out_zones_use_case.md +++ b/doc/use_cases/keep_out_zones_use_case.md @@ -1,5 +1,5 @@ # Keep Out Zones -As a Robot user I want to be able to designate keep-out zones or areas on a map so that my robot will go around those areas instead of through them +As a Robot user I want to be able to designate keep-out zones or areas on a map so that my robot will go around those areas instead of through them ## More details - Why is this needed? @@ -10,5 +10,3 @@ As a Robot user I want to be able to designate keep-out zones or areas on a map - The user should be able to specify keep out zones for the robot to avoid - Are there any non-functional requirements? (build system, tools, performance, etc) - - diff --git a/doc/use_cases/multi-story-building_use_case.md b/doc/use_cases/multi-story-building_use_case.md index e86b3a77c68..6b6a3d6b2fc 100644 --- a/doc/use_cases/multi-story-building_use_case.md +++ b/doc/use_cases/multi-story-building_use_case.md @@ -3,13 +3,11 @@ As a Robot user I want my robot to be able to navigate stairways, ramps or eleva ## More details - Why is this needed? - - Example: a delivery robot in an office building + - Example: a delivery robot in an office building - What is the expected user interaction? - The user should be able to specify stairways, ramps and elevators on a map for a robot to use or not use - - via a GUI + - via a GUI - via a config file or API so that it can be done by another program - Are there any non-functional requirements? (build system, tools, performance, etc) - - diff --git a/doc/use_cases/outdoor_localization_use_case.md b/doc/use_cases/outdoor_localization_use_case.md index f9f9e926fc0..af8e83d1528 100644 --- a/doc/use_cases/outdoor_localization_use_case.md +++ b/doc/use_cases/outdoor_localization_use_case.md @@ -11,5 +11,3 @@ As a Robot user I want my robot to know its location on a given map of an outdoo - The robot should be able to deduce it's own position on a map autonomously - Are there any non-functional requirements? (build system, tools, performance, etc) - - diff --git a/doc/use_cases/outdoor_navigation_use_case.md b/doc/use_cases/outdoor_navigation_use_case.md index 8eb8da7cba5..050a8804337 100644 --- a/doc/use_cases/outdoor_navigation_use_case.md +++ b/doc/use_cases/outdoor_navigation_use_case.md @@ -7,8 +7,6 @@ As a Robot user I want my robot to autonomously navigate to a given location on - Example: a delivery robot on a college campus - What is the expected user interaction? - - The user should be able to specify a map to use and a location on that map for the robot to move to. + - The user should be able to specify a map to use and a location on that map for the robot to move to. - Are there any non-functional requirements? (build system, tools, performance, etc) - - diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index dab5b9a8a06..f0d34789c0e 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -9,7 +9,7 @@ The nav2_behavior_tree module provides: See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://docs.nav2.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://docs.nav2.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin. -See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. ## The bt_action_node Template and the Behavior Tree Engine diff --git a/nav2_behavior_tree/plugins_list.hpp.in b/nav2_behavior_tree/plugins_list.hpp.in index 148583b9271..40ffed01ed5 100644 --- a/nav2_behavior_tree/plugins_list.hpp.in +++ b/nav2_behavior_tree/plugins_list.hpp.in @@ -1,6 +1,6 @@ // This was automativally generated by cmake -namespace nav2::details +namespace nav2::details { const char* BT_BUILTIN_PLUGINS = "@plugin_libs@"; } diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp index 19ddaf9e9bf..9a37bffab54 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -36,7 +36,7 @@ class CancelSpinServer : public TestActionServer goal_handle) { while (!goal_handle->is_canceling()) { - // Spining here until goal cancels + // Spinning here until goal cancels std::this_thread::sleep_for(std::chrono::milliseconds(15)); } } @@ -135,7 +135,7 @@ TEST_F(CancelSpinActionTestFixture, test_ports) // Setting target yaw goal_msg.target_yaw = 1.57; - // Spining for server and sending a goal + // Spinning for server and sending a goal client_->wait_for_action_server(); client_->async_send_goal(goal_msg, send_goal_options); diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md index be0ee32461f..2b91d9752bb 100644 --- a/nav2_behaviors/README.md +++ b/nav2_behaviors/README.md @@ -15,4 +15,4 @@ See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/ See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. The `TimedBehavior` template makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). -The `AssistedTeleop` behavior makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). \ No newline at end of file +The `AssistedTeleop` behavior makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index a5f6b9c3dc4..ffbfd1bf8cc 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -15,19 +15,13 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - GroupAction, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch.actions import (DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node -from launch_ros.actions import PushROSNamespace +from launch_ros.actions import Node, PushROSNamespace from launch_ros.descriptions import ParameterFile from nav2_common.launch import RewrittenYaml diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index 823f56ba2a7..a588fdd63e2 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -20,24 +20,14 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - ForEach, - GroupAction, - IncludeLaunchDescription, - LogInfo, - OpaqueFunction, - RegisterEventHandler, -) - +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + ForEach, GroupAction, IncludeLaunchDescription, LogInfo, + OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, TextSubstitution - import yaml diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 909b81eb3e1..87134e19d0c 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -15,16 +15,12 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction -from launch.actions import SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition -from launch.substitutions import EqualsSubstitution -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch.substitutions import NotEqualsSubstitution -from launch_ros.actions import LoadComposableNodes, SetParameter -from launch_ros.actions import Node +from launch.substitutions import (EqualsSubstitution, LaunchConfiguration, NotEqualsSubstitution, + PythonExpression) +from launch_ros.actions import LoadComposableNodes, Node, SetParameter from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 987c398992c..f8d08a4ac26 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -15,13 +15,11 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes, SetParameter -from launch_ros.actions import Node +from launch_ros.actions import LoadComposableNodes, Node, SetParameter from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 425dac4edee..1cda9d6ea7a 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -15,7 +15,6 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler from launch.event_handlers import OnProcessExit diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index f9294b711d2..7d851577252 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -15,7 +15,6 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition diff --git a/nav2_bringup/launch/tb3_loopback_simulation.launch.py b/nav2_bringup/launch/tb3_loopback_simulation.launch.py index 3553852597c..f32114eb29a 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation.launch.py @@ -17,19 +17,13 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - GroupAction, - IncludeLaunchDescription, -) +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node, SetParameter from launch_ros.descriptions import ParameterFile - from nav2_common.launch import RewrittenYaml diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 5f8659f95b7..ed12d8bcea2 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -18,20 +18,13 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression - from launch_ros.actions import Node diff --git a/nav2_bringup/launch/tb4_loopback_simulation.launch.py b/nav2_bringup/launch/tb4_loopback_simulation.launch.py index 5373efbfab3..aeb966c8a6e 100644 --- a/nav2_bringup/launch/tb4_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb4_loopback_simulation.launch.py @@ -17,19 +17,13 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - GroupAction, - IncludeLaunchDescription, -) +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import Command, LaunchConfiguration from launch_ros.actions import Node, SetParameter from launch_ros.descriptions import ParameterFile - from nav2_common.launch import RewrittenYaml diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index 5930a1b73b8..f71b3e950e0 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -18,21 +18,13 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import Command, LaunchConfiguration, PythonExpression - from launch_ros.actions import Node diff --git a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py index 862a0083799..838c69f3a9e 100644 --- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py @@ -25,18 +25,10 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - GroupAction, - IncludeLaunchDescription, - LogInfo, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + GroupAction, IncludeLaunchDescription, LogInfo, OpaqueFunction, + RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index eca25192b6e..a34976cce23 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -36,7 +36,7 @@ The zones around the robot can take the following shapes: * Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface. * Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time. * Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape. -* VelocityPolygon: allow switching of polygons based on the command velocity. When the velocity is covered by multiple sub polygons, the first sub polygon in the `velocity_polygons` list will be used. This is useful for robots to set different safety zones based on their velocity (e.g. a robot that has a larger safety zone when moving at 1.0 m/s than when moving at 0.5 m/s). +* VelocityPolygon: allow switching of polygons based on the command velocity. When the velocity is covered by multiple sub polygons, the first sub polygon in the `velocity_polygons` list will be used. This is useful for robots to set different safety zones based on their velocity (e.g. a robot that has a larger safety zone when moving at 1.0 m/s than when moving at 0.5 m/s). The data may be obtained from different data sources: diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py index 548aca4e2bb..491c2e8f6ec 100644 --- a/nav2_collision_monitor/launch/collision_detector_node.launch.py +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -17,15 +17,11 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch.substitutions import NotEqualsSubstitution -from launch_ros.actions import LoadComposableNodes, SetParameter -from launch_ros.actions import Node -from launch_ros.actions import PushROSNamespace +from launch.substitutions import LaunchConfiguration, NotEqualsSubstitution, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node, PushROSNamespace, SetParameter from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index 3dd9d01a0d4..c54d890e28b 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -17,15 +17,11 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch.substitutions import NotEqualsSubstitution -from launch_ros.actions import LoadComposableNodes, SetParameter -from launch_ros.actions import Node -from launch_ros.actions import PushROSNamespace +from launch.substitutions import LaunchConfiguration, NotEqualsSubstitution, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node, PushROSNamespace, SetParameter from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index d6b3cd25fda..eac7486913c 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -81,7 +81,7 @@ collision_monitor: theta_max: 1.0 # This is the last polygon to be checked, it should cover the entire range of robot's velocities # It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity - # is not covered by any of the other sub-polygons + # is not covered by any of the other sub-polygons stopped: points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]" linear_min: -1.0 diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake index 3f4977193c8..5bf034baf9e 100644 --- a/nav2_common/cmake/nav2_package.cmake +++ b/nav2_common/cmake/nav2_package.cmake @@ -29,10 +29,10 @@ macro(nav2_package) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) - if ("cxx_std_17" IN_LIST CMAKE_CXX_COMPILE_FEATURES) + if("cxx_std_17" IN_LIST CMAKE_CXX_COMPILE_FEATURES) set(CMAKE_CXX_STANDARD 17) else() - message( FATAL_ERROR "cxx_std_17 could not be found.") + message(FATAL_ERROR "cxx_std_17 could not be found.") endif() endif() diff --git a/nav2_common/nav2_common/launch/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py index 41f005bbc50..6e386128ce0 100644 --- a/nav2_common/nav2_common/launch/has_node_params.py +++ b/nav2_common/nav2_common/launch/has_node_params.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import List -from typing import Text +from typing import List, Text import launch import yaml @@ -37,9 +36,8 @@ def __init__( :param: node_name the name of the node to check """ - from launch.utilities import ( - normalize_to_list_of_substitutions, - ) # import here to avoid loop + # import here to avoid loop + from launch.utilities import normalize_to_list_of_substitutions self.__source_file = normalize_to_list_of_substitutions(source_file) self.__node_name = node_name diff --git a/nav2_common/nav2_common/launch/replace_string.py b/nav2_common/nav2_common/launch/replace_string.py index 076ac16f21f..cb55b883380 100644 --- a/nav2_common/nav2_common/launch/replace_string.py +++ b/nav2_common/nav2_common/launch/replace_string.py @@ -33,9 +33,9 @@ def __init__( ) -> None: super().__init__() - from launch.utilities import ( - normalize_to_list_of_substitutions, - ) # import here to avoid loop + from launch.utilities import normalize_to_list_of_substitutions + + # import here to avoid loop self.__source_file = normalize_to_list_of_substitutions(source_file) self.__replacements = {} diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 11aa77227e3..5434f611f51 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -58,9 +58,8 @@ def __init__( :param: convert_types whether to attempt converting the string to a number or boolean """ - from launch.utilities import ( - normalize_to_list_of_substitutions, - ) # import here to avoid loop + # import here to avoid loop + from launch.utilities import normalize_to_list_of_substitutions self.__source_file = normalize_to_list_of_substitutions(source_file) self.__param_rewrites = {} diff --git a/nav2_controller/README.md b/nav2_controller/README.md index c718a4e56f7..d2b468b1b4d 100644 --- a/nav2_controller/README.md +++ b/nav2_controller/README.md @@ -4,7 +4,7 @@ The Nav2 Controller is a Task Server in Nav2 that implements the `nav2_msgs::act An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with multiple plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. It also contains progress checkers and goal checker plugins to abstract out that logic from specific controller implementations. -See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available controller plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available controller plugins. See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html). diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md index a794f6430ff..0677cedb23c 100644 --- a/nav2_costmap_2d/README.md +++ b/nav2_costmap_2d/README.md @@ -1,10 +1,10 @@ # Nav2 Costmap_2d -The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors. A plugin interface allows for the layers to be combined into the costmap and finally inflated via an inflation radius based on the robot footprint. The nav2 version of the costmap_2d package is mostly a direct ROS2 port of the ROS1 navigation stack version, with minimal notable changes necessary due to support in ROS2. +The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors. A plugin interface allows for the layers to be combined into the costmap and finally inflated via an inflation radius based on the robot footprint. The nav2 version of the costmap_2d package is mostly a direct ROS2 port of the ROS1 navigation stack version, with minimal notable changes necessary due to support in ROS2. See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-costmaps.html) for additional parameter descriptions for the costmap and its included plugins. The [tutorials](https://docs.nav2.org/tutorials/index.html) and [first-time setup guides](https://docs.nav2.org/setup_guides/index.html) also provide helpful context for working with the costmap 2D package and its layers. A [tutorial](https://docs.nav2.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html) is also provided to explain how to create costmap plugins. -See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. ## To visualize the voxels in RVIZ: - Make sure `publish_voxel_map` in `voxel_layer` param's scope is set to `True`. diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index 16e2f4ba61a..6d2bde4a1c3 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -35,4 +35,3 @@ - diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 490b4815e8b..c8ba5291340 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -165,4 +165,4 @@ ament_add_test(test_costmap_subscriber_exec # nav2_util::nav2_utils_core # rclcpp::rclcpp # tf2_ros::tf2_ros -# ) \ No newline at end of file +# ) diff --git a/nav2_costmap_2d/test/integration/costmap_tests_launch.py b/nav2_costmap_2d/test/integration/costmap_tests_launch.py index 928c4a14ec1..5980f8f699b 100755 --- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py +++ b/nav2_costmap_2d/test/integration/costmap_tests_launch.py @@ -17,10 +17,8 @@ import os import sys -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription +from launch import LaunchDescription, LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_ros.actions from launch_testing.legacy import LaunchTestService diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 3c266192248..3cbf22336e8 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -535,7 +535,7 @@ TEST_F(TestNode, testInflation2) waitForMap(slayer); - // Creat a small L-Shape all at once + // Create a small L-Shape all at once addObservation(olayer, 1, 1, MAX_Z); addObservation(olayer, 2, 1, MAX_Z); addObservation(olayer, 2, 2, MAX_Z); diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index 7dd0b4eb3a9..56d422e678c 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -188,7 +188,7 @@ TEST_F(TestNode, testRaytracing2) { ASSERT_EQ(obs_before, 20); // The sensor origin will be <0,0>. So if we add an obstacle at 9,9, - // we would expect cells <0, 0> thru <8, 8> to be traced through + // we would expect cells <0, 0> through <8, 8> to be traced through // however the static map is not cleared by obstacle layer addObservation(olayer, 9.5, 9.5, MAX_Z / 2, 0.5, 0.5, MAX_Z / 2); layers.updateMap(0, 0, 0); diff --git a/nav2_costmap_2d/test/module_tests.cpp b/nav2_costmap_2d/test/module_tests.cpp index 5f368144b58..fb8fa9eb388 100644 --- a/nav2_costmap_2d/test/module_tests.cpp +++ b/nav2_costmap_2d/test/module_tests.cpp @@ -915,7 +915,7 @@ TEST(costmap, testInflation2) { ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); - // Creat a small L-Shape all at once + // Create a small L-Shape all at once pcl::PointCloud c0; c0.points.resize(3); c0.points[0].x = 1; @@ -1039,7 +1039,7 @@ TEST(costmap, testRaytracing2) { 100.0, MAX_Z, 100.0, 1, MAP_10_BY_10, THRESHOLD); // The sensor origin will be <0,0>. So if we add an obstacle at 9,9, we would expect cells - // <0, 0> thru <8, 8> to be traced through + // <0, 0> through <8, 8> to be traced through pcl::PointCloud c0; c0.points.resize(1); c0.points[0].x = 9.5; diff --git a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp index a45cb38a7f9..c4afee34e00 100644 --- a/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp +++ b/nav2_costmap_2d/test/regression/costmap_bresenham_2d.cpp @@ -100,7 +100,7 @@ TEST(costmap_2d, bresenham2DBoundariesCheck) CostmapTest ct(sz_x, sz_y, 0.1, 0.0, 0.0); CostmapAction ca(ct.getCostmap(), ct.getSize()); - // Initial point - some assymetrically standing point in order to cover most corner cases + // Initial point - some asymmetrically standing point in order to cover most corner cases const unsigned int x0 = 2; const unsigned int y0 = 4; // (x1, y1) point will move diff --git a/nav2_docking/README.md b/nav2_docking/README.md index 0352ee95aef..4e4db71d13f 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -26,7 +26,7 @@ Want to learn more? Checkout the ROSCon 2024 talk on Docking by clicking on the The Docking Framework has 5 main components: - `DockingServer`: The main action server and logic for performing the docking/undocking actions -- `Navigator`: A NavigateToPose action client to navigate the robot to the dock's staging pose if not with the prestaging tolerances +- `Navigator`: A NavigateToPose action client to navigate the robot to the dock's staging pose if not with the prestaging tolerances - `DockDatabase`: A database of dock instances in an environment and their associated interfaces for transacting with each type. An arbitrary number of types are supported. - `Controller`: A spiral-based graceful controller to use for the vision-control loop for docking - `ChargingDock` and `NonChargingDock`: Plugins that describe the dock and how to transact with it (check if charging, detection, etc). You can find these plugin headers in the `opennav_docking_core` package. @@ -38,7 +38,7 @@ The docking procedure is as follows: 2. If the robot is not within the prestaging tolerance of the dock's staging pose, navigate to the staging pose 3. Use the dock's plugin to initially detect the dock and return the docking pose 4. Enter a vision-control loop where the robot attempts to reach the docking pose while its actively being refined by the vision system -5. Exit the vision-control loop once contact has been detected or charging has started +5. Exit the vision-control loop once contact has been detected or charging has started 6. Wait until charging starts (if applicable) and return success. If anywhere this procedure is unsuccessful, `N` retries may be made by driving back to the dock's staging pose and trying again. If still unsuccessful, it will return a failure code to indicate what kind of failure occurred to the client. @@ -48,19 +48,19 @@ Undocking works more simply: 2. Find the staging pose for this dock and back out to that pose 3. Check if successfully backed out to the pose and charging has stopped -## Interfaces +## Interfaces ### Docking Action -The docking action can either operate on a dock in the `DockDatabase` or from a dock specified in the docking request. This second option is useful for testing or when dock's locales are not necessarily known in advance. +The docking action can either operate on a dock in the `DockDatabase` or from a dock specified in the docking request. This second option is useful for testing or when dock's locales are not necessarily known in advance. If `use_dock_id = true`, it uses the `dock_id` field to specify which dock in the database to use. Else, you must populate the `dock_pose` and `dock_type` fields. If you wish for the docking server to stage your robot at the the dock's staging pose for you, `navigate_to_staging_pose` must be true. -Else, you can send your robot to this pose and it will be skipped as long as the robot is within the prestaging tolerances. +Else, you can send your robot to this pose and it will be skipped as long as the robot is within the prestaging tolerances. You may set the maximum time for navigation using `max_staging_time`. -In return, you obtain the `num_retries`, for the number of attempted retries of the action; `success`, if the action worked and the robot is successfully charging; and `error_code` to return a semantically meaningful error code about what kind of error occurred, if any. See `DockRobot.action` for more details. +In return, you obtain the `num_retries`, for the number of attempted retries of the action; `success`, if the action worked and the robot is successfully charging; and `error_code` to return a semantically meaningful error code about what kind of error occurred, if any. See `DockRobot.action` for more details. While the action is performing, you can obtain feedback about the current `state` of docking, how much time `docking_time` has elapsed, and the current number of retries attempted. @@ -174,7 +174,7 @@ some robots. `getStagingPose` applies a parameterized translational and rotational offset to the dock pose to obtain the staging pose. -`getRefinedPose` can be used in two ways. +`getRefinedPose` can be used in two ways. 1. A blind approach where the returned dock pose will simply be equal to whatever was passed in from the dock database. This may work with a reduced success rate on a real robot (due to global localization error), but is useful for initial testing and simulation. 2. The more realistic use case is to use an AR marker, dock pose detection algorithm, etc. The plugin will subscribe to a `geometry_msgs/PoseStamped` topic `detected_dock_pose`. This can be used with the `image_proc/TrackMarkerNode` for Apriltags or other custom detectors for your dock. It is unlikely the detected pose is actually the pose you want to dock with, so several parameters are supplied to represent your docked pose relative to the detected feature's pose. @@ -259,4 +259,3 @@ Note: The external detection rotation angles are setup to work out of the box wi ### On Staging Poses Staging poses are where the robot should navigate to in order to start the docking procedure. This pose should be close enough to the dock to accurately detect the dock's presence, but far enough that if its moved slightly or the robot's localization isn't perfect it can still be detected and have enough room to adjust. The robot's charging contacts or charging location should be pointed towards the dock in this staging pose. That way, a feasible global planner can be used to model your robot's real constraints while getting to the docking pose (non-circular, non-holonomic), rather than complicating the docking process itself. - diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index f402f7970e3..560ad02e6f1 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -34,7 +34,6 @@ from sensor_msgs.msg import BatteryState from tf2_ros import TransformBroadcaster - # This test can be run standalone with: # python3 -u -m pytest test_docking_server.py -s @@ -42,6 +41,7 @@ # try to identify flaky ness. # python3 -u -m pytest --force-flaky --min-passes 3 --max-runs 5 -s -v test_docking_server.py + @pytest.mark.rostest # @pytest.mark.flaky # @pytest.mark.flaky(max_runs=5, min_passes=3) diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 64757f31691..4f168a7d8df 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -514,7 +514,7 @@ DWBLocalPlanner::transformGlobalPlan( auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance( global_plan_.poses.begin(), global_plan_.poses.end(), forward_prune_distance_); - // Find the first pose in the plan (upto prune_point) that's less than transform_start_threshold + // Find the first pose in the plan (up to prune_point) that's less than transform_start_threshold // from the robot. auto transformation_begin = std::find_if( begin(global_plan_.poses), prune_point, diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index 0154550682e..331a91352b1 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -13,30 +13,30 @@ The smooth control law is a pose-following kinematic control law that generates ## Parameters -| Parameter | Description | +| Parameter | Description | |-----|----| -| `transform_tolerance` | The TF transform tolerance. | +| `transform_tolerance` | The TF transform tolerance. | | `motion_target_dist` | The lookahead distance to use to find the motion_target point. This distance should be a value around 1.0m but not much farther away. Greater values will cause the robot to generate smoother paths but not necessarily follow the path as closely. | -| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | -| `k_phi` | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem. If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r. **Note**: This variable is called k1 in earlier versions of the paper. | -| `k_delta` | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem. The bigger the value, the robot converge faster to the reference heading. **Note**: This variable is called k2 in earlier versions of the paper. | -| `beta` | Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases. | -| `lambda` | Constant factor applied to the path curvature. This value must be greater or equal to 1. Determines the sharpness of the curve: higher lambda implies sharper curves. | -| `v_linear_min` | Minimum linear velocity. Units: meters/sec. | -| `v_linear_max` | Maximum linear velocity. Units: meters/sec. | -| `v_angular_max` | Maximum angular velocity produced by the control law. Units: radians/sec. | -| `slowdown_radius` | Radius around the goal pose in which the robot will start to slow down. Units: meters. | -| `initial_rotation` | Enable a rotation in place to the goal before starting the path. The control law may generate large sweeping arcs to the goal pose, depending on the initial robot orientation and k_phi, k_delta. | -| `initial_rotation_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `initial_rotation` is enabled. | -| `final_rotation` | Similar to `initial_rotation`, the control law can generate large arcs when the goal orientation is not aligned with the path. If this is enabled, the final pose will be ignored and the robot will follow the orientation of he path and will make a final rotation in place to the goal orientation. | -| `rotation_scaling_factor` | The scaling factor applied to the rotation in place velocity. | +| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | +| `k_phi` | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem. If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r. **Note**: This variable is called k1 in earlier versions of the paper. | +| `k_delta` | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem. The bigger the value, the robot converge faster to the reference heading. **Note**: This variable is called k2 in earlier versions of the paper. | +| `beta` | Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases. | +| `lambda` | Constant factor applied to the path curvature. This value must be greater or equal to 1. Determines the sharpness of the curve: higher lambda implies sharper curves. | +| `v_linear_min` | Minimum linear velocity. Units: meters/sec. | +| `v_linear_max` | Maximum linear velocity. Units: meters/sec. | +| `v_angular_max` | Maximum angular velocity produced by the control law. Units: radians/sec. | +| `slowdown_radius` | Radius around the goal pose in which the robot will start to slow down. Units: meters. | +| `initial_rotation` | Enable a rotation in place to the goal before starting the path. The control law may generate large sweeping arcs to the goal pose, depending on the initial robot orientation and k_phi, k_delta. | +| `initial_rotation_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `initial_rotation` is enabled. | +| `final_rotation` | Similar to `initial_rotation`, the control law can generate large arcs when the goal orientation is not aligned with the path. If this is enabled, the final pose will be ignored and the robot will follow the orientation of he path and will make a final rotation in place to the goal orientation. | +| `rotation_scaling_factor` | The scaling factor applied to the rotation in place velocity. | | `allow_backward` | Whether to allow the robot to move backward. | ## Topics -| Topic | Type | Description | +| Topic | Type | Description | |-----|----|----| -| `transformed_global_plan` | `nav_msgs/Path` | The global plan transformed into the robot frame. | -| `local_plan` | `nav_msgs/Path` | The local plan generated by appliyng iteratively the control law upon a set of motion targets along the global plan. | -| `motion_target` | `geometry_msgs/PointStamped` | The current motion target used by the controller to compute the velocity commands. | -| `slowdown` | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | +| `transformed_global_plan` | `nav_msgs/Path` | The global plan transformed into the robot frame. | +| `local_plan` | `nav_msgs/Path` | The local plan generated by appliyng iteratively the control law upon a set of motion targets along the global plan. | +| `motion_target` | `geometry_msgs/PointStamped` | The current motion target used by the controller to compute the velocity commands. | +| `slowdown` | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | diff --git a/nav2_lifecycle_manager/README.md b/nav2_lifecycle_manager/README.md index e4622119885..e001c98d691 100644 --- a/nav2_lifecycle_manager/README.md +++ b/nav2_lifecycle_manager/README.md @@ -15,4 +15,4 @@ The diagram below shows an _example_ of a list of managed nodes, and how it inte The UML diagram below shows the sequence of service calls once the _startup_ is requested from the lifecycle manager. - \ No newline at end of file + diff --git a/nav2_lifecycle_manager/test/launch_bond_test.py b/nav2_lifecycle_manager/test/launch_bond_test.py index 728a061001a..8825405ca9f 100755 --- a/nav2_lifecycle_manager/test/launch_bond_test.py +++ b/nav2_lifecycle_manager/test/launch_bond_test.py @@ -16,8 +16,7 @@ import os import sys -from launch import LaunchDescription -from launch import LaunchService +from launch import LaunchDescription, LaunchService from launch.actions import ExecuteProcess from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService diff --git a/nav2_lifecycle_manager/test/launch_lifecycle_test.py b/nav2_lifecycle_manager/test/launch_lifecycle_test.py index 06582361858..de9a1a2ce78 100755 --- a/nav2_lifecycle_manager/test/launch_lifecycle_test.py +++ b/nav2_lifecycle_manager/test/launch_lifecycle_test.py @@ -16,8 +16,7 @@ import os import sys -from launch import LaunchDescription -from launch import LaunchService +from launch import LaunchDescription, LaunchService from launch.actions import ExecuteProcess from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService diff --git a/nav2_loopback_sim/launch/loopback_simulation.launch.py b/nav2_loopback_sim/launch/loopback_simulation.launch.py index 0124cb848f5..48883f0cb6a 100644 --- a/nav2_loopback_sim/launch/loopback_simulation.launch.py +++ b/nav2_loopback_sim/launch/loopback_simulation.launch.py @@ -15,7 +15,6 @@ import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 6f75ef442cc..18cbcaf5199 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -15,8 +15,8 @@ import math -from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, TwistStamped -from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 +from geometry_msgs.msg import (PoseWithCovarianceStamped, Quaternion, TransformStamped, Twist, + TwistStamped, Vector3) from nav2_simple_commander.line_iterator import LineIterator from nav_msgs.msg import Odometry from nav_msgs.srv import GetMap @@ -29,13 +29,8 @@ from tf2_ros import Buffer, TransformBroadcaster, TransformListener import tf_transformations -from .utils import ( - addYawToQuat, - getMapOccupancy, - matrixToTransform, - transformStampedToMatrix, - worldToMap, -) +from .utils import (addYawToQuat, getMapOccupancy, matrixToTransform, transformStampedToMatrix, + worldToMap) """ This is a loopback simulator that replaces a physics simulator to create a diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index 468103e94e8..ff5c6012e37 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -19,7 +19,6 @@ import numpy as np import tf_transformations - """ Transformation utilities for the loopback simulator """ diff --git a/nav2_loopback_sim/setup.py b/nav2_loopback_sim/setup.py index 0d56733614b..8d25201e070 100644 --- a/nav2_loopback_sim/setup.py +++ b/nav2_loopback_sim/setup.py @@ -3,7 +3,6 @@ from setuptools import setup - package_name = 'nav2_loopback_sim' setup( diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index 4e811a24277..c095898b639 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -96,7 +96,7 @@ $ process_with_multiple_map_servers __params:=combined_params.yaml The parameter for the initial map (yaml_filename) has to be set, but an empty string can be used if no initial map should be loaded. In this case, no map is loaded during -on_configure or published during on_activate. The _load_map_-service should the be used to load and publish a map. +on_configure or published during on_activate. The _load_map_-service should the be used to load and publish a map. #### Map Saver @@ -140,4 +140,3 @@ Service usage examples: $ ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: /ros/maps/map.yaml}" $ ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: my_map, image_format: pgm, map_mode: trinary, free_thresh: 0.25, occupied_thresh: 0.65}" ``` - diff --git a/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake b/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake index c04356b4aa8..e86f249afde 100644 --- a/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake +++ b/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake @@ -31,4 +31,4 @@ find_library(GRAPHICSMAGICKCPP_LIBRARIES find_package_handle_standard_args( GRAPHICSMAGICKCPP GRAPHICSMAGICKCPP_LIBRARIES - GRAPHICSMAGICKCPP_INCLUDE_DIRS) \ No newline at end of file + GRAPHICSMAGICKCPP_INCLUDE_DIRS) diff --git a/nav2_map_server/test/component/test_map_saver_launch.py b/nav2_map_server/test/component/test_map_saver_launch.py index e33e03a9864..34f51bb61ec 100755 --- a/nav2_map_server/test/component/test_map_saver_launch.py +++ b/nav2_map_server/test/component/test_map_saver_launch.py @@ -17,10 +17,8 @@ import os import sys -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription +from launch import LaunchDescription, LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.legacy import LaunchTestService diff --git a/nav2_map_server/test/component/test_map_server_launch.py b/nav2_map_server/test/component/test_map_server_launch.py index 473feea97eb..ee3d5a1d3fb 100755 --- a/nav2_map_server/test/component/test_map_server_launch.py +++ b/nav2_map_server/test/component/test_map_server_launch.py @@ -17,10 +17,8 @@ import os import sys -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription +from launch import LaunchDescription, LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_testing.legacy import LaunchTestService diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index e8ca1e5afa9..0810664d85f 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -84,7 +84,7 @@ This process is then repeated a number of times and returns a converged solution | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | cost_weight | double | Default 4.0. Weight to apply to critic term. | - | cost_power | int | Default 1. Power order to apply to term. + | cost_power | int | Default 1. Power order to apply to term. #### Goal Angle Critic | Parameter | Type | Definition | @@ -113,7 +113,7 @@ Uses estimated distances from obstacles using cost and inflation parameters to a | cost_power | int | Default 1. Power order to apply to term. | | collision_cost | double | Default 100000.0. Cost to apply to a true collision in a trajectory. | | collision_margin_distance | double | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. | - | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. + | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | #### Cost Critic @@ -123,11 +123,11 @@ Uses inflated costmap cost directly to avoid obstacles | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | - | cost_weight | double | Default 3.81. Weight to apply to critic to avoid obstacles. | + | cost_weight | double | Default 3.81. Weight to apply to critic to avoid obstacles. | | cost_power | int | Default 1. Power order to apply to term. | | collision_cost | double | Default 1000000.0. Cost to apply to a true collision in a trajectory. | | critical_cost | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles. | - | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. + | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | | trajectory_point_step | int | Default 2. Step of trajectory points to evaluate for costs since otherwise so dense represents multiple points for a single costmap cell. | @@ -159,7 +159,7 @@ Uses inflated costmap cost directly to avoid obstacles | cost_weight | double | Default 5.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | | offset_from_furthest | int | Default 6. Number of path points after furthest one any trajectory achieves to drive path tracking relative to. | - | threshold_to_consider | float | Default 1.4. Distance between robot and goal above which path follow cost stops being considered | + | threshold_to_consider | float | Default 1.4. Distance between robot and goal above which path follow cost stops being considered | #### Prefer Forward Critic | Parameter | Type | Definition | @@ -302,7 +302,7 @@ Visualization of the trajectories using `visualize` uses compute resources to ba The most common parameters you might want to start off changing are the velocity profiles (`vx_max`, `vx_min`, `wz_max`, and `vy_max` if holonomic) and the `motion_model` to correspond to your vehicle. Its wise to consider the `prune_distance` of the path plan in proportion to your maximum velocity and prediction horizon. The only deeper parameter that will likely need to be adjusted for your particular settings is the Obstacle critics' `repulsion_weight` since the tuning of this is proportional to your inflation layer's radius. Higher radii should correspond to reduced `repulsion_weight` due to the penalty formation (e.g. `inflation_radius - min_dist_to_obstacle`). If this penalty is too high, the robot will slow significantly when entering cost-space from non-cost space or jitter in narrow corridors. It is noteworthy, but likely not necessary to be changed, that the Obstacle critic may use the full footprint information if `consider_footprint = true`, though comes at an increased compute cost. -If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics. +If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics. By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. @@ -318,14 +318,14 @@ The Path Follow critic cannot drive velocities greater than the projectable dist ### Obstacle, Inflation Layer, and Path Following -There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. The default behavior was tuned for small AMRs (e.g. turtlebots or similar), so if using a larger robot, you may want to reduce the `repulsion_weight` in kind. It may also perform awkward maneuvers when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task. +There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. The default behavior was tuned for small AMRs (e.g. turtlebots or similar), so if using a larger robot, you may want to reduce the `repulsion_weight` in kind. It may also perform awkward maneuvers when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task. Thus, care should be taken to select weights of the obstacle critic in conjunction with the costmap inflation radius and scale so that a robot does not have such issues. How I (Steve, your friendly neighborhood navigator) tuned this was to first create the appropriate obstacle critic behavior desirable in conjunction with the inflation layer parameters. Its worth noting that the Obstacle critic converts the cost into a distance from obstacles, so the nature of the distribution of costs in the inflation isn't overly significant. However, the inflation radius and the scale will define the cost at the end of the distribution where free-space meets the lowest cost value within the radius. So testing for quality behavior when going over that threshold should be considered. As you increase or decrease your weights on the Obstacle, you may notice the aforementioned behaviors (e.g. won't overcome free to non-free threshold). To overcome them, increase the FollowPath critic cost to increase the desire for the trajectory planner to continue moving towards the goal. Make sure to not overshoot this though, keep them balanced. A desirable outcome is smooth motion roughly in the center of spaces without significant close interactions with obstacles. It shouldn't be perfectly following a path yet nor should the output velocity be wobbling jaggedly. -Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off. +Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off. ### MFMA and AVX2 Optimizations -This MPPI is made possible to run on CPU-only by using a very well optimized implementation that rely on CPU vectorization through AVX2 and MFMA. All even remotely modern computers support this (2013+), but if using a very old computer you may not be able to use the plugin. Note that MPC is computationally heavy to begin with, so computers circa-2013 even if it were to have those compiler flags available probably wouldn't run it at a satisfactory rate anyway. +This MPPI is made possible to run on CPU-only by using a very well optimized implementation that rely on CPU vectorization through AVX2 and MFMA. All even remotely modern computers support this (2013+), but if using a very old computer you may not be able to use the plugin. Note that MPC is computationally heavy to begin with, so computers circa-2013 even if it were to have those compiler flags available probably wouldn't run it at a satisfactory rate anyway. diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index e1934affeec..924c127b32f 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -168,7 +168,7 @@ TEST(CriticTests, GoalAngleCritic) critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); - // Lets move it even closer, just to be sure it still doesn't trigger + // Let's move it even closer, just to be sure it still doesn't trigger state.pose.pose.position.x = 9.2; critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 901759449b8..34beba3bd3e 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -629,7 +629,7 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) EXPECT_NEAR(traj.y(1, i), i * 0.2 /*vel*/ * 0.1 /*dt*/, 1e-3); } - // Lets add some angular motion to the mix + // Let's add some angular motion to the mix state.vy = Eigen::ArrayXXf::Zero(1000, 50); state.wz = 0.2 * Eigen::ArrayXXf::Ones(1000, 50); state.wz.col(0) = Eigen::ArrayXf::Zero(1000); diff --git a/nav2_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv index c5ca48a3099..93f0f360161 100644 --- a/nav2_msgs/srv/GetCosts.srv +++ b/nav2_msgs/srv/GetCosts.srv @@ -4,4 +4,4 @@ bool use_footprint geometry_msgs/PoseStamped[] poses --- float32[] costs -bool success \ No newline at end of file +bool success diff --git a/nav2_msgs/srv/IsPathValid.srv b/nav2_msgs/srv/IsPathValid.srv index e178847085a..a51bf949114 100644 --- a/nav2_msgs/srv/IsPathValid.srv +++ b/nav2_msgs/srv/IsPathValid.srv @@ -5,4 +5,4 @@ uint8 max_cost 253 bool consider_unknown_as_obstacle false --- bool is_valid -int32[] invalid_pose_indices +int32[] invalid_pose_indices diff --git a/nav2_msgs/srv/SetInitialPose.srv b/nav2_msgs/srv/SetInitialPose.srv index ec1e50b65f1..7089d1a5871 100644 --- a/nav2_msgs/srv/SetInitialPose.srv +++ b/nav2_msgs/srv/SetInitialPose.srv @@ -1,3 +1,2 @@ geometry_msgs/PoseWithCovarianceStamped pose --- - diff --git a/nav2_navfn_planner/README.md b/nav2_navfn_planner/README.md index 8b4a07e051d..68dd5240e77 100644 --- a/nav2_navfn_planner/README.md +++ b/nav2_navfn_planner/README.md @@ -2,6 +2,6 @@ The NavfnPlanner is a global planner plugin for the Nav2 Planner server. It implements the Navigation Function planner with either A\* or Dij. expansions. It is largely equivalent to its counterpart in ROS 1 Navigation. The Navfn planner assumes a circular robot (or a robot that can be approximated as circular for the purposes of global path planning) and operates on a weighted costmap. -The `global_planner` package from ROS (1) is a refactor on NavFn to make it more easily understandable, but it lacks in run-time performance and introduces suboptimal behaviors. As NavFn has been extremely stable for about 10 years at the time of porting, the maintainers felt no compelling reason to port over another, largely equivalent (but poorer functioning) planner. +The `global_planner` package from ROS (1) is a refactor on NavFn to make it more easily understandable, but it lacks in run-time performance and introduces suboptimal behaviors. As NavFn has been extremely stable for about 10 years at the time of porting, the maintainers felt no compelling reason to port over another, largely equivalent (but poorer functioning) planner. See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-navfn.html) for additional parameter descriptions. diff --git a/nav2_planner/README.md b/nav2_planner/README.md index 302fed50db1..50efaf3bb7e 100644 --- a/nav2_planner/README.md +++ b/nav2_planner/README.md @@ -4,6 +4,6 @@ The Nav2 planner is a Task Server in Nav2 that implements the `nav2_behavior_tre A planning module implementing the `nav2_behavior_tree::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It loads a map of potential planner plugins to do the path generation in different user-defined situations. -See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-planner-server.html) for additional parameter descriptions and a [tutorial about writing planner plugins](https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html). diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 81244d804df..f49f9b31420 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -22,7 +22,7 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this ```bibtex @article{macenski2023regulated, - title={Regulated Pure Pursuit for Robot Path Tracking}, + title={Regulated Pure Pursuit for Robot Path Tracking}, author={Steve Macenski and Shrijit Singh and Francisco Martin and Jonatan Gines}, year={2023}, journal = {Autonomous Robots} @@ -31,7 +31,7 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this ## Pure Pursuit Basics -The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance. +The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance. In order to simply book-keeping, the global path is continuously pruned to the closest point to the robot (see the figure below) so that we only have to process useful path points. Then, the section of the path within the local costmap bounds is transformed to the robot frame and a lookahead point is determined using a predefined distance. @@ -55,42 +55,42 @@ The major improvements that this work implements is the regulations on the linea The cost functions penalize the robot's speed based on its proximity to obstacles and the curvature of the path. This is helpful to slow the robot when moving close to things in narrow spaces and scaling down the linear velocity by curvature helps to stabilize the controller over a larger range of lookahead point distances. This also has the added benefit of removing the sensitive tuning of the lookahead point / range, as the robot will track paths far better. Tuning is still required, but it is substantially easier to get reasonable behavior with minor adjustments. -An unintended tertiary benefit of scaling the linear velocities by curvature is that a robot will natively rotate to rough path heading when using holonomic planners that don't start aligned with the robot pose orientation. As the curvature will be very high, the linear velocity drops and the angular velocity takes over to rotate to heading. While not perfect, it does dramatically reduce the need to rotate to a close path heading before following and opens up a broader range of planning techniques. Pure Pursuit controllers otherwise would be completely unable to recover from this in even modestly confined spaces. +An unintended tertiary benefit of scaling the linear velocities by curvature is that a robot will natively rotate to rough path heading when using holonomic planners that don't start aligned with the robot pose orientation. As the curvature will be very high, the linear velocity drops and the angular velocity takes over to rotate to heading. While not perfect, it does dramatically reduce the need to rotate to a close path heading before following and opens up a broader range of planning techniques. Pure Pursuit controllers otherwise would be completely unable to recover from this in even modestly confined spaces. -Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. +Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. Note: The maximum allowed time to collision is thresholded by the lookahead point, starting in Humble. This is such that collision checking isn't significantly overshooting the path, which can cause issues in constrained environments. For example, if there were a straight-line path going towards a wall that then turned left, if this parameter was set to high, then it would detect a collision past the point of actual robot intended motion. Thusly, if a robot is moving fast, selecting further out lookahead points is not only a matter of behavioral stability for Pure Pursuit, but also gives a robot further predictive collision detection capabilities. The max allowable time parameter is still in place for slow commands, as described in detail above. ## Configuration -| Parameter | Description | +| Parameter | Description | |-----|----| -| `desired_linear_vel` | The desired maximum linear velocity to use. | -| `lookahead_dist` | The lookahead distance to use to find the lookahead point | -| `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | -| `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | -| `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | -| `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | -| `transform_tolerance` | The TF transform tolerance | -| `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | -| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | -| `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. | +| `desired_linear_vel` | The desired maximum linear velocity to use. | +| `lookahead_dist` | The lookahead distance to use to find the lookahead point | +| `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | +| `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | +| `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | +| `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | +| `transform_tolerance` | The TF transform tolerance | +| `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | +| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | +| `approach_velocity_scaling_dist` | Integrated distance from end of transformed path at which to start applying velocity scaling. This defaults to the forward extent of the costmap minus one costmap cell length. | | `use_collision_detection` | Whether to enable collision detection. | | `max_allowed_time_to_collision_up_to_carrot` | The time to project a velocity command to check for collisions when `use_collision_detection` is `true`. It is limited to maximum distance of lookahead distance selected. | -| `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | -| `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | -| `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | -| `cost_scaling_gain` | A multiplier gain, which should be <= 1.0, used to further scale the speed when an obstacle is within `cost_scaling_dist`. Lower value reduces speed more quickly. | -| `inflation_cost_scaling_factor` | The value of `cost_scaling_factor` set for the inflation layer in the local costmap. The value should be exactly the same for accurately computing distance from obstacles using the inflated cell values | -| `regulated_linear_scaling_min_radius` | The turning radius for which the regulation features are triggered. Remember, sharper turns have smaller radii | -| `regulated_linear_scaling_min_speed` | The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature. | -| `use_fixed_curvature_lookahead` | Enable fixed lookahead for curvature detection. Useful for systems with long lookahead. | -| `curvature_lookahead_dist` | Distance to lookahead to determine curvature for velocity regulation purposes. Only used if `use_fixed_curvature_lookahead` is enabled. | -| `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | -| `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | -| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | -| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | -| `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) | +| `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | +| `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | +| `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | +| `cost_scaling_gain` | A multiplier gain, which should be <= 1.0, used to further scale the speed when an obstacle is within `cost_scaling_dist`. Lower value reduces speed more quickly. | +| `inflation_cost_scaling_factor` | The value of `cost_scaling_factor` set for the inflation layer in the local costmap. The value should be exactly the same for accurately computing distance from obstacles using the inflated cell values | +| `regulated_linear_scaling_min_radius` | The turning radius for which the regulation features are triggered. Remember, sharper turns have smaller radii | +| `regulated_linear_scaling_min_speed` | The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature. | +| `use_fixed_curvature_lookahead` | Enable fixed lookahead for curvature detection. Useful for systems with long lookahead. | +| `curvature_lookahead_dist` | Distance to lookahead to determine curvature for velocity regulation purposes. Only used if `use_fixed_curvature_lookahead` is enabled. | +| `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | +| `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | +| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | +| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | +| `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) | Example fully-described XML with default parameter values: @@ -146,10 +146,10 @@ controller_server: ## Topics -| Topic | Type | Description | +| Topic | Type | Description | |-----|----|----| -| `lookahead_point` | `geometry_msgs/PointStamped` | The current lookahead point on the path | -| `lookahead_arc` | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision_up_to_carrot`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | +| `lookahead_point` | `geometry_msgs/PointStamped` | The current lookahead point on the path | +| `lookahead_arc` | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision_up_to_carrot`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed gauge. diff --git a/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml index d695bcec7f8..b7371017e6e 100644 --- a/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml +++ b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml @@ -7,4 +7,3 @@ - diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 675a2aefe3c..f6d09dfe5e4 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -1,8 +1,8 @@ # Nav2 Rotation Shim Controller -This is a controller (local trajectory planner) that implements a "shim" controller plugin. It was developed by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). +This is a controller (local trajectory planner) that implements a "shim" controller plugin. It was developed by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). -The Rotation Shim Controller stands between the controller server and the main controller plugin to implement a specific behavior often troublesome for other algorithms. This shim will rotate a robot in place to the rough heading of a newly received path. Afterwards, it will forward all future commands on that path to the main controller. It will take in a ``primary_controller`` parameter, containing the actual controller to use for path tracking once aligned with the path. +The Rotation Shim Controller stands between the controller server and the main controller plugin to implement a specific behavior often troublesome for other algorithms. This shim will rotate a robot in place to the rough heading of a newly received path. Afterwards, it will forward all future commands on that path to the main controller. It will take in a ``primary_controller`` parameter, containing the actual controller to use for path tracking once aligned with the path. This is useful for situations when working with plugins that are either too specialized or tuned for a particular task that they can fail to adequately solve the full local planning problem performantly. Examples include: @@ -17,7 +17,7 @@ When the `rotate_to_goal_heading` parameter is set to true, this controller is a The Rotation Shim Controller is suitable for: - Robots that can rotate in place, such as differential and omnidirectional robots. - Preference to rotate in place rather than 'spiral out' when starting to track a new path that is at a significantly different heading than the robot's current heading. -- Using planners that are non-kinematically feasible, such as NavFn, Theta\*, or Smac 2D (Feasible planners such as Smac Hybrid-A* and State Lattice will start search from the robot's actual starting heading, requiring no rotation). +- Using planners that are non-kinematically feasible, such as NavFn, Theta\*, or Smac 2D (Feasible planners such as Smac Hybrid-A* and State Lattice will start search from the robot's actual starting heading, requiring no rotation). This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). It will host an internal plugin of your actual path tracker (e.g. MPPI, RPP, DWB, TEB, etc) that will be used after the robot has rotated to the rough starting heading of the path. @@ -29,15 +29,15 @@ See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/ ## Configuration -| Parameter | Description | +| Parameter | Description | |-----|----| -| `angular_dist_threshold` | Maximum angular distance, in radians, away from the path heading to trigger rotation until within. | -| `forward_sampling_distance` | Forward distance, in meters, along path to select a sampling point to use to approximate path heading | -| `rotate_to_heading_angular_vel` | Angular rotational velocity, in rad/s, to rotate to the path heading | -| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading | -| `max_angular_accel` | Maximum angular acceleration for rotation to heading | -| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. | -| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading | +| `angular_dist_threshold` | Maximum angular distance, in radians, away from the path heading to trigger rotation until within. | +| `forward_sampling_distance` | Forward distance, in meters, along path to select a sampling point to use to approximate path heading | +| `rotate_to_heading_angular_vel` | Angular rotational velocity, in rad/s, to rotate to the path heading | +| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading | +| `max_angular_accel` | Maximum angular acceleration for rotation to heading | +| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. | +| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading | Example fully-described XML with default parameter values: diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 2b900437fec..f13cf8491bf 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -118,6 +118,6 @@ The `nav2_simple_commander` has a few examples to highlight the API functions av The `nav2_simple_commander` has a few demonstrations to highlight a couple of simple autonomy applications you can build using the `nav2_simple_commander` API: -- `demo_security.py` - A simple security robot application, showing how to have a robot follow a security route using Navigate Through Poses to do a patrol route, indefinitely. +- `demo_security.py` - A simple security robot application, showing how to have a robot follow a security route using Navigate Through Poses to do a patrol route, indefinitely. - `demo_picking.py` - A simple item picking application, showing how to have a robot drive to a specific shelf in a warehouse to either pick an item or have a person place an item into a basket and deliver it to a destination for shipping using Navigate To Pose. - `demo_inspection.py` - A simple shelf inspection application, showing how to use the Waypoint Follower and task executors to take pictures, RFID scans, etc of shelves to analyze the current shelf statuses and locate items in the warehouse. diff --git a/nav2_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py index ad3119ce441..39172c22185 100644 --- a/nav2_simple_commander/launch/assisted_teleop_example_launch.py +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -18,16 +18,9 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py index b07fef9ba68..7e6b9f2ad4d 100644 --- a/nav2_simple_commander/launch/follow_path_example_launch.py +++ b/nav2_simple_commander/launch/follow_path_example_launch.py @@ -17,16 +17,9 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py index be98c14150d..8320de3a828 100644 --- a/nav2_simple_commander/launch/inspection_demo_launch.py +++ b/nav2_simple_commander/launch/inspection_demo_launch.py @@ -17,16 +17,9 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/nav2_simple_commander/launch/nav_through_poses_example_launch.py b/nav2_simple_commander/launch/nav_through_poses_example_launch.py index 593ee1e8816..696bba3a6cc 100644 --- a/nav2_simple_commander/launch/nav_through_poses_example_launch.py +++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py @@ -17,16 +17,9 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/nav2_simple_commander/launch/nav_to_pose_example_launch.py b/nav2_simple_commander/launch/nav_to_pose_example_launch.py index c3e22fb08d3..6c49217d570 100644 --- a/nav2_simple_commander/launch/nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py @@ -17,16 +17,9 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py index f4bd8346013..33db3c070c1 100644 --- a/nav2_simple_commander/launch/picking_demo_launch.py +++ b/nav2_simple_commander/launch/picking_demo_launch.py @@ -17,16 +17,9 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py index c7afd73f31c..554a0386400 100644 --- a/nav2_simple_commander/launch/recoveries_example_launch.py +++ b/nav2_simple_commander/launch/recoveries_example_launch.py @@ -17,16 +17,9 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py index 240f430b236..23c41cdae28 100644 --- a/nav2_simple_commander/launch/security_demo_launch.py +++ b/nav2_simple_commander/launch/security_demo_launch.py @@ -17,16 +17,9 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py index 10f36ce7922..9bdcfbcb07d 100644 --- a/nav2_simple_commander/launch/waypoint_follower_example_launch.py +++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py @@ -17,16 +17,9 @@ import tempfile from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, -) +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index 6f2951d012a..8559609e84c 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -15,10 +15,15 @@ from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult - import rclpy from rclpy.duration import Duration +""" +Basic item picking demo. In this demonstration, the expectation +is that there is a person at the item shelf to put the item on the robot +and at the pallet jack to remove it +(probably with some kind of button for 'got item, robot go do next task'). +""" # Shelf positions for picking shelf_positions = { @@ -35,13 +40,6 @@ 'frieght_bay_3': [-6.0, -5.0, 3.14], } -""" -Basic item picking demo. In this demonstration, the expectation -is that there is a person at the item shelf to put the item on the robot -and at the pallet jack to remove it -(probably with some kind of button for 'got item, robot go do next task'). -""" - def main(): # Received virtual request for picking item at Shelf A and bring to diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py index 112fb30a1bf..7675ce199e2 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py @@ -18,7 +18,6 @@ import rclpy from rclpy.duration import Duration - """ Basic recoveries demo. In this demonstration, the robot navigates to a dead-end where recoveries such as backup and spin are used @@ -64,7 +63,7 @@ def main(): ) # Robot hit a dead end, back it up - print('Robot hit a dead end (lets pretend), backing up...') + print("Robot hit a dead end (let's pretend), backing up...") navigator.backup(backup_dist=0.5, backup_speed=0.1) i = 0 diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index 1c483363e75..d0cf0af5bf7 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -17,11 +17,9 @@ from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult - import rclpy from rclpy.duration import Duration - """ Basic security route patrol demo. In this demonstration, the expectation is that there are security cameras mounted on the robots recording or being diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py index 15769a05cfb..8f825c42693 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -17,7 +17,6 @@ from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy - """ Basic navigation demo to follow a given path after smoothing """ diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 035465caf09..ed277bb2e51 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -19,30 +19,19 @@ from action_msgs.msg import GoalStatus from builtin_interfaces.msg import Duration -from geometry_msgs.msg import Point -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState -from nav2_msgs.action import AssistedTeleop, BackUp, DriveOnHeading, Spin -from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose -from nav2_msgs.action import ( - DockRobot, - FollowGPSWaypoints, - FollowPath, - FollowWaypoints, - NavigateThroughPoses, - NavigateToPose, - UndockRobot, -) -from nav2_msgs.action import SmoothPath -from nav2_msgs.srv import ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap -from nav2_msgs.srv import GetCostmap, LoadMap, ManageLifecycleNodes +from nav2_msgs.action import (AssistedTeleop, BackUp, ComputePathThroughPoses, ComputePathToPose, + DockRobot, DriveOnHeading, FollowGPSWaypoints, FollowPath, + FollowWaypoints, NavigateThroughPoses, NavigateToPose, SmoothPath, + Spin, UndockRobot) +from nav2_msgs.srv import (ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap, + GetCostmap, LoadMap, ManageLifecycleNodes) import rclpy from rclpy.action import ActionClient from rclpy.duration import Duration as rclpyDuration from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy -from rclpy.qos import QoSProfile, QoSReliabilityPolicy +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class TaskResult(Enum): diff --git a/nav2_simple_commander/pytest.ini b/nav2_simple_commander/pytest.ini index 50d6d012576..fe55d2ed64b 100644 --- a/nav2_simple_commander/pytest.ini +++ b/nav2_simple_commander/pytest.ini @@ -1,2 +1,2 @@ [pytest] -junit_family=xunit2 \ No newline at end of file +junit_family=xunit2 diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index 4b49f89a450..74a15c63541 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -3,7 +3,6 @@ from setuptools import setup - package_name = 'nav2_simple_commander' setup( diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 40d68b5f4bc..98a960cb017 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -207,7 +207,7 @@ Seeing the figures below, you'll see an attempt to plan into a "U" shaped region By simply increasing the footprint (a bit hackier, the best solution is to edit the map to make this area impassable), then that gap is now properly blocked as un-navigable. In the second figure, you can see that the heuristics influence the expansion down a navigable route and is able to find a path in less than 10,000 iterations (or about 110ms). It is easy now! -As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isn't actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. +As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you don't want your robot actually using B) probably isn't actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. ![](media/A.png) ![](media/B.png) diff --git a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h index 5aa4d530753..c6b0fd6009a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h +++ b/nav2_smac_planner/include/nav2_smac_planner/thirdparty/robin_hood.h @@ -575,7 +575,7 @@ struct nothrow { struct is_transparent_tag {}; // A custom pair implementation is used in the map because std::pair is not is_trivially_copyable, -// which means it would not be allowed to be used in std::memcpy. This struct is copyable, which is +// which means it would not be allowed to be used in std::memcpy. This struct is copiable, which is // also tested. template struct pair { diff --git a/nav2_smac_planner/lattice_primitives/README.md b/nav2_smac_planner/lattice_primitives/README.md index cfdebe36aa4..57e134b1797 100644 --- a/nav2_smac_planner/lattice_primitives/README.md +++ b/nav2_smac_planner/lattice_primitives/README.md @@ -37,7 +37,7 @@ The directory to save the visualizations can be specified by passing in a path w Note: None of these parameters have defaults. They all must be specified through the [config.json](config.json) file. **motion_model** (string) - + The type of motion model used. Accepted values: - `ackermann`: Only forward and reversing trajectories - `diff`: Forward moving trajectories + rotation in place by a single angular bin @@ -119,11 +119,11 @@ A list of dictionaries where each dictionary represents an individual motion pri This section describes how the various portions of the generation algorithm works. ### Angle Discretization -Dividing a full turn into uniform angular headings presents several problems. The biggest problem is that it will create angles for which a straight trajectory does not land nicely on an endpoint that aligns with the grid. Instead we discretize the angles in a way that ensures straight paths will land on endpoints aligned with the grid. +Dividing a full turn into uniform angular headings presents several problems. The biggest problem is that it will create angles for which a straight trajectory does not land nicely on an endpoint that aligns with the grid. Instead we discretize the angles in a way that ensures straight paths will land on endpoints aligned with the grid. ![ ](docs/angle_discretization.png) -The image shows how the angular headings are generated. The same idea can be extended to a higher number of headings. As a result, the number of headings parameter is restricted to multiples of 8. +The image shows how the angular headings are generated. The same idea can be extended to a higher number of headings. As a result, the number of headings parameter is restricted to multiples of 8. ### Trajectory Generator 1. Create two lines. Line 1 passes through start point with angle of start angle, and line 2 passes through the end point with angle of end angle @@ -165,4 +165,4 @@ The lattice generator is generally based on the generation of the control set as 6. Steps 1-5 are repeated for all possible start angles between 0 and 90. -7. The resulting control set will only contain trajectories in quadrant 1. To get the final control set we exploit symmetry across the axess and flip the trajectories in different ways. \ No newline at end of file +7. The resulting control set will only contain trajectories in quadrant 1. To get the final control set we exploit symmetry across the axess and flip the trajectories in different ways. diff --git a/nav2_smac_planner/lattice_primitives/config.json b/nav2_smac_planner/lattice_primitives/config.json index 27671c337d1..12a8470bdb3 100644 --- a/nav2_smac_planner/lattice_primitives/config.json +++ b/nav2_smac_planner/lattice_primitives/config.json @@ -4,4 +4,4 @@ "grid_resolution": 0.05, "stopping_threshold": 5, "num_of_headings": 16 -} \ No newline at end of file +} diff --git a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py index 9ec50287193..a64d7bea283 100644 --- a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py +++ b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py @@ -21,11 +21,9 @@ import constants from lattice_generator import LatticeGenerator - import matplotlib.pyplot as plt import numpy as np - logging.basicConfig(level=logging.INFO) logger = logging.getLogger(__name__) diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py index a2b0340cb9b..f8356ab9322 100644 --- a/nav2_smac_planner/lattice_primitives/lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -16,13 +16,9 @@ from enum import Enum from helper import angle_difference, interpolate_yaws - import numpy as np - from rtree import index - from trajectory import Path, Trajectory, TrajectoryParameters - from trajectory_generator import TrajectoryGenerator diff --git a/nav2_smac_planner/lattice_primitives/requirements.txt b/nav2_smac_planner/lattice_primitives/requirements.txt index c76f9d4ba62..7ae56b050f2 100644 --- a/nav2_smac_planner/lattice_primitives/requirements.txt +++ b/nav2_smac_planner/lattice_primitives/requirements.txt @@ -1,3 +1,3 @@ numpy>=1.17.4 matplotlib>=3.1.2 -Rtree>=0.9.7 \ No newline at end of file +Rtree>=0.9.7 diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json index 0df406dfdc1..57e1e6ae423 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json @@ -3929,4 +3929,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json index 984bbb62d9e..e1c5ee5ec05 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/diff/output.json @@ -4873,4 +4873,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json index 8ea504f6c38..19a62926fca 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/0.5m_turning_radius/omni/output.json @@ -5857,4 +5857,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json index 646513c0aa0..3cf19c742fc 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/ackermann/output.json @@ -3997,4 +3997,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json index 9146355201c..a22f58074dc 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/diff/output.json @@ -4941,4 +4941,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json index 95f8dc2d85f..95a4b11bdd6 100644 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/omni/output.json @@ -6445,4 +6445,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py index fb6fa2c7b42..5263167ca0e 100644 --- a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py +++ b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py @@ -26,7 +26,6 @@ from pathlib import Path import matplotlib.pyplot as plt - import numpy as np diff --git a/nav2_smac_planner/lattice_primitives/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py index 0dcb6edc07e..4b2ed6537b2 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory.py +++ b/nav2_smac_planner/lattice_primitives/trajectory.py @@ -15,7 +15,6 @@ from dataclasses import dataclass from helper import angle_difference, normalize_angle - import numpy as np diff --git a/nav2_smac_planner/lattice_primitives/trajectory_generator.py b/nav2_smac_planner/lattice_primitives/trajectory_generator.py index 5fa7d638962..8efe06149a3 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py @@ -16,7 +16,6 @@ from typing import Tuple, Union import numpy as np - from trajectory import Path, Trajectory, TrajectoryParameters logger = logging.getLogger(__name__) diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index 4372c10127d..be417fa1be7 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -184,7 +184,7 @@ bool Smoother::smoothImpl( last_path = new_path; } - // Lets do additional refinement, it shouldn't take more than a couple milliseconds + // Let's do additional refinement, it shouldn't take more than a couple milliseconds // but really puts the path quality over the top. if (do_refinement_ && refinement_ctr_ < refinement_num_) { refinement_ctr_++; diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index 9431776fc15..949b48a5305 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -144,7 +144,7 @@ TEST(SmacTest, test_smac_lattice_reconfigure) } catch (...) { } - // So instead, lets call manually on a change + // So instead, let's call manually on a change std::vector parameters; parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI"))); EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error); diff --git a/nav2_smoother/README.md b/nav2_smoother/README.md index 04498a0034d..a62abd4b624 100644 --- a/nav2_smoother/README.md +++ b/nav2_smoother/README.md @@ -4,7 +4,7 @@ The Nav2 smoother is a Task Server in Nav2 that implements the `nav2_behavior_tr A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface is responsible for improving path smoothness and/or quality, typically given an unsmoothed path from the planner module in `nav2_planner`. It loads a map of potential smoother plugins to do the path smoothing in different user-defined situations. -See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available smoother plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available smoother plugins. See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions. diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index 2da176b12f3..20b296a9f42 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -151,7 +151,7 @@ bool SavitzkyGolaySmoother::smoothImpl( const auto initial_path_poses = path.poses; applyFilterOverAxes(path.poses, initial_path_poses); - // Lets do additional refinement, it shouldn't take more than a couple milliseconds + // Let's do additional refinement, it shouldn't take more than a couple milliseconds if (do_refinement_) { for (int i = 0; i < refinement_num_; i++) { const auto reined_initial_path_poses = path.poses; diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 5b4c2fff6e0..d1957e38e35 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -184,7 +184,7 @@ void SimpleSmoother::smoothImpl( last_path = new_path; } - // Lets do additional refinement, it shouldn't take more than a couple milliseconds + // Let's do additional refinement, it shouldn't take more than a couple milliseconds // but really puts the path quality over the top. if (do_refinement_ && refinement_ctr_ < refinement_num_) { refinement_ctr_++; diff --git a/nav2_system_tests/maps/map_circular.yaml b/nav2_system_tests/maps/map_circular.yaml index 3a479308a73..4e5acec88f1 100644 --- a/nav2_system_tests/maps/map_circular.yaml +++ b/nav2_system_tests/maps/map_circular.yaml @@ -5,4 +5,3 @@ negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 map_type: occupancy - diff --git a/nav2_system_tests/models/cardboard_box.sdf b/nav2_system_tests/models/cardboard_box.sdf index 97a40603c21..28d5a3ea1cb 100644 --- a/nav2_system_tests/models/cardboard_box.sdf +++ b/nav2_system_tests/models/cardboard_box.sdf @@ -6,4 +6,4 @@ - \ No newline at end of file + diff --git a/nav2_system_tests/scripts/ctest_loop.bash b/nav2_system_tests/scripts/ctest_loop.bash index 29f0e278289..a074b698d0a 100755 --- a/nav2_system_tests/scripts/ctest_loop.bash +++ b/nav2_system_tests/scripts/ctest_loop.bash @@ -47,4 +47,3 @@ for ((i=1; i<=$loopcount; i++)) done echo $failcount " FAILURES / " $loopcount echo $failcount " FAILURES / " $loopcount >> $outfile - diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py index ece57eefed7..3a9556278bc 100755 --- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -18,19 +18,12 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml diff --git a/nav2_system_tests/src/behaviors/backup/backup_tester.py b/nav2_system_tests/src/behaviors/backup/backup_tester.py index ad6f724bf8f..711441bd4cb 100755 --- a/nav2_system_tests/src/behaviors/backup/backup_tester.py +++ b/nav2_system_tests/src/behaviors/backup/backup_tester.py @@ -21,14 +21,11 @@ from nav2_msgs.action import BackUp from nav2_msgs.msg import Costmap from nav2_msgs.srv import ManageLifecycleNodes - import rclpy - from rclpy.action import ActionClient from rclpy.duration import Duration from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.qos import QoSProfile +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class BackupTest(Node): diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py index c8696a6ad4f..b7ca717286d 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior.launch.py @@ -18,15 +18,9 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py b/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py index 2f057620bac..852f3bee6a9 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_tester.py @@ -21,14 +21,11 @@ from nav2_msgs.action import DriveOnHeading from nav2_msgs.msg import Costmap from nav2_msgs.srv import ManageLifecycleNodes - import rclpy - from rclpy.action import ActionClient from rclpy.duration import Duration from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.qos import QoSProfile +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class DriveTest(Node): diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py index 232e5a8c093..2b308537f1d 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior.launch.py @@ -18,15 +18,9 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node diff --git a/nav2_system_tests/src/behaviors/spin/spin_tester.py b/nav2_system_tests/src/behaviors/spin/spin_tester.py index b2d6941987f..917df80aeb9 100755 --- a/nav2_system_tests/src/behaviors/spin/spin_tester.py +++ b/nav2_system_tests/src/behaviors/spin/spin_tester.py @@ -21,14 +21,11 @@ from nav2_msgs.action import Spin from nav2_msgs.msg import Costmap from nav2_msgs.srv import ManageLifecycleNodes - import rclpy - from rclpy.action import ActionClient from rclpy.duration import Duration from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.qos import QoSProfile +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class SpinTest(Node): diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py index e1f7c991b1e..38c205c6508 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior.launch.py @@ -18,15 +18,9 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index b13024b2d86..da70e9b04da 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -18,19 +18,12 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml from nav2_simple_commander.utils import kill_os_processes diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index bb43001b5b9..ff554956e8f 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -20,20 +20,13 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 281aed36250..31c55ccc9db 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -20,20 +20,13 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py index 3b3f723095b..c978e570b13 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -23,21 +23,16 @@ from typing import Optional from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Pose -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState from nav2_msgs.action import NavigateToPose from nav2_msgs.msg import SpeedLimit from nav2_msgs.srv import ManageLifecycleNodes -from nav_msgs.msg import OccupancyGrid -from nav_msgs.msg import Path - +from nav_msgs.msg import OccupancyGrid, Path import rclpy from rclpy.action import ActionClient from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.qos import QoSProfile +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy from sensor_msgs.msg import PointCloud2 diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index 80ff2cd7d10..24ce673726a 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -18,12 +18,7 @@ import time from geometry_msgs.msg import PoseStamped -from nav2_msgs.action import ( - ComputePathThroughPoses, - ComputePathToPose, - FollowPath, - SmoothPath, -) +from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose, FollowPath, SmoothPath from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult from nav_msgs.msg import Path import rclpy diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py index fd8072b43c6..3ca61767624 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes_launch.py +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -16,10 +16,8 @@ import os import sys -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ExecuteProcess -from launch.actions import GroupAction +from launch import LaunchDescription, LaunchService +from launch.actions import ExecuteProcess, GroupAction from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService diff --git a/nav2_system_tests/src/gps_navigation/CMakeLists.txt b/nav2_system_tests/src/gps_navigation/CMakeLists.txt index 742d004f115..5473b38b00e 100644 --- a/nav2_system_tests/src/gps_navigation/CMakeLists.txt +++ b/nav2_system_tests/src/gps_navigation/CMakeLists.txt @@ -6,4 +6,3 @@ ament_add_test(test_gps_waypoint_follower ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} ) - diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml index e46ce5d1a8d..c853d75f379 100644 --- a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml @@ -124,4 +124,4 @@ navsat_transform: broadcast_cartesian_transform: true publish_filtered_gps: true use_odometry_yaw: true - wait_for_datum: false + wait_for_datum: false diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index 3e0042b6638..19f3b0e0a2d 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -194,7 +194,7 @@ global_costmap: robot_radius: 0.22 resolution: 0.1 # When using GPS navigation you will potentially traverse huge environments which are not practical to - # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to + # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174 rolling_window: True width: 50 @@ -384,4 +384,4 @@ docking_server: k_phi: 3.0 k_delta: 2.0 v_linear_min: 0.15 - v_linear_max: 0.15 \ No newline at end of file + v_linear_max: 0.15 diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index cb11b2c36d7..ef42a91e4b7 100755 --- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -18,19 +18,12 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index 46572469c98..6829dc6a494 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -21,9 +21,7 @@ from nav2_msgs.action import ComputePathToPose, FollowGPSWaypoints from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters - import rclpy - from rclpy.action import ActionClient from rclpy.node import Node from rclpy.parameter import Parameter diff --git a/nav2_system_tests/src/localization/README.md b/nav2_system_tests/src/localization/README.md index a5e20c395f6..1bd74e9957c 100644 --- a/nav2_system_tests/src/localization/README.md +++ b/nav2_system_tests/src/localization/README.md @@ -9,7 +9,7 @@ First, build the package ``` colcon build --symlink-install ``` -After building, from /build/nav2_system_tests directory run: +After building, from /build/nav2_system_tests directory run: ``` ctest -V -R test_localization ``` diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index 619b989510d..1520ac09bbe 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -19,10 +19,8 @@ import sys from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import AppendEnvironmentVariable, ExecuteProcess -from launch.actions import IncludeLaunchDescription +from launch import LaunchDescription, LaunchService +from launch.actions import AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_ros.actions from launch_testing.legacy import LaunchTestService diff --git a/nav2_system_tests/src/planning/README.md b/nav2_system_tests/src/planning/README.md index 6e95d215ba3..0dcfb157d13 100644 --- a/nav2_system_tests/src/planning/README.md +++ b/nav2_system_tests/src/planning/README.md @@ -12,4 +12,4 @@ Below is an example of the output from randomized testing. Blue spheres represen *Note: Currently robot size is 1x1 cells, no obstacle inflation is done on the costmap* -*Note: The Navfn algorithm sometimes fails to generate a path as you can see from the 'orphan' spheres.* \ No newline at end of file +*Note: The Navfn algorithm sometimes fails to generate a path as you can see from the 'orphan' spheres.* diff --git a/nav2_system_tests/src/planning/test_planner_costmaps_launch.py b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py index 09bd3d34eee..fd8cadb4f9d 100755 --- a/nav2_system_tests/src/planning/test_planner_costmaps_launch.py +++ b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py @@ -17,10 +17,8 @@ import os import sys -from launch import LaunchDescription -from launch import LaunchService +from launch import LaunchDescription, LaunchService from launch.actions import ExecuteProcess - from launch_testing.legacy import LaunchTestService diff --git a/nav2_system_tests/src/planning/test_planner_random_launch.py b/nav2_system_tests/src/planning/test_planner_random_launch.py index a7747e513ab..105063110f8 100755 --- a/nav2_system_tests/src/planning/test_planner_random_launch.py +++ b/nav2_system_tests/src/planning/test_planner_random_launch.py @@ -17,10 +17,8 @@ import os import sys -from launch import LaunchDescription -from launch import LaunchService +from launch import LaunchDescription, LaunchService from launch.actions import ExecuteProcess - from launch_testing.legacy import LaunchTestService diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml index 9d71b2912a5..fba3a67a5b6 100644 --- a/nav2_system_tests/src/system/nav2_system_params.yaml +++ b/nav2_system_tests/src/system/nav2_system_params.yaml @@ -375,4 +375,3 @@ docking_server: k_delta: 2.0 v_linear_min: 0.15 v_linear_max: 0.15 - diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py index 7ff35af2aa8..f24729cd79d 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_error_msg_node.py @@ -17,26 +17,19 @@ import argparse import sys import time - from typing import Optional from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Pose -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState from nav2_msgs.action import NavigateThroughPoses from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters - import rclpy - from rclpy.action.client import ActionClient from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.qos import QoSProfile - +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy from std_msgs.msg import String diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py index d3a2a8251f4..6109223c82a 100755 --- a/nav2_system_tests/src/system/nav_through_poses_tester_node.py +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -17,23 +17,17 @@ import argparse import sys import time - from typing import Optional from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Pose -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState from nav2_msgs.action import NavigateThroughPoses from nav2_msgs.srv import ManageLifecycleNodes - import rclpy - from rclpy.action import ActionClient from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.qos import QoSProfile +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class NavTester(Node): diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 0b2c777b860..b931272bdf0 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -18,23 +18,17 @@ import math import sys import time - from typing import Optional from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Pose -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState from nav2_msgs.action import NavigateToPose from nav2_msgs.srv import ManageLifecycleNodes - import rclpy - from rclpy.action import ActionClient from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.qos import QoSProfile +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class NavTester(Node): diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index 612c050a043..ed8bdc00e3b 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -18,18 +18,12 @@ import sys from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription, LaunchService -from launch.actions import ( - ExecuteProcess, - GroupAction, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch.actions import (ExecuteProcess, GroupAction, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import TextSubstitution from launch_ros.actions import Node, PushROSNamespace - from launch_testing.legacy import LaunchTestService diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index fb4d3625880..ce604fc454a 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -20,20 +20,13 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py index b31f19bdd61..b6fb8104ec4 100755 --- a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py +++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py @@ -20,20 +20,13 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index 31e1b8af411..a55b0540575 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -20,19 +20,13 @@ import sys from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 0cbc76533e2..3e7f918e174 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -20,20 +20,13 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index 9cea2f6118d..9f296ff0d46 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -21,19 +21,14 @@ from typing import Optional from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Pose -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState from nav2_msgs.action import NavigateToPose from nav2_msgs.srv import ManageLifecycleNodes - import rclpy - from rclpy.action import ActionClient from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.qos import QoSProfile +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class NavTester(Node): diff --git a/nav2_system_tests/src/updown/README.md b/nav2_system_tests/src/updown/README.md index 43564543e1e..24a3b35bb17 100644 --- a/nav2_system_tests/src/updown/README.md +++ b/nav2_system_tests/src/updown/README.md @@ -1,6 +1,6 @@ # Nav2 Updown Test -This is a 'top level' system test which tests the lifecycle bringup and shutdown of the system. +This is a 'top level' system test which tests the lifecycle bringup and shutdown of the system. ## To run the test ``` diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py index 81e0cc1fda0..8a70be4e727 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -14,9 +14,7 @@ import os -from ament_index_python.packages import get_package_prefix -from ament_index_python.packages import get_package_share_directory - +from ament_index_python.packages import get_package_prefix, get_package_share_directory import launch.actions from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node diff --git a/nav2_system_tests/src/updown/test_updown_reliability b/nav2_system_tests/src/updown/test_updown_reliability index 18388443d89..72bd182bc31 100755 --- a/nav2_system_tests/src/updown/test_updown_reliability +++ b/nav2_system_tests/src/updown/test_updown_reliability @@ -3,7 +3,7 @@ for i in `seq 1 1000`; do echo "======= START OF RUN: $i =========" - # Start with a new ros2 daemon + # Start with a new ros2 daemon ros2 daemon stop # There shouldn't be any nodes in the list diff --git a/nav2_system_tests/src/waypoint_follower/test_case_launch.py b/nav2_system_tests/src/waypoint_follower/test_case_launch.py index ea7f581eab3..eb581b373ad 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_launch.py +++ b/nav2_system_tests/src/waypoint_follower/test_case_launch.py @@ -18,20 +18,13 @@ import sys from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch import LaunchService -from launch.actions import ( - AppendEnvironmentVariable, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable, -) +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService - from nav2_common.launch import RewrittenYaml diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 85dba0be401..87c7b332c94 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -21,13 +21,11 @@ from nav2_msgs.action import ComputePathToPose, FollowWaypoints from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters - import rclpy from rclpy.action import ActionClient from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy -from rclpy.qos import QoSProfile +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class WaypointFollowerTest(Node): diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 0ac1fa9b7aa..a88045d992f 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -3,7 +3,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-thetastar.html) for additional parameter descriptions. -## Features +## Features - The planner uses A\* search along with line of sight (LOS) checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* - As it also considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases) - Uses the costs from the costmap to penalise high cost regions @@ -51,7 +51,7 @@ w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2` ## Parameters The parameters of the planner are : - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 -- ` .w_euc_cost ` : weight applied on the length of the path. +- ` .w_euc_cost ` : weight applied on the length of the path. - ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs. Below are the default values of the parameters : ``` diff --git a/nav2_util/README.md b/nav2_util/README.md index adf71e61c2e..1c25071190e 100644 --- a/nav2_util/README.md +++ b/nav2_util/README.md @@ -13,9 +13,9 @@ The long-term aim is for these utilities to find more permanent homes in other p ## Twist Publisher and Twist Subscriber for commanded velocities -### Background +### Background -The Twist Publisher and Twist Subscriber are utility classes to assist NAV2 transition from +The Twist Publisher and Twist Subscriber are utility classes to assist NAV2 transition from [Twist](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/Twist.msg) to [TwistStamped](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/TwistStamped.msg). Details on the migration are found in [#1594](https://github.com/ros-planning/navigation2/issues/1594). @@ -28,4 +28,4 @@ The utility has the following effect: Every node in `nav2` that subscribes or publishes velocity commands with `Twist` now supports this optional behavior. The behavior up through ROS 2 Iron is preserved - using `Twist`. In a future ROS 2 version, when enough of the -ROS ecosystem has moved to `TwistStamped`, the default may change. +ROS ecosystem has moved to `TwistStamped`, the default may change. diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 0ae2ffe0c0e..1482fbe0fa1 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -114,7 +114,7 @@ class FibonacciServerNode : public rclcpp::Node return; } - // Check if we've gotten an new goal, pre-empting the current one + // Check if we've gotten an new goal, preempting the current one if (do_premptions_ && action_server_->is_preempt_requested()) { action_server_->accept_pending_goal(); goto preempted; diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index 579b3a6737a..cd5c0548653 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -3,7 +3,7 @@ The ``nav2_velocity_smoother`` is a package containing a lifecycle-component node for smoothing velocities sent by Nav2 to robot controllers. The aim of this package is to implement velocity, acceleration, and deadband smoothing from Nav2 to reduce wear-and-tear on robot motors and hardware controllers by smoothing out the accelerations/jerky movements that might be present with some local trajectory planners' control efforts. -It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some interpretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). +It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some interpretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions. @@ -87,4 +87,4 @@ The minimum and maximum velocities for rotation (e.g. ``Vw``) represent left and Deadband velocities are minimum thresholds, below which we set its value to `0`. This can be useful when your robot's breaking torque from stand still is non-trivial so sending very small values will pull high amounts of current. -The `VelocitySmoother` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). \ No newline at end of file +The `VelocitySmoother` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). diff --git a/nav2_voxel_grid/README.md b/nav2_voxel_grid/README.md index a10d519e2f0..9aec987ce79 100644 --- a/nav2_voxel_grid/README.md +++ b/nav2_voxel_grid/README.md @@ -1,9 +1,9 @@ # Nav2 Voxel Grid -The `nav2_voxel_grid` package contains the VoxelGrid used by the `Voxel Layer` inside of `nav2_costmap_2d`. The voxel grid itself is simply a 2D char pointer array of the map size with bit locations corresponding to voxel values (free, unknown, occupied , etc). +The `nav2_voxel_grid` package contains the VoxelGrid used by the `Voxel Layer` inside of `nav2_costmap_2d`. The voxel grid itself is simply a 2D char pointer array of the map size with bit locations corresponding to voxel values (free, unknown, occupied , etc). -It is branched out as a separate package for use in other applications where a dense voxel grid representation may be useful. It also contains implementations of 3D raycasting. +It is branched out as a separate package for use in other applications where a dense voxel grid representation may be useful. It also contains implementations of 3D raycasting. ## ROS1 Comparison -This package is a direct port to ROS2 for use in the voxel layer. +This package is a direct port to ROS2 for use in the voxel layer. diff --git a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp index cce36e7fc48..6219ff41c09 100644 --- a/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp +++ b/nav2_voxel_grid/test/voxel_grid_bresenham_3d.cpp @@ -69,7 +69,7 @@ TEST(voxel_grid, bresenham3DBoundariesCheck) nav2_voxel_grid::VoxelGrid vg(sz_x, sz_y, sz_z); TestVoxel tv(vg.getData(), sz_x, sz_y); - // Initial point - some assymetrically standing point in order to cover most corner cases + // Initial point - some asymmetrically standing point in order to cover most corner cases const double x0 = 2.2; const double y0 = 3.8; const double z0 = 0.4; diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index 822314fbf39..c906eba2ff9 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -33,10 +33,10 @@ Neither is better than the other, it highly depends on the tasks your robot(s) a `nav2_waypoint_follower` provides an action server named `FollowGPSWaypoints` which accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower` itself. `robot_localization`'s `navsat_transform_node` provides a service `fromLL`, which is used to convert pure GPS coordinates(longitude, latitude, alitude) -to cartesian coordinates in map frame(x,y), then the existent action named `FollowWaypoints` from `nav2_waypoint_follower` is used to get robot go through each converted waypoints. +to cartesian coordinates in map frame(x,y), then the existent action named `FollowWaypoints` from `nav2_waypoint_follower` is used to get robot go through each converted waypoints. The action msg definition for GPS waypoint following can be found [here](../nav2_msgs/action/FollowGPSWaypoints.action). -In a common use case, an client node can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`. +In a common use case, an client node can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`. For instance, ```cpp @@ -45,4 +45,4 @@ rclcpp_action::Client::SharedPtr gps_waypoint_follower_action_client_; gps_waypoint_follower_action_client_ = rclcpp_action::create_client(this, "follow_gps_waypoints"); ``` -All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor. \ No newline at end of file +All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor. diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp index 2a8537007e2..8be79fcb8e0 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp @@ -28,7 +28,7 @@ namespace nav2_waypoint_follower { /** - * @brief Simple plugin based on WaypointTaskExecutor, lets robot to wait for a + * @brief Simple plugin based on WaypointTaskExecutor, let's robot to wait for a * user input at waypoint arrival. */ class InputAtWaypoint : public nav2_core::WaypointTaskExecutor diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp index 69498aa267a..c6e6ab09fd0 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp @@ -26,7 +26,7 @@ namespace nav2_waypoint_follower { /** - * @brief Simple plugin based on WaypointTaskExecutor, lets robot to sleep for a + * @brief Simple plugin based on WaypointTaskExecutor, let's robot to sleep for a * specified amount of time at waypoint arrival. You can reference this class to define * your own task and rewrite the body for it. * diff --git a/nav2_waypoint_follower/plugins.xml b/nav2_waypoint_follower/plugins.xml index ab8c01f050f..fe9f497c175 100644 --- a/nav2_waypoint_follower/plugins.xml +++ b/nav2_waypoint_follower/plugins.xml @@ -1,18 +1,18 @@ - Lets robot sleep for a specified amount of time at waypoint arrival + Let's robot sleep for a specified amount of time at waypoint arrival - Run-time plugin that takes photos at waypoint arrivals when using waypoint follower node. + Run-time plugin that takes photos at waypoint arrivals when using waypoint follower node. Saves the taken photos to specified directory. - Lets robot wait for input at waypoint arrival + Let's robot wait for input at waypoint arrival diff --git a/tools/.codespell_ignore_words b/tools/.codespell_ignore_words new file mode 100644 index 00000000000..41230c8f09e --- /dev/null +++ b/tools/.codespell_ignore_words @@ -0,0 +1,15 @@ +master +uint +jupyter +blacklist +whitelist +stdio +deque +thirdparty +dur +segway +ue +hist +nav2 +ot +ws diff --git a/tools/pyproject.toml b/tools/pyproject.toml new file mode 100644 index 00000000000..e0001ef7bff --- /dev/null +++ b/tools/pyproject.toml @@ -0,0 +1,14 @@ +[tool.codespell] +builtin = "clear,rare,informal,usage,code,names" +check-filenames = true +check-hidden = true +ignore-words = "tools/.codespell_ignore_words" +interactive = 0 +quiet = 34 +skip="*.pgm,./build/*,./install/*,./log*,./.venv/*,./.git*,*.toml" +uri-ignore-words-list = "segue" +write-changes = false +[tool.isort] +profile = "google" +force_single_line = false +line_length = 99 diff --git a/tools/skip_keys.txt b/tools/skip_keys.txt index 8285089c341..262f3239dac 100644 --- a/tools/skip_keys.txt +++ b/tools/skip_keys.txt @@ -4,4 +4,4 @@ fastrtps libopensplice69 rti-connext-dds-5.3.1 slam_toolbox -urdfdom_headers \ No newline at end of file +urdfdom_headers diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md index d1f870094b3..0be09114eff 100644 --- a/tools/smoother_benchmarking/README.md +++ b/tools/smoother_benchmarking/README.md @@ -52,22 +52,22 @@ index c7a90bcb..6f93edbf 100644 +++ b/nav2_planner/src/planner_server.cpp @@ -381,7 +381,10 @@ void PlannerServer::computePlanThroughPoses() } - + // Get plan from start -> goal + auto planning_start = steady_clock_.now(); nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + auto planning_duration = steady_clock_.now() - planning_start; + result->planning_time = planning_duration; - + if (!validatePath(curr_goal, curr_path, goal->planner_id)) { throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); @@ -398,7 +401,7 @@ void PlannerServer::computePlanThroughPoses() publishPlan(result->path); - + auto cycle_duration = steady_clock_.now() - start_time; - result->planning_time = cycle_duration; + // result->planning_time = cycle_duration; - + if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { RCLCPP_WARN( diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp @@ -75,13 +75,13 @@ index ada1f664..610e9512 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -253,8 +253,6 @@ bool SmootherServer::findSmootherId( - + void SmootherServer::smoothPlan() { - auto start_time = steady_clock_.now(); - RCLCPP_INFO(get_logger(), "Received a path to smooth."); - + auto result = std::make_shared(); @@ -271,6 +269,8 @@ void SmootherServer::smoothPlan() // Perform smoothing diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py index fa01be9754f..9c8b716c214 100644 --- a/tools/smoother_benchmarking/metrics.py +++ b/tools/smoother_benchmarking/metrics.py @@ -25,7 +25,6 @@ import rclpy from transforms3d.euler import euler2quat - # Note: Map origin is assumed to be (0,0) diff --git a/tools/underlay.repos b/tools/underlay.repos index e443ce12489..8cfb9a2a691 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -18,11 +18,11 @@ repositories: # ros/diagnostics: # type: git # url: https://github.com/ros/diagnostics.git - # version: ros2-devel + # version: ros2-devel # ros/geographic_info: # type: git # url: https://github.com/ros-geographic-info/geographic_info.git - # version: ros2 + # version: ros2 # ompl/ompl: # type: git # url: https://github.com/ompl/ompl.git @@ -39,4 +39,3 @@ repositories: type: git url: https://github.com/ros2/common_interfaces.git version: rolling - diff --git a/tools/update_readme_table.py b/tools/update_readme_table.py index b743a988842..b85f942df9c 100755 --- a/tools/update_readme_table.py +++ b/tools/update_readme_table.py @@ -61,11 +61,17 @@ # Set which distributions you care about Distros = ['humble', 'iron', 'jazzy'] + def getSrcPath(package, prefix, OS): return f'https://build.ros2.org/job/{prefix}src_u{OS[0]}__{package}__ubuntu_{OS}__source/' + def getBinPath(package, prefix, OS): - return f'https://build.ros2.org/job/{prefix}bin_u{OS[0]}64__{package}__ubuntu_{OS}_amd64__binary/' + return ( + f'https://build.ros2.org/job/{prefix}bin_u{OS[0]}64__{package}__ubuntu_{OS}_' + 'amd64__binary/' + ) + def createPreamble(Distros): table = '| Package | ' @@ -77,6 +83,7 @@ def createPreamble(Distros): table += ' :---: | :---: |' return table + def main(): header = createPreamble(Distros) @@ -98,13 +105,15 @@ def main(): entry += f'[![Build Status]({binURL}badge/icon)]({binURL}) | ' entry += '\n' body += entry - + # Special case for Opennav Docking for directory structure of Nav2 body = body.replace('| opennav_docking |', '| nav2_docking |') # Special case for reducing the label length - body = body.replace('| nav2_regulated_pure_pursuit_controller |', '| nav2_regulated_pure_pursuit |') - + body = body.replace('| nav2_regulated_pure_pursuit_controller |', + '| nav2_regulated_pure_pursuit |') + print(header + '\n' + body) + if __name__ == '__main__': main() From d9cc4ba320e327c32ffaa52aa1fdca3fc4af1d2f Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Tue, 25 Mar 2025 03:28:20 +0900 Subject: [PATCH 02/70] Fix path comparison to avoid unnecessary updates (#5009) Signed-off-by: Tatsuro Sakaguchi --- nav2_behavior_tree/plugins/action/follow_path_action.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 081f36caee0..875c1b71c44 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -66,7 +66,7 @@ void FollowPathAction::on_wait_for_result( getInput("path", new_path); // Check if it is not same with the current one - if (goal_.path != new_path) { + if (goal_.path != new_path && new_path != nav_msgs::msg::Path()) { // the action server on the next loop iteration goal_.path = new_path; goal_updated_ = true; From 46dbd07191337e18f2a8fea99b10deacf6ffcfe3 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Tue, 25 Mar 2025 14:58:37 +0100 Subject: [PATCH 03/70] nav2_rviz_plugins: Don't use non-existent slot (#5016) The definition of the slot was removed in commit e6f500e5 ("nav2_rviz_plugins: Remove slots without implementation (#4974)", 2025-03-10), because it had no implementation. But we forgot to remove the reference to this slot, because the compiler cannot detect it. We remove the reference now. Without this, rviz shows warnings like: QObject::connect: No such slot nav2_rviz_plugins::CostmapCostTool::updateAutoDeactivate() QObject::connect: (sender name: 'Single click') Signed-off-by: Michal Sojka --- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 03435a913e4..4e97ee1eb2e 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -30,7 +30,7 @@ CostmapCostTool::CostmapCostTool() auto_deactivate_property_ = new rviz_common::properties::BoolProperty( "Single click", true, "Switch away from this tool after one click.", - getPropertyContainer(), SLOT(updateAutoDeactivate()), this); + getPropertyContainer(), nullptr, this); } CostmapCostTool::~CostmapCostTool() {} From d8dcb8d3a2448782b68b6d4ecbf1fe07d2c8cece Mon Sep 17 00:00:00 2001 From: suchetanrs <79915569+suchetanrs@users.noreply.github.com> Date: Tue, 25 Mar 2025 20:29:52 +0530 Subject: [PATCH 04/70] * Parametrize collision checking in nav2_graceful_controller (#5006) * * Parametrize collision checking in nav2_graceful_controller Signed-off-by: suchetanrs * * Fix linting errors Signed-off-by: suchetanrs * * Address PR comments * Add parameter to dynamic reconfigure Signed-off-by: suchetanrs * * Add test for the use_collision_detection parameter Signed-off-by: suchetanrs --------- Signed-off-by: suchetanrs --- .../nav2_graceful_controller/parameter_handler.hpp | 1 + nav2_graceful_controller/src/graceful_controller.cpp | 10 ++++++---- nav2_graceful_controller/src/parameter_handler.cpp | 6 ++++++ .../test/test_graceful_controller.cpp | 4 ++++ 4 files changed, 17 insertions(+), 4 deletions(-) diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp index dd4d4df09ce..3d6ad16f33b 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp @@ -53,6 +53,7 @@ struct Parameters double rotation_scaling_factor; bool allow_backward; double in_place_collision_resolution; + bool use_collision_detection; }; /** diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index 3e6857d0287..da880c7dc8e 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -56,8 +56,10 @@ void GracefulController::configure( params_->v_linear_min, params_->v_linear_max, params_->v_angular_max); // Initialize footprint collision checker - collision_checker_ = std::make_unique>(costmap_ros_->getCostmap()); + if(params_->use_collision_detection) { + collision_checker_ = std::make_unique>(costmap_ros_->getCostmap()); + } // Publishers transformed_plan_pub_ = node->create_publisher("transformed_global_plan", 1); @@ -177,7 +179,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands( next_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); geometry_msgs::msg::PoseStamped costmap_pose; tf2::doTransform(next_pose, costmap_pose, costmap_transform); - if (inCollision( + if (params_->use_collision_detection && inCollision( costmap_pose.pose.position.x, costmap_pose.pose.position.y, tf2::getYaw(costmap_pose.pose.orientation))) { @@ -348,7 +350,7 @@ bool GracefulController::simulateTrajectory( // Check for collision geometry_msgs::msg::PoseStamped global_pose; tf2::doTransform(next_pose, global_pose, costmap_transform); - if (inCollision( + if (params_->use_collision_detection && inCollision( global_pose.pose.position.x, global_pose.pose.position.y, tf2::getYaw(global_pose.pose.orientation))) { diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index f8ba2aa075c..7b2f7c42936 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -70,6 +70,8 @@ ParameterHandler::ParameterHandler( node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".in_place_collision_resolution", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead); @@ -103,6 +105,8 @@ ParameterHandler::ParameterHandler( node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward); node->get_parameter( plugin_name_ + ".in_place_collision_resolution", params_.in_place_collision_resolution); + node->get_parameter( + plugin_name_ + ".use_collision_detection", params_.use_collision_detection); if (params_.initial_rotation && params_.allow_backward) { RCLCPP_WARN( @@ -187,6 +191,8 @@ ParameterHandler::dynamicParametersCallback(std::vector param continue; } params_.allow_backward = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_collision_detection") { + params_.use_collision_detection = parameter.as_bool(); } } } diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index de95598bb0f..98e9dbd4e60 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -248,6 +248,8 @@ TEST(GracefulControllerTest, dynamicParameters) { node, "test.initial_rotation", rclcpp::ParameterValue(true)); nav2_util::declare_parameter_if_not_declared( node, "test.allow_backward", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, "test.use_collision_detection", rclcpp::ParameterValue(true)); // Create controller auto controller = std::make_shared(); @@ -282,6 +284,7 @@ TEST(GracefulControllerTest, dynamicParameters) { rclcpp::Parameter("test.prefer_final_rotation", false), rclcpp::Parameter("test.rotation_scaling_factor", 13.0), rclcpp::Parameter("test.allow_backward", true), + rclcpp::Parameter("test.use_collision_detection", false), rclcpp::Parameter("test.in_place_collision_resolution", 15.0)}); // Spin @@ -305,6 +308,7 @@ TEST(GracefulControllerTest, dynamicParameters) { EXPECT_EQ(node->get_parameter("test.prefer_final_rotation").as_bool(), false); EXPECT_EQ(node->get_parameter("test.rotation_scaling_factor").as_double(), 13.0); EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), true); + EXPECT_EQ(node->get_parameter("test.use_collision_detection").as_bool(), false); EXPECT_EQ(node->get_parameter("test.in_place_collision_resolution").as_double(), 15.0); // Set initial rotation to true From a904802fe222f961d4a211fa0c813349672c18eb Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Tue, 25 Mar 2025 08:49:45 -0700 Subject: [PATCH 05/70] Update smac planner types (#4927) * Update smac planner types Signed-off-by: Michael Carlstrom * Test ament_mypy Signed-off-by: Michael Carlstrom * Add packages Signed-off-by: Michael Carlstrom * Fix arg name Signed-off-by: Michael Carlstrom * Add ** Signed-off-by: Michael Carlstrom * Specific package Signed-off-by: Michael Carlstrom * re-run ci Signed-off-by: Michael Carlstrom * re-run ci Signed-off-by: Michael Carlstrom --------- Signed-off-by: Michael Carlstrom --- .github/workflows/lint.yml | 13 ++++ .../generate_motion_primitives.py | 31 +++++--- .../lattice_primitives/helper.py | 13 +++- .../lattice_primitives/lattice_generator.py | 55 ++++++++++---- nav2_smac_planner/lattice_primitives/py.typed | 0 .../tests/test_lattice_generator.py | 12 +-- .../tests/test_trajectory_generator.py | 74 ++++++++++++++++--- .../tests/trajectory_visualizer.py | 6 +- .../lattice_primitives/trajectory.py | 37 ++++++---- .../trajectory_generator.py | 51 +++++++------ 10 files changed, 208 insertions(+), 84 deletions(-) create mode 100644 nav2_smac_planner/lattice_primitives/py.typed diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index c181fe25f4c..28bfe3f8c5c 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -19,3 +19,16 @@ jobs: linter: ${{ matrix.linter }} distribution: rolling package-name: "*" + + ament_lint_mypy: + name: ament_mypy + runs-on: ubuntu-latest + container: + image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest + steps: + - uses: actions/checkout@v4 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + linter: mypy + distribution: rolling + package-name: "nav2_smac_planner" diff --git a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py index a64d7bea283..49ea5301d5a 100644 --- a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py +++ b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py @@ -18,17 +18,26 @@ import logging from pathlib import Path import time +from typing import Any, cast, Dict, List, TypedDict import constants -from lattice_generator import LatticeGenerator +from lattice_generator import ConfigDict, LatticeGenerator import matplotlib.pyplot as plt import numpy as np +from trajectory import Trajectory logging.basicConfig(level=logging.INFO) logger = logging.getLogger(__name__) -def handle_arg_parsing(): +class HeaderDict(TypedDict): + version: float + date_generated: str + lattice_metadata: Dict[str, Any] + primitives: List[Dict[str, Any]] + + +def handle_arg_parsing() -> argparse.Namespace: """ Handle the parsing of arguments. @@ -64,7 +73,8 @@ def handle_arg_parsing(): return parser.parse_args() -def create_heading_angle_list(minimal_set_trajectories: dict) -> list: +def create_heading_angle_list(minimal_set_trajectories: Dict[float, List[Trajectory]] + ) -> List[float]: """ Create a sorted list of heading angles from the minimal trajectory set. @@ -83,7 +93,7 @@ def create_heading_angle_list(minimal_set_trajectories: dict) -> list: return sorted(heading_angles, key=lambda x: (x < 0, x)) -def read_config(config_path) -> dict: +def read_config(config_path: Path) -> ConfigDict: """ Read in the user defined parameters via JSON. @@ -101,10 +111,11 @@ def read_config(config_path) -> dict: with open(config_path) as config_file: config = json.load(config_file) - return config + return cast(ConfigDict, config) -def create_header(config: dict, minimal_set_trajectories: dict) -> dict: +def create_header(config: ConfigDict, minimal_set_trajectories: Dict[float, List[Trajectory]] + ) -> HeaderDict: """ Create a dict containing all the fields to populate the header with. @@ -121,7 +132,7 @@ def create_header(config: dict, minimal_set_trajectories: dict) -> dict: A dictionary containing the fields to populate the header with """ - header_dict = { + header_dict: HeaderDict = { 'version': constants.VERSION, 'date_generated': datetime.today().strftime('%Y-%m-%d'), 'lattice_metadata': {}, @@ -142,7 +153,7 @@ def create_header(config: dict, minimal_set_trajectories: dict) -> dict: def write_to_json( - output_path: Path, minimal_set_trajectories: dict, config: dict + output_path: Path, minimal_set_trajectories: Dict[float, List[Trajectory]], config: ConfigDict ) -> None: """ Write the minimal spanning set to an output file. @@ -171,7 +182,7 @@ def write_to_json( minimal_set_trajectories[start_angle], key=lambda x: x.parameters.end_angle ): - traj_info = {} + traj_info: Dict[str, Any] = {} traj_info['trajectory_id'] = idx traj_info['start_angle_index'] = heading_lookup[ trajectory.parameters.start_angle @@ -202,7 +213,7 @@ def write_to_json( def save_visualizations( - visualizations_folder: Path, minimal_set_trajectories: dict + visualizations_folder: Path, minimal_set_trajectories: Dict[float, List[Trajectory]] ) -> None: """ Draw the visualizations for every trajectory and save it as an image. diff --git a/nav2_smac_planner/lattice_primitives/helper.py b/nav2_smac_planner/lattice_primitives/helper.py index e29c7877389..8d6265c8a97 100644 --- a/nav2_smac_planner/lattice_primitives/helper.py +++ b/nav2_smac_planner/lattice_primitives/helper.py @@ -12,10 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. Reserved. +from typing import Any, Optional + import numpy as np +from numpy.typing import NDArray -def normalize_angle(angle): +def normalize_angle(angle: float) -> float: """ Normalize the angle to between [0, 2pi). @@ -37,7 +40,8 @@ def normalize_angle(angle): return angle -def angle_difference(angle_1, angle_2, left_turn=None): +def angle_difference(angle_1: float, angle_2: float, + left_turn: Optional[float] = None) -> float: """ Calculate the difference between two angles based on a given direction. @@ -76,7 +80,8 @@ def angle_difference(angle_1, angle_2, left_turn=None): return 2 * np.pi - abs(angle_1 - angle_2) -def interpolate_yaws(start_angle, end_angle, left_turn, steps): +def interpolate_yaws(start_angle: float, end_angle: float, + left_turn: bool, steps: int) -> Any: """ Create equally spaced yaws between two angles. @@ -110,7 +115,7 @@ def interpolate_yaws(start_angle, end_angle, left_turn, steps): return yaws -def get_rotation_matrix(angle): +def get_rotation_matrix(angle: float) -> NDArray[np.floating[Any]]: """ Return a rotation matrix that is equivalent to a 2D rotation of angle. diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py index f8356ab9322..e038183cf2b 100644 --- a/nav2_smac_planner/lattice_primitives/lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -14,14 +14,25 @@ from collections import defaultdict from enum import Enum +from typing import Any, Dict, List, Tuple, TypedDict from helper import angle_difference, interpolate_yaws import numpy as np from rtree import index -from trajectory import Path, Trajectory, TrajectoryParameters + +from trajectory import AnyFloat, FloatNDArray, Path, Trajectory, TrajectoryParameters + from trajectory_generator import TrajectoryGenerator +class ConfigDict(TypedDict): + grid_resolution: float + turning_radius: float + stopping_threshold: int + num_of_headings: int + motion_model: str + + class LatticeGenerator: """ Handles all the logic for computing the minimal control set. @@ -46,7 +57,7 @@ class Flip(Enum): Y = 2 BOTH = 3 - def __init__(self, config: dict): + def __init__(self, config: ConfigDict): """Init the lattice generator from the user supplied config.""" self.trajectory_generator = TrajectoryGenerator(config) self.grid_resolution = config['grid_resolution'] @@ -60,7 +71,7 @@ def __init__(self, config: dict): self.DISTANCE_THRESHOLD = 0.5 * self.grid_resolution self.ROTATION_THRESHOLD = 0.5 * (2 * np.pi / self.num_of_headings) - def _get_wave_front_points(self, pos: int) -> np.array: + def _get_wave_front_points(self, pos: int) -> FloatNDArray: """ Calculate the end points that lie on the wave front. @@ -97,7 +108,7 @@ def _get_wave_front_points(self, pos: int) -> np.array: return np.array(positions) - def _get_heading_discretization(self, number_of_headings: int) -> list: + def _get_heading_discretization(self, number_of_headings: int) -> List[int]: """ Calculate the heading discretization based on the number of headings. @@ -131,7 +142,8 @@ def _get_heading_discretization(self, number_of_headings: int) -> list: return sorted([np.arctan2(j, i) for i, j in zip(outer_edge_x, outer_edge_y)]) - def _point_to_line_distance(self, p1: np.array, p2: np.array, q: np.array) -> float: + def _point_to_line_distance(self, p1: FloatNDArray, p2: FloatNDArray, + q: FloatNDArray) -> AnyFloat: """ Return the shortest distance from a point to a line segment. @@ -241,7 +253,7 @@ def _compute_min_trajectory_length(self) -> float: return self.turning_radius * min(heading_diff) - def _generate_minimal_spanning_set(self) -> dict: + def _generate_minimal_spanning_set(self) -> Dict[float, List[Trajectory]]: """ Generate the minimal spanning set. @@ -255,7 +267,7 @@ def _generate_minimal_spanning_set(self) -> dict: a list of trajectories that begin at that angle """ - quadrant1_end_poses = defaultdict(list) + quadrant1_end_poses: Dict[int, List[Tuple[Any, int]]] = defaultdict(list) # Since we only compute for quadrant 1 we only need headings between # 0 and 90 degrees @@ -338,7 +350,7 @@ def _generate_minimal_spanning_set(self) -> dict: # we can leverage symmetry to create the complete minimal set return self._create_complete_minimal_spanning_set(quadrant1_end_poses) - def _flip_angle(self, angle: float, flip_type: Flip) -> float: + def _flip_angle(self, angle: int, flip_type: Flip) -> float: """ Return the the appropriate flip of the angle in self.headings. @@ -370,8 +382,8 @@ def _flip_angle(self, angle: float, flip_type: Flip) -> float: return self.headings[int(heading_idx)] def _create_complete_minimal_spanning_set( - self, single_quadrant_minimal_set: dict - ) -> dict: + self, single_quadrant_minimal_set: Dict[int, List[Tuple[Any, int]]] + ) -> Dict[float, List[Trajectory]]: """ Create the full minimal spanning set from a single quadrant set. @@ -390,7 +402,7 @@ def _create_complete_minimal_spanning_set( in all quadrants """ - all_trajectories = defaultdict(list) + all_trajectories: Dict[float, List[Trajectory]] = defaultdict(list) for start_angle in single_quadrant_minimal_set.keys(): @@ -425,6 +437,9 @@ def _create_complete_minimal_spanning_set( ) ) + if unflipped_trajectory is None or flipped_x_trajectory is None: + raise ValueError('No trajectory was found') + all_trajectories[ unflipped_trajectory.parameters.start_angle ].append(unflipped_trajectory) @@ -459,6 +474,9 @@ def _create_complete_minimal_spanning_set( ) ) + if unflipped_trajectory is None or flipped_y_trajectory is None: + raise ValueError('No trajectory was found') + all_trajectories[ unflipped_trajectory.parameters.start_angle ].append(unflipped_trajectory) @@ -513,6 +531,10 @@ def _create_complete_minimal_spanning_set( ) ) + if (unflipped_trajectory is None or flipped_y_trajectory is None or + flipped_x_trajectory is None or flipped_xy_trajectory is None): + raise ValueError('No trajectory was found') + all_trajectories[ unflipped_trajectory.parameters.start_angle ].append(unflipped_trajectory) @@ -528,7 +550,8 @@ def _create_complete_minimal_spanning_set( return all_trajectories - def _handle_motion_model(self, spanning_set: dict) -> dict: + def _handle_motion_model(self, spanning_set: Dict[float, List[Trajectory]] + ) -> Dict[float, List[Trajectory]]: """ Add the appropriate motions for the user supplied motion model. @@ -565,7 +588,8 @@ def _handle_motion_model(self, spanning_set: dict) -> dict: print('No handling implemented for Motion Model: ' + f'{self.motion_model}') raise NotImplementedError - def _add_in_place_turns(self, spanning_set: dict) -> dict: + def _add_in_place_turns(self, spanning_set: Dict[float, List[Trajectory]] + ) -> Dict[float, List[Trajectory]]: """ Add in place turns to the spanning set. @@ -623,7 +647,8 @@ def _add_in_place_turns(self, spanning_set: dict) -> dict: return spanning_set - def _add_horizontal_motions(self, spanning_set: dict) -> dict: + def _add_horizontal_motions(self, spanning_set: Dict[float, List[Trajectory]] + ) -> Dict[float, List[Trajectory]]: """ Add horizontal sliding motions to the spanning set. @@ -723,7 +748,7 @@ def _add_horizontal_motions(self, spanning_set: dict) -> dict: return spanning_set - def run(self): + def run(self) -> Dict[float, List[Trajectory]]: """ Run the lattice generator. diff --git a/nav2_smac_planner/lattice_primitives/py.typed b/nav2_smac_planner/lattice_primitives/py.typed new file mode 100644 index 00000000000..e69de29bb2d diff --git a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py index 30a7a24eb2a..fad6db00ab3 100644 --- a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py @@ -14,7 +14,7 @@ import unittest -from lattice_generator import LatticeGenerator +from lattice_generator import ConfigDict, LatticeGenerator import numpy as np MOTION_MODEL = 'ackermann' @@ -28,7 +28,7 @@ class TestLatticeGenerator(unittest.TestCase): """Contains the unit tests for the TrajectoryGenerator.""" def setUp(self) -> None: - config = { + config: ConfigDict = { 'motion_model': MOTION_MODEL, 'turning_radius': TURNING_RADIUS, 'grid_resolution': GRID_RESOLUTION, @@ -40,7 +40,7 @@ def setUp(self) -> None: self.minimal_set = lattice_gen.run() - def test_minimal_set_lengths_are_positive(self): + def test_minimal_set_lengths_are_positive(self) -> None: # Test that lengths are all positive for start_angle in self.minimal_set.keys(): @@ -51,7 +51,7 @@ def test_minimal_set_lengths_are_positive(self): self.assertGreaterEqual(trajectory.parameters.end_straight_length, 0) self.assertGreaterEqual(trajectory.parameters.total_length, 0) - def test_minimal_set_end_points_lie_on_grid(self): + def test_minimal_set_end_points_lie_on_grid(self) -> None: # Test that end points lie on the grid resolution for start_angle in self.minimal_set.keys(): @@ -66,7 +66,7 @@ def test_minimal_set_end_points_lie_on_grid(self): self.assertAlmostEqual(div_x, np.round(div_x), delta=0.00001) self.assertAlmostEqual(div_y, np.round(div_y), delta=0.00001) - def test_minimal_set_end_angle_is_correct(self): + def test_minimal_set_end_angle_is_correct(self) -> None: # Test that end angle agrees with the end angle parameter for start_angle in self.minimal_set.keys(): @@ -76,7 +76,7 @@ def test_minimal_set_end_angle_is_correct(self): self.assertEqual(end_point_angle, trajectory.parameters.end_angle) - def test_output_angles_in_correct_range(self): + def test_output_angles_in_correct_range(self) -> None: # Test that the outputted angles always lie within 0 to 2*pi for start_angle in self.minimal_set.keys(): diff --git a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py index ea3f51930fa..053faefcd6a 100644 --- a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py @@ -12,10 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. Reserved. +from typing import cast import unittest import numpy as np -from trajectory_generator import TrajectoryGenerator +from trajectory import Trajectory +from trajectory_generator import TrajectoryGenerator, TrajectoryGeneratorConfigDict TURNING_RADIUS = 1 STEP_DISTANCE = 0.1 @@ -25,16 +27,19 @@ class TestTrajectoryGenerator(unittest.TestCase): """Contains the unit tests for the TrajectoryGenerator.""" def setUp(self) -> None: - config = {'turning_radius': TURNING_RADIUS} + config: TrajectoryGeneratorConfigDict = {'turning_radius': TURNING_RADIUS} self.trajectory_generator = TrajectoryGenerator(config) - def test_generate_trajectory_only_arc(self): + def test_generate_trajectory_only_arc(self) -> None: # Quadrant 1 end_point = np.array([1, 1]) trajectory = self.trajectory_generator.generate_trajectory( end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -44,6 +49,9 @@ def test_generate_trajectory_only_arc(self): end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -53,6 +61,9 @@ def test_generate_trajectory_only_arc(self): end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -62,16 +73,22 @@ def test_generate_trajectory_only_arc(self): end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) - def test_generate_trajectory_only_line(self): + def test_generate_trajectory_only_line(self) -> None: # Quadrant 1 end_point = np.array([1, 1]) trajectory = self.trajectory_generator.generate_trajectory( end_point, np.deg2rad(45), np.deg2rad(45), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -81,6 +98,9 @@ def test_generate_trajectory_only_line(self): end_point, np.deg2rad(135), np.deg2rad(135), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -90,6 +110,9 @@ def test_generate_trajectory_only_line(self): end_point, -np.deg2rad(135), -np.deg2rad(135), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -99,16 +122,22 @@ def test_generate_trajectory_only_line(self): end_point, -np.deg2rad(45), -np.deg2rad(45), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) - def test_generate_trajectory_line_to_arc(self): + def test_generate_trajectory_line_to_arc(self) -> None: # Quadrant 1 end_point = np.array([2, 1]) trajectory = self.trajectory_generator.generate_trajectory( end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -118,6 +147,9 @@ def test_generate_trajectory_line_to_arc(self): end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -127,6 +159,9 @@ def test_generate_trajectory_line_to_arc(self): end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -136,16 +171,22 @@ def test_generate_trajectory_line_to_arc(self): end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) - def test_generate_trajectory_line_to_end(self): + def test_generate_trajectory_line_to_end(self) -> None: # Quadrant 1 end_point = np.array([1, 2]) trajectory = self.trajectory_generator.generate_trajectory( end_point, np.deg2rad(0), np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -155,6 +196,9 @@ def test_generate_trajectory_line_to_end(self): end_point, -np.deg2rad(180), np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -164,6 +208,9 @@ def test_generate_trajectory_line_to_end(self): end_point, -np.deg2rad(180), -np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -173,10 +220,13 @@ def test_generate_trajectory_line_to_end(self): end_point, np.deg2rad(0), -np.deg2rad(90), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) - def test_generate_trajectory_radius_too_small(self): + def test_generate_trajectory_radius_too_small(self) -> None: # Quadrant 1 end_point = np.array([0.9, 0.9]) trajectory = self.trajectory_generator.generate_trajectory( @@ -209,13 +259,16 @@ def test_generate_trajectory_radius_too_small(self): self.assertEqual(trajectory, None) - def test_generate_trajectory_parallel_lines_coincident(self): + def test_generate_trajectory_parallel_lines_coincident(self) -> None: # Quadrant 1 end_point = np.array([5, 0]) trajectory = self.trajectory_generator.generate_trajectory( end_point, np.deg2rad(0), np.deg2rad(0), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) @@ -225,10 +278,13 @@ def test_generate_trajectory_parallel_lines_coincident(self): end_point, -np.deg2rad(180), -np.deg2rad(180), STEP_DISTANCE ) + self.assertIsNotNone(trajectory) + trajectory = cast(Trajectory, trajectory) + self.assertEqual(len(trajectory.path.xs), len(trajectory.path.ys)) self.assertGreater(len(trajectory.path.xs), 0) - def test_generate_trajectory_parallel_lines_not_coincident(self): + def test_generate_trajectory_parallel_lines_not_coincident(self) -> None: # Quadrant 1 end_point = np.array([0, 3]) trajectory = self.trajectory_generator.generate_trajectory( diff --git a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py index 5263167ca0e..be4ad5cbea9 100644 --- a/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py +++ b/nav2_smac_planner/lattice_primitives/tests/trajectory_visualizer.py @@ -24,12 +24,14 @@ import json from pathlib import Path +from typing import Any import matplotlib.pyplot as plt import numpy as np -def plot_arrow(x, y, yaw, length=1.0, fc='r', ec='k'): +def plot_arrow(x: float, y: float, yaw: float, length: float = 1.0, fc: str = 'r', ec: str = 'k' + ) -> None: """Plot arrow.""" plt.arrow( x, @@ -43,7 +45,7 @@ def plot_arrow(x, y, yaw, length=1.0, fc='r', ec='k'): plt.plot(0, 0) -def read_trajectories_data(file_path): +def read_trajectories_data(file_path: Path) -> Any: with open(file_path) as data_file: trajectory_data = json.load(data_file) diff --git a/nav2_smac_planner/lattice_primitives/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py index 4b2ed6537b2..efc3ec79ae4 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory.py +++ b/nav2_smac_planner/lattice_primitives/trajectory.py @@ -13,9 +13,15 @@ # limitations under the License. Reserved. from dataclasses import dataclass +from typing import Any, Union from helper import angle_difference, normalize_angle import numpy as np +from numpy.typing import NDArray + +AnyFloat = np.floating[Any] +TrajectoryFloat = Union[float, AnyFloat] +FloatNDArray = NDArray[AnyFloat] @dataclass(frozen=True) @@ -41,41 +47,42 @@ class TrajectoryParameters: arc_end_point: Coordinates of the ending position of the arc """ - turning_radius: float + turning_radius: TrajectoryFloat x_offset: float y_offset: float - end_point: np.array + end_point: FloatNDArray start_angle: float end_angle: float left_turn: bool - arc_start_point: float - arc_end_point: float + arc_start_point: FloatNDArray + arc_end_point: FloatNDArray @property - def arc_length(self): + def arc_length(self) -> TrajectoryFloat: """Arc length of the trajectory.""" return self.turning_radius * angle_difference( self.start_angle, self.end_angle, self.left_turn ) @property - def start_straight_length(self): - """Length of the straight line from start to arc.""" + def start_straight_length(self) -> AnyFloat: + """Length of the straight line fromnorarc_start_pointm start to arc.""" return np.linalg.norm(self.arc_start_point) @property - def end_straight_length(self): + def end_straight_length(self) -> AnyFloat: """Length of the straight line from arc to end.""" return np.linalg.norm(self.end_point - self.arc_end_point) @property - def total_length(self): + def total_length(self) -> AnyFloat: """Total length of trajectory.""" return self.arc_length + self.start_straight_length + self.end_straight_length @staticmethod - def no_arc(end_point, start_angle, end_angle): + def no_arc(end_point: FloatNDArray, start_angle: float, + end_angle: float) -> 'TrajectoryParameters': """Create the parameters for a trajectory with no arc.""" return TrajectoryParameters( turning_radius=0.0, @@ -100,11 +107,11 @@ class Path: yaws: Yaws of poses along trajectory """ - xs: np.array - ys: np.array - yaws: np.array + xs: FloatNDArray + ys: FloatNDArray + yaws: FloatNDArray - def __add__(self, rhs): + def __add__(self, rhs: 'Path') -> 'Path': """Add two paths together by concatenating them.""" if self.xs is None: return rhs @@ -115,7 +122,7 @@ def __add__(self, rhs): return Path(xs, ys, yaws) - def to_output_format(self): + def to_output_format(self) -> Any: """Return the path data in a format suitable for outputting.""" output_xs = self.xs.round(5) output_ys = self.ys.round(5) diff --git a/nav2_smac_planner/lattice_primitives/trajectory_generator.py b/nav2_smac_planner/lattice_primitives/trajectory_generator.py index 8efe06149a3..37f10d02cc3 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py @@ -13,24 +13,29 @@ # limitations under the License. Reserved. import logging -from typing import Tuple, Union +from typing import Any, List, Tuple, TypedDict, Union import numpy as np -from trajectory import Path, Trajectory, TrajectoryParameters + +from trajectory import FloatNDArray, Path, Trajectory, TrajectoryFloat, TrajectoryParameters logger = logging.getLogger(__name__) +class TrajectoryGeneratorConfigDict(TypedDict): + turning_radius: float + + class TrajectoryGenerator: """Handles all the logic for generating trajectories.""" - def __init__(self, config: dict): + def __init__(self, config: TrajectoryGeneratorConfigDict): """Init TrajectoryGenerator using the user supplied config.""" self.turning_radius = config['turning_radius'] def _get_arc_point( - self, trajectory_params: TrajectoryParameters, t: float - ) -> Tuple[float, float, float]: + self, trajectory_params: TrajectoryParameters, t: TrajectoryFloat + ) -> Tuple[float, float, TrajectoryFloat]: """ Get point on the arc trajectory using the following parameterization. @@ -100,8 +105,8 @@ def _get_arc_point( return x, y, yaw def _get_line_point( - self, start_point: np.array, end_point: np.array, t: float - ) -> Tuple[float, float]: + self, start_point: FloatNDArray, end_point: FloatNDArray, t: float + ) -> FloatNDArray: """ Get point on a line segment using the following parameterization. @@ -169,7 +174,7 @@ def _create_path( xs = [] ys = [] - yaws = [] + yaws: List[TrajectoryFloat] = [] for i in range(1, number_of_steps + 1): @@ -182,7 +187,7 @@ def _create_path( x, y = self._get_line_point( np.array([0, 0]), trajectory_params.arc_start_point, line_t ) - yaw = trajectory_params.start_angle + yaw: TrajectoryFloat = trajectory_params.start_angle # Handle the arc elif cur_t <= transition_points[1]: @@ -206,20 +211,20 @@ def _create_path( cur_t += t_step # Convert to numpy arrays - xs = np.array(xs) - ys = np.array(ys) - yaws = np.array(yaws) + xs_np = np.array(xs) + ys_np = np.array(ys) + yaws_np = np.array(yaws) # The last point may be slightly off due to rounding issues # so we correct the last point to be exactly the end point - xs[-1], ys[-1] = trajectory_params.end_point - yaws[-1] = trajectory_params.end_angle + xs_np[-1], ys_np[-1] = trajectory_params.end_point + yaws_np[-1] = trajectory_params.end_angle - return Path(xs, ys, yaws) + return Path(xs_np, ys_np, yaws_np) def _get_intersection_point( self, m1: float, c1: float, m2: float, c2: float - ) -> np.array: + ) -> FloatNDArray: """ Get the intersection point of two lines. @@ -243,14 +248,14 @@ def _get_intersection_point( """ - def line1(x): + def line1(x: float) -> float: return m1 * x + c1 x_point = (c2 - c1) / (m1 - m2) return np.array([x_point, line1(x_point)]) - def _is_left_turn(self, intersection_point: np.array, end_point: np.array) -> bool: + def _is_left_turn(self, intersection_point: FloatNDArray, end_point: FloatNDArray) -> Any: """ Determine if a trajectory will be a left turn. @@ -277,7 +282,7 @@ def _is_left_turn(self, intersection_point: np.array, end_point: np.array) -> bo return det >= 0 def _is_dir_vec_correct( - self, point1: np.array, point2: np.array, line_angle: float + self, point1: FloatNDArray, point2: FloatNDArray, line_angle: float ) -> bool: """ Check that the direction vector agrees with the line angle. @@ -327,7 +332,7 @@ def _is_dir_vec_correct( return False def _calculate_trajectory_params( - self, end_point: np.array, start_angle: float, end_angle: float + self, end_point: FloatNDArray, start_angle: float, end_angle: float ) -> Union[TrajectoryParameters, None]: """ Calculate the parameters for a trajectory with the desired constraints. @@ -478,13 +483,13 @@ def _calculate_trajectory_params( if m1 == 0: # If line 1 has gradient 0 then it is the x-axis. - def perp_line2(x): + def perp_line2(x: float) -> Any: return -1 / m2 * (x - x2) + y2 circle_center = np.array([x1, perp_line2(x1)]) elif m2 == 0: - def perp_line1(x): + def perp_line1(x: float) -> Any: return -1 / m1 * (x - x1) + y1 circle_center = np.array([x2, perp_line1(x2)]) @@ -525,7 +530,7 @@ def perp_line1(x): def generate_trajectory( self, - end_point: np.array, + end_point: FloatNDArray, start_angle: float, end_angle: float, primitive_resolution: float, From b3ca86ed9dde20f6c699259c710c570fb58388f9 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Wed, 26 Mar 2025 18:25:16 +0100 Subject: [PATCH 06/70] Declare_parameter_if_not_declared in docking navigator (#5023) Signed-off-by: Alberto Tudela --- .../opennav_docking/src/navigator.cpp | 3 ++- .../opennav_docking/test/test_navigator.cpp | 22 +++++++++++++++++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/nav2_docking/opennav_docking/src/navigator.cpp b/nav2_docking/opennav_docking/src/navigator.cpp index e792411a75f..ff76c5eecd7 100644 --- a/nav2_docking/opennav_docking/src/navigator.cpp +++ b/nav2_docking/opennav_docking/src/navigator.cpp @@ -24,7 +24,8 @@ Navigator::Navigator(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent) : node_(parent) { auto node = node_.lock(); - node->declare_parameter("navigator_bt_xml", std::string("")); + nav2_util::declare_parameter_if_not_declared( + node, "navigator_bt_xml", rclcpp::ParameterValue(std::string(""))); node->get_parameter("navigator_bt_xml", navigator_bt_xml_); } diff --git a/nav2_docking/opennav_docking/test/test_navigator.cpp b/nav2_docking/opennav_docking/test/test_navigator.cpp index 74df97e62d0..09f81494a79 100644 --- a/nav2_docking/opennav_docking/test/test_navigator.cpp +++ b/nav2_docking/opennav_docking/test/test_navigator.cpp @@ -81,6 +81,28 @@ class DummyNavigationServer : rclcpp::Node bool toggle_{false}; }; +TEST(NavigatorTests, TestNavigatorReconfigure) +{ + auto node = std::make_shared("test_node"); + auto navigator = std::make_unique(node); + node->configure(); + node->activate(); + navigator->activate(); + navigator->deactivate(); + navigator.reset(); + + // Create and activate again + EXPECT_NO_THROW(navigator = std::make_unique(node)); + navigator->activate(); + navigator->deactivate(); + + // Reset the node + navigator.reset(); + node->deactivate(); + node->cleanup(); + node->shutdown(); +} + TEST(NavigatorTests, TestNavigator) { auto dummy_navigator_node = std::make_shared(); From 7b40dc06aa13ec29515505876beb31bd6ac98e11 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 27 Mar 2025 12:18:21 -0700 Subject: [PATCH 07/70] Fix naming of launch file Signed-off-by: Steve Macenski --- ...ack_simulation.launch.py => tb4_loopback_simulation_launch.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename nav2_bringup/launch/{tb4_loopback_simulation.launch.py => tb4_loopback_simulation_launch.py} (100%) diff --git a/nav2_bringup/launch/tb4_loopback_simulation.launch.py b/nav2_bringup/launch/tb4_loopback_simulation_launch.py similarity index 100% rename from nav2_bringup/launch/tb4_loopback_simulation.launch.py rename to nav2_bringup/launch/tb4_loopback_simulation_launch.py From 6513b51570f62219d20afb7abd3b531a6a433d39 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 27 Mar 2025 12:18:40 -0700 Subject: [PATCH 08/70] Fixed naming conventions of launch file Signed-off-by: Steve Macenski --- ...ack_simulation.launch.py => tb3_loopback_simulation_launch.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename nav2_bringup/launch/{tb3_loopback_simulation.launch.py => tb3_loopback_simulation_launch.py} (100%) diff --git a/nav2_bringup/launch/tb3_loopback_simulation.launch.py b/nav2_bringup/launch/tb3_loopback_simulation_launch.py similarity index 100% rename from nav2_bringup/launch/tb3_loopback_simulation.launch.py rename to nav2_bringup/launch/tb3_loopback_simulation_launch.py From b5adf6149d7464dd5ad7041ebc46abaf1c40502d Mon Sep 17 00:00:00 2001 From: Yancey <135342560+Yancey2023@users.noreply.github.com> Date: Fri, 28 Mar 2025 10:08:00 +0800 Subject: [PATCH 09/70] navfn : fix performance issue (#4945) Signed-off-by: Yancey <135342560+Yancey2023@users.noreply.github.com> --- nav2_navfn_planner/src/navfn_planner.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 7c94751a761..1dde217dd1b 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -293,11 +293,13 @@ NavfnPlanner::makePlan( p.position.x = goal.position.x - tolerance; while (p.position.x <= goal.position.x + tolerance) { potential = getPointPotential(p.position); - double sdist = squared_distance(p, goal); - if (potential < POT_HIGH && sdist < best_sdist) { - best_sdist = sdist; - best_pose = p; - found_legal = true; + if (potential < POT_HIGH) { + double sdist = squared_distance(p, goal); + if (sdist < best_sdist) { + best_sdist = sdist; + best_pose = p; + found_legal = true; + } } p.position.x += resolution; } From 7123183bfa2f7110aeefde097c8f9d1205baa042 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Fri, 28 Mar 2025 17:23:16 +0000 Subject: [PATCH 10/70] Configured mypy strict for nav2_smac_planner (#5022) * Configured mypy strict on nav2_smac_planner. Signed-off-by: Leander Stephen D'Souza * Modify workflow to contain mypy configuration. Signed-off-by: Leander Stephen D'Souza * Added dependency for type support Signed-off-by: Leander Stephen D'Souza --------- Signed-off-by: Leander Stephen D'Souza --- .github/workflows/lint.yml | 11 ++++++++++- nav2_common/package.xml | 1 + nav2_smac_planner/lattice_primitives/__init__.py | 0 .../lattice_primitives/generate_motion_primitives.py | 6 +++--- .../lattice_primitives/lattice_generator.py | 9 ++++----- .../tests/test_lattice_generator.py | 2 +- .../tests/test_trajectory_generator.py | 5 +++-- nav2_smac_planner/lattice_primitives/trajectory.py | 5 +++-- .../lattice_primitives/trajectory_generator.py | 4 ++-- tools/pyproject.toml | 8 ++++++++ 10 files changed, 35 insertions(+), 16 deletions(-) create mode 100644 nav2_smac_planner/lattice_primitives/__init__.py diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 28bfe3f8c5c..85eb94e6cd9 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -25,10 +25,19 @@ jobs: runs-on: ubuntu-latest container: image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest + strategy: + matrix: + mypy_packages: + - "nav2_smac_planner" steps: - uses: actions/checkout@v4 + + - name: Install typeshed + run: sudo apt update && sudo apt install -y python3-typeshed + - uses: ros-tooling/action-ros-lint@v0.1 with: linter: mypy distribution: rolling - package-name: "nav2_smac_planner" + package-name: "${{ join(matrix.mypy_packages, ' ') }}" + arguments: --config tools/pyproject.toml diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 1584fafa646..f2cebe1c8f7 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -12,6 +12,7 @@ osrf_pycommon rclpy python3-yaml + python3-types-pyyaml ament_cmake_core diff --git a/nav2_smac_planner/lattice_primitives/__init__.py b/nav2_smac_planner/lattice_primitives/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py index 49ea5301d5a..bf9e64559ab 100644 --- a/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py +++ b/nav2_smac_planner/lattice_primitives/generate_motion_primitives.py @@ -20,11 +20,11 @@ import time from typing import Any, cast, Dict, List, TypedDict -import constants -from lattice_generator import ConfigDict, LatticeGenerator import matplotlib.pyplot as plt +from nav2_smac_planner.lattice_primitives import constants +from nav2_smac_planner.lattice_primitives.lattice_generator import ConfigDict, LatticeGenerator +from nav2_smac_planner.lattice_primitives.trajectory import Trajectory import numpy as np -from trajectory import Trajectory logging.basicConfig(level=logging.INFO) logger = logging.getLogger(__name__) diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py index e038183cf2b..7aacd36e156 100644 --- a/nav2_smac_planner/lattice_primitives/lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -16,14 +16,13 @@ from enum import Enum from typing import Any, Dict, List, Tuple, TypedDict -from helper import angle_difference, interpolate_yaws +from nav2_smac_planner.lattice_primitives.helper import angle_difference, interpolate_yaws +from nav2_smac_planner.lattice_primitives.trajectory import (AnyFloat, FloatNDArray, Path, + Trajectory, TrajectoryParameters) +from nav2_smac_planner.lattice_primitives.trajectory_generator import TrajectoryGenerator import numpy as np from rtree import index -from trajectory import AnyFloat, FloatNDArray, Path, Trajectory, TrajectoryParameters - -from trajectory_generator import TrajectoryGenerator - class ConfigDict(TypedDict): grid_resolution: float diff --git a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py index fad6db00ab3..710f574480d 100644 --- a/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/tests/test_lattice_generator.py @@ -14,7 +14,7 @@ import unittest -from lattice_generator import ConfigDict, LatticeGenerator +from nav2_smac_planner.lattice_primitives.lattice_generator import ConfigDict, LatticeGenerator import numpy as np MOTION_MODEL = 'ackermann' diff --git a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py index 053faefcd6a..9a02d610b1e 100644 --- a/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/tests/test_trajectory_generator.py @@ -15,9 +15,10 @@ from typing import cast import unittest +from nav2_smac_planner.lattice_primitives.trajectory import Trajectory +from nav2_smac_planner.lattice_primitives.trajectory_generator import ( + TrajectoryGenerator, TrajectoryGeneratorConfigDict) import numpy as np -from trajectory import Trajectory -from trajectory_generator import TrajectoryGenerator, TrajectoryGeneratorConfigDict TURNING_RADIUS = 1 STEP_DISTANCE = 0.1 diff --git a/nav2_smac_planner/lattice_primitives/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py index efc3ec79ae4..0ba9141411c 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory.py +++ b/nav2_smac_planner/lattice_primitives/trajectory.py @@ -15,7 +15,7 @@ from dataclasses import dataclass from typing import Any, Union -from helper import angle_difference, normalize_angle +from nav2_smac_planner.lattice_primitives.helper import angle_difference, normalize_angle import numpy as np from numpy.typing import NDArray @@ -61,9 +61,10 @@ class TrajectoryParameters: @property def arc_length(self) -> TrajectoryFloat: """Arc length of the trajectory.""" - return self.turning_radius * angle_difference( + result: TrajectoryFloat = self.turning_radius * angle_difference( self.start_angle, self.end_angle, self.left_turn ) + return result @property def start_straight_length(self) -> AnyFloat: diff --git a/nav2_smac_planner/lattice_primitives/trajectory_generator.py b/nav2_smac_planner/lattice_primitives/trajectory_generator.py index 37f10d02cc3..f576b14c188 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py @@ -15,10 +15,10 @@ import logging from typing import Any, List, Tuple, TypedDict, Union +from nav2_smac_planner.lattice_primitives.trajectory import (FloatNDArray, Path, Trajectory, + TrajectoryFloat, TrajectoryParameters) import numpy as np -from trajectory import FloatNDArray, Path, Trajectory, TrajectoryFloat, TrajectoryParameters - logger = logging.getLogger(__name__) diff --git a/tools/pyproject.toml b/tools/pyproject.toml index e0001ef7bff..663c8243821 100644 --- a/tools/pyproject.toml +++ b/tools/pyproject.toml @@ -12,3 +12,11 @@ write-changes = false profile = "google" force_single_line = false line_length = 99 + +[tool.mypy] +explicit_package_bases = true +strict = true + +[[tool.mypy.overrides]] +module = ["matplotlib.*", "rtree.*"] +ignore_missing_imports = true From a5ffa3fc77eb215af9f1f2c0a61c6614b3c70991 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Mon, 31 Mar 2025 03:28:12 +0100 Subject: [PATCH 11/70] Mypy nav2 common (#5031) * Configured mypy for nav2_common Signed-off-by: Leander Stephen D'Souza * Added nav2_common to the workflow. Signed-off-by: Leander Stephen D'Souza * Removed all instances of Any. Signed-off-by: Leander Stephen D'Souza * Fixed unsafe fixes using ruff. Signed-off-by: Leander Stephen D'Souza --------- Signed-off-by: Leander Stephen D'Souza --- .github/workflows/lint.yml | 1 + .../nav2_common/launch/has_node_params.py | 17 ++-- .../nav2_common/launch/replace_string.py | 26 +++--- .../nav2_common/launch/rewritten_yaml.py | 83 ++++++++++++------- tools/pyproject.toml | 7 +- 5 files changed, 84 insertions(+), 50 deletions(-) diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 85eb94e6cd9..3a68be4216f 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -29,6 +29,7 @@ jobs: matrix: mypy_packages: - "nav2_smac_planner" + - "nav2_common" steps: - uses: actions/checkout@v4 diff --git a/nav2_common/nav2_common/launch/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py index 6e386128ce0..fa36f280785 100644 --- a/nav2_common/nav2_common/launch/has_node_params.py +++ b/nav2_common/nav2_common/launch/has_node_params.py @@ -12,13 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import List, Text - import launch import yaml -class HasNodeParams(launch.Substitution): +class HasNodeParams(launch.Substitution): # type: ignore[misc] """ Substitution that checks if a param file contains parameters for a node. @@ -26,7 +24,7 @@ class HasNodeParams(launch.Substitution): """ def __init__( - self, source_file: launch.SomeSubstitutionsType, node_name: Text + self, source_file: launch.SomeSubstitutionsType, node_name: str ) -> None: super().__init__() """ @@ -39,21 +37,22 @@ def __init__( # import here to avoid loop from launch.utilities import normalize_to_list_of_substitutions - self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__source_file: list[launch.Substitution] = \ + normalize_to_list_of_substitutions(source_file) self.__node_name = node_name @property - def name(self) -> List[launch.Substitution]: + def name(self) -> list[launch.Substitution]: """Getter for name.""" return self.__source_file - def describe(self) -> Text: + def describe(self) -> str: """Return a description of this substitution as a string.""" return '' - def perform(self, context: launch.LaunchContext) -> Text: + def perform(self, context: launch.LaunchContext) -> str: yaml_filename = launch.utilities.perform_substitutions(context, self.name) - data = yaml.safe_load(open(yaml_filename, 'r')) + data = yaml.safe_load(open(yaml_filename)) if self.__node_name in data.keys(): return 'True' diff --git a/nav2_common/nav2_common/launch/replace_string.py b/nav2_common/nav2_common/launch/replace_string.py index cb55b883380..e379c045457 100644 --- a/nav2_common/nav2_common/launch/replace_string.py +++ b/nav2_common/nav2_common/launch/replace_string.py @@ -13,12 +13,12 @@ # limitations under the License. import tempfile -from typing import Dict, List, Optional, Text +from typing import Optional import launch -class ReplaceString(launch.Substitution): +class ReplaceString(launch.Substitution): # type: ignore[misc] """ Substitution that replaces strings on a given file. @@ -28,7 +28,7 @@ class ReplaceString(launch.Substitution): def __init__( self, source_file: launch.SomeSubstitutionsType, - replacements: Dict, + replacements: dict[str, launch.SomeSubstitutionsType], condition: Optional[launch.Condition] = None, ) -> None: super().__init__() @@ -37,7 +37,8 @@ def __init__( # import here to avoid loop - self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__source_file: list[launch.Substitution] = \ + normalize_to_list_of_substitutions(source_file) self.__replacements = {} for key in replacements: self.__replacements[key] = normalize_to_list_of_substitutions( @@ -46,7 +47,7 @@ def __init__( self.__condition = condition @property - def name(self) -> List[launch.Substitution]: + def name(self) -> list[launch.Substitution]: """Getter for name.""" return self.__source_file @@ -55,17 +56,19 @@ def condition(self) -> Optional[launch.Condition]: """Getter for condition.""" return self.__condition - def describe(self) -> Text: + def describe(self) -> str: """Return a description of this substitution as a string.""" return '' - def perform(self, context: launch.LaunchContext) -> Text: - yaml_filename = launch.utilities.perform_substitutions(context, self.name) + def perform(self, context: launch.LaunchContext) -> str: + yaml_filename: str = launch.utilities.perform_substitutions( + context, self.name + ) if self.__condition is None or self.__condition.evaluate(context): output_file = tempfile.NamedTemporaryFile(mode='w', delete=False) replacements = self.resolve_replacements(context) try: - input_file = open(yaml_filename, 'r') + input_file = open(yaml_filename) self.replace(input_file, output_file, replacements) except Exception as err: # noqa: B902 print('ReplaceString substitution error: ', err) @@ -76,7 +79,7 @@ def perform(self, context: launch.LaunchContext) -> Text: else: return yaml_filename - def resolve_replacements(self, context): + def resolve_replacements(self, context: launch.LaunchContext) -> dict[str, str]: resolved_replacements = {} for key in self.__replacements: resolved_replacements[key] = launch.utilities.perform_substitutions( @@ -84,7 +87,8 @@ def resolve_replacements(self, context): ) return resolved_replacements - def replace(self, input_file, output_file, replacements): + def replace(self, input_file: launch.SomeSubstitutionsType, + output_file: launch.SomeSubstitutionsType, replacements: dict[str, str]) -> None: for line in input_file: for key, value in replacements.items(): if isinstance(key, str) and isinstance(value, str): diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 5434f611f51..49ec39b5377 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -12,27 +12,30 @@ # See the License for the specific language governing permissions and # limitations under the License. +from collections.abc import Generator import tempfile -from typing import Dict, List, Optional, Text +from typing import Optional, TypeAlias, Union import launch import yaml +YamlValue: TypeAlias = Union[str, int, float, bool] + class DictItemReference: - def __init__(self, dictionary, key): + def __init__(self, dictionary: dict[str, YamlValue], key: str): self.dictionary = dictionary self.dictKey = key - def key(self): + def key(self) -> str: return self.dictKey - def setValue(self, value): + def setValue(self, value: YamlValue) -> None: self.dictionary[self.dictKey] = value -class RewrittenYaml(launch.Substitution): +class RewrittenYaml(launch.Substitution): # type: ignore[misc] """ Substitution that modifies the given YAML file. @@ -42,10 +45,10 @@ class RewrittenYaml(launch.Substitution): def __init__( self, source_file: launch.SomeSubstitutionsType, - param_rewrites: Dict, + param_rewrites: dict[str, launch.SomeSubstitutionsType], root_key: Optional[launch.SomeSubstitutionsType] = None, - key_rewrites: Optional[Dict] = None, - convert_types=False, + key_rewrites: Optional[dict[str, launch.SomeSubstitutionsType]] = None, + convert_types: bool = False, ) -> None: super().__init__() """ @@ -61,7 +64,8 @@ def __init__( # import here to avoid loop from launch.utilities import normalize_to_list_of_substitutions - self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__source_file: list[launch.Substitution] = \ + normalize_to_list_of_substitutions(source_file) self.__param_rewrites = {} self.__key_rewrites = {} self.__convert_types = convert_types @@ -79,19 +83,19 @@ def __init__( self.__root_key = normalize_to_list_of_substitutions(root_key) @property - def name(self) -> List[launch.Substitution]: + def name(self) -> list[launch.Substitution]: """Getter for name.""" return self.__source_file - def describe(self) -> Text: + def describe(self) -> str: """Return a description of this substitution as a string.""" return '' - def perform(self, context: launch.LaunchContext) -> Text: + def perform(self, context: launch.LaunchContext) -> str: yaml_filename = launch.utilities.perform_substitutions(context, self.name) rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) param_rewrites, keys_rewrites = self.resolve_rewrites(context) - data = yaml.safe_load(open(yaml_filename, 'r')) + data = yaml.safe_load(open(yaml_filename)) self.substitute_params(data, param_rewrites) self.add_params(data, param_rewrites) self.substitute_keys(data, keys_rewrites) @@ -103,7 +107,8 @@ def perform(self, context: launch.LaunchContext) -> Text: rewritten_yaml.close() return rewritten_yaml.name - def resolve_rewrites(self, context): + def resolve_rewrites(self, context: launch.LaunchContext) -> \ + tuple[dict[str, str], dict[str, str]]: resolved_params = {} for key in self.__param_rewrites: resolved_params[key] = launch.utilities.perform_substitutions( @@ -116,7 +121,8 @@ def resolve_rewrites(self, context): ) return resolved_params, resolved_keys - def substitute_params(self, yaml, param_rewrites): + def substitute_params(self, yaml: dict[str, YamlValue], + param_rewrites: dict[str, str]) -> None: # substitute leaf-only parameters for key in self.getYamlLeafKeys(yaml): if key.key() in param_rewrites: @@ -132,7 +138,8 @@ def substitute_params(self, yaml, param_rewrites): yaml_keys = path.split('.') yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val) - def add_params(self, yaml, param_rewrites): + def add_params(self, yaml: dict[str, YamlValue], + param_rewrites: dict[str, str]) -> None: # add new total path parameters yaml_paths = self.pathify(yaml) for path in param_rewrites: @@ -142,7 +149,10 @@ def add_params(self, yaml, param_rewrites): if 'ros__parameters' in yaml_keys: yaml = self.updateYamlPathVals(yaml, yaml_keys, new_val) - def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): + def updateYamlPathVals( + self, yaml: dict[str, YamlValue], + yaml_key_list: list[str], rewrite_val: YamlValue) -> dict[str, YamlValue]: + for key in yaml_key_list: if key == yaml_key_list[-1]: yaml[key] = rewrite_val @@ -153,12 +163,15 @@ def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): yaml[int(key)], yaml_key_list, rewrite_val ) else: - yaml[key] = self.updateYamlPathVals( - yaml.get(key, {}), yaml_key_list, rewrite_val + yaml[key] = self.updateYamlPathVals( # type: ignore[assignment] + yaml.get(key, {}), # type: ignore[arg-type] + yaml_key_list, + rewrite_val ) return yaml - def substitute_keys(self, yaml, key_rewrites): + def substitute_keys( + self, yaml: dict[str, YamlValue], key_rewrites: dict[str, str]) -> None: if len(key_rewrites) != 0: for key in list(yaml.keys()): val = yaml[key] @@ -169,20 +182,31 @@ def substitute_keys(self, yaml, key_rewrites): if isinstance(val, dict): self.substitute_keys(val, key_rewrites) - def getYamlLeafKeys(self, yamlData): - try: - for key in yamlData.keys(): - for k in self.getYamlLeafKeys(yamlData[key]): - yield k - yield DictItemReference(yamlData, key) - except AttributeError: + def getYamlLeafKeys(self, yamlData: dict[str, YamlValue]) -> \ + Generator[DictItemReference, None, None]: + if not isinstance(yamlData, dict): return - def pathify(self, d, p=None, paths=None, joinchar='.'): + for key in yamlData.keys(): + child = yamlData[key] + + if isinstance(child, dict): + # Recursively process nested dictionaries + yield from self.getYamlLeafKeys(child) + + yield DictItemReference(yamlData, key) + + def pathify( + self, d: Union[dict[str, YamlValue], list[YamlValue], YamlValue], + p: Optional[str] = None, + paths: Optional[dict[str, YamlValue]] = None, + joinchar: str = '.') -> dict[str, YamlValue]: if p is None: paths = {} self.pathify(d, '', paths, joinchar=joinchar) return paths + + assert paths is not None pn = p if p != '': pn += joinchar @@ -195,8 +219,9 @@ def pathify(self, d, p=None, paths=None, joinchar='.'): self.pathify(e, pn + str(idx), paths, joinchar=joinchar) else: paths[p] = d + return paths - def convert(self, text_value): + def convert(self, text_value: str) -> YamlValue: if self.__convert_types: # try converting to int or float try: diff --git a/tools/pyproject.toml b/tools/pyproject.toml index 663c8243821..ab81e85d120 100644 --- a/tools/pyproject.toml +++ b/tools/pyproject.toml @@ -18,5 +18,10 @@ explicit_package_bases = true strict = true [[tool.mypy.overrides]] -module = ["matplotlib.*", "rtree.*"] +module = [ + "matplotlib.*", + "rtree.*", + "launch.*", +] + ignore_missing_imports = true From 49363184fc1727ef5560a8c2a89ebb7fe358ec2b Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Mon, 31 Mar 2025 21:25:19 +0100 Subject: [PATCH 12/70] Clear costmap if reset distance exceeds costmap bounds. (#5010) * Migrate costmap bound check to clearArea for STVL override Signed-off-by: Leander Stephen D'Souza * Added unbounded map to world function. Signed-off-by: Leander Stephen D'Souza * Added test for mapToWorldNoBounds Signed-off-by: Leander Stephen D'Souza --------- Signed-off-by: Leander Stephen D'Souza --- .../include/nav2_costmap_2d/costmap_2d.hpp | 9 +++ nav2_costmap_2d/src/clear_costmap_service.cpp | 4 +- nav2_costmap_2d/src/costmap_2d.cpp | 6 ++ nav2_costmap_2d/src/costmap_layer.cpp | 13 +++- nav2_costmap_2d/test/unit/CMakeLists.txt | 5 ++ .../test/unit/coordinate_transform_test.cpp | 59 +++++++++++++++++++ 6 files changed, 92 insertions(+), 4 deletions(-) create mode 100644 nav2_costmap_2d/test/unit/coordinate_transform_test.cpp diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index b30179977d5..fbacf15aa24 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -173,6 +173,15 @@ class Costmap2D */ void mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const; + /** + * @brief Convert from map coordinates to world coordinates with no bounds checking + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + */ + void mapToWorldNoBounds(int mx, int my, double & wx, double & wy) const; + /** * @brief Convert from world coordinates to map coordinates * @param wx The x world coordinate diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index e11aca2ce5a..97e0b6f5c14 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -135,8 +135,8 @@ void ClearCostmapService::clearLayerRegion( double end_point_y = start_point_y + reset_distance; int start_x, start_y, end_x, end_y; - costmap->worldToMapEnforceBounds(start_point_x, start_point_y, start_x, start_y); - costmap->worldToMapEnforceBounds(end_point_x, end_point_y, end_x, end_y); + costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y); + costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y); costmap->clearArea(start_x, start_y, end_x, end_y, invert); diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 1d9997de8da..40d8284f6f4 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -282,6 +282,12 @@ void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double wy = origin_y_ + (my + 0.5) * resolution_; } +void Costmap2D::mapToWorldNoBounds(int mx, int my, double & wx, double & wy) const +{ + wx = origin_x_ + (mx + 0.5) * resolution_; + wy = origin_y_ + (my + 0.5) * resolution_; +} + bool Costmap2D::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const { if (wx < origin_x_ || wy < origin_y_) { diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index 93f20f664dc..72a0120cd58 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -66,10 +66,19 @@ void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, boo { current_ = false; unsigned char * grid = getCharMap(); - for (int x = 0; x < static_cast(getSizeInCellsX()); x++) { + + int size_x = getSizeInCellsX(); + int size_y = getSizeInCellsY(); + + start_x = std::clamp(start_x, 0, size_x); + start_y = std::clamp(start_y, 0, size_y); + end_x = std::clamp(end_x, 0, size_x); + end_y = std::clamp(end_y, 0, size_y); + + for (int x = 0; x < size_x; x++) { bool xrange = x > start_x && x < end_x; - for (int y = 0; y < static_cast(getSizeInCellsY()); y++) { + for (int y = 0; y < size_y; y++) { if ((xrange && y > start_y && y < end_y) == invert) { continue; } diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 291ffb062f9..559ef4ae4e2 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -62,3 +62,8 @@ ament_add_gtest(lifecycle_test lifecycle_test.cpp) target_link_libraries(lifecycle_test nav2_costmap_2d_core ) + +ament_add_gtest(coordinate_transform_test coordinate_transform_test.cpp) +target_link_libraries(coordinate_transform_test + nav2_costmap_2d_core +) diff --git a/nav2_costmap_2d/test/unit/coordinate_transform_test.cpp b/nav2_costmap_2d/test/unit/coordinate_transform_test.cpp new file mode 100644 index 00000000000..b221943a782 --- /dev/null +++ b/nav2_costmap_2d/test/unit/coordinate_transform_test.cpp @@ -0,0 +1,59 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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. Reserved. + +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" + + +/** + * Test for mapToWorldNoBounds + */ + +TEST(mapToWorldNoBounds, MapToWorldNoBoundsNegativeMapCoords) +{ + double wx, wy; + + std::unique_ptr map; + + map = std::make_unique(10, 10, 1.0, 0.0, 0.0); + map->mapToWorldNoBounds(-1, -1, wx, wy); + EXPECT_DOUBLE_EQ(wx, -0.5); + EXPECT_DOUBLE_EQ(wy, -0.5); + + map = std::make_unique(10, 10, 1.0, 1.0, 2.0); + map->mapToWorldNoBounds(-5, -5, wx, wy); + EXPECT_DOUBLE_EQ(wx, -3.5); + EXPECT_DOUBLE_EQ(wy, -2.5); + + map = std::make_unique(10, 10, 2.0, 3.0, 4.0); + map->mapToWorldNoBounds(-10, -10, wx, wy); + EXPECT_DOUBLE_EQ(wx, -16.0); + EXPECT_DOUBLE_EQ(wy, -15.0); +} + + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(0, nullptr); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +} From e90717de5d2895d288bb06de7373c2c7553af4f1 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 2 Apr 2025 01:02:31 +0800 Subject: [PATCH 13/70] Add near collision cost and warnings for misaligned parameter settings in MPPI critics (#4996) * Add warning when settings not aligned; Add tunable option for near collision Signed-off-by: mini-1235 * Linting Signed-off-by: mini-1235 * Add more information for warning Signed-off-by: mini-1235 * Add test for coverage Signed-off-by: mini-1235 * Throw controller exception instead of invalid argument Signed-off-by: mini-1235 --------- Signed-off-by: mini-1235 --- nav2_bringup/params/nav2_params.yaml | 3 +- .../critics/cost_critic.hpp | 1 + .../src/critics/cost_critic.cpp | 36 ++++++--- .../src/critics/obstacles_critic.cpp | 14 ++++ nav2_mppi_controller/test/critics_tests.cpp | 75 +++++++++++++++++++ .../gps_navigation/nav2_no_map_params.yaml | 3 +- 6 files changed, 119 insertions(+), 13 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 04021ac6d52..0c670571b24 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -161,8 +161,9 @@ controller_server: enabled: true cost_power: 1 cost_weight: 3.81 + near_collision_cost: 253 critical_cost: 300.0 - consider_footprint: true + consider_footprint: false collision_cost: 1000000.0 near_goal_distance: 1.0 trajectory_point_step: 2 diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index f9f1f8d7939..c285a14b664 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -133,6 +133,7 @@ class CostCritic : public CriticFunction float circumscribed_cost_{0.0f}; float collision_cost_{0.0f}; float critical_cost_{0.0f}; + unsigned int near_collision_cost_{253}; float weight_{0}; unsigned int trajectory_point_step_; diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 730f13423aa..c39d669bde3 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -15,6 +15,7 @@ #include #include "nav2_mppi_controller/critics/cost_critic.hpp" +#include "nav2_core/controller_exceptions.hpp" namespace mppi::critics { @@ -26,6 +27,7 @@ void CostCritic::initialize() getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.81f); getParam(critical_cost_, "critical_cost", 300.0f); + getParam(near_collision_cost_, "near_collision_cost", 253); getParam(collision_cost_, "collision_cost", 1000000.0f); getParam(near_goal_distance_, "near_goal_distance", 0.5f); getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); @@ -54,6 +56,22 @@ void CostCritic::initialize() " for full instructions. This will substantially impact run-time performance."); } + if (costmap_ros_->getUseRadius() == consider_footprint_) { + RCLCPP_WARN( + logger_, + "Inconsistent configuration in collision checking. Please verify the robot's shape settings " + "in both the costmap and the cost critic."); + if (costmap_ros_->getUseRadius()) { + throw nav2_core::ControllerException( + "Considering footprint in collision checking but no robot footprint provided in the " + "costmap."); + } + } + + if(near_collision_cost_ > 253) { + RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE"); + } + RCLCPP_INFO( logger_, "InflationCostCritic instantiated with %d power and %f / %f weights. " @@ -153,28 +171,24 @@ void CostCritic::score(CriticData & data) // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it // So the center point has more information than the footprint if (!worldToMapFloat(Tx, Ty, x_i, y_i)) { - if (!is_tracking_unknown_) { - traj_cost = collision_cost_; - trajectory_collide = true; - break; - } pose_cost = 255.0f; // NO_INFORMATION in float } else { pose_cost = static_cast(costmap->getCost(getIndex(x_i, y_i))); if (pose_cost < 1.0f) { continue; // In free space } - if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) { - traj_cost = collision_cost_; - trajectory_collide = true; - break; - } + } + + if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) { + traj_cost = collision_cost_; + trajectory_collide = true; + break; } // Let near-collision trajectory points be punished severely // Note that we collision check based on the footprint actual, // but score based on the center-point cost regardless - if (pose_cost >= 253.0f /*INSCRIBED_INFLATED_OBSTACLE in float*/) { + if (pose_cost >= static_cast(near_collision_cost_)) { traj_cost += critical_cost_; } else if (!near_goal) { // Generally prefer trajectories further from obstacles traj_cost += pose_cost; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 5d05dcd3e69..c469388fce8 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -16,6 +16,8 @@ #include #include "nav2_mppi_controller/critics/obstacles_critic.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" +#include "nav2_core/controller_exceptions.hpp" + namespace mppi::critics { @@ -44,6 +46,18 @@ void ObstaclesCritic::initialize() " for full instructions. This will substantially impact run-time performance."); } + if (costmap_ros_->getUseRadius() == consider_footprint_) { + RCLCPP_WARN( + logger_, + "Inconsistent configuration in collision checking. Please verify the robot's shape settings " + "in both the costmap and the obstacle critic."); + if (costmap_ros_->getUseRadius()) { + throw nav2_core::ControllerException( + "Considering footprint in collision checking but no robot footprint provided in the " + "costmap."); + } + } + RCLCPP_INFO( logger_, "ObstaclesCritic instantiated with %d power and %f / %f weights. " diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 924c127b32f..323c0d94378 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -126,6 +126,81 @@ TEST(CriticTests, ConstraintsCritic) EXPECT_NEAR(costs(1), 0.48, 0.01); } +TEST(CriticTests, ObstacleCriticMisalignedParams) { + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + ParametersHandler param_handler(node); + auto getParam = param_handler.getParamGetter("critic"); + bool consider_footprint; + getParam(consider_footprint, "consider_footprint", true); + + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + ObstaclesCritic critic; + // Expect throw when settings mismatched + EXPECT_THROW( + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler), + nav2_core::ControllerException + ); +} + +TEST(CriticTests, ObstacleCriticAlignedParams) { + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + ParametersHandler param_handler(node); + auto getParam = param_handler.getParamGetter("critic"); + bool consider_footprint; + getParam(consider_footprint, "consider_footprint", false); + + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + ObstaclesCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); +} + + +TEST(CriticTests, CostCriticMisAlignedParams) { + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + auto getParam = param_handler.getParamGetter("critic"); + bool consider_footprint; + getParam(consider_footprint, "consider_footprint", true); + costmap_ros->on_configure(lstate); + + CostCritic critic; + // Expect throw when settings mismatched + EXPECT_THROW( + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler), + nav2_core::ControllerException + ); +} + +TEST(CriticTests, CostCriticAlignedParams) { + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + ParametersHandler param_handler(node); + rclcpp_lifecycle::State lstate; + auto getParam = param_handler.getParamGetter("critic"); + bool consider_footprint; + getParam(consider_footprint, "consider_footprint", false); + costmap_ros->on_configure(lstate); + + CostCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); +} + TEST(CriticTests, GoalAngleCritic) { // Standard preamble diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index 19f3b0e0a2d..aa6abeb782c 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -109,8 +109,9 @@ controller_server: enabled: true cost_power: 1 cost_weight: 3.81 + near_collision_cost: 253 critical_cost: 300.0 - consider_footprint: true + consider_footprint: false collision_cost: 1000000.0 near_goal_distance: 1.0 trajectory_point_step: 2 From 08268f9922f92511f12ad9b951148d1c1d30096b Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Tue, 1 Apr 2025 18:08:42 +0100 Subject: [PATCH 14/70] Added pre-commit with ament ignores to the workflow. (#5029) * Added pre-commit with ament ignores to the workflow. Signed-off-by: Leander Stephen D'Souza * Removed codespell from workflow and enabled write changes in pre-commit. Signed-off-by: Leander Stephen D'Souza * Update pull request template with a testing description section. Signed-off-by: Leander Stephen D'Souza --------- Signed-off-by: Leander Stephen D'Souza --- .github/PULL_REQUEST_TEMPLATE.md | 7 +++++++ .github/workflows/codespell.yml | 14 -------------- .github/workflows/lint.yml | 16 ++++++++++++++++ tools/pyproject.toml | 2 +- 4 files changed, 24 insertions(+), 15 deletions(-) delete mode 100644 .github/workflows/codespell.yml diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 2a2e2339ee5..5b818d8e6e5 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -13,6 +13,13 @@ --- +## Description of testing performed + + ## Description of contribution in a few bullet points + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml new file mode 100644 index 00000000000..c25970af506 --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.xml @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_core/include/nav2_core/route_exceptions.hpp b/nav2_core/include/nav2_core/route_exceptions.hpp new file mode 100644 index 00000000000..aed9b304279 --- /dev/null +++ b/nav2_core/include/nav2_core/route_exceptions.hpp @@ -0,0 +1,83 @@ +// Copyright (c) 2023, Samsung Research America +// +// 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 NAV2_CORE__ROUTE_EXCEPTIONS_HPP_ +#define NAV2_CORE__ROUTE_EXCEPTIONS_HPP_ + +#include +#include +#include + +namespace nav2_core +{ + +class RouteException : public std::runtime_error +{ +public: + explicit RouteException(const std::string & description) + : std::runtime_error(description) {} +}; + +class OperationFailed : public RouteException +{ +public: + explicit OperationFailed(const std::string & description) + : RouteException(description) {} +}; + +class NoValidRouteCouldBeFound : public RouteException +{ +public: + explicit NoValidRouteCouldBeFound(const std::string & description) + : RouteException(description) {} +}; + +class TimedOut : public RouteException +{ +public: + explicit TimedOut(const std::string & description) + : RouteException(description) {} +}; + +class RouteTFError : public RouteException +{ +public: + explicit RouteTFError(const std::string & description) + : RouteException(description) {} +}; + +class NoValidGraph : public RouteException +{ +public: + explicit NoValidGraph(const std::string & description) + : RouteException(description) {} +}; + +class IndeterminantNodesOnGraph : public RouteException +{ +public: + explicit IndeterminantNodesOnGraph(const std::string & description) + : RouteException(description) {} +}; + +class InvalidEdgeScorerUse : public RouteException +{ +public: + explicit InvalidEdgeScorerUse(const std::string & description) + : RouteException(description) {} +}; + +} // namespace nav2_core + +#endif // NAV2_CORE__ROUTE_EXCEPTIONS_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index 4f50614876c..955a8c80af4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -65,6 +65,11 @@ class CostmapSubscriber */ void costmapUpdateCallback(const nav2_msgs::msg::CostmapUpdate::SharedPtr update_msg); + std::string getFrameID() const + { + return frame_id_; + } + protected: bool isCostmapReceived() {return costmap_ != nullptr;} void processCurrentCostmapMsg(); @@ -81,6 +86,7 @@ class CostmapSubscriber nav2_msgs::msg::Costmap::SharedPtr costmap_msg_; std::string topic_name_; + std::string frame_id_; std::mutex costmap_msg_mutex_; rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; }; diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index 0426b87be30..bdbc5e11ccb 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -73,6 +73,7 @@ void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr { std::lock_guard lock(costmap_msg_mutex_); costmap_msg_ = msg; + frame_id_ = costmap_msg_->header.frame_id; } if (!isCostmapReceived()) { costmap_ = std::make_shared( diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 4319129ecbc..dc49bf40f01 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -31,9 +31,13 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" + "msg/WaypointStatus.msg" + "msg/Route.msg" + "msg/RouteNode.msg" + "msg/RouteEdge.msg" + "msg/EdgeCost.msg" "msg/Trajectory.msg" "msg/TrajectoryPoint.msg" - "msg/WaypointStatus.msg" "srv/GetCosts.srv" "srv/GetCostmap.srv" "srv/IsPathValid.srv" @@ -45,6 +49,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SaveMap.srv" "srv/SetInitialPose.srv" "srv/ReloadDockDatabase.srv" + "srv/SetRouteGraph.srv" + "srv/DynamicEdges.srv" "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" @@ -61,6 +67,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/FollowGPSWaypoints.action" "action/DockRobot.action" "action/UndockRobot.action" + "action/ComputeRoute.action" + "action/ComputeAndTrackRoute.action" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs ) diff --git a/nav2_msgs/action/ComputeAndTrackRoute.action b/nav2_msgs/action/ComputeAndTrackRoute.action new file mode 100644 index 00000000000..0631ee20f8b --- /dev/null +++ b/nav2_msgs/action/ComputeAndTrackRoute.action @@ -0,0 +1,34 @@ +#goal definition +uint16 start_id +geometry_msgs/PoseStamped start +uint16 goal_id +geometry_msgs/PoseStamped goal + +bool use_start # Whether to use the start field or find the start pose in TF +bool use_poses # Whether to use the poses or the IDs fields for request +--- +#result definition + +# Error codes +uint16 NONE=0 +uint16 UNKNOWN=400 +uint16 TF_ERROR=401 +uint16 NO_VALID_GRAPH=402 +uint16 INDETERMINANT_NODES_ON_GRAPH=403 +uint16 TIMEOUT=404 +uint16 NO_VALID_ROUTE=405 +uint16 OPERATION_FAILED=406 +uint16 INVALID_EDGE_SCORER_USE=407 + +builtin_interfaces/Duration execution_duration +uint16 error_code 0 +string error_msg +--- +#feedback definition +uint16 last_node_id +uint16 next_node_id +uint16 current_edge_id +Route route +nav_msgs/Path path +string[] operations_triggered +bool rerouted diff --git a/nav2_msgs/action/ComputeRoute.action b/nav2_msgs/action/ComputeRoute.action new file mode 100644 index 00000000000..60bee2205b8 --- /dev/null +++ b/nav2_msgs/action/ComputeRoute.action @@ -0,0 +1,28 @@ +#goal definition +uint16 start_id +geometry_msgs/PoseStamped start +uint16 goal_id +geometry_msgs/PoseStamped goal + +bool use_start # Whether to use the start field or find the start pose in TF +bool use_poses # Whether to use the poses or the IDs fields for request +--- +#result definition + +# Error codes +uint16 NONE=0 +uint16 UNKNOWN=400 +uint16 TF_ERROR=401 +uint16 NO_VALID_GRAPH=402 +uint16 INDETERMINANT_NODES_ON_GRAPH=403 +uint16 TIMEOUT=404 +uint16 NO_VALID_ROUTE=405 +uint16 INVALID_EDGE_SCORER_USE=407 + +builtin_interfaces/Duration planning_time +nav_msgs/Path path +Route route +uint16 error_code 0 +string error_msg +--- +#feedback definition diff --git a/nav2_msgs/action/__init__.py b/nav2_msgs/action/__init__.py index a4036af8cda..e3dee59a2e0 100644 --- a/nav2_msgs/action/__init__.py +++ b/nav2_msgs/action/__init__.py @@ -1,7 +1,9 @@ from nav2_msgs.action._assisted_teleop import AssistedTeleop from nav2_msgs.action._back_up import BackUp +from nav2_msgs.action._compute_and_track_route import ComputeAndTrackRoute from nav2_msgs.action._compute_path_through_poses import ComputePathThroughPoses from nav2_msgs.action._compute_path_to_pose import ComputePathToPose +from nav2_msgs.action._compute_route import ComputeRoute from nav2_msgs.action._dock_robot import DockRobot from nav2_msgs.action._drive_on_heading import DriveOnHeading from nav2_msgs.action._dummy_behavior import DummyBehavior @@ -18,8 +20,10 @@ __all__ = [ 'AssistedTeleop', 'BackUp', + 'ComputeAndTrackRoute', 'ComputePathThroughPoses', 'ComputePathToPose', + 'ComputeRoute', 'DockRobot', 'DriveOnHeading', 'DummyBehavior', diff --git a/nav2_msgs/msg/EdgeCost.msg b/nav2_msgs/msg/EdgeCost.msg new file mode 100644 index 00000000000..5200aee851f --- /dev/null +++ b/nav2_msgs/msg/EdgeCost.msg @@ -0,0 +1,3 @@ +# Edge cost to use with nav2_msgs/srv/DynamicEdges to adjust route edge costs +uint16 edgeid +float32 cost diff --git a/nav2_msgs/msg/Route.msg b/nav2_msgs/msg/Route.msg new file mode 100644 index 00000000000..0ebc591c9c8 --- /dev/null +++ b/nav2_msgs/msg/Route.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +float32 route_cost +RouteNode[] nodes # ordered set of nodes of the route +RouteEdge[] edges # ordered set of edges that connect nodes diff --git a/nav2_msgs/msg/RouteEdge.msg b/nav2_msgs/msg/RouteEdge.msg new file mode 100644 index 00000000000..d257c9edce4 --- /dev/null +++ b/nav2_msgs/msg/RouteEdge.msg @@ -0,0 +1,3 @@ +uint16 edgeid +geometry_msgs/Point start +geometry_msgs/Point end diff --git a/nav2_msgs/msg/RouteNode.msg b/nav2_msgs/msg/RouteNode.msg new file mode 100644 index 00000000000..89f328a50e3 --- /dev/null +++ b/nav2_msgs/msg/RouteNode.msg @@ -0,0 +1,2 @@ +uint16 nodeid +geometry_msgs/Point position diff --git a/nav2_msgs/msg/__init__.py b/nav2_msgs/msg/__init__.py index a6e8fcd4219..6ab637b85c1 100644 --- a/nav2_msgs/msg/__init__.py +++ b/nav2_msgs/msg/__init__.py @@ -9,6 +9,7 @@ from nav2_msgs.msg._missed_waypoint import MissedWaypoint from nav2_msgs.msg._particle import Particle from nav2_msgs.msg._particle_cloud import ParticleCloud +from nav2_msgs.msg._route import Route from nav2_msgs.msg._speed_limit import SpeedLimit from nav2_msgs.msg._voxel_grid import VoxelGrid @@ -24,6 +25,7 @@ 'MissedWaypoint', 'Particle', 'ParticleCloud', + 'Route', 'SpeedLimit', 'VoxelGrid', ] diff --git a/nav2_msgs/srv/DynamicEdges.srv b/nav2_msgs/srv/DynamicEdges.srv new file mode 100644 index 00000000000..e725c7ef75a --- /dev/null +++ b/nav2_msgs/srv/DynamicEdges.srv @@ -0,0 +1,5 @@ +uint16[] closed_edges +uint16[] opened_edges +EdgeCost[] adjust_edges +--- +bool success diff --git a/nav2_msgs/srv/SetRouteGraph.srv b/nav2_msgs/srv/SetRouteGraph.srv new file mode 100644 index 00000000000..d2d8092b67c --- /dev/null +++ b/nav2_msgs/srv/SetRouteGraph.srv @@ -0,0 +1,3 @@ +string graph_filepath +--- +bool success diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index f949f980a95..7d29119f31b 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -384,6 +384,7 @@ void PlannerServer::computePlanThroughPoses() auto goal = action_server_poses_->get_current_goal(); auto result = std::make_shared(); nav_msgs::msg::Path concat_path; + RCLCPP_INFO(get_logger(), "Computing path through poses to goal."); geometry_msgs::msg::PoseStamped curr_start, curr_goal; @@ -515,6 +516,7 @@ PlannerServer::computePlan() // Initialize the ComputePathToPose goal and result auto goal = action_server_pose_->get_current_goal(); auto result = std::make_shared(); + RCLCPP_INFO(get_logger(), "Computing path to goal."); geometry_msgs::msg::PoseStamped start; diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt new file mode 100644 index 00000000000..db7825a0c96 --- /dev/null +++ b/nav2_route/CMakeLists.txt @@ -0,0 +1,269 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_route CXX) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav2_util REQUIRED) +find_package(angles REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nanoflann REQUIRED) +find_package(nlohmann_json REQUIRED) + +nav2_package() + +set(executable_name route_server) +set(library_name ${executable_name}_core) + +include_directories( + include +) + +# Main library +add_library(${library_name} SHARED + src/route_server.cpp + src/route_planner.cpp + src/route_tracker.cpp + src/edge_scorer.cpp + src/operations_manager.cpp + src/node_spatial_tree.cpp + src/path_converter.cpp + src/graph_loader.cpp + src/graph_saver.cpp + src/goal_intent_extractor.cpp +) + +target_include_directories(${library_name} + PUBLIC + "$" + "$" +) +target_link_libraries(${library_name} PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${std_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + nlohmann_json::nlohmann_json +) +target_link_libraries(${library_name} PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 +) + +# Main executable +add_executable(${executable_name} + src/main.cpp +) + +target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp) + +rclcpp_components_register_nodes(${library_name} "nav2_route::RouteServer") + +# Edge scoring plugins +add_library(edge_scorers SHARED + src/plugins/edge_cost_functions/distance_scorer.cpp + src/plugins/edge_cost_functions/time_scorer.cpp + src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp + src/plugins/edge_cost_functions/penalty_scorer.cpp + src/plugins/edge_cost_functions/costmap_scorer.cpp + src/plugins/edge_cost_functions/semantic_scorer.cpp + src/plugins/edge_cost_functions/goal_orientation_scorer.cpp + src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp +) +target_include_directories(edge_scorers + PUBLIC + "$" + "$" +) +target_link_libraries(edge_scorers PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${std_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + angles::angles + nlohmann_json::nlohmann_json +) +target_link_libraries(edge_scorers PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 +) + +# Route operations plugins +add_library(route_operations SHARED + src/plugins/route_operations/adjust_speed_limit.cpp + src/plugins/route_operations/trigger_event.cpp + src/plugins/route_operations/rerouting_service.cpp + src/plugins/route_operations/collision_monitor.cpp + src/plugins/route_operations/time_marker.cpp +) +target_include_directories(route_operations + PUBLIC + "$" + "$" +) +target_link_libraries(route_operations PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${std_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + nlohmann_json::nlohmann_json +) +target_link_libraries(route_operations PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 +) + +# Graph Parser plugins +add_library(graph_file_loaders SHARED + src/plugins/graph_file_loaders/geojson_graph_file_loader.cpp +) +target_include_directories(graph_file_loaders + PUBLIC + "$" + "$" +) +target_link_libraries(graph_file_loaders PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${std_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + nlohmann_json::nlohmann_json +) +target_link_libraries(graph_file_loaders PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 +) + +add_library(graph_file_savers SHARED + src/plugins/graph_file_savers/geojson_graph_file_saver.cpp +) +target_include_directories(graph_file_savers + PUBLIC + "$" + "$" +) +target_link_libraries(graph_file_savers PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${geometry_msgs_TARGETS} + ${visualization_msgs_TARGETS} + ${std_msgs_TARGETS} + ${nav_msgs_TARGETS} + tf2_ros::tf2_ros + nlohmann_json::nlohmann_json +) +target_link_libraries(graph_file_savers PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 +) + +pluginlib_export_plugin_description_file(nav2_route plugins.xml) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS ${executable_name} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(TARGETS ${library_name} edge_scorers route_operations graph_file_loaders graph_file_savers + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY graphs DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY test/test_graphs DESTINATION share/${PROJECT_NAME}/test) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) +endif() + +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_dependencies( + rclcpp + rclcpp_lifecycle + rclcpp_components + std_msgs + geometry_msgs + nav2_costmap_2d + pluginlib + visualization_msgs + nav_msgs + tf2_ros + nav2_core + nanoflann + nlohmann_json +) +ament_export_libraries(${library_name} edge_scorers route_operations graph_file_loaders graph_file_savers) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/nav2_route/README.md b/nav2_route/README.md new file mode 100644 index 00000000000..a434276bff8 --- /dev/null +++ b/nav2_route/README.md @@ -0,0 +1,427 @@ +# Nav2 Route Server + +The Route Server is a Nav2 Task server to compliment the Planner Server's free-space planning capabilities with pre-defined Navigation Route Graph planning, created by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) at [Open Navigation](https://www.opennav.org/) with assistance from [Josh Wallace](https://www.linkedin.com/in/joshua-wallace-b4848a113/) at Locus Robotics. It can be used to: +* Fully replace free-space planning when following a particular route closely is required (less Controller plugins' tuning to deviate or track the path closely), or +* Augment the global planner with long-distance routing to a goal and using free-space feasible planning in a more localized fashion for the immediate 10m, 100m, etc future. + +This graph has very few rules associated with it and may be generated manually or automatically via AI, geometric, or probabilistic techniques. +docs.nav2.org includes tutorials for how to generate such a graph by annotations on a grid map created via SLAM, but can also be procedurally generated. +This package then takes a planning request and uses this graph to find a valid route through the environment via an optimal search-based algorithm. It uses plugin-based scoring functions applied each edge based on arbitrary user-defined semantic information and the chosen optimization criteria(s). + +The Nav2 Route Server may also live monitor and analyze the route's process to execute custom behaviors on entering or leaving edges or achieving particular graph nodes. These behaviors are defined as another type of plugin and can leverage the graph's edges' and nodes' arbitrary semantic data. + +There are plugin interfaces throughout the server to enable a great deal of application-specific customization: +- Custom search-prioritization behavior with edge scoring plugins (e.g. minimize distance or time, mark blocked routes, enact static or dynamic penalties for danger and application-specific knowledge, prefer main arteries) +- Custom operations to perform during route execution: (a) triggered when entering or leaving an edge, (b) achieving a graph node on the route, (c) performed consistently (e.g. open door, pause at node to wait for clearance, adjust maximum speed, turn on lights, change mode, check for future collisions) +- Parsers of navigation graph files to use any type of format desirable (e.g. geoJSON, OpenStreetMap) + +Additionally, the server leverages **additional arbitrary metadata** specified within the navigation graph files to store information such as speed limits, added costs, or operations to perform. +Thus, we do not restrict the data that can be embedded in the navigation route graph for an application and this metadata is communicated to the edge scoring and operations plugins to adjust behavior as demanded by the application. +Note that plugins may also use outside information from topics, services, and actions for dynamic behavior or centralized knowledge sharing as well if desired. + +[A rudimentary demo of the basic features can be seen here](https://www.youtube.com/watch?v=T57pac6q4RU) using the `route_example_launch.py` provided in `nav2_simple_commander`. + +## Features + +- 98% Unit Test Coverage +- Optimized Dikjstra's planning algorithm modeled off of the Smac Planner A* implementation +- Use of Kd-trees for finding the nearest node(s) to arbitrary start and goal poses in the graph for pose-based planning requests. Optional use of Breadth-First Search to refine those nearest node(s) from simply Euclidean distance to considering traversibility length in the costmap. +- Highly efficient graph representation to maximize caching in a single data structure containing both edges' and nodes' objects and relationships with localized information +- All edges are directional allowing for single-direction lanes or planning +- Data in files may be with respect to any frame in the TF tree and are transformed to a centralized frame automatically +- Action interface response returns both a sparse route of nodes and edges for client applications with navigation graph knowledge and `nav_msgs/Path` dense paths minimicking freespace planning for drop-in behavior replacement of the Planner Server. +- Action interface request can process requests with start / goal node IDs or euclidean poses +- Service interface to change navigation route graphs at run-time +- Edge scoring dynamic plugins return a cost for traversing an edge and may mark an edge as invalid in current conditions from sensor or system state information +- Graph file parsing dynamic plugins allow for use of custom or proprietary formats +- Operation dynamic plugins to perform arbitrary tasks at a given node or when entering or leaving an edge on the route +- Operation may be graph-centric (e.g. graph file identifies operation to perform at a time) or plugin-centric (e.g. plugins self-identify nodes and edges to act upon during execution) +- Operations may trigger rerouting if necessary (e.g. due to new information, blockages, multi-robot data, etc) +- The nodes and edges metadata may be modified or used to communicate information across plugins including different types across different runs +- The Route Tracking action returns regular feedback on important events or state updates (e.g. rerouting requests, passed a node, triggered an operation, etc) +- If rerouting occurs during Route Tracking along the previous current edge, that state will be retained for improved behavior and provide an interpolated nav_msgs/Path from the closest point on the edge to the edge's end (or rerouting's starting node) to minimize free-space planning connections where a known edge exists and is being continued. + +## Practical Architectures for Use + +There are several practical architectures and designs for how this Route Serve can be assembled into a robotics solution. +Which to use depends on the nature of an application's needs and behavior. +This is not an exhaustive list, but enough to get users started thinking about how to fit this into their system. +Architectures (1) and (2) are implemented and tested in `nav2_bt_navigator/behavior_trees` for immediate use! + +* 1. Route Server's output dense path -> Controller Server for direct route following + - This is most useful when close or exact route following is required, fully replaces the Planner Server + - Considering adding in the Smoother Server or spline fitting between the Route Server and Controller Server in a Behavior Tree to smooth out the edge transitions for following + - Consider ComputingPathToPose for first-mile and last-mile on and off the route if leaving the graph is desirable. +* 2. Route Server's output sparse route -> Planner Server to plan to the next node(s) -> Controller Server to track global path + - This is useful when you want to follow the general route, but wish to have the flexibility to leave the route when obstacles are in the way and need the free-space planner to do so + - This is also useful in conjunction with (1) above as a fallback behavior to follow the route when possible, leave when blocked (after some time, or proactively when it sees blocked in immediate future), and then track the route again. This is great in situations where you want to only leave the route when required but otherwise track the route closely + - Consider using ComputePathToPose to plan to the next node in the route and change nodes as you approach the next + - Consider using ComputePathThroughPoses to plan through several nodes in the future to have smooth interchange +* 3. Route Server's output sparse route -> Waypoint Follower or Navigate Through Poses to navigate with task + - Similar to (2), this is useful to take the nodes and edges and navigate along the intended route using intelligent navigation + - This architecturally puts the route planning in the higher-level application-autonomy logic rather than with in the main navigation-planning task logic, which could be useful separation of concerns for some applications. +* 4. Create a behavior tree to NavigateToPose to the first node in the graph, then select (1) or (2) to track the route, finally NavigateToPose to get to the final goal pose off of the graph + - This is useful when the start and/or goal poses are not on the navigation graph, and thus the robot needs to navigate to its starting node or final goal pose in a 'first-mile' and 'last-mile' style task +* 5. Route Server's `ComputeAndTrackRoute` instead of `ComputeRoute` and send the dense path in (1) or sparse route in (2) or (3) + - This is useful to track the progress of the route in the Route Server while following the route as an application sees fit. This process allows for the triggering of spatial, graph, or contextual behaviors while executing the task like adjusting speeds, turning on lights, rerouting due to multi-robot coordination resource constraints, opening doors, etc. This has a wide ranging set of applications. +* 6. Teleoping a robot -> having a script which automatically stores new nodes and/or operator manually triggers a node capture -> saving this to file -> annotating file with operation plugin to do at each waypoint (if any) -> later using the graph to navigate the robot and perform tasks + - This is one possible way to setup a Teach-and-Repeat behavior using the route server with custom behaviors at each node. There are likely many. +* 7. Multi-floor navigation using graph nodes as terminals for stairs, elevators, etc + - Then free-space planning can be used between on-floor nodes and graph connections for floor connectors to enact specific behaviors to call elevators, climb stairs, etc. + +## Design + +The Nav2 Route Server is designed as several composed objects to make the system easy to understand and easily unit testable. The breakdown exists between different classes of capabilities like ROS 2 Interfaces (e.g. actions, services, debugging topics), the core search algorithm, scoring factory, route progress tracking, operations factory, file parsing, and action request 'intent' extraction. This distinction makes a relatively complex system easier to grasp, as there are a number of moving pieces. Luckily, few of these pieces (tracker is the exception) don't need to know much about each other so they can be isolated. + +The diagram below provides context for how the package is structured from the consititutent files you'll find in this project. + +

+ +

+ +Each have their own complete unit testing files named similarly in the `test/` directory. As the diagram suggests, plugin interfaces exist in the file loader, edge scorer, and operations manager to enable customizable behavior for different applications. + +### Plugin Interfaces + +Several plugin interfaces are provided to enable customizable behavior in the route search, route operation, and route graph file formatting. This allows for a great deal of customization for any number of applications which might want to (1) prioritize time, distance, or other application-specific criteria in routing; (2) perform custom operations or rerouting mechanics at run-time while progressing along the route such as adjusting speed or opening doors; (3) be able to integrate your own custom file format or another format of your interest. + +The interface definitions can be found in the `include/nav2_route/interfaces` directory and are mostly self explanatory via their method names and provided doxygen documentation. + +## Metrics + +A set of 1,000 experiments were run with the route planner with randomly selected start and goal poses, with various sized graphs. These metrics provide some reasonable benchmarking for performance of how the route server will perform in your application with various sized graphs that represent your environment: + +| Graph size | Ave. Search Time | +| ----------- | ------------------- | +| 100 | 0.0031 ms | +| 10,000 | 0.232 ms | +| 90,000 | 3.75 ms | +| 250,000 | 11.36 ms | +| 1,000,000 | 44.07 ms | + +This is in comparison with typical run-times of free-space global planners between 50 ms - 400 ms (depending on environment size and structure). Thus, the typical performance of using the route planner - even in truly massive environments to need hundreds of thousands of nodes of interest or importance - is well in excess of freespace planning. This enables Nav2 to operate in much larger spaces and still perform global routing. There's little reason then to use a heuristic-driven A* when Dikjstra's Algorithm is plenty fast for even massively connected graphs. + +The script used for this analysis can be found in `test/performance_benchmarking.cpp`. + +## Parameters + +``` +route_server: + ros__parameters: + + base_frame: "base_link" # Robot's base frame + route_frame: "map" # Global reference frame + path_density: 0.05 # Density of points for generating the dense nav_msgs/Path from route (m) + max_iterations: 0 # Maximum number of search iterations, if 0, uses maximum possible + max_planning_time: 2.0 # Maximum planning time (seconds) + costmap_topic: 'global_costmap/costmap_raw' # Costmap topic when enable_nn_search is enabled. May also be used by the collision monitor operation and/or the costmap edge scorer if using the same topic to share resources. + + graph_file_loader: "GeoJsonGraphFileLoader" # Name of default file loader + plugin: nav2_route::GeoJsonGraphFileLoader # file loader plugin to use + graph_filepath: "" # file path to graph to use + + edge_cost_functions: ["DistanceScorer", "DynamicEdgesScorer"] # Edge scoring cost functions to use + DistanceScorer: + plugin: "nav2_route::DistanceScorer" + DynamicEdgesScorer: + plugin: "nav2_route::DynamicEdgesScorer" + + operations: ["AdjustSpeedLimit", "ReroutingService"] # Route operations plugins to use + AdjustSpeedLimit: + plugin: "nav2_route::AdjustSpeedLimit" + ReroutingService: + plugin: "nav2_route::ReroutingService" + + tracker_update_rate: 50.0 # Rate at which to check the status of path tracking + aggregate_blocked_ids: false # Whether to aggregate the blocked IDs reported by route operations over the lifespan of the navigation request or only use the currently blocked IDs. + boundary_radius_to_achieve_node: 1.0 # Radius (m) near boundary nodes (e.g. start/end) to enable evaluation of achievement metric + radius_to_achieve_node: 2.0 # Radius (m) near route nodes as preliminary condition for evaluation of achievement metric + + max_prune_dist_from_edge: 8.0 # Max distance from an edge to consider pruning it as in-progress (e.g. if we're too far away from the edge, its nonsensical to prune it) + min_prune_dist_from_goal: 0.15 # Min distance from goal node away from goal pose to consider goal node pruning as considering it as being passed (in case goal pose is very close to a goal node, but not exact) + min_prune_dist_from_start: 0.10 # Min distance from start node away from start pose to consider start node pruning as considering it as being passed (in case start pose is very close to a start node, but not exact) + prune_goal: true # Whether pruning the goal nodes from the route due to being past the goal pose requested is possible (pose requests only) + enable_nn_search: true # Whether to enable breadth first search considering the costmap to find the node closest to the start and edge poses, rather than using the euclidean nearest neighbor alone. + max_nn_search_iterations: 10000 # Maximum number of iterations for breadth-first search. + num_nearest_nodes: 5 # The number of nearest-neighbors to extract for breadth-first search to consider. +``` + +#### `CostmapScorer` + +This edge scoring plugin will score based on the costmap values of the edge (e.g. using either maximum cost on the edge or average cost) + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| weight | Relative scoring weight (1.0) | +| costmap_topic | Costmap topic for collision checking (`global_costmap/costmap_raw`) | +| max_cost | Maximum cost to consider an route blocked (253.0) | +| use_maximum | Whether to score based on single maximum or average (true) | +| invalid_on_collision | Whether to consider collision status as a terminal condition (true) | +| invalid_off_map | Whether to consider route going off the map invalid (true) | +| check_resolution | Resolution to check costs at (1 = costmap resolution, 2 = 2x costmap resolution, etc) | + +#### `DistanceScorer` + +This edge scoring plugin will score based on the distance length of the edge, weighted proportionally to percentage-based speed limits (if set in the graph) + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| weight | Relative scoring weight (1.0) | +| speed_tag | Graph metadata key to look for % speed limits (speed_limit) | + +#### `DynamicEdgesScorer` + +This edge scoring plugin will score based on the requested values from a 3rd party application via a service interface. It can set dynamically any cost for any edge and also be used to close and reopen particular edges if they are blocked, in use by other robots locking out its shared use by other robots, higher cost due to overlap with other platforms in service, increased cost due to fleet manager analytics that this space is underperforming throughput, or otherwise temporarily non-traversible. For example, if other robots report an edge to be blocked, all robots can avoid this edge/aisle/etc. + +#### `TimeScorer` + +This edge scoring plugin will score based on the time to traverse the length of the edge. This will use the distance of the edge weighted in proportion to the absolute speed limits of the robot over an edge. If none is set in the graph, a parameterized maximum speed will be used. If an actual, measured time of a previous traversal is in the edge's metadata, this will be used. + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| weight | Relative scoring weight (1.0) | +| speed_tag | Graph metadata key to look for abs speed limits (abs_speed_limit) | +| time_tag | Graph metadata key to look for abs traversal times (abs_time_taken) | +| max_vel | Maximum velocity to use if speed limit or time taken is not set | + +#### `PenaltyScorer` + +This edge scoring plugin will score based on a statically set penalty in the graph file for a particular edge. This can be based on application known logic to weight preferences of navigation tactics in a space. + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| weight | Relative scoring weight (1.0) | +| penalty_tag | Graph metadata key to look for penalty value (penalty) | + +#### `SemanticScorer` + +This edge scoring plugin will score based on semantic information provided in the graph file. It can either check for the edge's semantic class via a parameterized key's value **or** search all key names to match known semantic classes to apply weight (e.g. `class: highway` or `highway: `). + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| weight | Relative scoring weight (1.0) | +| semantic_classes | A list of semantic classes possible in your graph | +| `` | The cost to apply for a class (e.g. `highway: 8.4`) | +| semantic_key | Key to search edges for for semantic data (class). If empty string, will look at key names instead. | + +#### `StartPoseOrientationScorer` + +This edge scoring plugin will score an edge starting at the start node (vector from start->goal) based on its angular proximity to the starting pose's orientation. +This will either score a weighted-angular distance or reject traversals that are outside of a set threshold to force the route to go down a particular direction (i.e. direction robot is already facing). + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| orientation_weight | Relative scoring weight (1.0) | +| use_orientation_threshold | Whether to use orient. threshold or weighted-angular distance scoring (false) | +| orientation_tolerance | The angular threshold to reject edges' angles if greater than this w.r.t. starting pose, when `use_orientation_threshold: true` (PI/2) | + +#### `GoalPoseOrientationScorer` + +This edge scoring plugin will score a an edge with terminus of the goal node (vector from start->goal) based on its angular proximity to the goal pose's orientation. +This will either score a weighted-angular distance or reject traversals that are outside of a set threshold to force the route to go down a particular direction (i.e. direction robot wants to be facing). + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| orientation_weight | Relative scoring weight (1.0) | +| use_orientation_threshold | Whether to use orient. threshold or weighted-angular distance scoring (false) | +| orientation_tolerance | The angular threshold to reject edges' angles if greater than this w.r.t. goal pose, when `use_orientation_threshold: true` (PI/2) | + +#### `AdjustSpeedLimit` + +This route operation will check the graph at each state change (e.g. node passed) if the new edge entered contains speed limit restrictions. If so, it will publish those to the speed limit topic to be received by the controller server. + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| speed_limit_topic | Topic to publish new speed limits to (speed_limit) | +| speed_tag | Graph metadata key to look for % speed limits (speed_limit) | + +#### `CollisionMonitor` + +This route operation will evaluate a future-looking portion of the route for validity w.r.t. collision in the costmap. If it is blocked, it sets the edge blocked as blocked for rerouting around the blocked edge. + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| costmap_topic | Costmap topic (global_costmap/costmap_raw) | +| rate | Throttled rate to evaluate collision at, rather than `tracker_update_rate` which might be excessively expensive for forward looking non-control collision checking (1.0 hz) | +| max_cost | Max cost to be considered invalid (253.0) | +| max_collision_dist | How far ahead to evaluate the route's validity (5.0 m) | +| check_resolution | Resolution to check costs at (1 = costmap resolution, 2 = 2x costmap resolution, etc) | +| reroute_on_collision | Whether to reroute on collision or exit tracking as failed when a future collision is detected (default: true) | + +#### `ReroutingService` + +This route operation will receive service requests from a 3rd party application to cause a rerouting request. + +#### `TimeMarker` + +This route operation will track times taken to traverse particular edges to write times to for later improved navigation time estimation. + +| Parameter | Description | +|-----------------------|---------------------------------------------------------------------| +| time_tag | Graph edge metadata key to write to (abs_time_taken) | + +#### `TriggerEvent` + +This route operation will trigger an external service when a graph node or edge contains a route operation of this name. It uses a `std_srvs/Trigger` interface and is a demonstration of the `RouteOperationClient` base class which can be used to trigger other events of other types of other names as desired (opening doors, calling elevators, etc). + +## ROS Interfaces + + +| Topic | Type | +|-----------------|-------------------------------------| +| plan | nav_msgs/msg/Path | +| speed_limit | nav2_msgs/msg/SpeedLimit | +| route_graph | visualization_msgs/msg/MarkerArray | + +| Service | Description | +|--------------------------------------------------|----------------------------------------------------------------| +| `route_server/set_route_graph` | Sets new route graph to use | +| `route_server//adjust_edges` | Sets adjust edge values and closure from 3rd party application | +| `route_server//reroute` | Trigger a reroute from a 3rd party | + +| Action | Description | +|----------------------------------|-----------------------------------------------------------------------| +| `compute_route` | Computes and returns a sparse graph route and dense interpolated path | +| `compute_and_track_route` | Computes and tracks a route for route operations and progress, returns route and path via process feedback also containing current state and operations triggered | + +## File Formats + +The graphs may be stored in one of the formats the parser plugins can understand or implement your own parser for a particular format of your interest! +A parser is provided for GeoJSON formats. +The only three required features of the navigation graph is (1) for the nodes and edges to have identifiers from each other to be unique for referencing and (2) for edges to have the IDs of the nodes belonging to the start and end of the edge and (3) nodes contain coordinates. +This is strictly required for the Route Server to operate properly in all of its features. + +Besides this, we establish some conventions for route graph files to standardize this information to offer consistent and documented behavior with our provided plugins. +The unique identifier for each node and edge should be given as `id`. The edge's nodes are `startid` and `endid`. The coordinates are given in an array called `coordinates`. +While technically optional, it is highly recommended to also provide: +- The node's frame of reference (`frame`), if not the global frame and you want it transformed +- The Operation's `trigger` (e.g. enter, exit edge, node achieved) and `type` (e.g. action to perform), as relevant + +While optional, it is somewhat recommended to provide, if relevant to your needs: +- The edge's `cost`, if it is fixed and known at graph generation time or edge scoring plugins are not used +- Whether the edge's cost is `overridable` with edge scoring plugins, if those plugins are provided. + +Otherwise, the Node, Edge, and Operations may contain other arbitrary application-specific fields with key-value pairs under the `metadata` key-name. +These can be primitive types (float, int, string, etc), vector types (e.g. a polygon or other vector of information), or even contain information nested under namespaces - whereas a metadata object may exist as a key's value within `metadata`. + +While GeoJSON is not YAML-based, the following YAML file is provided as a more human-readable example for illustration of the conventions above. +Usable real graph file demos can be found in the `graphs/` directory. However, the +`sample_graph.geojson` in the `graph/` directory exists to show the different API for options +for operations, metadata, recursive metadata and vectors. + +``` +example_graph.yaml + +Node1: // <-- If provided by format, stored as name in metadata + id: 1 // <-- Required + coordinates: [0.32, 4.0] // <-- Required + frame: "map" // <-- Highly recommended + metadata: + class: "living_room" // <-- Metadata for node (arbitrary) + operation: + pause: // <-- If provided by format, stored as name in metadata + type: "stop" // <-- Required + trigger: ON_ENTER // <-- Required + metadata: + wait_for: 5.0 // <-- Metadata for operation (arbitrary) + +Edge1: // <-- If provided by format, stored as name in metadata + id: 2 // <-- Required + startid: 1 // <-- Required + endid: 3 // <-- Required + overridable: False // <-- Recommended + cost: 6.0 // <-- Recommended, if relevant + metadata: + speed_limit: 85 // <-- Metadata for edge (arbitrary). Use abs_speed_limit if not a percentage + operations: + open_door: // <-- If provided by format, stored as name in metadata + type: "open_door" // <-- Required + trigger: ON_EXIT // <-- Required + metadata: + door_id: 54 // <-- metadata for operation (arbitrary) + service_name "open-door" // <-- metadata for operation (Recommended) +``` + +### Metadata Conventions for Convenience + +While other metadata fields are not required nor necessarily needed, there are some useful standards which may make your life easier within in the Route Server framework. +These are default fields for the provided plugins, but you're free to embed this information anyway you like (but may come at the cost of needing to re-implement provided capabilities). +A set of conventions are shown in the table below. The details regarding the convention and where they are used within the codebase follows in more detail. + +| Graph File Key | Information Stored | +| --------------- | ------------- | +| `speed_limit` | Speed limit, represented in percentage 0.0-100.0. | +| `abs_speed_limit` | Speed limit, represented in `m/s`. | +| `penalty` | A cost penalty to apply to a node or edge. | +| `class` | The semanic class this node belongs to. | +| `service_name` | Which service to call, if node or edge should trigger an event. | +| `abs_time_taken` | Time taken to traverse an edge in `s`. | + +
+ Click me to see expansive detail about these conventions and uses + + The `DistanceScorer` edge scoring plugin will score the L2 norm between the poses of the nodes constituting the edge as its unweighted score. + It contains a parameter `speed_tag` (Default: `speed_limit`) which will check the edge's metadata if it contains an entry for speed limit information, represented as a percentage of the maximum speed. + If so, it will adjust the score to instead be proportional to the time rather than distance. + Further, the `AdjustSpeedLimit` Route Operation plugin will utilize the same parameter and default `speed_limit` value to check each edge entered for a set speed limit percentage. + If present, it will request an adjusted speed limit by the controller server on each edge entered. + Thus, by convention, we say that `speed_limit` attribute of an edge should contain this information. + + The `TimeScorer` plugin operates with an exact analog to the `DistanceScorer`, except rather than using speed limits based on percentages of maximum, it uses actual speed limits in `m/s`. As such the `speed_tag` parameter (Default: `abs_speed_limit`) is used to represent absolute speed limits in the metadata files. By convention, we say that the `abs_speed_limit` attribute of an edge should contain this information. + + Similarly, the `penalty_tag` parameter (Default: `penalty`) in the `PenaltyScorer` can be used to represent a static unweighted cost to add to the sum total of the edge scoring plugins. + This is useful to penalize certain routes over others knowing some application-specific information about that route segment. Technically this may be negative to incentivize rather than penalize. + Thus, by convention, we say the `penalty` attribute of an edge should contain this information. + + By convention, to use the `SemanticScorer` edge scoring plugin, we expect the semantic class of a node or edge to be stored in the `class` metadata key (though is reconfigurable to look at any using the `semantic_key` parameter). This can be used to store arbitrary semantic metadata about a node or edge such as `living_room`, `bathroom`, `work cell 2`, `aisle45`, etc which is then correlated to a set of additional class costs in the plugin's configuration. However, if the `semantic_key` parameter is set to empty string, then instead it checks **all keys** in the metadata of a node or edge if they match any names of the semantic classes. This way, semantic information may be embedded as keys with other values (for another application) or as values themselves to the `class` key if only needing to specify its membership. + +The Route Operation `TriggerEvent` and more broadly any operation plugins derived from `RouteOperationClient` (a service-typed template route operation base class to simplify adding in custom plugins based on service calls) relies on the parameter and matching metadata key `service_name` to indicate the service name to call with the corresponding route operation. When set in the parameter file, this will be used for all instances when called in the navigation route graph. When `service_name` is set in the operation metadata in the route graph, it can be used to specify a particular service name of that service type to use at that particular node/edge, created on the fly (when a conflict exists, uses the navigation graph as the more specific entry). + That way both design patterns work for a Route Operation `OpenDoor` of service type `nav2_msgs/srv/OpenDoor`, for example: + - A `open_door/door1` (and `door2` and so on) service specific to each node containing a door to open may be called contextually and correctly at each individual door in the graph file metadata. This way you can have individual services (if desired) without having to have individual repetitive operation plugin definitions. + - A `open_doors` general service specified in the parameter file to call to open a door specified in the service's `request` field, so that one service is called for all instances of doors in the graph file without repetition in the graph file and storing client resources (just adding info about which one from the node metadata). + + Thus, we say that `service_name` is a key to correspond to a string of the service's name to call in an operation to use `TriggerEvent` and `RouteOperationClient` plugins and base classes. + + By convention, we reserve the edge metadata field `abs_time_taken` for representing actual navigation times along the edge. This is populated by the `TimeMarker` route operation plugin and utilized by the `TimeScorer` to more accurately represent the times to navigate along an edge based on the real execution time. While this is used as an internal mechanism to share data between live operations and route planning, it is also possible to set `last_time_taken` in your navigation graph file based on many execution runs to use in the `TimeScorer` to optimize system performance for a fleet or over long durations (and remove `TimeMarker` operation for live updates). + + +
+ +## Etc. Notes + +### Metadata Communication + +The metadata contained in the graph's nodes and edges can serve a secondary purpose to communicating arbitrary information from the graph file for use in routing behavior or operations. It may also be used to communicate or store information about a node or edge during run-time to query from a plugin in a future iteration, from another plugin in the system, or from another plugin type entirely. + +For example: +- If the collision monitor Route Operation identifies an edge as being blocked on a regular basis, a counter can be used to track the number of times this edge is blocked and if exceeding a threshold, it adds additional costs to that edge to incentivize taking another direction in an Edge Scorer. +- If we want to minimize the time to traverse the space, rather than estimating the times to traverse an edge, we can store actual times to navigate into the metadata of the edges. This can be stored as part of a route operation after completing an edge and retrieved at planning time by an edge scorer. + +All of this is made possible by the centralized graph representation and pointers back to its memory locations at each stage of the system. + +### Node Achievement + +The Route Tracker will track the progress of a robot following a defined route over time. When we achieve a node, that is to say, we pass it, that triggers events based on reaching a node (Also: exiting an old edge, entering a new edge). Thus, the specification of node achievement is worth some discussion for users so they can best use this powerful feature. + +The node achievement logic will first check if the robot is within a configurable radius of a node. This radius should be **generous** and not equatable to the goal tolerance. This should be sufficiently large that by the mechanics of your control, you can achieve a node when you pass it considering realistic deviations of path tracking by your trajectory planner. This may not need to be very large if using an exact path follower controller but may need to be relatively large for a dynamic obstacle avoidance planner. If in doubt, make it **larger** since this is merely the first stage for checking node achievement, not the metric itself. + +Once we're within the range of a node that we need to consider whether or not we've achieved the node, we evaluate a mathematical operation to see if the robot has moved from the regime of the previous edge into the next edge spatially. We do this by finding the bisecting vector of the two edge vectors of interest (e.g. last edge and next edge) and comparing that with the distance vector from the node. When the dot product is 0, that means the we're at the orthogonal vector to the bisector moving from one regime to the next. Thus, we can check the sign of this dot product to indicate when we transition from one edge to the next while allowing for significant deviation from the node itself due to path tracking error or dynamic behavior. + +For the edge boundary cases where there is no last edge (e.g. starting) or next edge (e.g. finishing), we use the boundary radius only (separate param from otherwise used radius). Recall that this only applies the routing part of the navigation request, the controller will still continue tracking the path until your goal achievement defined in your local trajectory planner's configurations. However, any Route Operations to be performed at the start or end nodes will be slightly preempted when compared to the others, by the choice of this radius. + +A special case exists for rerouting, where as if we reroute along the same edge as we were previously routing through, the tracker state will be reloaded. While the current edge is not reported in the route message (because the last node is passed), that information is inserted back in the tracker to perform edge exit route operations -- and most importantly, the refined bisector-based node achievement criteria (which is why Steve went through the sheer pain to make it possible). + +### Gap and Trivial Routing + +If a routing or rerouting request is made using poses which are not known to be nodes in the graph, there may be a gap between the robot's pose and the start of the route (as well as the end of the route and the goal pose) -- or a "last-mile" problem. This gap may be trivial to overcome with denser graphs and small distances between nodes that a local trajectory planner can overcome. However, with sparser graphs, some kind of augmentation would likely be required. For example: +- (A) Using a behavior tree which uses free-space planning to connect the robot and goal poses with the start / end of the route +- (B) Using the waypoint follower (or navigate through poses or otherwise) that takes the nodes and uses them as waypoints terminating with the goal pose for freespace navigation using the route as a general prior +- (C) Using the Route as a goal route to the goal, global planner to make a feasible plan for the next 100 meters (or so), and use the controller to track the global feasible path rather than the general route +- (D) some other solution which will get the robot from its pose, through the route, to the goal +- (E) The trajectory planner can handle these gaps itself, so Route dense path to the controller directly + +A special condition of this when a routing or rerouting request is made up of only 1-2 edges, whereas both the start and the goal are not 'on or near' the terminal nodes (e.g. they're already along an edge meaningfully, so going to a goal node and then the final pose would involve back tracking), the edges may be pruned away leaving only a single node as the route. This is since we have already passed the starting node of the initial edge and/or the goal is along the edge before the ending node of the final edge. This is not different than the other conditions, but is a useful edge case to consider while developing your application solution. + +This is an application problem which can be addressed above but may have other creative solutions for your application. It is on you as an application developer to determine if this is a problem for you and what the most appropriate solution is, since different applications will have different levels of flexibility of deviating from the route or extremely strict route interpretations. + +Note that there are parameters like `prune_route`, `min_prune_distance_from_start` and `min_prune_distance_from_goal` which impact the pruning behavior while tracking a route using poses. There is also the option to request routes using NodeIDs instead of poses, which obviously would never have this issue since they are definitionally on the route graph at all times. That bypasses the entire issue if the goal(s) are always known to be on the graph. diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson new file mode 100644 index 00000000000..2e6a43af148 --- /dev/null +++ b/nav2_route/graphs/aws_graph.geojson @@ -0,0 +1,132 @@ +{ +"type": "FeatureCollection", +"name": "graph", +"crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, +"date_generated": "Wed Feb 22 05:41:45 PM EST 2025", +"features": [ +{ "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, +{ "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, +{ "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, +{ "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, +{ "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, +{ "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, +{ "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, +{ "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, +{ "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, +{ "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, +{ "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, +{ "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, +{ "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, +{ "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, +{ "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, +{ "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, +{ "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, +{ "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, +{ "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } +] +} diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson new file mode 100644 index 00000000000..162a1a88884 --- /dev/null +++ b/nav2_route/graphs/sample_graph.geojson @@ -0,0 +1,121 @@ +{ + "type": "FeatureCollection", + "name": "graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "date_generated": "Tue Feb 28 07:48:03 PM EST 2025", + "features": [ + { + "type": "Feature", + "properties": + { + "id": 0, + "frame": "map", + "metadata": + { + "region": + { + "x_values": [1.0, -1.0, -1.0, 1.0], + "y_values": [1.0, 1.0, -1.0, -1.0], + "properties": { + "class_type": "living_room", + "number_of_lights": 10 + } + } + }, + "operations": + { + "stop": + { + "type": "stop", + "trigger": "NODE", + "metadata": + { + "wait_for": 5.0 + } + } + } + }, + "geometry": + { + "type": "Point", + "coordinates": [ 0.0, 0.0 ] + } + }, + { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 2.0 ] } }, + { + "type": "Feature", + "properties": + { + "id": 9, + "startid": 0, + "endid": 1, + "overridable": false, + "cost": 10.0, + "metadata": + { + "speed_limit": 0.85, + "aisle_number": 14 + }, + "operations": + { + "open_door": + { + "type": "open_door", + "trigger": "ON_ENTER", + "metadata": + { + "door_id": 54, + "service_name": "open_door" + } + }, + "take_picture": + { + "type": "take_picture", + "trigger": "ON_EXIT", + "metadata": + { + "type": "jpg", + "resolution": [1080, 720] + } + } + } + }, + "geometry": + { + "type": "MultiLineString", + "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] + } + }, + { "type": "Feature", "properties": { "id": 10, "startid": 1, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 11, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 12, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 13, "startid": 0, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 0.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 14, "startid": 3, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 16, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 17, "startid": 2, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 18, "startid": 5, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 19, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 20, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 22, "startid": 5, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 23, "startid": 3, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 24, "startid": 6, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 25, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 26, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 27, "startid": 5, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 28, "startid": 8, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 29, "startid": 6, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 30, "startid": 7, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 31, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 32, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 1.0, 2.0 ] ] ] } } + + ] +} diff --git a/nav2_route/graphs/scripts/README.md b/nav2_route/graphs/scripts/README.md new file mode 100644 index 00000000000..d1bfc83d3a4 --- /dev/null +++ b/nav2_route/graphs/scripts/README.md @@ -0,0 +1 @@ +These scripts are used to help automate parts of route graph generation process. diff --git a/nav2_route/graphs/scripts/export_shapefiles.py b/nav2_route/graphs/scripts/export_shapefiles.py new file mode 100644 index 00000000000..e6fe5515f94 --- /dev/null +++ b/nav2_route/graphs/scripts/export_shapefiles.py @@ -0,0 +1,60 @@ +#! /usr/bin/env python3 +# Copyright 2021 Josh Wallace +# +# 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. + +from datetime import datetime +import sys + +import geopandas as gpd +import pandas as pd + +try: + file_prefix = sys.argv[1] + edges = gpd.read_file(sys.argv[2]) + nodes = gpd.read_file(sys.argv[3]) +except Exception: + raise Exception('Incorrect arguments provide') + +# Rename columns to match the expected output +nodes = nodes.rename(columns={'start_node': 'startid'}) +edges = edges.rename(columns={'start_node': 'startid'}) +nodes = nodes.rename(columns={'end_node': 'endid'}) +edges = edges.rename(columns={'end_node': 'endid'}) + +# Remove edgeid and use standard id +edges = edges.rename(columns={'edge_id': 'id'}) + +# Make sure IDs are integers +if 'id' in nodes.columns: + nodes['id'] = nodes['id'].astype(int) +if 'id' in edges.columns: + edges['id'] = edges['id'].astype(int) + +now = datetime.now() + +graph = pd.concat([nodes, edges]) + +# Set start/endids to integers, for nodes, set to -1 (not used) +if 'startid' in graph.columns: + graph['startid'] = graph['startid'].fillna(-1).astype(int) +if 'endid' in graph.columns: + graph['endid'] = graph['endid'].fillna(-1).astype(int) +if 'startid' in graph.columns: + graph['startid'] = graph['startid'].astype(int) +if 'endid' in graph.columns: + graph['endid'] = graph['endid'].astype(int) + +file_name = file_prefix + '_' + now.strftime('%m_%d_%Y_%H_%M_%S') + '.geojson' + +graph.to_file(file_name, driver='GeoJSON') diff --git a/nav2_route/graphs/scripts/generate_start_and_end_id.sql b/nav2_route/graphs/scripts/generate_start_and_end_id.sql new file mode 100644 index 00000000000..e0e34cd5d88 --- /dev/null +++ b/nav2_route/graphs/scripts/generate_start_and_end_id.sql @@ -0,0 +1,22 @@ +WITH start_points AS +( + SELECT n.id AS node_id + ,e.id AS edge_id + ,e.geometry AS edge_geometry + FROM edges AS e, nodes AS n + WHERE ST_INTERSECTS(n.geometry, ST_StartPoint(e.geometry)) +), end_points AS +( + SELECT n.id AS node_id + ,e.id AS edge_id + ,e.geometry AS edge_geometry + FROM edges AS e, nodes AS n + WHERE ST_INTERSECTS(n.geometry, ST_EndPoint(e.geometry)) +) +SELECT sp.edge_id AS edge_id + ,sp.node_id AS start_node + ,ep.node_id AS end_node + ,sp.edge_geometry AS geometry +FROM start_points AS sp +JOIN end_points AS ep +ON sp.edge_id = ep.edge_id diff --git a/nav2_route/graphs/scripts/increment_edge_id.json b/nav2_route/graphs/scripts/increment_edge_id.json new file mode 100644 index 00000000000..0328f9cd081 --- /dev/null +++ b/nav2_route/graphs/scripts/increment_edge_id.json @@ -0,0 +1,21 @@ +{ + "author": "josh", + "exported_at": "2025-03-15T19:27:03", + "expressions": [ + { + "description": "\n\n


", + "expression": "if(maximum(\"id\") is null, 10000, maximum(\"id\")+1)", + "group": "user", + "name": "increment_edge_id", + "type": "expression" + }, + { + "description": "\n\n


", + "expression": "if(maximum(\"id\") is null, 0, maximum(\"id\")+1)", + "group": "user", + "name": "increment_node_id", + "type": "expression" + } + ], + "qgis_version": "3.22.4-Białowieża" +} diff --git a/nav2_route/graphs/scripts/increment_node_id.json b/nav2_route/graphs/scripts/increment_node_id.json new file mode 100644 index 00000000000..4f399c62951 --- /dev/null +++ b/nav2_route/graphs/scripts/increment_node_id.json @@ -0,0 +1,14 @@ +{ + "author": "josh", + "exported_at": "2025-03-15T19:22:14", + "expressions": [ + { + "description": "\n\n


", + "expression": "if(maximum(\"id\") is null, 0, maximum(\"id\")+1)", + "group": "user", + "name": "increment_node_id", + "type": "expression" + } + ], + "qgis_version": "3.22.4-Białowieża" +} diff --git a/nav2_route/graphs/turtlebot3_graph.geojson b/nav2_route/graphs/turtlebot3_graph.geojson new file mode 100644 index 00000000000..ba7aff0c6d2 --- /dev/null +++ b/nav2_route/graphs/turtlebot3_graph.geojson @@ -0,0 +1,92 @@ +{ + "type": "FeatureCollection", + "name": "turtlebot3_graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "features": [ + { "type": "Feature", "properties": { "id": 1 }, "geometry": { "type": "Point", "coordinates": [ -0.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 2 }, "geometry": { "type": "Point", "coordinates": [ 0.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 3 }, "geometry": { "type": "Point", "coordinates": [ 0.0, -1.7446 ] } }, + { "type": "Feature", "properties": { "id": 4 }, "geometry": { "type": "Point", "coordinates": [ -1.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 5 }, "geometry": { "type": "Point", "coordinates": [ -0.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 6 }, "geometry": { "type": "Point", "coordinates": [ 0.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 7 }, "geometry": { "type": "Point", "coordinates": [ -1.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 8 }, "geometry": { "type": "Point", "coordinates": [ -1.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 9 }, "geometry": { "type": "Point", "coordinates": [ -0.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 10 }, "geometry": { "type": "Point", "coordinates": [ 0.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 11 }, "geometry": { "type": "Point", "coordinates": [ 1.6, 0.8554 ] } }, + { "type": "Feature", "properties": { "id": 12 }, "geometry": { "type": "Point", "coordinates": [ 1.6, -0.3446 ] } }, + { "type": "Feature", "properties": { "id": 13 }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.2554 ] } }, + { "type": "Feature", "properties": { "id": 14 }, "geometry": { "type": "Point", "coordinates": [ 1.6, -1.3446 ] } }, + { "type": "Feature", "properties": { "id": 15 }, "geometry": { "type": "Point", "coordinates": [ -1.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 16 }, "geometry": { "type": "Point", "coordinates": [ -0.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 17 }, "geometry": { "type": "Point", "coordinates": [ 0.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 18 }, "geometry": { "type": "Point", "coordinates": [ 1.6, 1.8554 ] } }, + { "type": "Feature", "properties": { "id": 19 }, "geometry": { "type": "Point", "coordinates": [ -2.0, 0.2554 ] } }, + { "type": "Feature", "properties": { "id": 20 }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.2554 ] } }, + { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.3446 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 22, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 23, "startid": 8, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 24, "startid": 16, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 25, "startid": 9, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 26, "startid": 5, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 27, "startid": 2, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 28, "startid": 6, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 29, "startid": 10, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 30, "startid": 17, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ 1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 31, "startid": 18, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 1.8554 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 32, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 33, "startid": 12, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 34, "startid": 14, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -1.3446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 35, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ -1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 36, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 37, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ -1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 38, "startid": 15, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 1.8554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 39, "startid": 17, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 40, "startid": 10, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 41, "startid": 6, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 42, "startid": 2, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ 1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 43, "startid": 14, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -1.3446 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 44, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 1.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 46, "startid": 18, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 1.8554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 1.8554 ], [ -1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 48, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 49, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -1.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 50, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -1.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 51, "startid": 1, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 52, "startid": 5, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 53, "startid": 9, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 54, "startid": 16, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 55, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 56, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 57, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 58, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 59, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 60, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 61, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 62, "startid": 5, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -0.3446 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 63, "startid": 7, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 64, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 0.8554 ], [ -1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 65, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 66, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 0.8554 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 67, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 0.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 68, "startid": 12, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 0.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 69, "startid": 6, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -0.3446 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 70, "startid": 3, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -1.7446 ], [ 0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 71, "startid": 3, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -1.7446 ], [ -0.6, -1.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 72, "startid": 1, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, -1.3446 ], [ 0.0, -1.7446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 73, "startid": 2, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, -1.3446 ], [ 0.0, -1.7446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 74, "startid": 11, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, 0.8554 ], [ 2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 75, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.2554 ], [ 1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 76, "startid": 12, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.6, -0.3446 ], [ 2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 77, "startid": 13, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.2554 ], [ 1.6, 0.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 78, "startid": 16, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -0.6, 1.8554 ], [ 0.0, 2.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 79, "startid": 20, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.2554 ], [ 0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 80, "startid": 17, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.6, 1.8554 ], [ 0.0, 2.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 81, "startid": 20, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.2554 ], [ -0.6, 1.8554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 82, "startid": 8, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, 0.8554 ], [ -2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 83, "startid": 19, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, 0.2554 ], [ -1.6, -0.3446 ] ] ] } }, + { "type": "Feature", "properties": { "id": 84, "startid": 7, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -1.6, -0.3446 ], [ -2.0, 0.2554 ] ] ] } }, + { "type": "Feature", "properties": { "id": 85, "startid": 19, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, 0.2554 ], [ -1.6, 0.8554 ] ] ] } } + ] +} diff --git a/nav2_route/graphs/turtlebot4_graph.geojson b/nav2_route/graphs/turtlebot4_graph.geojson new file mode 100644 index 00000000000..f7b922e9e5a --- /dev/null +++ b/nav2_route/graphs/turtlebot4_graph.geojson @@ -0,0 +1,119 @@ +{ + "type": "FeatureCollection", + "name": "turtlebot4_graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "features": [ + { "type": "Feature", "properties": { "id": 0 }, "geometry": { "type": "Point", "coordinates": [ 0.624282608695647, 12.880652173913044 ] } }, + { "type": "Feature", "properties": { "id": 1 }, "geometry": { "type": "Point", "coordinates": [ 0.584239130434777, 7.835173913043479 ] } }, + { "type": "Feature", "properties": { "id": 2 }, "geometry": { "type": "Point", "coordinates": [ 0.544195652173907, 2.569456521739133 ] } }, + { "type": "Feature", "properties": { "id": 3 }, "geometry": { "type": "Point", "coordinates": [ 7.651913043478253, 7.915260869565218 ] } }, + { "type": "Feature", "properties": { "id": 4 }, "geometry": { "type": "Point", "coordinates": [ 9.453869565217383, 0.727456521739134 ] } }, + { "type": "Feature", "properties": { "id": 5 }, "geometry": { "type": "Point", "coordinates": [ 13.79858695652173, 6.914173913043479 ] } }, + { "type": "Feature", "properties": { "id": 6 }, "geometry": { "type": "Point", "coordinates": [ 13.698478260869557, 1.328108695652176 ] } }, + { "type": "Feature", "properties": { "id": 7 }, "geometry": { "type": "Point", "coordinates": [ 15.820782608695643, 6.974239130434784 ] } }, + { "type": "Feature", "properties": { "id": 8 }, "geometry": { "type": "Point", "coordinates": [ 15.900869565217382, 9.036478260869565 ] } }, + { "type": "Feature", "properties": { "id": 9 }, "geometry": { "type": "Point", "coordinates": [ 15.940913043478252, 11.719391304347827 ] } }, + { "type": "Feature", "properties": { "id": 10 }, "geometry": { "type": "Point", "coordinates": [ 18.383565217391293, 6.934195652173914 ] } }, + { "type": "Feature", "properties": { "id": 11 }, "geometry": { "type": "Point", "coordinates": [ 18.3235, 9.036478260869565 ] } }, + { "type": "Feature", "properties": { "id": 12 }, "geometry": { "type": "Point", "coordinates": [ 18.283456521739122, 11.8195 ] } }, + { "type": "Feature", "properties": { "id": 13 }, "geometry": { "type": "Point", "coordinates": [ 20.125456521739121, 11.839521739130435 ] } }, + { "type": "Feature", "properties": { "id": 14 }, "geometry": { "type": "Point", "coordinates": [ 20.065391304347816, 9.116565217391305 ] } }, + { "type": "Feature", "properties": { "id": 15 }, "geometry": { "type": "Point", "coordinates": [ 19.985304347826077, 6.974239130434784 ] } }, + { "type": "Feature", "properties": { "id": 16 }, "geometry": { "type": "Point", "coordinates": [ 22.167673913043469, 6.914173913043479 ] } }, + { "type": "Feature", "properties": { "id": 17 }, "geometry": { "type": "Point", "coordinates": [ 22.047543478260859, 9.116565217391305 ] } }, + { "type": "Feature", "properties": { "id": 18 }, "geometry": { "type": "Point", "coordinates": [ 23.869521739130423, 11.77945652173913 ] } }, + { "type": "Feature", "properties": { "id": 19 }, "geometry": { "type": "Point", "coordinates": [ 23.8495, 9.216673913043479 ] } }, + { "type": "Feature", "properties": { "id": 20 }, "geometry": { "type": "Point", "coordinates": [ 23.789434782608684, 6.934195652173914 ] } }, + { "type": "Feature", "properties": { "id": 21 }, "geometry": { "type": "Point", "coordinates": [ 26.071913043478251, 6.954217391304349 ] } }, + { "type": "Feature", "properties": { "id": 22 }, "geometry": { "type": "Point", "coordinates": [ 25.951782608695641, 9.196652173913044 ] } }, + { "type": "Feature", "properties": { "id": 23 }, "geometry": { "type": "Point", "coordinates": [ 25.971804347826076, 11.8195 ] } }, + { "type": "Feature", "properties": { "id": 24 }, "geometry": { "type": "Point", "coordinates": [ 28.234260869565205, 11.85954347826087 ] } }, + { "type": "Feature", "properties": { "id": 25 }, "geometry": { "type": "Point", "coordinates": [ 28.55460869565216, 6.954217391304349 ] } }, + { "type": "Feature", "properties": { "id": 26 }, "geometry": { "type": "Point", "coordinates": [ 28.59465217391303, 5.552695652173915 ] } }, + { "type": "Feature", "properties": { "id": 27 }, "geometry": { "type": "Point", "coordinates": [ 28.59465217391303, 3.130065217391307 ] } }, + { "type": "Feature", "properties": { "id": 28 }, "geometry": { "type": "Point", "coordinates": [ 28.614673913043465, 1.448239130434786 ] } }, + { "type": "Feature", "properties": { "id": 29 }, "geometry": { "type": "Point", "coordinates": [ 25.150913043478248, 1.348130434782611 ] } }, + { "type": "Feature", "properties": { "id": 30 }, "geometry": { "type": "Point", "coordinates": [ 22.508043478260859, 1.388173913043481 ] } }, + { "type": "Feature", "properties": { "id": 31 }, "geometry": { "type": "Point", "coordinates": [ 19.825130434782597, 1.348130434782611 ] } }, + { "type": "Feature", "properties": { "id": 32 }, "geometry": { "type": "Point", "coordinates": [ 16.86191304347825, 1.368152173913046 ] } }, + { "type": "Feature", "properties": { "id": 33 }, "geometry": { "type": "Point", "coordinates": [ 13.698478260869557, 1.328108695652176 ] } }, + { "type": "Feature", "properties": { "id": 10000, "startid": 1, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 0.584239130434777, 7.835173913043479 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10001, "startid": 3, "endid": 1 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 0.584239130434777, 7.835173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10002, "startid": 0, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 0.624282608695647, 12.880652173913044 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10003, "startid": 3, "endid": 0 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 0.624282608695647, 12.880652173913044 ] ] } }, + { "type": "Feature", "properties": { "id": 10004, "startid": 2, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 0.544195652173907, 2.569456521739133 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10005, "startid": 3, "endid": 2 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 0.544195652173907, 2.569456521739133 ] ] } }, + { "type": "Feature", "properties": { "id": 10006, "startid": 6, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 9.453869565217383, 0.727456521739134 ] ] } }, + { "type": "Feature", "properties": { "id": 10006, "startid": 33, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 9.453869565217383, 0.727456521739134 ] ] } }, + { "type": "Feature", "properties": { "id": 10007, "startid": 4, "endid": 6 }, "geometry": { "type": "LineString", "coordinates": [ [ 9.453869565217383, 0.727456521739134 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10007, "startid": 4, "endid": 33 }, "geometry": { "type": "LineString", "coordinates": [ [ 9.453869565217383, 0.727456521739134 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10008, "startid": 4, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 9.453869565217383, 0.727456521739134 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10009, "startid": 3, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 9.453869565217383, 0.727456521739134 ] ] } }, + { "type": "Feature", "properties": { "id": 10010, "startid": 3, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 7.651913043478253, 7.915260869565218 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10011, "startid": 5, "endid": 3 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 7.651913043478253, 7.915260869565218 ] ] } }, + { "type": "Feature", "properties": { "id": 10012, "startid": 5, "endid": 6 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10012, "startid": 5, "endid": 33 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10013, "startid": 6, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10013, "startid": 33, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10014, "startid": 5, "endid": 7 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.79858695652173, 6.914173913043479 ], [ 15.820782608695643, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10015, "startid": 7, "endid": 5 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.820782608695643, 6.974239130434784 ], [ 13.79858695652173, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10016, "startid": 7, "endid": 8 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.820782608695643, 6.974239130434784 ], [ 15.900869565217382, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10017, "startid": 8, "endid": 7 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.900869565217382, 9.036478260869565 ], [ 15.820782608695643, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10018, "startid": 8, "endid": 9 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.900869565217382, 9.036478260869565 ], [ 15.940913043478252, 11.719391304347827 ] ] } }, + { "type": "Feature", "properties": { "id": 10019, "startid": 9, "endid": 8 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.940913043478252, 11.719391304347827 ], [ 15.900869565217382, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10020, "startid": 6, "endid": 32 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 16.86191304347825, 1.368152173913046 ] ] } }, + { "type": "Feature", "properties": { "id": 10020, "startid": 33, "endid": 32 }, "geometry": { "type": "LineString", "coordinates": [ [ 13.698478260869557, 1.328108695652176 ], [ 16.86191304347825, 1.368152173913046 ] ] } }, + { "type": "Feature", "properties": { "id": 10021, "startid": 32, "endid": 6 }, "geometry": { "type": "LineString", "coordinates": [ [ 16.86191304347825, 1.368152173913046 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10021, "startid": 32, "endid": 33 }, "geometry": { "type": "LineString", "coordinates": [ [ 16.86191304347825, 1.368152173913046 ], [ 13.698478260869557, 1.328108695652176 ] ] } }, + { "type": "Feature", "properties": { "id": 10022, "startid": 7, "endid": 10 }, "geometry": { "type": "LineString", "coordinates": [ [ 15.820782608695643, 6.974239130434784 ], [ 18.383565217391293, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10023, "startid": 10, "endid": 7 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.383565217391293, 6.934195652173914 ], [ 15.820782608695643, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10024, "startid": 10, "endid": 11 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.383565217391293, 6.934195652173914 ], [ 18.3235, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10025, "startid": 11, "endid": 10 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.3235, 9.036478260869565 ], [ 18.383565217391293, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10026, "startid": 11, "endid": 12 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.3235, 9.036478260869565 ], [ 18.283456521739122, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10027, "startid": 12, "endid": 11 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.283456521739122, 11.8195 ], [ 18.3235, 9.036478260869565 ] ] } }, + { "type": "Feature", "properties": { "id": 10028, "startid": 12, "endid": 13 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.283456521739122, 11.8195 ], [ 20.125456521739121, 11.839521739130435 ] ] } }, + { "type": "Feature", "properties": { "id": 10029, "startid": 13, "endid": 12 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.125456521739121, 11.839521739130435 ], [ 18.283456521739122, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10030, "startid": 13, "endid": 14 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.125456521739121, 11.839521739130435 ], [ 20.065391304347816, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10031, "startid": 14, "endid": 13 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.065391304347816, 9.116565217391305 ], [ 20.125456521739121, 11.839521739130435 ] ] } }, + { "type": "Feature", "properties": { "id": 10032, "startid": 14, "endid": 15 }, "geometry": { "type": "LineString", "coordinates": [ [ 20.065391304347816, 9.116565217391305 ], [ 19.985304347826077, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10033, "startid": 15, "endid": 14 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.985304347826077, 6.974239130434784 ], [ 20.065391304347816, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10034, "startid": 15, "endid": 10 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.985304347826077, 6.974239130434784 ], [ 18.383565217391293, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10035, "startid": 10, "endid": 15 }, "geometry": { "type": "LineString", "coordinates": [ [ 18.383565217391293, 6.934195652173914 ], [ 19.985304347826077, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10036, "startid": 32, "endid": 31 }, "geometry": { "type": "LineString", "coordinates": [ [ 16.86191304347825, 1.368152173913046 ], [ 19.825130434782597, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10037, "startid": 31, "endid": 32 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.825130434782597, 1.348130434782611 ], [ 16.86191304347825, 1.368152173913046 ] ] } }, + { "type": "Feature", "properties": { "id": 10038, "startid": 31, "endid": 30 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.825130434782597, 1.348130434782611 ], [ 22.508043478260859, 1.388173913043481 ] ] } }, + { "type": "Feature", "properties": { "id": 10039, "startid": 30, "endid": 31 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.508043478260859, 1.388173913043481 ], [ 19.825130434782597, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10040, "startid": 30, "endid": 29 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.508043478260859, 1.388173913043481 ], [ 25.150913043478248, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10041, "startid": 29, "endid": 30 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.150913043478248, 1.348130434782611 ], [ 22.508043478260859, 1.388173913043481 ] ] } }, + { "type": "Feature", "properties": { "id": 10042, "startid": 29, "endid": 28 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.150913043478248, 1.348130434782611 ], [ 28.614673913043465, 1.448239130434786 ] ] } }, + { "type": "Feature", "properties": { "id": 10043, "startid": 28, "endid": 29 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.614673913043465, 1.448239130434786 ], [ 25.150913043478248, 1.348130434782611 ] ] } }, + { "type": "Feature", "properties": { "id": 10044, "startid": 28, "endid": 27 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.614673913043465, 1.448239130434786 ], [ 28.59465217391303, 3.130065217391307 ] ] } }, + { "type": "Feature", "properties": { "id": 10045, "startid": 27, "endid": 28 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 3.130065217391307 ], [ 28.614673913043465, 1.448239130434786 ] ] } }, + { "type": "Feature", "properties": { "id": 10046, "startid": 27, "endid": 26 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 3.130065217391307 ], [ 28.59465217391303, 5.552695652173915 ] ] } }, + { "type": "Feature", "properties": { "id": 10047, "startid": 26, "endid": 27 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 5.552695652173915 ], [ 28.59465217391303, 3.130065217391307 ] ] } }, + { "type": "Feature", "properties": { "id": 10048, "startid": 26, "endid": 25 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.59465217391303, 5.552695652173915 ], [ 28.55460869565216, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10049, "startid": 25, "endid": 26 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.55460869565216, 6.954217391304349 ], [ 28.59465217391303, 5.552695652173915 ] ] } }, + { "type": "Feature", "properties": { "id": 10050, "startid": 15, "endid": 16 }, "geometry": { "type": "LineString", "coordinates": [ [ 19.985304347826077, 6.974239130434784 ], [ 22.167673913043469, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10051, "startid": 16, "endid": 15 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.167673913043469, 6.914173913043479 ], [ 19.985304347826077, 6.974239130434784 ] ] } }, + { "type": "Feature", "properties": { "id": 10052, "startid": 16, "endid": 17 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.167673913043469, 6.914173913043479 ], [ 22.047543478260859, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10053, "startid": 17, "endid": 16 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.047543478260859, 9.116565217391305 ], [ 22.167673913043469, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10054, "startid": 17, "endid": 18 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.047543478260859, 9.116565217391305 ], [ 23.869521739130423, 11.77945652173913 ] ] } }, + { "type": "Feature", "properties": { "id": 10055, "startid": 18, "endid": 17 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.869521739130423, 11.77945652173913 ], [ 22.047543478260859, 9.116565217391305 ] ] } }, + { "type": "Feature", "properties": { "id": 10056, "startid": 18, "endid": 19 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.869521739130423, 11.77945652173913 ], [ 23.8495, 9.216673913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10057, "startid": 19, "endid": 18 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.8495, 9.216673913043479 ], [ 23.869521739130423, 11.77945652173913 ] ] } }, + { "type": "Feature", "properties": { "id": 10058, "startid": 19, "endid": 20 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.8495, 9.216673913043479 ], [ 23.789434782608684, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10059, "startid": 20, "endid": 19 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.789434782608684, 6.934195652173914 ], [ 23.8495, 9.216673913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10060, "startid": 20, "endid": 16 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.789434782608684, 6.934195652173914 ], [ 22.167673913043469, 6.914173913043479 ] ] } }, + { "type": "Feature", "properties": { "id": 10061, "startid": 16, "endid": 20 }, "geometry": { "type": "LineString", "coordinates": [ [ 22.167673913043469, 6.914173913043479 ], [ 23.789434782608684, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10062, "startid": 20, "endid": 21 }, "geometry": { "type": "LineString", "coordinates": [ [ 23.789434782608684, 6.934195652173914 ], [ 26.071913043478251, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10063, "startid": 21, "endid": 20 }, "geometry": { "type": "LineString", "coordinates": [ [ 26.071913043478251, 6.954217391304349 ], [ 23.789434782608684, 6.934195652173914 ] ] } }, + { "type": "Feature", "properties": { "id": 10064, "startid": 21, "endid": 22 }, "geometry": { "type": "LineString", "coordinates": [ [ 26.071913043478251, 6.954217391304349 ], [ 25.951782608695641, 9.196652173913044 ] ] } }, + { "type": "Feature", "properties": { "id": 10065, "startid": 22, "endid": 21 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.951782608695641, 9.196652173913044 ], [ 26.071913043478251, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10066, "startid": 22, "endid": 23 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.951782608695641, 9.196652173913044 ], [ 25.971804347826076, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10067, "startid": 23, "endid": 22 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.971804347826076, 11.8195 ], [ 25.951782608695641, 9.196652173913044 ] ] } }, + { "type": "Feature", "properties": { "id": 10068, "startid": 23, "endid": 24 }, "geometry": { "type": "LineString", "coordinates": [ [ 25.971804347826076, 11.8195 ], [ 28.234260869565205, 11.85954347826087 ] ] } }, + { "type": "Feature", "properties": { "id": 10069, "startid": 24, "endid": 23 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.234260869565205, 11.85954347826087 ], [ 25.971804347826076, 11.8195 ] ] } }, + { "type": "Feature", "properties": { "id": 10070, "startid": 21, "endid": 25 }, "geometry": { "type": "LineString", "coordinates": [ [ 26.071913043478251, 6.954217391304349 ], [ 28.55460869565216, 6.954217391304349 ] ] } }, + { "type": "Feature", "properties": { "id": 10071, "startid": 25, "endid": 21 }, "geometry": { "type": "LineString", "coordinates": [ [ 28.55460869565216, 6.954217391304349 ], [ 26.071913043478251, 6.954217391304349 ] ] } } + ] +} diff --git a/nav2_route/include/nav2_route/edge_scorer.hpp b/nav2_route/include/nav2_route/edge_scorer.hpp new file mode 100644 index 00000000000..79653f92e3b --- /dev/null +++ b/nav2_route/include/nav2_route/edge_scorer.hpp @@ -0,0 +1,87 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__EDGE_SCORER_HPP_ +#define NAV2_ROUTE__EDGE_SCORER_HPP_ + +#include +#include +#include + +#include "tf2_ros/buffer.h" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +namespace nav2_route +{ + +/** + * @class nav2_route::EdgeScorer + * @brief An class to encapsulate edge scoring logic for plugins and different user + * specified algorithms to influence graph search. It has access to the edge, which + * in turn has access to the parent and child node of the connection. It also contains + * action and arbitrary user-defined metadata to enable edge scoring logic based on + * arbitrary properties of the graph you select (e.g. some regions have a multiplier, + * some actions are discouraged with higher costs like having to go through a door, + * edges with reduced speed limits are proportionally less preferred for optimality + * relative to the distance the edge represents to optimize time to goal) + */ +class EdgeScorer +{ +public: + /** + * @brief Constructor + */ + explicit EdgeScorer( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + const std::shared_ptr costmap_subscriber); + + /** + * @brief Destructor + */ + ~EdgeScorer() = default; + + /** + * @brief Score the edge with the set of plugins + * @param edge Ptr to edge for scoring + * @param goal_pose Pose Stamped of desired goal + * @param score of edge + * @param final_edge whether this edge brings us to the goal or not + * @return If edge is valid + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, + float & score); + + /** + * @brief Provide the number of plugisn in the scorer loaded + * @return Number of scoring plugins + */ + int numPlugins() const; + +protected: + pluginlib::ClassLoader plugin_loader_; + std::vector plugins_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__EDGE_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp new file mode 100644 index 00000000000..3072f2626c9 --- /dev/null +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -0,0 +1,144 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_ +#define NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_ + +#include +#include +#include + +#include "tf2_ros/transform_listener.h" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_msgs/action/compute_route.hpp" +#include "nav2_msgs/action/compute_and_track_route.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/node_spatial_tree.hpp" +#include "nav2_route/goal_intent_search.hpp" + +namespace nav2_route +{ + +/** + * @class nav2_route::GoalIntentExtractor + * @brief Extracts the start and goal nodes in the graph to use for the routing + * request from the action request. This may use additional information about the + * environment or simply find the corresponding nodes specified in the request. + */ +class GoalIntentExtractor +{ +public: + /** + * @brief Constructor + */ + GoalIntentExtractor() = default; + + /** + * @brief Destructor + */ + ~GoalIntentExtractor() = default; + + /** + * @brief Configure extractor + * @param node Node to use to create any interface needed + * @param graph Graph to populate kD tree using + * @param id_to_graph_map Remapping vector to correlate nodeIDs + * @param tf TF buffer for transformations + * @param costmap_subscriber Costmap subscriber to use for traversability + * @param route_frame Planning frame + * @param base_frame Robot reference frame + */ + void configure( + nav2_util::LifecycleNode::SharedPtr node, + Graph & graph, + GraphToIDMap * id_to_graph_map, + std::shared_ptr tf, + std::shared_ptr costmap_subscriber, + const std::string & route_frame, + const std::string & base_frame); + + /** + * @brief Sets a new graph when updated + * @param graph Graph to populate kD tree using + * @param graph id_to_graph_map to get graph IDX for node IDs + */ + void setGraph(Graph & graph, GraphToIDMap * id_to_graph_map); + + /** + * @brief Transforms a pose into the route frame + * @param pose Pose to transform (e.g. start, goal) + * @param frame_id Frame to transform to + */ + geometry_msgs::msg::PoseStamped transformPose( + geometry_msgs::msg::PoseStamped & pose, + const std::string & frame_id); + /** + * @brief Main API to find the start and goal graph IDX (not IDs) for routing + * @param goal Action request goal + * @return start, goal node IDx + */ + template + NodeExtents + findStartandGoal(const std::shared_ptr goal); + + /** + * @brief Prune the start and end nodes in a route if the start or goal poses, + * respectively, are already along the route to avoid backtracking + * @param inpute_route Route to prune + * @param goal Action request goal + * @param first_time If this is the first time routing + * @return Pruned route + */ + template + Route pruneStartandGoal( + const Route & input_route, + const std::shared_ptr goal, + ReroutingState & rerouting_info); + + /** + * @brief Override the start pose for use in pruning if it is externally overridden + * Usually by the rerouting logic + * @param start_pose Starting pose of robot to prune using + */ + void overrideStart(const geometry_msgs::msg::PoseStamped & start_pose); + + /** + * @brief gets the desired start pose + * @return PoseStamped of start pose + */ + geometry_msgs::msg::PoseStamped getStart(); + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("GoalIntentExtractor")}; + std::shared_ptr node_spatial_tree_; + GraphToIDMap * id_to_graph_map_; + Graph * graph_; + std::shared_ptr tf_; + std::shared_ptr costmap_subscriber_; + std::string route_frame_; + std::string base_frame_; + geometry_msgs::msg::PoseStamped start_, goal_; + bool prune_goal_, enable_search_; + int max_nn_search_iterations_; + float max_dist_from_edge_, min_dist_from_goal_, min_dist_from_start_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_ diff --git a/nav2_route/include/nav2_route/goal_intent_search.hpp b/nav2_route/include/nav2_route/goal_intent_search.hpp new file mode 100644 index 00000000000..cd5a91d5476 --- /dev/null +++ b/nav2_route/include/nav2_route/goal_intent_search.hpp @@ -0,0 +1,266 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__GOAL_INTENT_SEARCH_HPP_ +#define NAV2_ROUTE__GOAL_INTENT_SEARCH_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "std_msgs/msg/color_rgba.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_msgs/msg/route.hpp" +#include "nav2_route/types.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/line_iterator.hpp" + +namespace nav2_route +{ + +namespace GoalIntentSearch +{ + +/** + * @class Find which position is nearest to a given point on a costmap using breadth-first search + */ +class BreadthFirstSearch +{ +public: + /** + * @brief Constructor + * @param costmap Costmap to check + */ + explicit BreadthFirstSearch(std::shared_ptr costmap) + : costmap_(costmap) + { + } + + /** + * @brief Search for the closest node to the given reference node + * @param reference_node The reference node to search from + * @param candidate_nodes The candidate nodes to search for + * @param max_iterations The maximum number of iterations to perform + * @return True if a candidate node is found, false otherwise + */ + bool search( + const geometry_msgs::msg::PoseStamped & reference_node, + const std::vector & candidate_nodes, + const int max_iterations = std::numeric_limits::max()) + { + closest_node_idx_ = 0u; + + // Convert target to costmap space + unsigned int goal_x, goal_y; + if (!costmap_->worldToMap(reference_node.pose.position.x, reference_node.pose.position.y, + goal_x, goal_y)) + { + return false; + } + unsigned int goal_id = costmap_->getIndex(goal_x, goal_y); + + // Convert candidates to costmap space + std::vector candidate_ids; + candidate_ids.reserve(candidate_nodes.size()); + for (const auto & node : candidate_nodes) { + unsigned int node_x, node_y; + if (!costmap_->worldToMap(node.pose.position.x, node.pose.position.y, node_x, node_y)) { + // Off the costmap, consider this candidate invalid + continue; + } + candidate_ids.push_back(costmap_->getIndex(node_x, node_y)); + } + + if (candidate_ids.empty()) { + return false; + } + + // Main Breadth-First Search + std::queue search_queue; + std::unordered_set visited; + visited.insert(goal_id); + search_queue.push(goal_id); + int iterations = 0; + while (!search_queue.empty() && iterations < max_iterations) { + unsigned int current_id = search_queue.front(); + search_queue.pop(); + iterations++; + + // Check if the current node is a candidate + auto iter = std::find(candidate_ids.begin(), candidate_ids.end(), current_id); + if (iter != candidate_ids.end()) { + closest_node_idx_ = std::distance(candidate_ids.begin(), iter); + return true; + } + + // Get neighbors of the current node + std::vector neighbors = getNeighbors(current_id); + + // Add unvisited neighbors to the queue + for (const auto & neighbor : neighbors) { + if (visited.find(neighbor) == visited.end()) { + visited.insert(neighbor); + search_queue.push(neighbor); + } + } + } + + // No solution found with full expansion or iterations exceeded + return false; + } + + /** + * @brief Get the neighbors of a given node + * @param current_id The current node id + * @return A vector of neighbor node ids + */ + std::vector getNeighbors(const unsigned int current_id) + { + int size_x = static_cast(costmap_->getSizeInCellsX()); + const std::vector offsets = { + -1, 1, -size_x, size_x, + -size_x - 1, -size_x + 1, + size_x - 1, size_x + 1}; + int costmap_size = static_cast(costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY()); + + unsigned int p_mx, p_my; + costmap_->indexToCells(current_id, p_mx, p_my); + + std::vector neighbors; + for (const auto & offset : offsets) { + // If out of bounds of costmap + int signed_neighbor_id = static_cast(current_id) + offset; + if (signed_neighbor_id < 0 || signed_neighbor_id >= costmap_size) { + continue; + } + unsigned int neighbor_id = static_cast(signed_neighbor_id); + + // If wrapping around the costmap + unsigned int n_mx, n_my; + costmap_->indexToCells(neighbor_id, n_mx, n_my); + if (std::fabs(static_cast(p_mx) - static_cast(n_mx)) > 1 || + std::fabs(static_cast(p_my) - static_cast(n_my)) > 1) + { + continue; + } + + // Add if not in collision + if (!inCollision(neighbor_id)) { + neighbors.push_back(neighbor_id); + } + } + + return neighbors; + } + + /** + * @brief Check if a node is in collision with the costmap + * @param id The node id to check + * @return True if the node is in collision, false otherwise + */ + bool inCollision(const unsigned int id) + { + // Check if the node is in collision + float cost = static_cast(costmap_->getCost(id)); + return cost >= 253.0f && cost != 255.0f; + } + + /** + * @brief Get the output closest node in candidate indices + * @return closest_node The closest candidate node index to the given point by search + */ + unsigned int getClosestNodeIdx() + { + return closest_node_idx_; + } + + /** + * @brief Destructor + */ + ~BreadthFirstSearch() = default; + +protected: + std::shared_ptr costmap_; + unsigned int closest_node_idx_{0u}; +}; + +/** + * @class Find if a line segment is in collision with the costmap or not + */ +class LoSCollisionChecker +{ +public: + /** + * @brief Constructor + * @param costmap Costmap to check + */ + explicit LoSCollisionChecker(std::shared_ptr costmap) + : costmap_(costmap) + { + } + + /** + * @brief Destructor + */ + ~LoSCollisionChecker() = default; + + /** + * @brief Find the line segment in cosmap frame + * @param start Start point + * @param end End point + * @return True if the line segment is on the costmap, false otherwise + */ + bool worldToMap(const geometry_msgs::msg::Point & start, const geometry_msgs::msg::Point & end) + { + if (!costmap_->worldToMap(start.x, start.y, x0_, y0_) || + !costmap_->worldToMap(end.x, end.y, x1_, y1_)) + { + return false; + } + return true; + } + + /** + * @brief Check if the line segment is in collision with the costmap + * @return True if the line segment is in collision, false otherwise + */ + bool isInCollision() + { + nav2_util::LineIterator iter(x0_, y0_, x1_, y1_); + for (; iter.isValid(); iter.advance()) { + float cost = static_cast(costmap_->getCost(iter.getX(), iter.getY())); + if (cost >= 253.0f && cost != 255.0 /*unknown*/) { + return true; + } + } + return false; + } + +protected: + std::shared_ptr costmap_; + unsigned int x0_, x1_, y0_, y1_; +}; + +} // namespace GoalIntentSearch + +} // namespace nav2_route + +#endif // NAV2_ROUTE__GOAL_INTENT_SEARCH_HPP_ diff --git a/nav2_route/include/nav2_route/graph_loader.hpp b/nav2_route/include/nav2_route/graph_loader.hpp new file mode 100644 index 00000000000..ea602b11019 --- /dev/null +++ b/nav2_route/include/nav2_route/graph_loader.hpp @@ -0,0 +1,107 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__GRAPH_LOADER_HPP_ +#define NAV2_ROUTE__GRAPH_LOADER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/interfaces/graph_file_loader.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" + +namespace nav2_route +{ + +/** + * @class nav2_route::GraphLoader + * @brief An action server to load graph files into the graph object for search and processing + */ +class GraphLoader +{ +public: + /** + * @brief A constructor for nav2_route::GraphLoader + * @param options Additional options to control creation of the node. + */ + explicit GraphLoader( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf, + const std::string frame); + + /** + * @brief A destructor for nav2_route::GraphLoader + */ + ~GraphLoader() = default; + + /** + * @brief Loads a graph object with file information from a filepath + * @param graph Graph to populate + * @param idx_map A map translating nodeid's to graph idxs for use in graph modification + * services and idx-based route planning requests. This is much faster than using a + * map the full graph data structure. + * @param filepath The filepath to the graph data + * @param graph_file_loader_id The id of the GraphFileLoader + * @return bool If successful + */ + bool loadGraphFromFile( + Graph & graph, + GraphToIDMap & idx_map, + const std::string & filepath); + + /** + * @brief Loads a graph object with file information from ROS parameter, if provided + * @param graph Graph to populate + * @param idx_map A map translating nodeid's to graph idxs for use in graph modification + * services and idx-based route planning requests. This is much faster than using a + * map the full graph data structure. + * @param graph_file_loader_id The id of the GraphFileLoader + * @return bool If successful or none provided + */ + bool loadGraphFromParameter( + Graph & graph, + GraphToIDMap & idx_map); + + /** + * @brief Transform the coordinates in the graph to the route frame + * @param[in/out] graph The graph to be transformed + * @return True if transformation was successful + */ + bool transformGraph(Graph & graph); + +protected: + std::string route_frame_, graph_filepath_; + std::shared_ptr tf_; + rclcpp::Logger logger_{rclcpp::get_logger("GraphLoader")}; + + // Graph Parser + pluginlib::ClassLoader plugin_loader_; + GraphFileLoader::Ptr graph_file_loader_; + std::string default_plugin_id_; + std::string plugin_type_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__GRAPH_LOADER_HPP_ diff --git a/nav2_route/include/nav2_route/graph_saver.hpp b/nav2_route/include/nav2_route/graph_saver.hpp new file mode 100644 index 00000000000..328ec65840e --- /dev/null +++ b/nav2_route/include/nav2_route/graph_saver.hpp @@ -0,0 +1,91 @@ +// Copyright (c) 2024 John Chrosniak +// +// 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 NAV2_ROUTE__GRAPH_SAVER_HPP_ +#define NAV2_ROUTE__GRAPH_SAVER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/interfaces/graph_file_saver.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" + +namespace nav2_route +{ + +/** + * @class nav2_route::GraphSaver + * @brief An object to save graph objects to a file + */ +class GraphSaver +{ +public: + /** + * @brief A constructor for nav2_route::GraphSaver + * @param node Lifecycle node encapsulated by the GraphSaver + * @param tf A tf buffer + * @param frame Coordinate frame that the graph belongs to + */ + explicit GraphSaver( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf, + const std::string frame); + + /** + * @brief A destructor for nav2_route::GraphSaver + */ + ~GraphSaver() = default; + + /** + * @brief Saves a graph object to a file + * @param graph Graph to save + * @param filepath The filepath to the graph data + * @return bool If successful + */ + bool saveGraphToFile( + Graph & graph, + std::string filepath = ""); + + /** + * @brief Transform the coordinates in the graph to the route frame + * @param[in/out] graph The graph to be transformed + * @return True if transformation was successful + */ + bool transformGraph(Graph & graph); + +protected: + std::string route_frame_, graph_filepath_; + std::shared_ptr tf_; + rclcpp::Logger logger_{rclcpp::get_logger("GraphSaver")}; + + // Graph Parser + pluginlib::ClassLoader plugin_loader_; + GraphFileSaver::Ptr graph_file_saver_; + std::string default_plugin_id_; + std::string plugin_type_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__GRAPH_SAVER_HPP_ diff --git a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp new file mode 100644 index 00000000000..7aa9a3123ee --- /dev/null +++ b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp @@ -0,0 +1,88 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__INTERFACES__EDGE_COST_FUNCTION_HPP_ +#define NAV2_ROUTE__INTERFACES__EDGE_COST_FUNCTION_HPP_ + +#include +#include + +#include "tf2_ros/buffer.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "pluginlib/class_loader.hpp" +#include "nav2_route/types.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +namespace nav2_route +{ + +/** + * @class EdgeCostFunction + * @brief A plugin interface to score edges during graph search to modify + * the lowest cost path (e.g. by distance, maximum speed, regions prefer not to travel + * blocked by occupancy, or using arbitrarily defined user metadata stored in the + * edge and nodes of interest.) + */ +class EdgeCostFunction +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Constructor + */ + EdgeCostFunction() = default; + + /** + * @brief Virtual destructor + */ + virtual ~EdgeCostFunction() = default; + + /** + * @brief Configure the scorer, but do not store the node + * @param parent pointer to user's node + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) = 0; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + */ + virtual bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) = 0; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + virtual std::string getName() = 0; + + /** + * @brief Prepare for a new cycle, by resetting state, grabbing data + * to use for all immediate requests, or otherwise prepare for scoring + */ + virtual void prepare() {} +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__INTERFACES__EDGE_COST_FUNCTION_HPP_ diff --git a/nav2_route/include/nav2_route/interfaces/graph_file_loader.hpp b/nav2_route/include/nav2_route/interfaces/graph_file_loader.hpp new file mode 100644 index 00000000000..6774f0d1870 --- /dev/null +++ b/nav2_route/include/nav2_route/interfaces/graph_file_loader.hpp @@ -0,0 +1,71 @@ +// Copyright (c) 2025 Joshua Wallace +// +// 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 NAV2_ROUTE__INTERFACES__GRAPH_FILE_LOADER_HPP_ +#define NAV2_ROUTE__INTERFACES__GRAPH_FILE_LOADER_HPP_ + +#include +#include + + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/types.hpp" + +namespace nav2_route +{ + +/** + * @class GraphFileLoader + * @brief A plugin interface to parse a file into the graph + */ +class GraphFileLoader +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Constructor + */ + GraphFileLoader() = default; + + /** + * @brief Virtual destructor + */ + virtual ~GraphFileLoader() = default; + + /** + * @brief Configure the graph file loader, but do not store the node + * @param parent pointer to user's node + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; + + /** + * @brief Method to load the graph from the filepath + * @param graph The graph to populate + * @param filepath The file to parse + * @param idx_map A map translating nodeid's to graph idxs for use in graph modification + * services and idx-based route planning requests. This is much faster than using a + * map the full graph data structure. + * @return true if graph was successfully loaded + */ + virtual bool loadGraphFromFile( + Graph & graph, + GraphToIDMap & graph_to_id_map, + std::string filepath) = 0; +}; +} // namespace nav2_route + +#endif // NAV2_ROUTE__INTERFACES__GRAPH_FILE_LOADER_HPP_ diff --git a/nav2_route/include/nav2_route/interfaces/graph_file_saver.hpp b/nav2_route/include/nav2_route/interfaces/graph_file_saver.hpp new file mode 100644 index 00000000000..4851655e4dd --- /dev/null +++ b/nav2_route/include/nav2_route/interfaces/graph_file_saver.hpp @@ -0,0 +1,67 @@ +// Copyright (c) 2024 John Chrosniak +// +// 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 NAV2_ROUTE__INTERFACES__GRAPH_FILE_SAVER_HPP_ +#define NAV2_ROUTE__INTERFACES__GRAPH_FILE_SAVER_HPP_ + +#include +#include + + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/types.hpp" + +namespace nav2_route +{ + +/** + * @class GraphFileSaver + * @brief A plugin interface to parse a file into the graph + */ +class GraphFileSaver +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Constructor + */ + GraphFileSaver() = default; + + /** + * @brief Virtual destructor + */ + virtual ~GraphFileSaver() = default; + + /** + * @brief Configure the graph file saver, but do not store the node + * @param parent pointer to user's node + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0; + + /** + * @brief Method to save the graph to the filepath + * @param graph The graph to save + * @param filepath The path to save the file to + * @return true if graph was successfully saved + */ + virtual bool saveGraphToFile( + Graph & graph, + std::string filepath) = 0; +}; +} // namespace nav2_route + +#endif // NAV2_ROUTE__INTERFACES__GRAPH_FILE_SAVER_HPP_ diff --git a/nav2_route/include/nav2_route/interfaces/route_operation.hpp b/nav2_route/include/nav2_route/interfaces/route_operation.hpp new file mode 100644 index 00000000000..0c5d140f3ad --- /dev/null +++ b/nav2_route/include/nav2_route/interfaces/route_operation.hpp @@ -0,0 +1,138 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__INTERFACES__ROUTE_OPERATION_HPP_ +#define NAV2_ROUTE__INTERFACES__ROUTE_OPERATION_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "pluginlib/class_loader.hpp" +#include "nav2_route/types.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +namespace nav2_route +{ + +/** + * @struct OperationResult + * @brief a struct to hold return from an operation + */ +struct OperationResult +{ + bool reroute{false}; + std::vector blocked_ids; +}; + +/** + * @enum nav2_route::RouteOperationType + * @brief The type of operation plugin + */ +enum class RouteOperationType +{ + ON_GRAPH = 0, + ON_STATUS_CHANGE = 1, + ON_QUERY = 2 +}; + +/** + * @class RouteOperation + * @brief A plugin interface to perform an operation while tracking the route + * such as triggered from the graph file when a particular node is achieved, + * edge is entered or exited. The API also supports triggering arbitrary operations + * when a status has changed (e.g. any node is achieved) or at a regular frequency + * on query set at a fixed rate of tracker_update_rate. Operations can request the + * system to reroute and avoid certain edges. Example operations may be to: + * reroute when blocked or at a required rate (though may want to use BTs instead), + * adjust speed limits, wait, call an external service or action to perform a task + * such as calling an elevator or opening an automatic door, etc. + * Operations may throw nav2_core::OperationFailed exceptions in failure cases + */ +class RouteOperation +{ +public: + using Ptr = std::shared_ptr; + + /** + * @brief Constructor + */ + RouteOperation() = default; + + /** + * @brief Destructor + */ + virtual ~RouteOperation() = default; + + /** + * @brief Configure the operation plugin (get params, create interfaces, etc) + * @param node A node to use + * @param name the plugin's name set by the param file that may need to be used + * to correlate an operation instance to the navigation graph operation calls + */ + virtual void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) = 0; + + /** + * @brief An API to get the name of a particular operation for triggering, query + * or logging + * @return the plugin's name + */ + virtual std::string getName() = 0; + + /** + * @brief Indication of which type of route operation this plugin is. Whether it is + * be called by the graph's nodes or edges, whether it should be triggered at any + * status change, or whether it should be called constantly on any query. By default, + * it will create operations that are only called when a graph's node or edge requests it. + * Note that On Query, On Status Change, and On Graph are mutually exclusive since each + * operation type is merely a subset of the previous level's specificity. + * @return The type of operation (on graph call, on status changes, or constantly) + */ + virtual RouteOperationType processType() {return RouteOperationType::ON_GRAPH;} + + /** + * @brief The main route operation API to perform an operation when triggered. + * The return value indicates if the route operation is requesting rerouting when returning true. + * Could be if this operation is checking if a route is in collision or operation + * failed (to open a door, for example) and thus this current route is now invalid. + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, + * for additional context (must check nullptr if at goal) + * @param edge_entered Edge entered by node achievement, + * for additional context (must check if nullptr if no future edge, at goal) + * @param edge_exited Edge exited by node achievement, + * for additional context (must check if nullptr if no last edge, starting) + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + virtual OperationResult perform( + NodePtr node_achieved, + EdgePtr edge_entered, + EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * mdata = nullptr) = 0; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__INTERFACES__ROUTE_OPERATION_HPP_ diff --git a/nav2_route/include/nav2_route/node_spatial_tree.hpp b/nav2_route/include/nav2_route/node_spatial_tree.hpp new file mode 100644 index 00000000000..ac5347bc0bd --- /dev/null +++ b/nav2_route/include/nav2_route/node_spatial_tree.hpp @@ -0,0 +1,111 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__NODE_SPATIAL_TREE_HPP_ +#define NAV2_ROUTE__NODE_SPATIAL_TREE_HPP_ + +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" + +namespace nav2_route +{ + +// Search in XY dimension for nearest neighbors +const size_t DIMENSION = 2; + +/** + * @class nav2_route::GraphAdaptor + * @brief An adaptor for Nanoflann to operate on our graph object without copying + */ +struct GraphAdaptor +{ + explicit GraphAdaptor(const Graph & obj_) + : obj(obj_) {} + + inline size_t kdtree_get_point_count() const {return obj.size();} + + inline double kdtree_get_pt(const size_t idx, const size_t dim) const + { + if (dim == 0) { + return obj[idx].coords.x; + } + return obj[idx].coords.y; + } + + template bool kdtree_get_bbox(BBOX & /*bb*/) const {return false;} + + const Graph & obj; +}; + +typedef nanoflann::KDTreeSingleIndexAdaptor< + nanoflann::L2_Simple_Adaptor, GraphAdaptor, DIMENSION> kd_tree_t; + +/** + * @class nav2_route::NodeSpatialTree + * @brief An object to find kNNs of the graph to determining the start and end + * nodes to utilize for planning in a free-space-style request of start and goal poses. + * Since the graph does not change over time, we can precompute this quadtree and reuse it + * over many requests. + */ +class NodeSpatialTree +{ +public: + /** + * @brief Constructor + */ + NodeSpatialTree() = default; + + /** + * @brief Destructor + */ + ~NodeSpatialTree(); + + /** + * @brief Compute the kd-tree based on the graph node information + * @param graph The graph of nodes for the route + */ + void computeTree(Graph & graph); + + /** + * @brief Find the closest node to a given pose + * @param pose_in Pose to find node near + * @param node_id The return ID of the node + * @return if successfully found + */ + bool findNearestGraphNodesToPose( + const geometry_msgs::msg::PoseStamped & pose_in, + std::vector & node_ids); + + /** + * @brief Set the number of nodes to search in local area for + * @param num Numbers of nearest nodes to return + */ + void setNumOfNearestNodes(int num_of_nearest_nodes); + +protected: + kd_tree_t * kdtree_; + GraphAdaptor * adaptor_; + Graph * graph_; + int num_of_nearest_nodes_{3}; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__NODE_SPATIAL_TREE_HPP_ diff --git a/nav2_route/include/nav2_route/operations_manager.hpp b/nav2_route/include/nav2_route/operations_manager.hpp new file mode 100644 index 00000000000..b30045f94c9 --- /dev/null +++ b/nav2_route/include/nav2_route/operations_manager.hpp @@ -0,0 +1,126 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 +#include + +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/interfaces/route_operation.hpp" + +#ifndef NAV2_ROUTE__OPERATIONS_MANAGER_HPP_ +#define NAV2_ROUTE__OPERATIONS_MANAGER_HPP_ + +namespace nav2_route +{ + +/** + * @class nav2_route::OperationsManager + * @brief Manages operations plugins to call on route tracking + */ +class OperationsManager +{ +public: + typedef std::vector::const_iterator OperationsIter; + + /** + * @brief A constructor for nav2_route::OperationsManager + */ + explicit OperationsManager( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber); + + /** + * @brief A Destructor for nav2_route::OperationsManager + */ + ~OperationsManager() = default; + + /** + * @brief Finds the set of operations stored in the graph to trigger at this transition + * @param node Node to check + * @param edge_entered Edge entered to check for ON_ENTER events + * @param edge_exit Edge exit to check for ON_EXIT events + * @return OperationPtrs A vector of operation pointers to execute + */ + OperationPtrs findGraphOperations( + const NodePtr node, const EdgePtr edge_enter, const EdgePtr edge_exit); + + /** + * @brief Finds the set of operations stored in graph objects, by event + * @param node op_vec Operations vector to check + * @param trigger Trigger for which operations in op_vec should be included + * @param operations Output vector populated with relevant operations + */ + template + void findGraphOperationsToProcess( + T & obj, + const OperationTrigger & trigger, + OperationPtrs & operations); + + /** + * @brief Updates manager result state by an individual operation's result + * @param name Operations' name + * @param op_result Operations' result + * @param result Manager's result to update + */ + void updateResult( + const std::string & name, const OperationResult & op_result, OperationsResult & result); + + /** + * @brief Processes the operations at this tracker state + * @param status_change Whether something meaningful has changed + * @param state The route tracking state to check for state info + * @param route The raw route being tracked + * @param pose robot pose + * @param rerouting_info Rerouting information regarding previous partial state + * @return A result vector whether the operations are requesting something to occur + */ + OperationsResult process( + const bool status_change, + const RouteTrackingState & state, + const Route & route, + const geometry_msgs::msg::PoseStamped & pose, + const ReroutingState & rerouting_info); + +protected: + /** + * @brief Processes a vector of operations plugins, by trigger + * @param operations Operations to trigger + * @param Results to populate from operations + */ + void processOperationsPluginVector( + const std::vector & operations, OperationsResult & result, + const NodePtr node, + const EdgePtr edge_entered, + const EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & pose); + + pluginlib::ClassLoader plugin_loader_; + std::unordered_map graph_operations_; + std::vector change_operations_; + std::vector query_operations_; + rclcpp::Logger logger_{rclcpp::get_logger("OperationsManager")}; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__OPERATIONS_MANAGER_HPP_ diff --git a/nav2_route/include/nav2_route/path_converter.hpp b/nav2_route/include/nav2_route/path_converter.hpp new file mode 100644 index 00000000000..90b61898160 --- /dev/null +++ b/nav2_route/include/nav2_route/path_converter.hpp @@ -0,0 +1,88 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PATH_CONVERTER_HPP_ +#define NAV2_ROUTE__PATH_CONVERTER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" + +namespace nav2_route +{ +/** + * @class nav2_route::PathConverter + * @brief An helper to convert the route into dense paths + */ +class PathConverter +{ +public: + /** + * @brief A constructor for nav2_route::PathConverter + */ + PathConverter() = default; + + /** + * @brief A destructor for nav2_route::PathConverter + */ + ~PathConverter() = default; + + /** + * @brief Configure the object + * @param node Node to use to get params and create interfaces + */ + void configure(nav2_util::LifecycleNode::SharedPtr node); + + /** + * @brief Convert a Route into a dense path + * @param route Route object + * @param rerouting_info Re-Route info in case partial path to be populated + * @param frame Frame ID from planning + * @param now Time + * @return Path of the route + */ + nav_msgs::msg::Path densify( + const Route & route, + const ReroutingState & rerouting_info, + const std::string & frame, + const rclcpp::Time & now); + + /** + * @brief Convert an individual edge into a dense line + * @param x0 X initial + * @param y0 Y initial + * @param x1 X final + * @param y1 Y final + * @param poses Pose vector reference to populate + */ + void interpolateEdge( + float x0, float y0, float x1, float y1, + std::vector & poses); + +protected: + rclcpp_lifecycle::LifecyclePublisher::SharedPtr path_pub_; + float density_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PATH_CONVERTER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp new file mode 100644 index 00000000000..dfa632962a7 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp @@ -0,0 +1,94 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__COSTMAP_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__COSTMAP_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/line_iterator.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" + +namespace nav2_route +{ + +/** + * @class CostmapScorer + * @brief Scores edges by the average or maximum cost found while iterating over the + * edge's line segment in the global costmap + */ +class CostmapScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + CostmapScorer() = default; + + /** + * @brief destructor + */ + virtual ~CostmapScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + + /** + * @brief Prepare for a new cycle, by resetting state, grabbing data + * to use for all immediate requests, or otherwise prepare for scoring + */ + void prepare() override; + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("CostmapScorer")}; + rclcpp::Clock::SharedPtr clock_; + std::string name_; + bool use_max_, invalid_on_collision_, invalid_off_map_; + float weight_, max_cost_; + std::shared_ptr costmap_subscriber_; + std::shared_ptr costmap_{nullptr}; + unsigned int check_resolution_ {1u}; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__COSTMAP_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp new file mode 100644 index 00000000000..6e57c9a78c3 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/distance_scorer.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DISTANCE_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DISTANCE_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_route +{ + +/** + * @class DistanceScorer + * @brief Scores edges by the distance traversed, weighted by speed limit metadata + * to optimize for time to goal, when %-based speed limits are set + */ +class DistanceScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + DistanceScorer() = default; + + /** + * @brief destructor + */ + virtual ~DistanceScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + std::string name_; + std::string speed_tag_; + float weight_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DISTANCE_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp new file mode 100644 index 00000000000..e28c484600f --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp @@ -0,0 +1,95 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DYNAMIC_EDGES_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DYNAMIC_EDGES_SCORER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_msgs/srv/dynamic_edges.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_route +{ + +/** + * @class DynamicEdgesScorer + * @brief Rejects edges that are in the closed set of edges for navigation to prevent + * routes from containing paths blocked or otherwise deemed not currently traversable + */ +class DynamicEdgesScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + DynamicEdgesScorer() = default; + + /** + * @brief destructor + */ + virtual ~DynamicEdgesScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + + /** + * @brief Service callback to process edge changes + * @param request Service request containing newly closed edges or opened edges + * @param response Response to service (empty) + */ + void closedEdgesCb( + const std::shared_ptr request, + std::shared_ptr response); + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("DynamicEdgesScorer")}; + std::string name_; + std::set closed_edges_; + std::unordered_map dynamic_penalties_; + rclcpp::Service::SharedPtr service_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__DYNAMIC_EDGES_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp new file mode 100644 index 00000000000..562f5b67339 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp @@ -0,0 +1,92 @@ +// Copyright (c) 2024, Polymath Robotics 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 NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__GOAL_ORIENTATION_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__GOAL_ORIENTATION_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/line_iterator.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" +#include "angles/angles.h" + +namespace nav2_route +{ + +/** + * @class GoalOrientationScorer + * @brief Scores an edge leading to the goal node by comparing the orientation of the route + * pose and the orientation of the edge by multiplying the deviation from the desired + * orientation with a user defined weight. An alternative method can be selected, with + * the use_orientation_threshold flag, which rejects the edge it is greater than some + * tolerance + */ +class GoalOrientationScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + GoalOrientationScorer() = default; + + /** + * @brief destructor + */ + virtual ~GoalOrientationScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("GoalOrientationScorer")}; + std::string name_; + double orientation_tolerance_; + float orientation_weight_; + bool use_orientation_threshold_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__GOAL_ORIENTATION_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp new file mode 100644 index 00000000000..476fe2f269b --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp @@ -0,0 +1,80 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__PENALTY_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__PENALTY_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_route +{ + +/** + * @class PenaltyScorer + * @brief Adjusts the score of an edge by an amount set as metadata in the graph + */ +class PenaltyScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + PenaltyScorer() = default; + + /** + * @brief destructor + */ + virtual ~PenaltyScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + std::string name_; + std::string penalty_tag_; + float weight_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__PENALTY_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp new file mode 100644 index 00000000000..eabeb980478 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp @@ -0,0 +1,96 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__SEMANTIC_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__SEMANTIC_SCORER_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_route +{ + +/** + * @class SemanticScorer + * @brief Scores an edge based on arbitrary graph semantic data such as set priority/danger + * levels or regional attributes (e.g. living room, bathroom, work cell 2) + */ +class SemanticScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + SemanticScorer() = default; + + /** + * @brief destructor + */ + virtual ~SemanticScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Scores graph object based on metadata's semantic value at key + * @param mdata Metadata + * @param score to add to + */ + void metadataValueScorer(Metadata & mdata, float & score); + + /** + * @brief Scores graph object based on metadata's key values + * @param mdata Metadata + * @param score to add to + */ + void metadataKeyScorer(Metadata & mdata, float & score); + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + std::string name_, key_; + std::unordered_map semantic_info_; + float weight_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__SEMANTIC_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp new file mode 100644 index 00000000000..41d92970704 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp @@ -0,0 +1,94 @@ +// Copyright (c) 2025, Polymath Robotics 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 NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/line_iterator.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" +#include "angles/angles.h" + +namespace nav2_route +{ + +/** + * @class StartPoseOrientationScorer + * @brief Scores initial edge by comparing the orientation of the robot's current + * pose and the orientation of the edge by multiplying the deviation from the desired + * orientation with a user defined weight. An alternative method can be selected, with + * the use_orientation_threshold flag, which rejects the edge it is greater than some + * tolerance + */ +class StartPoseOrientationScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + StartPoseOrientationScorer() = default; + + /** + * @brief destructor + */ + virtual ~StartPoseOrientationScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + rclcpp::Logger logger_{rclcpp::get_logger("StartPoseOrientationScorer")}; + std::string name_; + std::shared_ptr tf_buffer_; + double orientation_tolerance_; + float orientation_weight_; + bool use_orientation_threshold_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__START_POSE_ORIENTATION_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp new file mode 100644 index 00000000000..1213a7d9391 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/time_scorer.hpp @@ -0,0 +1,82 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__TIME_SCORER_HPP_ +#define NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__TIME_SCORER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/edge_cost_function.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_route +{ + +/** + * @class TimeScorer + * @brief Scores edges by the time to traverse an edge. + * It uses previous times to navigate the edge primarily, then secondarily uses + * maximum speed and absolute speed limits to estimate with edge length. + */ +class TimeScorer : public EdgeCostFunction +{ +public: + /** + * @brief Constructor + */ + TimeScorer() = default; + + /** + * @brief destructor + */ + virtual ~TimeScorer() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Main scoring plugin API + * @param edge The edge pointer to score, which has access to the + * start/end nodes and their associated metadata and actions + * @param cost of the edge scored + * @return bool if this edge is open valid to traverse + */ + bool score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override; + +protected: + std::string name_; + std::string speed_tag_, prev_time_tag_; + float weight_, max_vel_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__EDGE_COST_FUNCTIONS__TIME_SCORER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp b/nav2_route/include/nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp new file mode 100644 index 00000000000..0cb9f34ae17 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp @@ -0,0 +1,148 @@ +// Copyright (c) 2025 Joshua Wallace +// +// 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 +#include + +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/interfaces/graph_file_loader.hpp" + +#ifndef NAV2_ROUTE__PLUGINS__GRAPH_FILE_LOADERS__GEOJSON_GRAPH_FILE_LOADER_HPP_ +#define NAV2_ROUTE__PLUGINS__GRAPH_FILE_LOADERS__GEOJSON_GRAPH_FILE_LOADER_HPP_ + +namespace nav2_route +{ + +/** + * @class nav2_route::GeoJsonGraphFileLoader + * @brief A GraphFileLoader plugin to load a geojson graph representation + */ +class GeoJsonGraphFileLoader : public GraphFileLoader +{ +public: + using Json = nlohmann::json; + + /** + * @brief Constructor + */ + GeoJsonGraphFileLoader() = default; + + /** + * @brief Destructor + */ + ~GeoJsonGraphFileLoader() = default; + + /** + * @brief Configure, but do not store the node + * @param parent pointer to user's node + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + + /** + * @brief Loads the geojson file into the graph + * @param graph The graph to be populated by the geojson file + * @param graph_to_id_map A map of node id's to the graph index + * @param filepath The path of the file to load + * @return True if the graph was successfully loaded + */ + bool loadGraphFromFile( + Graph & graph, + GraphToIDMap & graph_to_id_map, + std::string filepath) override; + +protected: + /** + * @brief Checks if a file even exists on the filesystem + * @param filepath The filepath to be checked + * @return True if the file path provided exists + */ + bool doesFileExist(const std::string & filepath); + + /** + * @brief Get the nodes and edges from features + * @param[in] features The features that contain the nodes and edges + * @param[out] nodes The nodes found within the features + * @param[out] edges The edges found within the features + */ + void getGraphElements( + const Json & features, std::vector & nodes, std::vector & edges); + + /** + * @brief Add nodes into the graph + * @param[out] graph The graph that will contain the new nodes + * @param[out] graph_to_id_map A map of node id to the graph index + * @param[in] nodes The nodes to be added into the graph + */ + void addNodesToGraph(Graph & graph, GraphToIDMap & graph_to_id_map, std::vector & nodes); + + /** + * @brief Add edges into the graph + * @param[out] graph The graph that will contain the new edges + * @param[in] graph_to_id_map A map of node id to the graph id + * @param[in] edges The edges to be added into the graph + */ + void addEdgesToGraph(Graph & graph, GraphToIDMap & graph_to_id_map, std::vector & edges); + + /** + * @brief Converts the coordinates from the json object into the Coordinates type + * @param node The json object that holds the coordinate data + * @return The coordinates found in the json object + */ + Coordinates convertCoordinatesFromJson(const Json & node); + + /** + * @brief Converts the metadata from the json object into the metadata type + * @param properties The json object that holds the metadata + * @param key The key for the metadata + * @return The converted metadata + */ + Metadata convertMetaDataFromJson(const Json & properties, const std::string & key = "metadata"); + + /** + * @brief Converts the operation from the json object into the operation type + * @param json_operation The json object that holds the operation data + * @return The converted operation data + */ + Operation convertOperationFromJson(const Json & json_operation); + + /** + * @brief Converts the operations data from the json object into the operations type if present + * @param properties The json object that holds the operations data + * @return Operations The converted operations data + */ + Operations convertOperationsFromJson(const Json & properties); + + /** + * @brief Converts the edge cost data from the json object into the edge cost type + * @param properties The json object that holds the edge cost data + * @return EdgeCost The converted edge cost data + */ + EdgeCost convertEdgeCostFromJson(const Json & properties); + + rclcpp::Logger logger_{rclcpp::get_logger("GeoJsonGraphFileLoader")}; +}; + +NLOHMANN_JSON_SERIALIZE_ENUM( + OperationTrigger, { + {OperationTrigger::NODE, "NODE"}, + {OperationTrigger::ON_ENTER, "ON_ENTER"}, + {OperationTrigger::ON_EXIT, "ON_EXIT"}, + }) + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__GRAPH_FILE_LOADERS__GEOJSON_GRAPH_FILE_LOADER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp b/nav2_route/include/nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp new file mode 100644 index 00000000000..6352462c8da --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2024 John Chrosniak +// +// 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 +#include + +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/interfaces/graph_file_saver.hpp" +#include "nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp" + +#ifndef NAV2_ROUTE__PLUGINS__GRAPH_FILE_SAVERS__GEOJSON_GRAPH_FILE_SAVER_HPP_ +#define NAV2_ROUTE__PLUGINS__GRAPH_FILE_SAVERS__GEOJSON_GRAPH_FILE_SAVER_HPP_ + +namespace nav2_route +{ + +/** + * @class nav2_route::GeoJsonGraphFileSaver + * @brief A GraphFileSaver plugin to save a geojson graph representation + */ +class GeoJsonGraphFileSaver : public GraphFileSaver +{ +public: + using Json = nlohmann::json; + + /** + * @brief Constructor + */ + GeoJsonGraphFileSaver() = default; + + /** + * @brief Destructor + */ + ~GeoJsonGraphFileSaver() = default; + + /** + * @brief Configure, but do not store the node + * @param parent pointer to user's node + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) override; + + /** + * @brief Saves the graph to a geojson file + * @param graph The graph to save to the geojson file + * @param filepath The path to save the graph to + * @return True if successful + */ + bool saveGraphToFile( + Graph & graph, + std::string filepath) override; + +protected: + /** + * @brief Add nodes into the graph + * @param[out] graph The graph that will contain the new nodes + * @param[in] json_features Json array to add the nodes to + */ + void loadNodesFromGraph(Graph & graph, std::vector & json_features); + + /** + * @brief Add edges into the graph + * @param[out] graph The graph that will contain the new edges + * @param[in] json_edges Json array to add the edges to + */ + void loadEdgesFromGraph(Graph & graph, std::vector & json_edges); + + /** + * @brief Convert graph metadata to Json + * @param metadata Metadata from a node or edge in the graph + * @param json_metadata Json entry to store metadata in + */ + void convertMetaDataToJson(const Metadata & metadata, Json & json_metadata); + + /** + * @brief Convert graph operation to Json + * @param Operations Operations information from the graph + * @param json_operations Json entry to store operation data in + */ + void convertOperationsToJson(const Operations & operations, Json & json_operations); + + rclcpp::Logger logger_{rclcpp::get_logger("GeoJsonGraphFileSaver")}; +}; +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__GRAPH_FILE_SAVERS__GEOJSON_GRAPH_FILE_SAVER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operation_client.hpp b/nav2_route/include/nav2_route/plugins/route_operation_client.hpp new file mode 100644 index 00000000000..e0862ed81e2 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operation_client.hpp @@ -0,0 +1,224 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__ROUTE_OPERATION_CLIENT_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATION_CLIENT_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace nav2_route +{ + +using namespace std::chrono_literals; // NOLINT + +/** + * @class RouteOperationClient + * @brief A route operation base class to trigger a service event at a node or edge. + * This is meant to be named accordingly to the event of query in the parameter file + * (e.g. OpenDoor, CallElevator). Thus, a single RouteOperationClient implementation can + * support many different operation instances calling different services. The template type + * may be overridden for any applications. It may be set up to either trigger a + * single service if the `service_name` is set in the parameter file at launch for all calls + * **or** trigger different services depending on the `service_name` set in the metadata of the + * node or edge operation given. + * + * For example: OpenDoor + * RouteOperationClient instance can open a door at service "/open_door" set in the parameter file + * that it calls every time the OpenDoor operation is triggered in the graph (ceneralized service). + * Or the OpenDoor instance can call any service whose `service_name` is set in the operation's + * metadata (e.g. "/open_door1", "/open_door2", ...) corresponding to other instances of similar + * use. In this case, the specification of "OpenDoor" operation is less to do with the service name + * and more to do with the type of service template (nav2_msgs) used to populate the request + * (decentralized). This allows for massive reuse of a single plugin implementation in centralized + * or decentralized applications. + * + * The templated nature of this node makes it a base class for any such service + * containing additional request or response fields by implementing the virtual interfaces + * configureEvent, populateRequest, processResponse appropriately. The std_srvs/Trigger + * example TriggerEvent can be thought of as a useful entry level demo and useful working primitive + */ +template +class RouteOperationClient : public RouteOperation +{ +public: + /** + * @brief Constructor + */ + RouteOperationClient() = default; + + /** + * @brief destructor + */ + virtual ~RouteOperationClient() = default; + + /** + * @brief Configure client with any necessary parameters, etc. + * May change or reset `main_srv_name_` variable to control + * main service name and existence. + */ + virtual void configureEvent( + const rclcpp_lifecycle::LifecycleNode::SharedPtr /*node*/, + const std::string & /*name*/) {} + + /** + * @brief Populate request with details for service, if necessary + */ + virtual void populateRequest( + std::shared_ptr/*request*/, const Metadata * /*mdata*/) {} + + /** + * @brief Process response from service to populate a result, if necessary + */ + virtual OperationResult processResponse( + std::shared_ptr/*response*/) {return OperationResult();} + +protected: + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr, + const std::string & name) final + { + RCLCPP_INFO(node->get_logger(), "Configuring route operation client: %s.", name.c_str()); + name_ = name; + logger_ = node->get_logger(); + node_ = node; + callback_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node->get_node_base_interface()); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".service_name", rclcpp::ParameterValue("")); + main_srv_name_ = node->get_parameter(getName() + ".service_name").as_string(); + + configureEvent(node, name); + + // There exists a single central service to use, create client. + // If this is set to empty string after configuration, then the individual nodes will + // indicate the endpoint for the particular service call. + if (!main_srv_name_.empty()) { + main_client_ = node->create_client( + main_srv_name_, rclcpp::SystemDefaultsQoS(), callback_group_); + } + } + + /** + * @brief The main operation to call a service of arbitrary type and arbitrary name + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, for additional context + * @param edge_entered Edge entered by node achievement, for additional context + * @param edge_exited Edge exited by node achievement, for additional context + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + OperationResult perform( + NodePtr node_achieved, + EdgePtr /*edge_entered*/, + EdgePtr /*edge_exited*/, + const Route & /*route*/, + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, + const Metadata * mdata) final + { + auto req = std::make_shared(); + std::shared_ptr response; + populateRequest(req, mdata); + + std::string srv_name; + srv_name = mdata->getValue("service_name", srv_name); + if (srv_name.empty() && !main_client_) { + throw nav2_core::OperationFailed( + "Route operation service (" + getName() + ") needs 'server_name' " + "set in the param file or in the operation's metadata!"); + } + + if (srv_name.empty()) { + srv_name = main_srv_name_; + response = callService(main_client_, req); + } else { + auto node = node_.lock(); + auto client = node->create_client( + srv_name, rclcpp::SystemDefaultsQoS(), callback_group_); + response = callService(client, req); + } + + RCLCPP_INFO( + logger_, + "%s: Processed operation at Node %i with service %s.", + name_.c_str(), node_achieved->nodeid, srv_name.c_str()); + return processResponse(response); + } + + std::shared_ptr callService( + typename rclcpp::Client::SharedPtr client, + std::shared_ptr req, + const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(500ms)) + { + auto node = node_.lock(); + if (!client->wait_for_service(1s)) { + throw nav2_core::OperationFailed( + "Route operation service " + + std::string(client->get_service_name()) + " is not available!"); + } + + auto result = client->async_send_request(req); + if (callback_group_executor_.spin_until_future_complete(result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + throw nav2_core::OperationFailed( + "Route operation service " + + std::string(client->get_service_name()) + " failed to call!"); + } + + return result.get(); + } + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override {return name_;} + + /** + * @brief Indication that the adjust speed limit route operation is performed + * on all state changes + * @return The type of operation (on graph call, on status changes, or constantly) + */ + RouteOperationType processType() final {return RouteOperationType::ON_GRAPH;} + + std::string name_, main_srv_name_; + std::atomic_bool reroute_; + rclcpp::Logger logger_{rclcpp::get_logger("RouteOperationClient")}; + typename rclcpp::Client::SharedPtr main_client_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATION_CLIENT_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operations/adjust_speed_limit.hpp b/nav2_route/include/nav2_route/plugins/route_operations/adjust_speed_limit.hpp new file mode 100644 index 00000000000..899e0837094 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operations/adjust_speed_limit.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__ADJUST_SPEED_LIMIT_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__ADJUST_SPEED_LIMIT_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" + +namespace nav2_route +{ + +/** + * @class AdjustSpeedLimit + * @brief A route operation to process on state changes to evaluate if speed limits + * should be adjusted based on graph edge metadata + */ +class AdjustSpeedLimit : public RouteOperation +{ +public: + /** + * @brief Constructor + */ + AdjustSpeedLimit() = default; + + /** + * @brief destructor + */ + virtual ~AdjustSpeedLimit() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override {return name_;} + + /** + * @brief Indication that the adjust speed limit route operation is performed + * on all state changes + * @return The type of operation (on graph call, on status changes, or constantly) + */ + RouteOperationType processType() override {return RouteOperationType::ON_STATUS_CHANGE;} + + /** + * @brief The main speed limit operation to adjust the maximum speed of the vehicle + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, for additional context + * @param edge_entered Edge entered by node achievement, for additional context + * @param edge_exited Edge exited by node achievement, for additional context + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + OperationResult perform( + NodePtr node_achieved, + EdgePtr edge_entered, + EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * mdata = nullptr) override; + +protected: + std::string name_; + std::string speed_tag_; + rclcpp::Logger logger_{rclcpp::get_logger("AdjustSpeedLimit")}; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr speed_limit_pub_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__ADJUST_SPEED_LIMIT_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp b/nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp new file mode 100644 index 00000000000..9c73ef8f395 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp @@ -0,0 +1,157 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__COLLISION_MONITOR_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__COLLISION_MONITOR_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/line_iterator.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_core/route_exceptions.hpp" + +namespace nav2_route +{ + +/** + * @class CollisionMonitor + * @brief A route operation to process a costmap to determine if a route is blocked + * requiring immediate rerouting. If using the local costmap topic, then it will check + * in the local time horizon only. if using the global, it may check the full route for + * continued validity. It is however recommended to specify a maximum collision distance + * for evaluation to prevent necessarily long-term evaluation of collision information which + * may not be representative of the conditions in that area by the time the robot gets there. + */ +class CollisionMonitor : public RouteOperation +{ +public: + struct LineSegment + { + unsigned int x0, y0, x1, y1; + }; + + /** + * @brief Constructor + */ + CollisionMonitor() = default; + + /** + * @brief destructor + */ + virtual ~CollisionMonitor() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override {return name_;} + + /** + * @brief Indication that the adjust speed limit route operation is performed + * on all state changes + * @return The type of operation (on graph call, on status changes, or constantly) + */ + RouteOperationType processType() override {return RouteOperationType::ON_QUERY;} + + /** + * @brief The main speed limit operation to adjust the maximum speed of the vehicle + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, for additional context + * @param edge_entered Edge entered by node achievement, for additional context + * @param edge_exited Edge exited by node achievement, for additional context + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + OperationResult perform( + NodePtr /*node*/, + EdgePtr curr_edge, + EdgePtr /*edge_exited*/, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * /*mdata*/) override; + +protected: + /** + * @brief Backs out the end coordinate along the line segment start-end to length dist + * @param start Start of line segment + * @param end End of line segment + * @param dist Distance along line segment to find the new end point along + * @return Coordinates of the new end point `dist` away from the start along the line + */ + Coordinates backoutValidEndPoint( + const Coordinates & start, const Coordinates & end, const float dist); + + /** + * @brief Backs out the line end coordinates of the start-end line segment + * where costmap transforms are possible + * @param start Start of line segment + * @param line LineSegment object to replace the x1/y1 values for along segment until invalid + * @return If any part s of the segment requested is valid + */ + bool backoutValidEndPoint(const Coordinates & start, LineSegment & line); + + /** + * @brief Converts a line segment start-end into a LineSegment struct in costmap frame + * @param start Start of line segment + * @param end End of line segment + * @param line LineSegment object to populate + * @return If line segment is valid (e.g. start and end both in costmap transforms) + */ + bool lineToMap(const Coordinates & start, const Coordinates & end, LineSegment & line); + + /** + * @brief Checks a line segment in costmap frame for validity + * @param line LineSegment object to collision check in costmap set + * @return If any part of the line segment is in collision + */ + bool isInCollision(const LineSegment & line); + + /** + * @brief Gets the latest costmap from the costmap subscriber + */ + void getCostmap(); + + std::string name_, topic_; + std::atomic_bool reroute_; + rclcpp::Logger logger_{rclcpp::get_logger("CollisionMonitor")}; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Time last_check_time_; + rclcpp::Duration checking_duration_{0, 0}; + float max_collision_dist_, max_cost_; + bool reroute_on_collision_; + unsigned int check_resolution_{1u}; + std::shared_ptr costmap_subscriber_; + std::shared_ptr costmap_{nullptr}; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__COLLISION_MONITOR_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operations/rerouting_service.hpp b/nav2_route/include/nav2_route/plugins/route_operations/rerouting_service.hpp new file mode 100644 index 00000000000..30e6e97353b --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operations/rerouting_service.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__REROUTING_SERVICE_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__REROUTING_SERVICE_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_util/node_utils.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace nav2_route +{ + +/** + * @class ReroutingService + * @brief A route operation to process requests from an external server for rerouting + */ +class ReroutingService : public RouteOperation +{ +public: + /** + * @brief Constructor + */ + ReroutingService() = default; + + /** + * @brief destructor + */ + virtual ~ReroutingService() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override {return name_;} + + /** + * @brief Indication that the adjust speed limit route operation is performed + * on all state changes + * @return The type of operation (on graph call, on status changes, or constantly) + */ + RouteOperationType processType() override {return RouteOperationType::ON_QUERY;} + + /** + * @brief The main speed limit operation to adjust the maximum speed of the vehicle + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, for additional context + * @param edge_entered Edge entered by node achievement, for additional context + * @param edge_exited Edge exited by node achievement, for additional context + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + OperationResult perform( + NodePtr node, + EdgePtr edge_entered, + EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * mdata = nullptr) override; + + /** + * @brief Service callback to trigger a reroute externally + * @param request, empty + * @param response, returns success + */ + void serviceCb( + const std::shared_ptr request, + std::shared_ptr response); + +protected: + std::string name_; + std::atomic_bool reroute_; + rclcpp::Logger logger_{rclcpp::get_logger("ReroutingService")}; + rclcpp::Service::SharedPtr service_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__REROUTING_SERVICE_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operations/time_marker.hpp b/nav2_route/include/nav2_route/plugins/route_operations/time_marker.hpp new file mode 100644 index 00000000000..584c5d49c9c --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operations/time_marker.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TIME_MARKER_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TIME_MARKER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" + +namespace nav2_route +{ + +/** + * @class TimeMarker + * @brief A route operation to add accurate times to the graph's edges after navigation + * to use in later iterations for a more refined estimate of actual travel time + */ +class TimeMarker : public RouteOperation +{ +public: + /** + * @brief Constructor + */ + TimeMarker() = default; + + /** + * @brief destructor + */ + virtual ~TimeMarker() = default; + + /** + * @brief Configure + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) override; + + /** + * @brief Get name of the plugin for parameter scope mapping + * @return Name + */ + std::string getName() override {return name_;} + + /** + * @brief Indication that the adjust speed limit route operation is performed + * on all state changes + * @return The type of operation (on graph call, on status changes, or constantly) + */ + RouteOperationType processType() override {return RouteOperationType::ON_STATUS_CHANGE;} + + /** + * @brief The main speed limit operation to adjust the maximum speed of the vehicle + * @param mdata Metadata corresponding to the operation in the navigation graph. + * If metadata is invalid or irrelevant, a nullptr is given + * @param node_achieved Node achieved, for additional context + * @param edge_entered Edge entered by node achievement, for additional context + * @param edge_exited Edge exited by node achievement, for additional context + * @param route Current route being tracked in full, for additional context + * @param curr_pose Current robot pose in the route frame, for additional context + * @return Whether to perform rerouting and report blocked edges in that case + */ + OperationResult perform( + NodePtr node_achieved, + EdgePtr edge_entered, + EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * mdata = nullptr) override; + +protected: + std::string name_; + std::string time_tag_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Time edge_start_time_; + unsigned int curr_edge_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TIME_MARKER_HPP_ diff --git a/nav2_route/include/nav2_route/plugins/route_operations/trigger_event.hpp b/nav2_route/include/nav2_route/plugins/route_operations/trigger_event.hpp new file mode 100644 index 00000000000..39e77532392 --- /dev/null +++ b/nav2_route/include/nav2_route/plugins/route_operations/trigger_event.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TRIGGER_EVENT_HPP_ +#define NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TRIGGER_EVENT_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "nav2_route/plugins/route_operation_client.hpp" + +namespace nav2_route +{ + +/** + * @class TriggerEvent + * @brief A route operation to trigger an event at a node or edge. This operation is meant to be + * named accordingly to the event of query in the parameter file (e.g. OpenDoor, CallElevator). + * Thus, a single TriggerEvent plugin type can support many different operation instances + * calling different services. It may be set up to either trigger a + * single service if the `service_name` is set in the parameter file at launch + * **or** trigger different services depending on the `service_name` set in the metadata of the + * node or edge operation given for centralized or decentralized events by node or edge. + * + * See the Route Operation Client for more details + */ +class TriggerEvent : public RouteOperationClient +{ +public: + /** + * @brief Constructor + */ + TriggerEvent() = default; + + /** + * @brief destructor + */ + virtual ~TriggerEvent() = default; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__PLUGINS__ROUTE_OPERATIONS__TRIGGER_EVENT_HPP_ diff --git a/nav2_route/include/nav2_route/route_planner.hpp b/nav2_route/include/nav2_route/route_planner.hpp new file mode 100644 index 00000000000..3a75c70bf34 --- /dev/null +++ b/nav2_route/include/nav2_route/route_planner.hpp @@ -0,0 +1,162 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__ROUTE_PLANNER_HPP_ +#define NAV2_ROUTE__ROUTE_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/edge_scorer.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_route +{ +/** + * @class nav2_route::RoutePlanner + * @brief An optimal planner to compute a route from a start to a goal in an arbitrary graph + */ +class RoutePlanner +{ +public: + /** + * @brief A constructor for nav2_route::RoutePlanner + */ + RoutePlanner() = default; + + /** + * @brief A destructor for nav2_route::RoutePlanner + */ + virtual ~RoutePlanner() = default; + + /** + * @brief Configure the route planner, get parameters + * @param node Node object to get parametersfrom + * @param tf_buffer TF buffer to use for transformations + * @param costmap_subscriber Costmap subscriber to use for blocked nodes + */ + void configure( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + const std::shared_ptr costmap_subscriber); + + /** + * @brief Find the route from start to goal on the graph + * @param graph Graph to search + * @param start Start index in the graph of the start node + * @param goal Goal index in the graph of the goal node + * @param blocked_ids A set of blocked node and edge IDs not to traverse + * @return Route object containing the navigation graph route + */ + virtual Route findRoute( + Graph & graph, unsigned int start_index, unsigned int goal_index, + const std::vector & blocked_ids, + const RouteRequest & route_request); + +protected: + /** + * @brief Reset the search state of the graph nodes + * @param graph Graph to reset + */ + inline void resetSearchStates(Graph & graph); + + /** + * @brief Dikstra's algorithm search on the graph + * @param graph Graph to search + * @param start Start Node pointer + * @param goal Goal node pointer + * @param blocked_ids A set of blocked node and edge IDs not to traverse + */ + void findShortestGraphTraversal( + Graph & graph, const NodePtr start_node, const NodePtr goal_node, + const std::vector & blocked_ids, + const RouteRequest & route_request); + + /** + * @brief Gets the traversal cost for an edge using edge scorers + * @param edge Edge pointer to find traversal cost for + * @param travel cost + * @param blocked_ids A set of blocked node and edge IDs not to traverse + * @return if this edge is valid for search + */ + inline bool getTraversalCost( + const EdgePtr edge, float & score, const std::vector & blocked_ids, + const RouteRequest & route_request); + + /** + * @brief Gets the next node in the priority queue for search + * @return Next node pointer in queue with cost + */ + inline NodeElement getNextNode(); + + /** + * @brief Adds a node to the priority queue for search + * @param cost Priority level + * @param node Node pointer to insert + */ + inline void addNode(const float cost, const NodePtr node); + + /** + * @brief Gets the edges from a given node + * @param node Node pointer to check + * @return A vector of edges that the node contains + */ + inline EdgeVector & getEdges(const NodePtr node); + + /** + * @brief Clears the priority queue + */ + inline void clearQueue(); + + /** + * @brief Checks if a given node is the goal node + * @param node Node to check + * @return bool If this node is the goal + */ + inline bool isGoal(const NodePtr node); + + /** + * @brief Checks if a given node is the start node + * @param node Node to check + * @return bool If this node is the start + */ + inline bool isStart(const NodePtr node); + + /** + * @brief Checks edge is a start or end edge + * @param edge Edge to check + * @return EdgeType identifying whether the edge is start, end or none + */ + nav2_route::EdgeType classifyEdge(const EdgePtr edge); + + int max_iterations_{0}; + unsigned int start_id_{0}; + unsigned int goal_id_{0}; + NodeQueue queue_; + std::unique_ptr edge_scorer_; + std::shared_ptr tf_buffer_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__ROUTE_PLANNER_HPP_ diff --git a/nav2_route/include/nav2_route/route_server.hpp b/nav2_route/include/nav2_route/route_server.hpp new file mode 100644 index 00000000000..a6b830c1b0e --- /dev/null +++ b/nav2_route/include/nav2_route/route_server.hpp @@ -0,0 +1,236 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 NAV2_ROUTE__ROUTE_SERVER_HPP_ +#define NAV2_ROUTE__ROUTE_SERVER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_util/lifecycle_node.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/create_timer_ros.h" +#include "nav2_util/simple_action_server.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/service_server.hpp" +#include "nav2_msgs/action/compute_route.hpp" +#include "nav2_msgs/action/compute_and_track_route.hpp" +#include "nav2_msgs/msg/route.hpp" +#include "nav2_msgs/msg/route_node.hpp" +#include "nav2_msgs/srv/set_route_graph.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/graph_loader.hpp" +#include "nav2_route/route_planner.hpp" +#include "nav2_route/path_converter.hpp" +#include "nav2_route/route_tracker.hpp" +#include "nav2_route/goal_intent_extractor.hpp" + +namespace nav2_route +{ +/** + * @class nav2_route::RouteServer + * @brief An action server implements a Navigation Route-Graph planner + * to compliment free-space planning in the Planner Server + */ +class RouteServer : public nav2_util::LifecycleNode +{ +public: + using ComputeRoute = nav2_msgs::action::ComputeRoute; + using ComputeRouteGoal = ComputeRoute::Goal; + using ComputeRouteResult = ComputeRoute::Result; + using ComputeRouteServer = nav2_util::SimpleActionServer; + + using ComputeAndTrackRoute = nav2_msgs::action::ComputeAndTrackRoute; + using ComputeAndTrackRouteGoal = ComputeAndTrackRoute::Goal; + using ComputeAndTrackRouteFeedback = ComputeAndTrackRoute::Feedback; + using ComputeAndTrackRouteResult = ComputeAndTrackRoute::Result; + using ComputeAndTrackRouteServer = nav2_util::SimpleActionServer; + + /** + * @brief A constructor for nav2_route::RouteServer + * @param options Additional options to control creation of the node. + */ + explicit RouteServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** + * @brief A destructor for nav2_route::RouteServer + */ + ~RouteServer() = default; + +protected: + /** + * @brief Configure member variables and initializes planner + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Activate member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Deactivate member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Reset member variables + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Called when in shutdown state + * @param state Reference to LifeCycle node state + * @return SUCCESS or FAILURE + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Main route action server callbacks for computing and tracking a route + */ + void computeRoute(); + void computeAndTrackRoute(); + + /** + * @brief Abstract method combining finding the starting/ending nodes and the route planner + * to find the Node locations of interest and route to the goal + * @param goal The request goal information + * @param blocked_ids The IDs of blocked graphs / edges + * @param updated_start_id The nodeID of an updated starting point when tracking + * a route that corresponds to the next point to route to from to continue progress + * @return A route of the request + */ + template + Route findRoute( + const std::shared_ptr goal, + ReroutingState & rerouting_info = ReroutingState()); + + /** + * @brief Main processing called by both action server callbacks to centralize + * the great deal of shared code between them + */ + template + void processRouteRequest( + std::shared_ptr> & action_server); + + /** + * @brief Find the planning duration of the request and log warnings + * @param start_time Start of planning time + * @return Duration of planning time + */ + rclcpp::Duration findPlanningDuration(const rclcpp::Time & start_time); + + /** + * @brief Find the routing request is valid (action server OK and not cancelled) + * @param action_server Actions server to check + * @return if the request is valid + */ + template + bool isRequestValid(std::shared_ptr> & action_server); + + /** + * @brief Populate result for compute route action + * @param result Result to populate + * @param route Route to use to populate result + * @param path Path to use to populate result + * @param planning_duration Time to create a valid route + */ + void populateActionResult( + std::shared_ptr result, + const Route & route, + const nav_msgs::msg::Path & path, + const rclcpp::Duration & planning_duration); + + /** + * @brief Populate result for compute and track route action + * @param result Result to populate + * @param route Route to use to populate result + * @param path Path to use to populate result + * @param planning_duration Time to create a valid route + */ + void populateActionResult( + std::shared_ptr/*result*/, + const Route & /*route*/, + const nav_msgs::msg::Path & /*path*/, + const rclcpp::Duration & /*planning_duration*/); + + /** + * @brief The service callback to set a new route graph + * @param request_header to the service + * @param request to the service + * @param response from the service + */ + void setRouteGraph( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Log exception warnings, templated by action message type + * @param goal Goal that failed + * @param exception Exception message + */ + template + void exceptionWarning(const std::shared_ptr goal, const std::exception & ex); + + std::shared_ptr compute_route_server_; + std::shared_ptr compute_and_track_route_server_; + + // TF + std::shared_ptr tf_; + std::shared_ptr transform_listener_; + + // Publish the route for visualization + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + graph_vis_publisher_; + + // Set or modify graph + nav2_util::ServiceServer>::SharedPtr set_graph_service_; + + // Internal tools + std::shared_ptr graph_loader_; + std::shared_ptr route_planner_; + std::shared_ptr route_tracker_; + std::shared_ptr path_converter_; + std::shared_ptr goal_intent_extractor_; + + std::shared_ptr costmap_subscriber_; + + // State Data + Graph graph_; + GraphToIDMap id_to_graph_map_; + std::string route_frame_, base_frame_; + double max_planning_time_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__ROUTE_SERVER_HPP_ diff --git a/nav2_route/include/nav2_route/route_tracker.hpp b/nav2_route/include/nav2_route/route_tracker.hpp new file mode 100644 index 00000000000..d4ccb66a51e --- /dev/null +++ b/nav2_route/include/nav2_route/route_tracker.hpp @@ -0,0 +1,134 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 + +#include "tf2_ros/transform_listener.h" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/simple_action_server.hpp" +#include "nav2_msgs/action/compute_and_track_route.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/operations_manager.hpp" + +#ifndef NAV2_ROUTE__ROUTE_TRACKER_HPP_ +#define NAV2_ROUTE__ROUTE_TRACKER_HPP_ + +namespace nav2_route +{ + +/** + * @class nav2_route::RouteTracker + * @brief Takes a processing route request and tracks it for progress + * in accordance with executing behavioral operations + */ +class RouteTracker +{ +public: + using ActionServerTrack = nav2_util::SimpleActionServer; + using Feedback = nav2_msgs::action::ComputeAndTrackRoute::Feedback; + + /** + * @brief A constructor for nav2_route::RouteTracker + */ + RouteTracker() = default; + + /** + * @brief A constructor for nav2_route::RouteTracker + */ + ~RouteTracker() = default; + + /** + * @brief Configure route tracker + * @param node Node to grab info from + * @param route_frame Frame of route navigation + */ + void configure( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + std::shared_ptr action_server, + const std::string & route_frame, + const std::string & base_frame); + + /** + * @brief Determine if a node is to be considered achieved at the current position + * @param pose Current robot pose to query + * @param state Tracker state + * @param route Route to check against + * @return bool if node is achieved + */ + bool nodeAchieved( + const geometry_msgs::msg::PoseStamped & pose, + RouteTrackingState & state, + const Route & route); + + /** + * @brief Determine if a node is the start or last node in the route + * @param idx Idx of the current edge being tracked + * @param route Route to check + * @return bool if this node is the last node + */ + bool isStartOrEndNode(RouteTrackingState & state, const Route & route); + + /** + * @brief Get the current robot's base_frame pose in route_frame + * @return Robot pose + */ + geometry_msgs::msg::PoseStamped getRobotPose(); + + /** + * @brief A utility to publish feedback for the action on important changes + * @param rerouted If the route has been rerouted + * @param next_node_id Id of the next node the route is to pass + * @param last_node_id Id of the last node the route passed + * @param edge_id Id of the current edge being processed + * @param operations A set of operations which were performed this iteration + */ + void publishFeedback( + const bool rereouted, + const unsigned int next_node_id, + const unsigned int last_node_id, + const unsigned int edge_id, + const std::vector & operations); + + /** + * @brief Main function to track route, manage state, and trigger operations + * @param route Route to track progress of + * @param path Path that comprises this route for publication of feedback messages + * @param blocked_ids A set of blocked IDs to modify if rerouting is necessary + * @return TrackerResult if the route is completed, should be rerouted, or was interrupted + */ + TrackerResult trackRoute( + const Route & route, const nav_msgs::msg::Path & path, + ReroutingState & rerouting_info); + +protected: + nav2_msgs::msg::Route route_msg_; + nav_msgs::msg::Path path_; + std::string route_frame_, base_frame_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("RouteTracker")}; + double radius_threshold_, boundary_radius_threshold_, tracker_update_rate_; + bool aggregate_blocked_ids_; + std::shared_ptr action_server_; + std::unique_ptr operations_manager_; + std::shared_ptr tf_buffer_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__ROUTE_TRACKER_HPP_ diff --git a/nav2_route/include/nav2_route/types.hpp b/nav2_route/include/nav2_route/types.hpp new file mode 100644 index 00000000000..dc31c5e01da --- /dev/null +++ b/nav2_route/include/nav2_route/types.hpp @@ -0,0 +1,304 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 +#include +#include +#include +#include +#include + +#ifndef NAV2_ROUTE__TYPES_HPP_ +#define NAV2_ROUTE__TYPES_HPP_ + +namespace nav2_route +{ + +/** + * @struct nav2_route::Metadata + * @brief An object to store arbitrary metadata regarding nodes from the graph file + */ +struct Metadata +{ + Metadata() {} + + // For retrieving metadata at run-time via plugins + template + T getValue(const std::string & key, T & default_val) const + { + auto it = data.find(key); + if (it == data.end()) { + return default_val; + } + return std::any_cast(it->second); + } + + // For populating metadata from file + template + void setValue(const std::string & key, T & value) + { + data[key] = value; + } + + std::unordered_map data; +}; + +struct Node; +typedef Node * NodePtr; +typedef std::vector NodeVector; +typedef NodeVector Graph; +typedef std::unordered_map GraphToIDMap; +typedef std::unordered_map> GraphToIncomingEdgesMap; +typedef std::vector NodePtrVector; +typedef std::pair NodeElement; +typedef std::pair NodeExtents; + +/** + * @struct nav2_route::NodeComparator + * @brief Node comparison for priority queue sorting + */ +struct NodeComparator +{ + bool operator()(const NodeElement & a, const NodeElement & b) const + { + return a.first > b.first; + } +}; + +typedef std::priority_queue, NodeComparator> NodeQueue; + +/** + * @struct nav2_route::EdgeCost + * @brief An object to store edge cost or cost metadata for scoring + */ +struct EdgeCost +{ + float cost{0.0}; + bool overridable{true}; // If overridable, may use plugin edge cost scorers +}; + +/** + * @enum nav2_route::OperationTrigger + * @brief The triggering events for an operation + */ +enum class OperationTrigger +{ + NODE = 0, + ON_ENTER = 1, + ON_EXIT = 2 +}; + +/** + * @struct nav2_route::Operation + * @brief An object to store operations to perform on events with types and metadata + */ +struct Operation +{ + std::string type; + OperationTrigger trigger; + Metadata metadata; +}; + +typedef std::vector Operations; +typedef std::vector OperationPtrs; + +/** + * @struct nav2_route::OperationsResult + * @brief Result information from the operations manager + */ +struct OperationsResult +{ + std::vector operations_triggered; + bool reroute{false}; + std::vector blocked_ids; +}; + +/** + * @struct nav2_route::DirectionalEdge + * @brief An object representing edges between nodes + */ +struct DirectionalEdge +{ + unsigned int edgeid; // Edge identifier + NodePtr start{nullptr}; // Ptr to starting node of edge + NodePtr end{nullptr}; // Ptr to ending node of edge + EdgeCost edge_cost; // Cost information associated with edge + Metadata metadata; // Any metadata stored in the graph file of interest + Operations operations; // Operations to perform related to the edge +}; + +typedef DirectionalEdge * EdgePtr; +typedef std::vector EdgeVector; +typedef std::vector EdgePtrVector; + +/** + * @struct nav2_route::SearchState + * @brief An object to store state related to graph searching of nodes + * This is an internal class users should not modify. + */ +struct SearchState +{ + EdgePtr parent_edge{nullptr}; + float integrated_cost{std::numeric_limits::max()}; + float traversal_cost{std::numeric_limits::max()}; + + void reset() + { + integrated_cost = std::numeric_limits::max(); + traversal_cost = std::numeric_limits::max(); + parent_edge = nullptr; + } +}; + +/** + * @struct nav2_route::Coordinates + * @brief An object to store Node coordinates in different frames + */ +struct Coordinates +{ + std::string frame_id{"map"}; + float x{0.0}, y{0.0}; +}; + +/** + * @struct nav2_route::Node + * @brief An object to store the nodes in the graph file + */ +struct Node +{ + unsigned int nodeid; // Node identifier + Coordinates coords; // Coordinates of node + EdgeVector neighbors; // Directed neighbors and edges of the node + Metadata metadata; // Any metadata stored in the graph file of interest + Operations operations; // Operations to perform related to the node + SearchState search_state; // State maintained by route search algorithm + + void addEdge( + EdgeCost & cost, NodePtr node, unsigned int edgeid, Metadata meta_data = {}, + Operations operations_data = {}) + { + neighbors.push_back({edgeid, this, node, cost, meta_data, operations_data}); + } +}; + +/** + * @struct nav2_route::Route + * @brief An ordered set of nodes and edges corresponding to the planned route + */ +struct Route +{ + NodePtr start_node; + EdgePtrVector edges; + float route_cost{0.0}; +}; + +/** + * @struct nav2_route::RouteRequest + * @brief An object to store salient features of the route request including its start + * and goal node ids, start and goal pose, and a flag to indicate if the start and goal + * poses are relevant + */ +struct RouteRequest +{ + unsigned int start_nodeid; // node id of start node + unsigned int goal_nodeid; // node id of goal node + geometry_msgs::msg::PoseStamped start_pose; // pose of start + geometry_msgs::msg::PoseStamped goal_pose; // pose of goal + bool use_poses; // whether the start and goal poses are used +}; + +/** + * @enum nav2_route::TrackerResult + * @brief Return result of the route tracker to the main server for processing + */ +enum class TrackerResult +{ + EXITED = 0, + INTERRUPTED = 1, + COMPLETED = 2 +}; + +/** + * @struct nav2_route::RouteTrackingState + * @brief Current state management of route tracking class + */ +struct RouteTrackingState +{ + NodePtr last_node{nullptr}, next_node{nullptr}; + EdgePtr current_edge{nullptr}; + int route_edges_idx{-1}; + bool within_radius{false}; +}; + +/** + * @struct nav2_route::ReroutingState + * @brief State shared to objects to communicate important rerouting data + * to avoid rerouting over blocked edges, ensure reroute from the current + * appropriate starting point along the route, and state of edges if pruned + * for seeding the Tracker's state. Admittedly, this is a bit complex, so more + * context is provided inline. + */ +struct ReroutingState +{ + // Communicate edges identified as blocked by the operational plugins like collision checkers. + // This is fully managed by the route tracker when populated. + std::vector blocked_ids; + + // Used to determine if this is the first planning iteration in the goal intent extractor + // to bypass pruning. Fully managed in the goal intent extractor. + bool first_time{true}; + + // Used to mark current edge being tracked by the route, if progress was made before rerouting. + // It is reset in the goal intent extractor if the previous progressed edge is different from + // the new edge from planning. Otherwise, used in the path converter to create new dense path + // with partial progress information and in the tracker to seed the state to continue. + // It is managed by both the goal intent extractor and the route tracker. + EdgePtr curr_edge{nullptr}; + Coordinates closest_pt_on_edge; + + // Used to mark the route tracking state before rerouting was requested. + // When route tracking made some progress, the Start ID and pose are populated + // and used by the goal intent extractor to override the initial request's + // start, current pose along the edge, and pruning criteria. Otherwise, the initial request + // information is used. This is managed by the route tracker but used by goal intent extractor. + unsigned int rerouting_start_id{std::numeric_limits::max()}; + geometry_msgs::msg::PoseStamped rerouting_start_pose; + + void reset() + { + rerouting_start_id = std::numeric_limits::max(); + blocked_ids.clear(); + first_time = true; + curr_edge = nullptr; + closest_pt_on_edge = Coordinates(); + rerouting_start_pose = geometry_msgs::msg::PoseStamped(); + } +}; + +/** + * @enum nav2_route::EdgeType + * @brief An enum class describing what type of edge connecting two nodes is + */ +enum class EdgeType +{ + NONE = 0, + START = 1, + END = 2 +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__TYPES_HPP_ diff --git a/nav2_route/include/nav2_route/utils.hpp b/nav2_route/include/nav2_route/utils.hpp new file mode 100644 index 00000000000..9ccf65eb41e --- /dev/null +++ b/nav2_route/include/nav2_route/utils.hpp @@ -0,0 +1,267 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 +#include "std_msgs/msg/color_rgba.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_msgs/msg/route.hpp" +#include "nav2_route/types.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/line_iterator.hpp" + +#ifndef NAV2_ROUTE__UTILS_HPP_ +#define NAV2_ROUTE__UTILS_HPP_ + +namespace nav2_route +{ + +namespace utils +{ + +/** + * @brief Convert the position into a pose + * @param x X Coordinates + * @param y Y Coordinates + * @return PoseStamped of the position + */ +inline geometry_msgs::msg::PoseStamped toMsg(const float x, const float y) +{ + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = x; + pose.pose.position.y = y; + return pose; +} + +/** + * @brief Convert the route graph into a visualization marker array for visualization + * @param graph Graph of nodes and edges + * @param frame Frame ID to use + * @param now Current time to use + * @return MarkerArray of the graph + */ +inline visualization_msgs::msg::MarkerArray toMsg( + const nav2_route::Graph & graph, const std::string & frame, const rclcpp::Time & now) +{ + visualization_msgs::msg::MarkerArray msg; + visualization_msgs::msg::Marker curr_marker; + curr_marker.header.frame_id = frame; + curr_marker.header.stamp = now; + curr_marker.action = 0; + + auto getSphereSize = []() { + geometry_msgs::msg::Vector3 v_msg; + v_msg.x = 0.05; + v_msg.y = 0.05; + v_msg.z = 0.05; + return v_msg; + }; + + auto getSphereColor = []() { + std_msgs::msg::ColorRGBA c_msg; + c_msg.r = 1.0; + c_msg.g = 0.0; + c_msg.b = 0.0; + c_msg.a = 1.0; + return c_msg; + }; + + auto getLineColor = []() { + std_msgs::msg::ColorRGBA c_msg; + c_msg.r = 0.0; + c_msg.g = 1.0; + c_msg.b = 0.0; + c_msg.a = 0.5; // So bi-directional connections stand out overlapping + return c_msg; + }; + + unsigned int marker_idx = 1; + for (unsigned int i = 0; i != graph.size(); i++) { + if (graph[i].nodeid == std::numeric_limits::max()) { + continue; // Skip "deleted" nodes + } + curr_marker.ns = "route_graph"; + curr_marker.id = marker_idx++; + curr_marker.type = visualization_msgs::msg::Marker::SPHERE; + curr_marker.pose.position.x = graph[i].coords.x; + curr_marker.pose.position.y = graph[i].coords.y; + curr_marker.scale = getSphereSize(); + curr_marker.color = getSphereColor(); + msg.markers.push_back(curr_marker); + + // Add text + curr_marker.ns = "route_graph_ids"; + curr_marker.id = marker_idx++; + curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + curr_marker.pose.position.x = graph[i].coords.x + 0.07; + curr_marker.pose.position.y = graph[i].coords.y; + curr_marker.text = std::to_string(graph[i].nodeid); + curr_marker.scale.z = 0.1; + msg.markers.push_back(curr_marker); + + for (unsigned int j = 0; j != graph[i].neighbors.size(); j++) { + curr_marker.ns = "route_graph"; + curr_marker.id = marker_idx++; + curr_marker.type = visualization_msgs::msg::Marker::LINE_LIST; + curr_marker.pose.position.x = 0; // Set to 0 since points are relative to this frame + curr_marker.pose.position.y = 0; // Set to 0 since points are relative to this frame + curr_marker.points.resize(2); + curr_marker.points[0].x = graph[i].coords.x; + curr_marker.points[0].y = graph[i].coords.y; + curr_marker.points[1].x = graph[i].neighbors[j].end->coords.x; + curr_marker.points[1].y = graph[i].neighbors[j].end->coords.y; + curr_marker.scale.x = 0.03; + curr_marker.color = getLineColor(); + msg.markers.push_back(curr_marker); + curr_marker.points.clear(); // Reset for next node marker + + // Add text + curr_marker.ns = "route_graph_ids"; + curr_marker.id = marker_idx++; + curr_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + curr_marker.pose.position.x = + graph[i].coords.x + ((graph[i].neighbors[j].end->coords.x - graph[i].coords.x) / 2.0) + + 0.07; + + // Deal with overlapping bi-directional text markers by offsetting locations + float y_offset = 0.0; + if (graph[i].nodeid > graph[i].neighbors[j].end->nodeid) { + y_offset = 0.05; + } else { + y_offset = -0.05; + } + + curr_marker.pose.position.y = + graph[i].coords.y + ((graph[i].neighbors[j].end->coords.y - graph[i].coords.y) / 2.0) + + y_offset; + curr_marker.text = std::to_string(graph[i].neighbors[j].edgeid); + curr_marker.scale.z = 0.1; + msg.markers.push_back(curr_marker); + } + } + + return msg; +} + +/** + * @brief Convert the route into a message + * @param route Route of nodes and edges + * @param frame Frame ID to use + * @param now Current time to use + * @return Route message + */ +inline nav2_msgs::msg::Route toMsg( + const nav2_route::Route & route, const std::string & frame, const rclcpp::Time & now) +{ + nav2_msgs::msg::Route msg; + msg.header.frame_id = frame; + msg.header.stamp = now; + msg.route_cost = route.route_cost; + + nav2_msgs::msg::RouteNode route_node; + nav2_msgs::msg::RouteEdge route_edge; + route_node.nodeid = route.start_node->nodeid; + route_node.position.x = route.start_node->coords.x; + route_node.position.y = route.start_node->coords.y; + msg.nodes.push_back(route_node); + + // Provide the Node info and Edge IDs we're traversing through + for (unsigned int i = 0; i != route.edges.size(); i++) { + route_edge.edgeid = route.edges[i]->edgeid; + route_edge.start.x = route.edges[i]->start->coords.x; + route_edge.start.y = route.edges[i]->start->coords.y; + route_edge.end.x = route.edges[i]->end->coords.x; + route_edge.end.y = route.edges[i]->end->coords.y; + msg.edges.push_back(route_edge); + + route_node.nodeid = route.edges[i]->end->nodeid; + route_node.position.x = route.edges[i]->end->coords.x; + route_node.position.y = route.edges[i]->end->coords.y; + msg.nodes.push_back(route_node); + } + + return msg; +} + +/** + * @brief Finds the normalized dot product of 2 vectors + * @param v1x Vector 1's x component + * @param v1y Vector 1's y component + * @param v2x Vector 2's x component + * @param v2y Vector 2's y component + * @return Value of dot product + */ +inline float normalizedDot( + const float v1x, const float v1y, + const float v2x, const float v2y) +{ + const float mag1 = std::hypotf(v1x, v1y); + const float mag2 = std::hypotf(v2x, v2y); + if (mag1 < 1e-6 || mag2 < 1e-6) { + return 0.0; + } + return (v1x / mag1) * (v2x / mag2) + (v1y / mag1) * (v2y / mag2); +} + +/** + * @brief Finds the closest point on the line segment made up of start-end to pose + * @param pose Pose to find point closest on the line with respect to + * @param start Start of line segment + * @param end End of line segment + * @return Coordinates of point on the line closest to the pose + */ +inline Coordinates findClosestPoint( + const geometry_msgs::msg::PoseStamped & pose, + const Coordinates & start, const Coordinates & end) +{ + Coordinates pt; + const float vx = end.x - start.x; + const float vy = end.y - start.y; + const float ux = start.x - pose.pose.position.x; + const float uy = start.y - pose.pose.position.y; + const float uv = vx * ux + vy * uy; + const float vv = vx * vx + vy * vy; + + // They are the same point, so only one option + if (vv < 1e-6) { + return start; + } + + const float t = -uv / vv; + if (t > 0.0 && t < 1.0) { + pt.x = (1.0 - t) * start.x + t * end.x; + pt.y = (1.0 - t) * start.y + t * end.y; + } else if (t <= 0.0) { + pt = start; + } else { + pt = end; + } + + return pt; +} + +inline float distance(const Coordinates & coords, const geometry_msgs::msg::PoseStamped & pose) +{ + return hypotf(coords.x - pose.pose.position.x, coords.y - pose.pose.position.y); +} + +} // namespace utils + +} // namespace nav2_route + +#endif // NAV2_ROUTE__UTILS_HPP_ diff --git a/nav2_route/media/architecture.png b/nav2_route/media/architecture.png new file mode 100644 index 0000000000000000000000000000000000000000..3bab56db5e6b6097162c113d89db6b68d6340d81 GIT binary patch literal 71688 zcmc$`c{tQ<|2O_YXtN~=AzKnsgzUP=lF(+~B5T&N8(S$<%9ef4mUZm=7$UN7nXyxh zZ7}x1Se|oq-S>Sx-{(GlzvsWlaX5}?e9rUpKHvNDI(zp(Lz(I{^Jxfzs8m%HA3+dV z0|b%wo}>WZn7A{QL6ASBs(9y#hw&1EGUO7vJ{v!gWd);RsUU%`J%yV}U#z9}r@H8i z7u+8IZbarT+GKjgDl40Ju9+i9BC;v*`~CM{ngwR>t7gPD-FSCi_3nGKES2}FPf}x> zCJ%%ro)0F*Gi`6W%DIYz4eAMDi^@o7e zf;F^kievIF=bwMeCvZwh5!g#gN{nY`X7*W}nf~=X2d!^5`DgzRofdzSZvPg!O*qgw zI_IAL78 z)4JjF=OHMWGjA8X&ll-ezbpIqcbar3D=olDXH3SjU+ElSJ?zb=%1LGkfB#v!!{l2b z#Z>sL@SiXI#mGo5{`nX|N_ykZCvNf+Pyc*UIeoI>&!@2SwElm{zsz?T`g1V(NAeI+ z&w!3)_(NxpKS8P_|4%j~dFkwY#E6`nyxw~wmulbLH}yrC#Oe`&l{3qxAx-5e2$3O4YHj1f_wDcHf6v(bC1X;lR<~O(dOUEff3DTus=7#eV z>tKGnjYlpGcS)Q{Np>B(D$YX?_l@f6>LZ_2(w@^#rf?R!rEm)jW)dfxy5ikRZs2QO zxphWW!ei7mTwbxcf|umtT36PkI-X^*6i?KP?Rd2X1Vz%8#oYe6li%k^Evy_G9EOL7 zkD`WAq>%sZ$mHb16f8ViZ&-pr%7AxVKm-{s-m+R=c4PO$=FZd6-IU;lxB{(4P(sP> z$@RZqDybtkWc{`SVKu#0%1)Zc>L?QMt% zy+^*48~IUk*-Q0{xkZ48Y95G73FRi$t%g>`jjuACKZuP=f=P>rhz#0glR)xsR5Ue@ z7+(e6_te&y=-M1*E`NqiW-9S z{bkdy^NZswy^iSc17ayJv}(WtbyY|yB#Bj<-7`*4kB}}Tj`MJH+dgurSb(5Zrv8C} zBT5tF#@Wt@b;@NOb~mjOu*NY03c2_!bSy^WjF{XO26Hsq(H0kxkU$XX5@Um)+h6nY z2)x6?)q{G~o-{T0jn<#A|6a*8q@$x#2m>qGALET99h$QDnXP-qfrCow+y<-mqXw}A zxX0mRPYS;-2{~ zIQU`~;8t^S({D{6yC#^StmhjJjmLa#q%EKrjMeDBm``u_EfUz^fmG@8NoYz>7`AzRlOal0ZFIdl z@3(@YVhB(kMaLoEEz>m&qipx}t2>mXRHvXR3E^vAbr^mDPm}{TrtmZk&}@BhF2pl} z*4A(St6pE}Gvn=5A*d(pvGIl<`8V7^S-G+i11k{TEuhC<9iKk9t62K3`K0vVD=ibw zL6?86FgR=jvFf}H1fu0X)ey|K^}}aD z%x?X|=6XDpB?~=x@%)6<{O8S)JMc%;lg>KUfRi_cnMrgjqr9fFu&#O( z8s47d5x{1!xXyjfg$ehnjHUW(j)Rj?4hp>YD*HqXQ-XpW#Vl}m-~kV%5nq?syTqy>^THlgXUQw$RO!)v$ z)g6K7RvHgLc1OsOwS&+Cc{^V=&Opmn`RY!Ayc%O29lT~|1=#)mf~?bKjYAV}?X_z_ z%RQJKAcxAW4-{}$R++~&8nKQCNavF-t-CBd2)8ivC-@3451+f4RHRNL6+!`{^HtWh z8F<#&J!!?WiamItNu;B~jP%zUt#iv6#(vNRrwtH`YjTLW>L1Vs{G_W!IEhR}! z6NdvNtG`tK5N{k01K`#pCq(D_WZ*}xjyR0!yb{*m9|Bq+Zkcc%x_l)AOvOje4KUwAd2rJmVlM z(i>(jOF*Ml;VY$Gc9EozViIth3>Hr|KUXf|s;+^YpxJ}P0a_9EO*X#}qh{$zvxdQ_ zwZ1h7=m@|=xEee4=qMWHros-&k|=LzrI^S4em1oz$CYET!n658j9Itm>Y1E;>m zBtw{7EZU*h`3}5G?8!hDe-I&}E2+oBpCHiq2ODzhi-Pi!3I$u0^~zR04(2k7A&E(| zj$xZ@J{qz1k0JlBeLg?STzt0EWHyQF)n}&#IhZqXUI_#Sg`NMvs7G#bgtAKqXL!3g zp-|{%SoB?s11QCn_rFd|71B?h>Rt~CewP=G4T(R5b&6escrX!<{LorV zJORsYi7_FWQW<8moH=Zw58G`C_iS80&dki}Qwb`Sjq7-Qj1!Qt6pZ*n zzU1$_Z&#R$3x0$b3nvq+h zfA3mehUFgVLdmZ;b*?k@dG{}S-2uWIt?0;j8_{PwxYJK0RBF_XR!ULV5*vPzWZS6W9e_ec;aS`l^oMN={iRfmI(~dWlmryyX6NX>4ojt z1;jwmmbb|Fj8`+QlCfcGWxQ5ciuTP1{=7`Wdqm-SK&&;00;Bb;(aX`Bxg)qvF@8us zukE5ZUT*Im|LDwtzhs&YUKOVwspqqciLtC@hb|>bxobQpY)5`uE9J&a3F>(Mb^knY z)T&cyQp89?cNKfoZdXeRmAx-i;9Xf3#`w5@pMLS!-6(p=_`u?K$ng5=FNM+q8tCKd zOdGu?9)ZvJnx#ErFE@8hC-~iEkN`;h9NiH1qKSWAVMGGyH9<4TlGW%%Zx#MjU0AubBj|MT3ZXX>r*z4kKFG*#gcobvu)pGzfH8-YNTj?)<&Z7#A1k zXH{WJ0)6{}4d!F1z3_WNCQ3NBES^G@SVW6Adk{lE zTY+hBIhj(ty)NWmM2u84tkIPp^-LbqjGZ^J&>XJ7`ws_F`fEQhF%cry5{)Rq)#QzF zv?u^oTOWGS=hpUh0iQD*NDf)wv^aBNf1`8%n+_ig>8>S~ya6Jita;*n{i>gKwPf-q zbnAbUwXw+7t9M@rornY{m(KEidB>alduW%N03`omLGo~XEF|8y@O{e%V5@iADXC)^ zg^XjD-JoDdzalW?Yh>XWvFdhqvwY&oW0*3v_HJkR*;ZHpt}9lCKdy9*+bqnBSc~VY zy{@MHBF?bNZQQODyt&#H8-4zBv|i{%kq!IB|I5k{q^~mrc-*SZy=2~cQG00zI4J!z%wrv7@M}2l3q|lrD zcZ|mUqN5Uc9a-z=a?R_^E5irO_ZGV>Sy-VjKPqVC&iRc$(eYFai2`ObBb<3yXpWhw zCnoUOmI)W2%iJ{~2^(`E?fN?8(>$)*i}~NoJXcr4hh+iX>p)lAgsw;Z9M>_zoV$4e zm`!9)XG!kdA4Rbtf&Th3I;59gL&b#4HLNqw-et*fYqu-4r-p7mMeVz1Y}C0HzI=D_ z3^0{8tIAi;j5FsLU2lzo770h6ee>r6qpa&MwXPcrBk)g$U;K71H3AE#5*+KpU&{s< z{`-xEkrPp!$v(WX_WF>2*gSB5*iUCFtFJM_V?p*~j$v#so$KjxlYpB2V}wlZ8JiYo ztkkYtO9}2HnufQVw^eBddUMpFf=DnY0w0ED)n+BW_aXb_6EU;I;VYE>7axh6M!4wK zqqSs@Q@WeGOGWFc=1(@IpZ!^uZgo^^gZ)$s-a2HlC(Kl7UOvnQ)|J?7SgNK}4E=lE zPd8hoyP+F@m@Ay?brfHLan`~7W~lub%bBvMcd{!C zL^`g)6ONDgLmCu~<>owpc8`hh=rlyDP-u*}cDMY%5JP`+geVM6`%23T zQ3uw4NGz?|uZ~IiwZLES874O&e!RkWZ-KW>D9$vt^u!$>r#CPl&ulHM<_Mzot22&C6(`zGw*E?& zm>?vt;(_S92T`b;1%FZO|CEv#vlx}4f~@|Ki5pVwPuDcbnSaP7QC*FC@z>^iukOWa zgYjDUCX4v6WHvw9oLf+XghMhuxcEtrW~urai1wWRtJtXbF+UF|{dcCfYBI_#9c1}@ zECs?!a-Zkq7Dh`r&;AHv_r1!infw`xhIb@MxzFrw%*~pTsaS6&4m#sU%572A`Nt75 z-?=WoMBhu@-14G^uEv18def0A=#a5T@auk^*A>yfJMxv(D%cLL0@ zsDhVRl;B1$RHnLVI9X!x;Vhf5;=R1wOzU41s>6N*S{k(J9H3qEtp)p0oAidqN4Su2 zRQwAO?!1h4(XkuMd+Q&#v%e^{)X6A2OxD}4Xf*fs2zOW|lphwJ7t-bbZh_Hu9LiLa z{%|QGfnTTa$5^#LnI^*Qg$j;wXd*TsYBp1>vk2)Ay+dhmd9^)!nc`#BTiw2Tm6 z&BLlxwfoteJcvils4j{j`)n)jIeP;syhn# zH;&i3TWlvvJTlZ19M@iQjbd+BQH3b{i`Kl-qoA|KzrGFHYVRCOzg8#=W9C1a-n4(D zKd6u_?P<1RoVVs>w5~)m9{w%oOh!#sw)fVc&McY|qMYkaiZu2is7_SHp$bR~*WV1? z`wwp$`OhZa8d%&FRj|qLVwN1P(Gl(UKwP)!{+Yy>SyhqJnIx{sCgWx9>k3()I7nL_ ze~VMS6RK}hd61ZA$WfsLx@vDeqFD+~JY794k;#B6vJwev8IkLr6!Ib9#ogh9#Le2v z1XI*Z??%~8){pPEL$suXZCuQL$!XP5?-8wf+9q%`$aH#3iJ#yVZWgQ>XgkOpl0)C-Kwyn9`FCKw4OHZ^9S{Kz6rj`VJa{&JO_&+e zf^~goQ%i#ke_e8Vcd4|e6Mh;pb(-yD%Hypfb`CI9#hL$F_)$rL?J_g1KjK;YAxnhL z;A84Rbb<*zq~DmPuoYpT#|3Jg+ICiAqk&9*2$&J`$3SWMTbChNGZ&r*Hh){MBb!M67RIH9sqB^?@XCMBHJ$=?ckf(gQ(`41Q8UrY2IsLD2K(HnaoL$7LCgt{y z7@;osNr-Oh`v=B;zaf={TYdp~`y7GS{?=ZU^{p8e1(v4K3j4_R_Ip?nd*x4;_!zGn z-p~n^h&ThBH~!m_V-bajbVpoAB(*nkx0=@u$LMXac8bUDq&T#q>8U511E zBEcHuPr6pTg>ZM zYTcGkQ&a*Lc1*X17NFrqwU*>}eGq=b;frTyNfg4$+bWMqVLj(CR04G1h~3uKdeTl+rmI9~l_f`gE`UYATp`%A4Z5 zXXB)cJ0Rc-(6@ALa5>bhfFb4Q85uiTKC%=LlEoY+1P=DL&QRPrI4Urxq^PX*J@CRJ zm}^|6c6WERX*Pc5JT8ER+Al%-ZM-QzUX!26NKI{c)Re^9%hE*M&=g1|!bsj{mAok1 znIwhYt1X7|6~0y44WE-1lf31PX^+wl$m08ps-cET#z9n66jj5+!-I0)U&u~j&Uc^r zL5H1G#H*nPo~<7ea2@H&p|7#TDy1cqT?n+HP{@FlX;8rwY$cKG03RJzIq5qp8934e zSLk%TUG;ILmMyZ}7Ofw;-pek%x4A%X$aZ=!&XV?HOy5;;W4M~|VpdyV!DuI(9C~OD zl4TyB8Diq&9r-_Gq5%p5#TTZ& z`q)p_`=DUUPFZ*EqX&938`Vv)Al%&;8zIp}_Z?LRe9h&6^TlIHDtNa_V0Fv>Xk7>e zd$-(!_7ULcFFi!j4xdju@$`VBVWRchx4&#hwY0QQlgz<>2U|L}h-a;z^x!r{gVSGI z2V^yvqoYiGckf=ib}b+6aMvFeFD7}f96bKM>g_awJ*%2_&Erck`+;0rHW*}&Mxb1J+7ROU4YpyE-vP~ zf=cZ9fj#9@MC%TCu)=tNw$Su}Nu@#+tG}qgNa^5nq2hWmm-${ai{qQw6_N`4FLja~ zki(r?Ne{e#7iP5#<#w=eeTMSsEf!uLkTfJ;Rbx>Zna+ie((4S%U|7WVeUVudenzb; z<0#inM;UE5C)6{&Cr``~4wO?i16iUwhwoWEC~4TnrX017SdmAEu-&y*_5f_q>w5Ir zFKdtRJpJ=xUF!=BQe6f@^j1A0$a5eQTyvfX*KkJF{8t`qzEE~sebHT22c$5CtpJ;s zMGuqc!c^5=JXRP@yg7hLmIY!*5brFZ3fN8i+0M9Uh(d)#1}O;ZW>?`j>3~3u*>wQ{ z(OOe@9B8yBkQN0#I50qF2i`ZRWz-K65?8uPqKhj=VIp}o9ad|)8w%jIJ>^}%n-s8L zEyEk)+mh2p4@u;MWU-SY8!OEcC-h;7NiyHI-HY)GgJF)8KyNt5V2WD@L2Y_JqDpqitOGrn7?nTr3Z&OViT_q`j8Yy50(JDP=W=AK_awSoXXmjar7*&_JyzDp z$bn)iRCe!bRs9!y=ZL9V(99S&-Rxqg*~U#eA__-!%ETww+zJS3lgjL>nSP_-)YF4V z(O$_ucD7Ads=7U#H7=66TnzakS0Q5=%5NHJx;yDe(dUsvIA`OMV7a^bnvdY9qH!mD zE8sNL%*Oit^J86ZV)X~I&#a%epN6x@p_8ZJV*2*!ePQB#6DkLnSbt*^gYWejn@Y*Y zkqoUo)N-lH_d&unCd|`i;Ct~*{#|Un3j^5(@X^K_$t1c#tk_Ani4>ep zc#YspTK~xqhXH{Uf=as zPBu6m8-`G zoIN&X)$`B~a3X^fYIk|F`jK794_GM*@Y{>9E!mMCSwg3mc2S`%C1`XC8CLb`XDKAR zMw?M3cEiq>SN`0MYX@nu~8(oZP$0%RA|V zPh#+JRVkJg8In-deP#$ka$|;G7IT<=caoHqy#a4Y|FcYz@4IJbvo%xnk1jCBZ^bST zm$oj(p_c7N%J`}ZffBYVG?fNg%FIfhiYv^P!HSJw^Nt*91gW|t)5~P`^(0{1ODv(I z2_SvQl8_;YUR8!@VViqltXDr+?kd{dJI?w3W#BSf2-@-8)(dp18h6IOOBG6foSE*&OX<>)d6_>CsQ4?%{Gu@pRIJ>Q8hy9an^TPEI8)2}s67f&HIgqRfPjxJ#gKpW z^q}O4X)pyN=5dUdnBh|lCHuqcK~ZytrnhDb_q)ArJ;_*mZuA!`Y1l=Na_Yt%mE-bb zk36V_-{b9m$-Uj_*Iw?IKNj1leLBI;z@S>EAM+px;-)YzP`Dxchf3eZw6)X5rXx$$ z{SKXkLV0Fe*vDi}7BRaU#Co9odKxtes6q9Eo_haDrw?8$Of%HImd(BmFH$*dstqzs zyLDl475jP(_an72pkR(onHi4a`N;vjd!>V=4G9GU{F5wj6DRAs*D<%|y*<0vjWzdOgUAr}^((ATsg4pO;9&e_=B(DEW2Z#&PI}pmU*5Q7#YYm!-4GU;jZ*J%6-y@ zceK(mW($ip0DT8THjsA|o>qO09_6}d@F&GZPVL;|$Fvgg$6JOGUJ2{!KEzy0F~0GT zY!&3M`98MYU%qha)p-<4du{&khJC1VTaNcW*g{7m_t(_|EI?M-*z@7%M!FnfNhGJB zz}1Jn93-@-oA$`$h1aJ;9VY5*t~(6OBawPQ^&kC%iRPg7GHF<2W~>Z4BEV|3 z-rEwY+p`&eO5UIY5oDD5;*C@-4iAt?HQFzf<|ZjhLVi2Hf_~!(Mw5*$pn0as^txPm zOqK$NJgTp*Qcs^87Dx$J&zbx1cT6*Js-a5Y#^PDU;OW3^-MYmvs16*g&{x-|oMseL zAlYqyMMfzg4zhMfu#`izDA6n>kQnp}tTL=1&3|Yh-KIb#3838dqXN?VxCE3~E-I%>r z;=Q{*wLXD>C49huif|nSO#LPsA>+ECnS*jcss=APC$*sYdhgwm#-h190Tk8S%D6Mh zQtl2;N$czDS75J}eX4->BNi@(W88ajRz1n(pXLU#HM_o>rnk=aXR4uO7X(Stz7Ps0 z@8HU{L8Z{$GhjN9X69?mnf8BjM^a#vOUsqMlc-Q(#q_*-7JVQrJ-{|7a({;Qcjrr4i32wgL$n~9>yRyNKbs;V z2xVN@6w5H@G%IS0Ix(59V4P%%k~ksulcU#XZ?o9zFV~%75rJGuI(_HcIK5<0l~?b4 zv@R;ev7>IlL+|&qo0N&eM#^j&?KNXp3Bi}Gpr81~6NX-fqN|djNisimVO7gLy~%oW zGNThO;U-i;t&aXR`5vTEbJp;K5GIlQr*N5{_nZ|{ zlO~m87DC4NJoglW4?GcGTc=klcMrBUvNTg1e3Ex@i6`fM<1zHbd|TdsD*wI<*Ka3T z!K2ek8ROB9pkR_SlP25?&f2wqpZg}6lk@Ydr9sH#+b=k8>qS!tvrygZHhwyHP@6{a zG%3IRU8(XAwUhdDlzF!0logTRxpYwZY zlV^36+&|j%ZEl2c-r0CeKR?lbykYh)&n_um&A~T;wqO6^*3Zl4vhL!EkR9H80s5bxDC;|;IV&_l&0`NrPj!u5Hca_akvolt(Hjs#gxdlY}c zv}1QhJ|g8=ONh;?u@;WbO(XHV1|;TZ?y~)|o)7X{ZKP~;73~xYqOd)n&K*CKM{q+y+DeEJ9ngCR4&Jce0IlRE9PSku9=NIV~S_zbl zhW=yU69_eJ>rlzZD4+{!YoJmu9>4pvlKsC0A$!d0$cn?qZP{zy7VmaY218~F6$>-q;e4UIj)G+EeVfk0ikH^J0n$nD65@1@It7kG6x1P zy7$M1@&~$Gd4~~YzfL^8%S%-nV=4kv?ums8;@l=p=K*%5=To0M&hSd(9W8h(XZHc_IEWCvC?#ZmVyVZ7|P1QF}$Xti(J=nxm*8dPG@b) znE3fXyjjOsP%0GZd-epRc)>n(PG`HQJznpy?knomJC)lcx_w)T9J+%O9ia9WmH4U0 z9y8;8Kb*M$@HljKXKk9rJGnq3%yo1A;fCOyTR$Xtn@a|lhY={TF7gI(@~2db-OC9I zP4+GqD>WWiu4wA%#SLOtKz5L?kmHJ6R%Xj_gS2nCNWqfrw_=w@$~%fPXOaI!gX^Zs zCU@e@W3+!1)Z#m_{7@OGy*Fc&j;=+gfV;64wxOcs1EWy><$SzNiVO@T>Qc$)qEav+ zm%Hffkp6ModZb4XAZZ*l0QeQY`y6R$^3MwT>k6~x^ox$IqbmWufyFVo{m=O_`l8gZUB@)CA} zOXzrebrYvgV=;y4R{9(}I$p0yzWoLBrKI@gI2#+M9rrRyB=pY$7qH;^@q#WBCsHc6 zePVGFGivsPKp z-StdQDhic*WdhzMIREoco~TyJGuiO>?D|BWc!oIydf$5ZMu(a~G;}uXBDXEitD zOeEqs@pqna=M^dL^?2*JrR*vh*Lj#4$B>Mi_xhA_xR)OiU;Cis!#!@92NY!ju^28U zP)Bl8v`w%zMjubhe!bop`I!`xv9UB5F=3m!vwk|mW#$AE!lgi9+(-9Qa;?CmA(DqC zWmVdA^}^o%#2InNj+~7ci{<(zVbfv#g`3Y`gHgBcTut(Z*GdA0Bkp+rGUXzk-AGbklHVvxFx;W(imFm*eT3eGLdp7i!D+@m5sGd-8TYMAC}+JM8=1(tojkx;Qx<&s?dZ4X|3_U4G??!o%vAb+VKn9nQzZ{>U- z6tbq>1WyjyGjV1zE9>eO3In6l_E%uj%9G-X-Fmb&Gy7-y*xVfDU~_i>#0L3 zs3!mCGu1MLHqdJC+P-~UNe;g%u_i{G;CIAyn2kx&H{y7nFKj)=o%dfz0v*Yi zv2UE7Yro$f;{{SJgm@num%{&p_)3X#G7K2YTGoQGxPP>a-!i0At!Sq~?7&cwT$~1Q z%4z#Ky*zW&x4XyBG;F#v>RI#(!~}O8D+&kntd0l#s*ig2`2i1c(D}Fg>Fg@E z)Y=5{D!QA>ws^Ub`G5KDuPK)6^ArD2g$K>)3i?|6_xm!2-k5}B9Cx8eZ?qVI-~KZ7Y@X7LG08dMYcSQ? z(C!+PO{a!4`ksT$+e>mjEq2pn@?4i>T$NsuS}1jV(o3c_0K@Kot3h!I0GKz&L=wD8 z@<`ITEsm(rl3h6>=)I0y^Yz*f7%xfj{0g+R~P z*9Ym?0HBiLl>oa8XMDZ4)L-~)PQ}49%DV~|R_C?VZg)h+&7*&zVoAn2ib{}6dX6!j z?;n)(-2#}q1u>5?EWxMLIb-@;L80rnoAv{IsyLA+r=NRuw2TulL^EN7R8DV>nN->$ zwJww$JM%MU8UGZce#PhuV_~mNPgdU#_jmx76)jm%?(;NZobprnQp2`#r;p|*A6IST zdT$%c6aHdvlc$wQ=k|&kByco|1pQ;jPDTWggVFvUv${wny7YX0&{<8PDA3e7NhKc4 zs$G)H7q^!BcgY{=iQj1vGU4Ulz7!ofX%t41S+@uC zq64LRcWMmfY}n%}Gq*J+-a%#t?o#|m-TN1BSh23;&qaYO(zJJ0vhSAbr0YVFH{2z; zLKk$A&Yzs};VG>PGRI&jaSBSCZ(ccT_ZZ0pZ5g7R^?U4PTDvybZbmQe+8h^jfAQ0n zKDrZ<3H)(v5KfQYaQLHp($0IUP)^PYnSZL^n>`gSpR({|wY{%gJ~QFf)ZhWCe>`AU zuq-Rd&lH8o-?&IsjBAB#K#3V)Nm2=KUC(Ri4GN)O>F`|R$ z^oM+Y+}4>d?-IzfoHe%V|0$!B2}?;sRsZhjh@l4fJ=7g=wFmg=Kc-N0F!Bsec7GfD zD^G*zFhqSA%;8Zq+}kufdzHrw@%a}+VgMRh{4CP1Iwta0&rs~hK%Pv!*C%V5qU{q2 zL<(LW=Way*zEovRFnNu`;mJhynOGj<*2;V65qKROi5MEZd*-+Y3@EK)I(c)iQ`<`D zMpH`hD@sR$QmIj{6D}w#;^|{u{>3d#a4~A|G6c4=-}0>AGc=JA+LS=<)tNX0rry{1 z7kSylb`r{D1#7+|GO|d+u|4k(koNs6>7|9Pv~)K;|E+Ista72hdM=jzv47hn`UNs2 zRRBUcehgd=I#$c3((zBYd_5bf9%HU?rJk_P`le=+K7|spy`%p0cNYZ@sy!pp=ky!$ zCjyAKNcufz?=2L$#FPdN};JxR5wJ31r~Q(`*|NZwNkMy(wy2bAWA>srEg?Jh*H@>0ah1zEHAE?(|=87uX_B~$3TJ_l50j847K z4rqx<;$mfVa3!iQK+1uvmID&EZ~q1BH^tmpxN!2^;cHH&+b`ntk36!+>O2QJxq9#2 z>b)~l<&H~UlJR)Z(iP8J{hcR5;%SD9H%>uS#%aJcxpkCyiApI{7T)CeFejrxYDJ%t zj;`---!>LZ>8J)UliYb_k8*=PeeRON{{W4}&JdERQ=6@Cr+GEngL6c(omviO&m4$vdL-)`IKyzheJZWV{QG-` zM*!vvFt=ra*&-z*Bg_DRUfI2ThVlB3Urj*;&b|+(#?ZnKn6I8;)nP4Z56r78qbnFq zi^kDSW{^L+W&YWgowA z&xJLekC{2R$4Zdn=jl9)Ye zVjTm$Q7@sK`aG_aNhnf%^kl$5R%YX^rw@tsz!(CiqjW#+x*mg!*-Ko(cJh4s$Fhk& zW!43*&`#HL!_>B+`mW!H*65EhOutU}Ly~+mRqrA*ZamPI%$HcYrfghtu-Npd7v{gY zh&I_?8s?Ha+ArO8r9Zu#gmyC49#l9T7!nPjZngD~4UXMPWG6`#@Myn<@*+I>wGEnT zpopkQt8Nuv&nTC;`uwdXMw;;#$O}aPj>eN-5;JHGu*NYUlQ6<^=L3CRc_Z;iZ{V>; zlB5NjEM)yga87Hf-Zc9+h)k&e7eDkNcJk#KHWDMZnnXO^r zFF{kU)0SNRQF@P=ag9Uz4!_r%LES|$yNJQ7syOknYqop(*`8RQjud;u1PDiY;)C8M zUvir$&$ROwq=iDXvILq-Vf!WT-yp@ynljn}z za@%=-*XZHRSLqhLZ*n@jPxqc>$Bhde_D73qyGC3r(6j@AP{>HR$NQji(owy_qMkRr z{F|dqocH#y)$A`4{~Nb`_R3~>olXv{8t59EjyH{6*xek^^x2%z_Sr&No^+f(CNIC{4padyMXCic5Damu=A+9y z-HQdB`=p8&_)k!_Yk#2%(iq)|n>l}>e0bG%V>st=gpM{7T6ya3@o-uF(TS$))iPeZoz1$E zzqH)+qL!q3D*|Q2zh@N~@K`-f%4U;8#~uqujNPA%!JCL{pL&ynI`|0n3cJlaStEF> z-n}axu|N2$q{Nw-3i;g9Ce!v&zM`1z{-rok$9m6;(B;>i@kgI_TDo1LkRGwX_1Be} z$5nrzn+B60Vej>`ZP#&q<}ysV_3}noT%U;&0;!ZN<7evoM8(>`da8)7f8ik1u=ShT z;@XX4R9_lHuwG6Xk88zTE(cfj9Ahd&zHa41Z=axQ_HdV(dh?&uZleA7lOoM{N^ERH z@7-0`?CIG*9X4;x#E;YCU!LAioZ8M?%q;qH?YV_$}v|eF8ObPApe!i5=LRo zI`Zs>`O+J7&ndUh#Q?$i@~u&JeZ*;750@_*Ueiy=ynNLilQ+)La%m*V3RcTJFBtvY zxqgv8bd1rY=Wx9(88KUUH>A?4JEH6zzu_IRbI(sE*Lx#&!#PAO|4P*``CV-A$vV)9~;Iu-6}+Q^vA02}|<3 z?1o9AU#x+AZzmM9`EbJ3^LrAhYqW8NVtMyoJ-scqL*d! zM`(n}xLS9zjIFN)D1{}lFCDZr@yiT1iwM1^N=#cFoMZGqQp1vEtUf*2TH>h}K<*#3 z;|za^o?CRX_R}cbUE@fv48&jF?4sjd zOW`_!;>b2olc}X}G!c=!H&gLpzxsVK96Hq@Spc2;K~7xL_i)$B*Zi1AmZnJpIxc>6v}vDmLgW*Pv1mIq#?_=slAB}SPhPSmUBlBCOzeJ{{x z0vK#HrSZ{1@R}_;2Y0_Z#}%ayeX8)6@9$xHrlUnA7ov9EGwki5Lohtg$SC{i>Rkt} zcM*HMC#I0$J-fX<6eiqJJOICUvT$du;x_HQX&&}Xhf)~En0t#W{Dm(U7)m$J%z$RW-xNIP03En=3+~tH5m8Cw zOBp(|li_0LCY$n><%t^lQHNhPoArVpC!$4D6ubE*EjPD4_p3s2R#wZRmfDG1V?=nz zoh{=u-BdY>C)(OIpFY~Vr8cHD?f}H!tN{mgXS|e&B(nCq)^)@e>bEmNy#701EdIPS z(^!r3l|$>#pj`p~Kf~{dCMGPj53XPVh`=ddf1O-zibLG zJT_zX*G!TeoZ8r495ScYP5B+j&hBCv1qNVJRfrI9 z8Q6B&C0Xw(a5pupuPv?61UZpfxl5#{XdgHQs%SsLa^*|>iV^k*AfP6m`dzsn`xH?R zAi@-*9*6tYV7#MtMI-Uz7_X5#K*ON66)bQQ2r5~44lE56i~L}&TkJiTyB2$0Jni?6edjC zEe#bn9`vaqKt?yu{H?%GN)Ai_4c6!Txb%>|h%GWzdwctUE&8`P16KEfGHZz#8>KHK zr=SAf=;d&gDA@QXCY1z&RKL=1So3<7@}qIbzfoHSJ9eO!7JN2oUs!0beHQ$0E|Em^CQRUYr`C83lNv8aB`AL?<G}~uQgFUet+ueG-xPpi zJqYFhdx8)W{r{%}tfC6qOG8$~(*bmJ0DzDv1Yj2sMcq@`c)dlR#_jdGI_bO^Eq1z@ zI!A3Q6*p=vYF>zlchJV=ZVO1oP%+>7;bmm?RGUrO{rpQ8>+HU*XV0!*bkF3 z&Fydl9|7sY^!%PIRp2xE< z9!vb@|ETU}g1!>&0z?KvXr8@;g6i`B>JEdEsQ;tEO(|o&#ME&O>;W%gH+{ z*BDSjixFX}tO%4IZ8P9RwDJ?-QF6-2%fYYb!v6nvE~l^P$l}f)hZ?;^Qalh;FvLqa zB;kvI3sgNEn}VbX}3O~`dEBq`_OF&CqwzI4ORi0rGOme!bUQxsgb=8 z2banhn`A6;`KG}ol|n#cg~K$=V*2{&BB(=pqae2;qwg$0shDeoRIb&3_Ho0V+eQ@r zCPlRJux%8ce}qMEfHV~E&Yaj(d$XNR9r%_tYjls}6oe|Qk4<(_vs@2dc6qusR{f-@ z-6n5_49m9qvi?v_;(srop$Mfrh{O%>$BjS}w6gMCN}6+~JDM|F7!*cW^ zH#uJAbYtf}r$>rGX1=9%wv|dFVVG!l_#~T8UgfCkyoTEtFUbyFd*guxHsv>q{0t$k zmG$m!|Bm;O#eq~}dwbDxrO~GBXaE1bNdPX&6Z?6eEJ@Nc4Hnr=B?r?Qywp53x@-98-P9Gl*)bUY zjZrOU80@!*LfndLbvI~&p$(%Rtd#WShr1ScW$IM(mT(WdN%l5+-|T7;jy;;sauUk7 zxl5<8^_oj)S{@~54pM*WBxm%)=CCM%$2z&;xlXrEA4oQu76<_erk!hTe;_%x>J~dn z!Jg*g^VTsyGk~fv(t&vh(vH|~e3RM^vuL~hs;h3AtXi4xFwe$&SbPO8q{?OP=9+7X zQ}|>vRQ_oEw>%R-o^f%F=H!>aE2@;4sd#Ot6l2maU8M5O=lM_3uDRr2Ks5HIfH>@j z|H+RO8I%It1KZ87CqL4ytS*rfiG(;0j_(zhD^Z819<8h`$RCB|e1{vTn6C+28+q05z==d(Rfj0PsG3G#1{L4uSrc%(*rb8;vn?ez+Bj!O z(X)cXkup1;lG-aF)G|39ha3pG%J705f$FpWJxpA(q=iO=CP-72HilsAJqe|K&Y~N` zKv_Pm%~R;}a2elZ1FI~;9mu|L$}*4o`TBs9Sp+&0n}>$5{6Wq1qqb;)ol`SqXU9iJ zMSO;^T($mVM=c9QFEa&i@fF>6}Xa+rbxq@Rq znorKBT5GS)w(6+WEv~BZJ@oQ7xsjR^l!s`pFD1jL=KJx}PcbiF>yb#5x7W^fbX#i( zslJ_w<~G{Khabj>f7E(WZc0VX{Qb=QIz`f$O`MRs>7Gnvf^-SsGr9W8dQ>biIAAk>G`^+^!BU)CY2@+ZwZaUv<4EYeYIE!gUOL`8e9S?mj!KwXOSZ538d)=bK% z`j&AxX|!8YDbC?l!eI7U>erW}`S6c&QaIyw$*}hB<)tjXvr>VEZx+*z5P8Fb0sqR_ zj)OiKr=Eh%;62{js%{_#_S`aki8Q2X>>3tsLoL78hWx^LHV(FMx|JG8LK(ja^n&BX zwD0aAwP{XTBKZjw=7`)?*tF&EE+F4MkzM~WVs9r#&g3s;Aan!L`m*=1n=VZ6dIJ@4`{1ifVG@Fhyo-qZ+pl8Co$ zGK&p=V5UIrp=%9S)m7IaA*j9 zc#x*(p;J$upl2H|QISt)RaMoDYu!V>Tgtvy-@?Purc-)42X6-N2LbN4B7iGnqe*k zmF-j+N2F+sIjH$IuBPL;NbSXiNR+LL;bPga(m_;9S5iWz&gAV*&f~QL<4Gmj)4162 zp|cp6rxKiy8B_@Te98b1K|nuguS7C$XE$5^IOpImV-5H2a;=ZP{KuN`^=_c;Oryn; z6ildp`*B~kW!QRFah2^v0Ek!GOLHFIvnALgb0R3AP8s;pc60Tj=rrC){kiQoL+6C( z==!X*l?6pU7gdqB66I$goS3^a+-!m6~_IiW6%s-==rV z-~aq7t)m&u64F$~1nlg2mW1kETg<1r_#J%rWf!NWM^cptWXoe>Vhr8hHedU2AjyI5 zM)YP2z{=MPmco!IYP*ppW+F)OC5lgASb$r~JrMQ#Oo!U0B->5r|Hs6} zk?(lEy-360!S~CLrs*6I!?POk`Gpdzq9}#O^7KZsexF&QgYX(YP%429x!(cv8R$|g z6HoZy^?;vtc+kSwpZCKN9+YEb?IiHUvBJJh7obGI+z~_c&*A>4gA=~wOc?b%>zD&I zrcI7hBwtPg7Z_FO+Lv$Qh*Fv6BHW~jE9rloFO(v;I&Pp2AHuAkg+uD2^cAFhG&9tj zkqEv1y@Ma$UGXC>C_o*DAn`P^^dRHP#iKxGGzc@9jQs~_a+gO6_1N4m&$+8i2I&yZ zhH6|+eodf0m4xHBUu<26HGgAyH74Eb3K_z+kfCCrO@{OMta%`#EP$A4$zZM#05UIR zS0KD!()FVOyYuL%?(bGyYmppE=ZY8aqW>vbF%|$-2qxeTTlo2vb}3!A`$y(?YHSrPACcnE{7E; zix^A1xDpsiS;2ly*}k+Q6sXgx!l9zw z7^bqx?T`HZIS^;ED;4&hxyQk~B3YZ%(~v=Vd#TT>>1z8P^;@h!=egl}3JMpxUi7{K zr$}bgo%UOsz?U_*-;F=EOR{x7YYJqBp?ibQl4e(EU(5u-AZ(%lekY%b;w7 zN(HhUZpYoXHpC|f!&>^QztUtiHK(*E5mop$h%%bm7d|-T4-xV7IY3@rEky`L=`Lt} zp`0%j$DHe6Rlb4Ah@1dQCnVju%KyqWeyicVf~~!6 zN_Ki}X0Inckz4L@$%2F=evNPzoG=Et`zPpBz`&5MI!_`y7L;9fk&2tiQ%E)gB46J$ zuRhLswQ>u}OLvZeT32T}Mn;_R9urB{fiYBpEazVsZu4X(qW6 zvDQO_0`;0?;5-oDA)9ZZ)5G~%atVT#@$~x;aHON{HYYL_16w&)D!zOnoX>eyGb0#1 zy(5jaOn9Thar*1y1%MW@pQSy#cW*ve`Kk}{`hmKP7!U|5uuq^mT8)l z#@dRyYs(?yqYW1FM9tRETe|=|p(V*SdF#@;zgdc!&HT7m;4B?)AU#)`Ex&5he zd9$tU;s?aOcrLrvh=n6m?kGep)%d=VO+cO}q>^s*vJBGCPF?ZN@_16P-e|(Gz z-zEDRxyH(mNETB^Z?R7R`Q|zE=)CtrN>OnL@7_TXmr?IbFz==YmwKHZPES|Y1#|3W z#NOVXdyLRk_U2+U>#>8-+=fuEG4Hxipf zP~E9P{+3Gy5SrK3pIs2OT?mvF>#@}T;LqS~J}X_`cR!Asv8BNsj){soi8u~$_O;nP zyyEJ)$+PJ`qiWs7*U3?DY;0WTsTZNL6F!krB4s!@O~;=vH`dypRFMUjMkfn(el?vs zZSVVwE8!-;XgH-3%G^aERLY>3>x15=Yq8cKj@FyO@V0jK?+>GYTUwN@|4mOUQ7Xw7 zY%s-_l(N>6C}xL9rw&{q!j{xlc$r)>6!s zTk9#8!}*EdvfkiL3u=7|}SpLMpzrI^lm#j@#cEqRMoV9gtWQ#&oRuwf`Q(7J-)|ZY98McYedrRB-Us^ZpDirQ zpi19oE;m~+9}o*g%j7vCK|0c$z!cJ@Vth1|96>ph+{ooKml-Nhs`+?U{TUMXmNT2T zi`sav=M2;=d1(iwd~VWy`U0Qsal*pAM;C>52g2wp47!q zuA~X%IftI?VYt-fHuOv?=PL^}ayikT?BNbgXuHUxPz2!Nd|0Zx3NPQX5xgWVJ8`M& z7}v6LsGSlbWI2r8ycNJw=6K~kw-NC}q8CX|P*$xuY56vKTz^TM9ZNp@@w~m)*${3z zb3c-oSqHw6xrfU5$=D?y6mMQ*_=_8Pw%YdIyP+Ekpg)eEE%V6NJP3B8WJs^$mqOI5Y zf&HLe;L8q_od-u|j{Xn-S!okVHOk`E(pPICpNKvqqx0LXV@i|*9!^Fb1pXqkp+4kZ zkLA)o{?evl2pmM3w|((&Xm)T1stv!P@`Fe*fk{G{M?#!p!tlQ@lSvX1)HV<6Mc<8> z^~A?G$v!KM_%_FHxKv(zm12CXi#WG?;@b-QY8Zmw@Fc?NykG4&fA4G zL=}hL#!XMmXzr2CJV81#Wvy^q9imc}F7leC(^MTjQp!S6^WLCQZrMVj>_H30qnNJu zgM+mHwS@Wzm!Zp9&uuY*DPg#g>wPjX>#z}0nK%GQlSZeI45=8BPo|(>&?EJ_WhJk9 zWt?{^=pz%P<9xUe`3RMB;GdamWt;$$f19xxd9(O`yh#VPr=GH8I+=xO4fHteM>m3c>Jk!1A9t4sf&Wxf4w+=-6xb4MQcsVGX8 z`Z~+>(irZPR`>~#+U zxbftNav@z;igSv5WWg+O>>}IL$N~~N$#Uj<$v=7G|3uoGRv)5?Ffn#BKb@ZfSy~gHOk7HYZ|m!OFp5ONi+F?H zdeC(CXO}s+j}#WjD27jRoB!Kp{~owRQ1cqrP4VuD#Y9EHe2M9B5IO{kKeTsKDvpoS zOsIcB*NyeC&&J_|pZ%60nRd=Y^GPK*h&<@rKr%;XmgVOUqFn4X%L8 z1HhpsAVw(tH}X;|w?}JubF4$&!1rNlBylyal@A(_-Em5We!CS1J*>QLv2Gf#wawH+ zYsXFt+P&_fBhWIA;MG*XY>_1fnoc0p|3-4Pd1t7B~)WAl_+#^BIm8{I~+vD|Wr-QWuJ#7CE zh@wJ(Bpss&?uqtdP`f+@;xVq2FF`R!0#VOu8M_RHt%9_-9!jz(TDbiXHIuT~wtafX z$Vq6vE?C9)9RsX|q816g`%4-9mX`MDC2K|$zT{05h#Wr+e*yNM!VnAWSh4q3exu{@ z7HN)ZPGD@^G|na-d(bd$V541uuY;alA_loYp+GH>dLkVeOK7Z{lAZS8%Io4A0=>!= zrjAw)??)(fr#3r{qvvd_`tn$F$VWfS4p#&f4xJGM&N$i?i0&SolL^A%H)<|~?dp3Y z8sG1+g4UYOEaMIjWVdVl+Ftc_90cEpWuiWsct^4SaQ;czs|WYF>Y-oM_0JY%pd@u! z0=nsIuompzgDUr@#tmpiiThD8^6CaDH?qdvIR;~^qD`bV2}UBM-c1TN4YKZ#J=i}U zBCGaT`OjVAMvLl^1pcXvN)!ryoU&Xs$o zhWIT8WLua0$!Nicb7UvEd*Zp!4OAD6EBslPEdyU>rX{lPPxtT2r%$mjojSeIydos} z*zTD?{>Jl8Dit>zKR@D(QLXH|<1$oZc7JUSf#zR93bu!4zacN#lo;%d4&U-6jWPa; zMqT~Ev37>HI>vYsi5PP#;r7d+c&+?)xR$P`jLNpny;lHK5lK+W+BQAT5-_?uNLKfQ zgfzHDa}zLp6o*tMF0DUM7@_lT2|8SllADokT2h|NT!98Iv`rAwFTq;$f{gj@{#eiS z8}e-d3F0|})1C!X|KgLID1*;pPJNx(rJn3MRf1C4w?EEBzf|I@zjy?%byoRNKAKL9 zD(r_kXLed}!b}m=2J(@$5Lo$O3{Amyx-rtU9ZEn;{{C6oz17nL_!1MAUiFq> zC{$Y*pRhHbIN{rHT{CJ@(adnw^EG_oZS*SKm*dvqCV^_=QQj?CQ>Xj=@ zK;N)NK;Cyy`Bn517v+k4xwJ-|d2^`s2ussm5Jfzw4;0bF0fDlGMQN zUZ!S-V41DNTqw~Z6@8G}wEkp~zQjR$9>~h7*1A+i)u>xcR;4LYS@tD+^GyMj%qqz1 zC<@g7Z0@oB_avoV{INXoh1_x_QgI1TEw!W7Gs4ZZrL8A`>`o5c|KZ5B*7Kq(F8VO) zaa7okp%(>rpxTkMfVB8kX|vp^L5SYQ1%Dxgc^$Le!DO8s{`9)TpP5w{f3EHC+RKob zf(S9{1!GfJ7VG)#T~)kozy)N2!uR%LH_(Zu#A1?9-=hI2dIwLo^H!CGdzcTSI88~+ z$4Xwh`XIMfXY4r6HT#2Ao?os)MAn{nnm>X)&V38^*fRwBpTXyfWaaXZjY2Tch)lmN zI)tsi3${O)i-g|5$w5B`Efi?j$>~AdFPqPQ1G*b?8?H8cD zT?BpdmX8={J~zC?^rpeeczw9a)|8CPEb=r!rw_#catS&GUtcgBO@4k>`|sm2hd3_B z*T+TteXq7yN;N}LQmyGQ)1aKbXFR>>7^0r+B`VFw7+XJZZeaC5N|7cSNah_ijZ4Yb z3u?JL7xZ`cSLN0~x0ey?lGCHDe8pI4l0SBQ2NP@lf48@*cwaZJJ2{h1$D}4fE=KOQB?ki zH2Jf&jI_Xjg#9S%h91cJKBuKZ$Y;QG#exAnd zVUO2r#ii%}4`wL#7?3J7Zs)2I;*^Ax{YZK%51djq#<7=R&NaAq7$fCk!L|691 z04|KS-8EY=w=w=myiwf$#b=VJ>alvdUUBz>H3fFM)qmKX{|vMiF;ME?0|kK}Xd=h; z^%2WfUvyq->T~Zl6A!>PB{{VLjZEI9s01lajXkk|zA_Cr73dY2e)xGg7LV!)=Ehgd z5$5=o5IQ2@o2A2?YoZ?k;%_}FI2~w)qN{xi-~J>F$K67*Bm3>F0S76JukLemjz1?f zN0+_3!y$97DJw{Mk|6s@v~}Odb8y(4YUCrD?`4W46J;aUf)aLRz8s&M?dz+S%8M5%;hu ztJafpg8Vwvq90u!FMkb7HUiwM2?0ICzO~qQ_+A(2gfAcl8vDsFQ-NlqmCn$}S2F7A zIlpwTKD>v))Q9&msgouhhJP&bQ--{6A(csSTp>3TC8S^nS_14 z8E{mXJM(E(_o-e|62>XxYy(Ww^ea~rrs^G`?*Yg>Y7)|yEUkzpoJQ0aCQwZJ1zlSW z8|qv@#C|;FciK04M3J9zo6{tW5jF%`j66F-j)X$?xcXJRYqV$YbrtHhHFo zMh=2(23`aBaIqkFpOBxlEKELx}YEgTn>h**oF%~Ld%vrvohr}qO0!*{Dw)_ zhKn0Y*|4wr0G-EL1*bqkKT4uV6~#Ihys~~vSdI#1ND^Wd-c)_$+Q{wlXZj`xISv~QwGuG009B+#^;l0 zK3rHc;8zu34VcW29$~iVG~7jm$Ri^?P8O{7p$#zBYe4-4%MGc46@=8e?H`Gsu=|Ph zu*5Sy(Q~+LzmbBOv$r9o6TbR(_8(JwM))(x!byvKoTZ*$+w@;|kgEDR zd+K)1`~kkMHrWh_m(&jLa#Ez$o@cdYta)FmyI#b!Lv_PDxE3jLpf0!6<|7_ zNf;T#u(io1LD|-G2)B||t7%Nm4p0dw%|eIFmNFQUmdPT)d342E^l}EhkU%uRi`L6|?_FXOTR?UrQcF6SnfGsj1moTv*tj zd1l{{6H=m0*L87i%#qrAZYKI99BF@&2s&qXq!EztwT35kfeBu;cxfwhQA;Hw@NRl! z+Is@^_#`oi3#KyrZZvZ1tByXBZ_k3WY)cn`(KJa(NzUKyLeeuTCaw|vNtTbp;$)FK zyNK0_5qa`7zwAG29}%Gfpi}$3cpHXfpEp>_Co1`nz@qT~@;&&FK!ba+ZQfgQdpBhu zR&$@;dcey!B{>Z&QjZR&%F=vmF2U^{?ybD`W$iPb3;>)WHfL{^x;QK()IU{R&mz(lk`*}NWJ=OJq1M?_aSHGZ1>^Oqe26l!6G6G|%8 zmw0zRd{O3;_zkLUIUk0+3AzeAlEfn)La2xbN{stuTEH{C4!Cc7_CygUNcNtWS~6XFBUk`2+($Uaiw~xCc+LjqOOui%N6dms%%h z#U8s&jijUTj{zYDN%Qbrb;tO@JOV*|N`Vt{)*X56giX$LRV$=m-JFj4JX@J*B7ysg zQWjnMwl)K5YLhkzP_TYiRQG8xAIwr15mXizpD#m32gnKUbJLs}3qQC^AUE5rjHOCk zF!h0zxV7;=h@~H8(5#8XzIn50sadaDrW-9|)MK?LHXJI#3Q$xfUqVZ$B{Z&(_R>7TbgMhQ46a$=>Hb9m7S2quwn zAbG;YL6P!CMt6V_?CMMD5jzzg@cRn?*GQtY!wsKg1K7r%vNG55dCevAj0%rsV97MR z0H)i5&2H@d*CYND6aBnIU@6x38nWgsZ?Xi;`1DCiA*L1j%S%3%h7x!M!wv8c-*4{PxcS&P1ny(LZ|FXg zRn$uoHz@ka@<_*j)z_%b=c`u|lHgedIPNXI0W5^PTcIkaqJPl)*U5__kI1>l@BL&t zauT!xF41f&qm{1PuW65pc}2=+w&gn(bVaV-F5Rx&I}OM<{M z5iwfm8h*yY^GcvYuI6KMyAvhoT6HRnv9&%3s{q~7S?wQ_!isntj<=IGD2IO5BaV(g zG6^b;4&vPKUZ3OhFr|p+Y)!fzXu`c=O1}5yq{Ge-`9{WQgK*m*2L|mbq;v}H?n(m? z^o7O5NPv5|G#6Ln+9QVj6FdIii3 z*PgCr7i@z}X1M*{fe!KYy>dObyAIse($d{MomTV~N-hdz>WTY~{N}U%f8unjYNnUp zuEWL|1IjM}?}A(Ud;33E){-&I_)uxV>4SS?K07u55P{wv!=V1Lmzml)yW-(f%|Hy18M}^J| zk@RG_IL!mJ!^g&Ul&$F2ITZkK@oo+`C5i=8QG$#m!i*5hD~=2rTGcgs=L^Pk)}EKY z0WH$qo5*@*x85xd#$1&d_a-(_!?!y-JFkM9u6Vh*kG34^EOJ_TNVcw~k8KQtOgj>; zkc`>c*e>q{b-BAoT#@V*I_;O zUe=Lwv^-i!us(h+IQ{6-g*u(QZEk~l?~qqw$m(2yVmkhcOEjp(|b1dhb(Zmx-&Uapsc=cg1hWxap<1A1;}G8K^l0oYw_(m1I4l*ma1sC~KLh8R6$CmxQnJ7F{^R^Cdce$XsIQPDmJdNN z%y`bM&*@B+!E=sU$%)|2Uf19&+-G<>4>qTZHy$2xc3So#)wRkP-MhR?&rgNY>3(&? zai-wIim;D^^P|RJWtv3_3^ZssRtj9@BhusVgv~R|>nOhEQIU_T&XcwndWj{a8x=dCqZbd;i$!3a&_D5&Yp)gpTQL}r4$*mTo{J^&!Clgncj@qt? zI99!=a>PxYK97w&bX*EGZ0B#6YUmFK7Q8>G!bw!6?hZEdz>s2SLRyyDX!zO$VUZ6H z*6WZ!rD6UPoVc6l>QnYdyy&^I3=|S%^2Umi|ZCq{2A<*V65@#Fq!8Hal z{P#2U$fXycrA#!d9y*`Q0ISpfiZJMCzj2qz0Z@5xSeNhM&(s~Wv zbh4#vdsu?yt8pQ%4SiPwrc@?0XnlSnWHVxOdG`AJNGyk(5BIb2P9@U|wtAMn*s@z^ zEBjE z_wsBFvsg{{V)Qm@Ux3tV>Fsr+XH$qB$zt-C5rr{b-z^m?zV}lmOi?JZ0YLx?$C3g4 zmt6v0t&!IOT~Nv?F^myU+^om(@T5DspWj(fD>nol3a1*G9B4qFt*I6XgB72W%Vkx;z=;Ud0d$fI{w3#&OF6*hqdGL5-%lG|KR9sx;7rfK zreHV9%4X!J5TAC24pP)TT0$$hv!I1gDp+XDy?IG_7jvq1qa@84zO)Y0!WCO;^~eDdT@E$29lo;;sLQ+Lm9tkZ6vZ_svZy zUum%g@V(yyjw=ZsU_M*3F2>zatptk0%66iugX|~{<1g4v6uMmi&MPPb@zgUN;+8+? z>0|k=a){Mfda-<8n42`e$}SQ2)(;>F0?Yks9UA1CzeUt(&v# zKF?EzN%Nobxt;Trnhd`3W>zivF(IJy7bN`Q?@1B-J=@%0{{HtMSSrAt(pjpqG|m8q zEU5sz6jB9hehcmAt{oU_z2}|#0sk)pP~Ft=1jpzVs3J)NEq=cz_09lzOxNSkgQ%hn zgwgr@2M}H3wcWXh5kQg;K8bVl@{#>&z}V>4MeI+PoPPcU=xZbGHpYleppIw@Jq!tM z_`8gM)?KZ#t!CzJ?b-0Be{oE|a|yd+5z5BKk4x7E0*l7%G74amuWT?Ne+&=Cgk03Do!k2|I<8H;Jm zg=%JQd(m7e33ySK>`Zsx6e*aVlhHBI?FL{Snrh6YfxDNE9W0;h=Jd5@?;jexX7_=Q zPEQME)vq3m&nLu{8Dm64GUIj|0<08cVC$3sp!vQI{E~R3yVw7$O?$-1DW&sw>1+!w zue*f2xdsAL8KC+vIye;$SoNJ(;CP2q0)+ec$nrXbK_GH*=ygu#gwV9#p5@~00(J~9 zfq$&x*hneI+aI&zX?ozDcBGfjh2>@TLf<2J=TTl>6I5RSB2e`45j8u~Kk3aem}kbT z)F1_SF#YEcOgO!kZl%JcF&pkRY=*v=iNJ-uHRqbU@(ye$^vd?F?>`~;oHv`i}ZBVG-O&i9K2${mzEh3wSh-53pN{Sv=2JB-HTS{3Y2S;0F)(=8sVR zUo*OL4 zD5M0eBZ|1x%UWK@HAyM0e6D^y$SLG{60I2T{Pe19zlKqx#-!{bx+*>^n`vw=F@b^q zwGx%L7CchVer1fi=W+1fN7tA=*3Jt;f}pbpTK5DKYOexBDJsn+<~e^4JhRq{+IFR{ z<9gtU8b_L05YY7beTMskZ7W zjBm46xM>6`s7F+@zK)`%3O`Aq>>-=77`6?MAj@VJm2ywr_h&nIB0o7k?=IiJ$Fo ztqf`=K`~S2C#eznTK(C>&&intlc}wt5GE(_(x27aGrTybsbSr&7m^#!TdNXlRY&Nh zqmFSFLXJs|(o@c_RFe(OvOyBz+F)lutn6IlySMO$?vweM(mp$EEBX=|iYd8rJ!yFEvM$180K0-=S z8X4QW{Mkjj=I&wY(t6viq45g+tn+kNaHC>3u!{Q3@(FG=dd7nHyP2b^)TIF1fn_h@ zC4DTLplLZg_C%VXg`#=)%$1GZs7KEt_8 z{fm#Qnwz>V5yh-Xdr6cECN*8_k_$26ih|sIEU3!RZVXg zK-)&_PD0rRg*5Eq(Tjh!*>X*}ylA=inx}i&J|WB5ckfOxkLaZB2fg}4HJojcTB-%| zHW5BZ13I;B6g@0Pt2h;=9S7|IJpS!or^$1# zZ3~BG=|+=wT-A~ML4__AlamYUedP>qe{Y(A79>&&SeD?t^2+S<;73kxa+l+6c5B(< zgm7>HB}hSEu$D&w5sIVkt!dX zpeYeUHJ3y#t}B@WZf9zoq_=exQ6@~5IXNZCPM%jWIa@yGtgkS%IicWLiaPr+wLNuq zk6hbD6c6NPJ#@q_BjO#O_Bg5?VK*VUpD()Vd-TnlFHOEgI2Rbxhb+da*IlaRNg1zE z_9g)IM-MAZRy3DKG~!mv8n$5p&-smZs;13?bjm=wnB}pt6#v-te6B)SIqRUi-Z0k_oENeMWOUM-at%+`&g+qSPT6`*vvJG?V_q+y38al7 zp+Iy*t3=Eab-v``AFS4$48{;_y#0c zv{q+V$HO+5*D2Xr6Tb^7#rJ4-Pl>MXitL;Qc~(5bUAZ0)%l5cR+e4?etA_tEa~Mk9 z@B`IRuh#w%|F4}LfE4u)h++vXLJukq7vc`D)yCsA>wkIhQBjoToY%cw_V0ptTFstd zkxz4;A8}$6x^J4MJC5%-rwrc2aSx$Gfi$6=7X56&pH(w+>Jf5CcT&*tVKpPQG4A8{ z*L=C(+8EG?q&LFhshs*^`YeM>2cPx1o%zc zUlj6D`EcBmCf8=rM%xSEh&!C{8%2Uw*8T9}>H{gmJ?xZEmA%Kv+Wl!_Ss6!gL+0?? ztp`md;8!Zxg1j8-R~Rvr)fv7Z`$VDh_-yXH(Cm|Q{jkNY7+Bk|+!Tzh>*1O=S^oV^ zOi+pKIM*H_Kv_3HaYxD%CcQ_dWxLDiuQuO^!n-*PT^H%$pwB}}tGCqSz&dX8qZV2O z_h%gv8oJW~a`2AoR)Q~P3xQVXY5{RLI*^*SbzAgcIP?DI=IHJ<>osh7lPW7ogR9!& zeMABAeGk?0d`K!hjax63XK#-yA3#^FsP-mQSz^WeLvijq3E8^!isbmRh$z3uUS$y@ z5ol+b8$(i3vwag~Cm=>b&oj@%vN+N@F>B=blqxq{Gba1#d?b(MP8s(R*6=8^gP=Hb zfw%Jq-VVQEdck=y6bRPv@-yfrb+-<&(^s9moq6PsjsqhZn%hoYE`JY$qQay@G#N2x z3TT%f8SxnN8QRmR_W2@jHiK`~^50KAVPR?mp7AbfmFWVK@oeE_0a3=))NWq@G3oaK zkp?T=*=Rsps%qKJiq~STKm7E7N@3K=AGGPr4av$AdNKHoB0%lO73ge2Ap7zq7T|ia zpN)Z>Bcn!Di6r9ja~-XR7PqzQB*?YPHSy$C?xXXZWRY7AK7~MoGTMuOgXDVn^*1XQN@VJ zS6-gm8@y;Pg?fW!hE(Fn-=VWYu9 z`zrDhM@x<%0Iuuj)8?ggf1O5WwJl2mY5E! zu^+`kjA0Lq;luihf%0RCC8RTH#D&6IpDf=Fz|ofNONLikH3`_N0@(bhQ1ZsFYe+Iy%nYfGdIj6v=EBFRS)QJ=-eT9;C=SpEv7qcxQs|j73Oa-U2Uo88b8M2b3 zS512P3kRw{a;-!}&r~6-lcO6ye2rf{zgzwrtv$a_=%4)9_0j@yrRsAViK9)P_RgHp z8_njSykF_)f1k2nrb1cOc+ocfYgx(S*B4hYqnR4wfz)A)uyC!Xwr7FKCiFq0d9&tU zf9I370u`A#&Obo4-_R{y<3YqPw9AFZ4L@@2=0bqzLHMuNuvS+7{LtKt3E~|`>q{q% zQ@ITqxq%y+mi>2?7O{@%e@pIA=|Cr9YdK^7ye#Hh>`xSFt~8NOSfu>L7rp_sB&!kO z_xx_?_ucDIji%xm6r{M$jtm?)WG9Vs*%2B}-hL9QY(&td>+vEpjImM#8d1|DWPkk*--7!?{mO{#BDY168b++Zoda$)r=m<7G+t}ToPnslMj2} zzJ0r{GvO+P)34OCsnyWH;4($JXS_yY2#;aB&g+Pjl$iK}@N!qMT*XG&mLLHPNbS55 z&y?4cgZipxzxIx53a9zSW6q3Ji~J;cz1>%(gt`Y0Fcj1SGqWF?_p1_^H-OS{0n5@89}%l^Cytdt@_i7pE7`SOhsxqp!GTH&FN(wZGPUX>wlK z5D!$09Ok{93wpW@X`qd_LneVOBEn^zhoKu=3=qjyd(p^bO{5Zjt*_p&(&)>%TpNQZ2`jq7i5;Es;Tm1ycfuA_|U^E zoF;{28YiYoa!E6nN;qfHBacd{ve(vn9`V!qPK~RXsG)Y`qaTBO#z2KR>FLSL$u*&t zmeoWelaV($xEEmp66J{XA>^gGHa~3-m8JtO@_}zT?1<9Iyrzl}X8kTaxA>mZdcxsu zj(Rhz{_Ut>wea#DeITKVmZs*I*kCvtrwbo5d(T=&Dw0qbS9TH`T~2P`vAl_y%3PVz zX;vEE_Uvrg;BQ7@d1qQ&c}xee`w?sn_;in3O1jpnt7=P5icbb(TU5oU}YSKpF;uBt_A)O*+WV zK~?SR1IN0YM%^rM-?7z43dN2lQx1}`eqB4H?lRy@$YK;qK6H(NuvA#@k#L$%d#{zO zi3;5=3~=LtfA6aBd+v|jLlmDqVKeB|^YqL7$u3{AQ9@5u5>C zo6PSV_?B+$O}7HIah-9XA<>y$bK$#QxlUuURi2#f&hb*^D-y`5g%z%~y&?_unb2y< z9g~#sK%>(djkN>O739`rWhdqLwzy)E(rWrgjNy4hBWUvGtF$H!^w8-n(>fXV9lEqE z@}BR5$)?ik?Pu|L8#lkIn-A`AFEdtV2YH_~ls$P7G7MpGinodu4ly zco>9pX*sheG0CJmpVUV{P582N_c3tMU>c3~L8StqCR>JS#nAvofQT5=$+1*h*0fXk zwXHKm_8~M}64bJ0>U8Rea*7(GnEB93{HOyQs~DsJoH73i1RTaEd?=ez+P)c+WPfGw ze&wYKnp8L|SAWZ0Z~$d^{p5dp&4rJ@DJ*?XRo4H9ueXk?a_QcODG5nIxsqve;IKx-(erk*UeDw7xBRN>d+u54HT&Q?ZwM~?>&=Aj3A@?ti?lZvj?ww_YvSv_ z2d-2AczqVv`DJFQHyHX5w7;*YBg(} z|E*H87=NCGMupP~o`>@(j_o+qyS2wa?jvCE%?z?^T{nh3u{ zaj)qcPqxB!_dd{B@Sz|T$Ut9M>4SZcFkQ(A>}5+= z?8+l)IkD*iww^s3*v75ey{$1d#`^e3rU@wwkFeLdwTc6IKwzjy0t(aw;D z!OID}#O#K8{K(=TkxZdE9kq`KvvtJdcoygp)R$$&)>ltEK4R&g){$EcC zd}DT?X~ZW-gqpsosv}#26(&l#JXW_Z9EM+>UN)B8f2nfNoo^DsYerqVy^maKKa<3v zS7j>p{$UdcLA2y1+(T}UJE!gvJ?IC;@163Xd=^EM&Ta3<+F-VsPHxp4t4`%!I0bH^ zOp{aygtZH1-jhNfG4P`@x2JRCDDAIIwEYiAJ5tj*$z8!L7D>|4pg`ak5i1xdJ%8Sk z#N7ZkRVo&1&a$BO(+xozE>0~1U$W6+CnkOtE2$xMnvzIPPG*x!MXP~Y$qs$X#RpDN zu+-DUvs&W3cmK@pXXI8`Yr$h@09Ga0^RnCzQ|(ADOBye=-wv4T1#h@78#|L~j5s9) zN6@uw%3yXcbYD5|ch%DupW_EW33(S=UF5fd{xWIWTyB%ZP5t8+|FJW;v3L3Qm^4D$ zrG)%$*iXySHBoP%5@89vA+YzFUk#NS4ZJii(vUbMswEZWsfYUYR6-PAO4@4Pna3cL^ zO*OL>{toLtzr){C*7}+EFo_L85H(oj#+v(mA-7<%vH&pHb7YNFt3VHg*sk)VX;ci= z)6N%V(E012|IhwJd`7lY*32hi=IH{C>W`gCVq!sF;Hd6`?W58z+OaA82joe;hbcRo zR=Lkyz49%hN78n`k0%bGys>b(h7?JE<~kkm3DMoJ|Ey~ZlEunm{}E|ISu8-PEH4b_ z$UFYHz~ITLYEFB%*ZZqELBGz9nrBZ5!jXpARtNJqhZ25ts$WjN0{d_kWEIU1S#E*$ z1X+H;dqb~D_iK-z|7Y$+O}vF^Gr8}8>w~ldY)3+7+kLkVR<=|CpvFftVM-8Iy6r#U zU;G|bw)DP$sw$-uK9cRcF>H^p4(&*@uk(E6vh{8Pev`4h=I8)Z1WtQ1QlQFTc|K+H zy09OHYdrh?XrN|!EHFw%DtWH#&8lA&$Be$Ezqs{sPqGqwtCvl+pyyvCu>T3X(DKAq zNcr>bZle&Rae5n7xVF8!CS@=6St2eS2yG|Ix;j-dP+%_BSC<6;uWvz|1*d1^jl@JV zV!r>trVo!=N01w_hAQT@c|Qe>jAeNrM}7u{g)h&xL~V~+IsY*rkXQRp$gLawYRog1 zf`l*S2BKtpNXV#3e@NGm#F4kR~D68O?1DrKfk_#0mPk5 zS@7lGv9pt&iqpIbg~+>9H(5{^QD49NHD~#_<@z~s{Py~5UOT7D@%Gt8ma6|6*x$$# zt^>fsUQaAZzg_BcI$c~QvZ~&IFuQl1_#Z5ot=OJbZ`g!qJ;gO^^ zmV|wcu-eQJsNHB|fmIcPWwZ7$xJs}Z<|!Fmu3MTBph4>i@ds#yE%wo85S`{bV^NeK z9vx)>xAG;zt(w`kO-*ME5&q`!!8f90FFSou+Rw4m63z6mT2DgJLuCVkj5!B7JH>k}lF%VC)OrBAd+9&o3Egy9ZepDiirWS50M095l zO+eTE8E;*M^ifi6x>Lf4mzJp#C2Y&%EIc!z!Ww%dxAyeD=t6w)*Wm3l4Ew$nm)eYL zJf$aE4m62Bm~uI%F{^>GNimpSTAb>d45UTcRct=+XAjy{T@OFSRb z8_KKNYLu)x-w}R+8~4l0z|p77;KYwZ#I+D-B}_*tyP=hJU1;GU$Tztp`jz+L~KdsxRMK)?XSJbzDVCitfr!yHOw9_ICM_*Q)Mu@4l@lDVf)KNQX7R(+2r7O1Lk}Ggd5j*I+}= zTT5qZ(LKQ)czbUC?kc(6p->;EtXp1Bo=l@tvt(2f2HC`QJZrxR=+;K{DR@H9l|R9Gk~)g#8ua&>T7#Tzsz5~n&hk3pcU0X z@5S-|f!tcOaEW1;lQj2I+CI;yX{6Ro@x^FOiXagmK(NTw4q^@$xX9$a^#|2fXq4B! zk`4ku)hy&`$>vA|R?TG>Hvb~UO2sd~gDVbU(nsQcw5U`{_Vl#MiKWr6ws)MwXmJ>D z=tF|1$F;3F9+u#lTu2HWAeqAf%=jZ2_6KiiLNYK$od|%f$P2`AtqyOr)p9M*#;?Ez z@lmPYKOlc_o_D5{!$|A2u_V2&D?aP6xH4Cw(XrV0WWm!AQxJ!zo}a2__u106#jln zD3eJeNn)eA&v%!z6bQg#q;b9mOuz88KnuRtzc9%a!u6HQm&JXeR9UQu^9k%>Sszg# zjjs286M%y>+G9Nn1$&WVI8V>l)!AA?L)>GF#berdvsSFR48e9&5>%YDPezTH()hWGp*w^E{>M0~0ZDor?j|uBohKjHS_n*ijfA**K|y2XBTx64!g0f!j0eR9#WvrKlt)0}!QM1v z#X0iGB|1q1-PN}x|8Uw%HTJFU#BGtEHl0zky`a;5&_zK-%T0LBIxHH@#{}9Pr;-8uS#Mm$f)Se zY~>^OKWW>aJ^5Mv7g0c5!}AL$!wp689h{Go}LW?oOlbeFMz8ljC6IRslcVg>$m1prp|~9EZ))=t1X3v<M(KSEL9~oqY*Z^PZj-v7 zI z0-X~r^XjX&q$>0eWH~$emPe_FR0C}f-ls;B!Ruu{&<*GvO)IzFwd}M0>~IigZj0G$ zbnKYc8{7oal<697~0yjxtqXiB-P`2S@?G8*b zXY2S_S&P(^#XU*hZ%zoK@0TpxC2h*;a@yGNzVnY%uh8!QaswF7jK zY|n%r#_V+*iYe=EgXPEp9Lh2X&U$|ab|Pr%KG{WVH(f*CAq~j8U9Txw)$=hk><2P~ z$*D=XXbs$VKdR&wUI9Vn6p{K#4U!#&RM10p;9*8?w{GQ|jMLpoHYaT!454?x7#@R? zQA|@={Gt~i!=bMR01H8=18c5~XgwD^b9b{h1+^<#+<-r#2pB^h_Wm308dA}c)kyw@ zL_&yOpK89ra|RXz*GrWb3b;Ap(A;uTOT;9QJ}Xp7yfl$mVoB2z%&l5>I;+KN$3E(Q zVYt@{ber=Q6qc5k`vEnjYl@mF5}YFrr<%bO{5)xTDfTmh`%8U~4Pxv0<8MfDyAB|6 zp9P)9{+RgxW|AFpYt%C*Gg2T(i8cFOY-jdbD^01?3Ou zxP$kJ0Vz~9n2+_7;0=^9Z`;Wj?+UT!q7^F1{%E_j0xE7DUFDtGSE*Y!0;VUR6J=iC zBiErT0-%cqPTmNp`A&g+INF?$2Fgj9>IK>#=M7Q1s=6&f{DAyUayQ}(Sdl;Zh@?MW zU&KSz?Cfmj^~RX6(OQq{aW5e1aa}}?H~|WNZ%+>_twsu|0Uf$+vc++-W~H5E&kl(6 zU6a3hB55Ncqt8#E>ZrP!nleNW6qJLTn3Pap-2rI*ahV|dF2BpSoPcCj^wiTt=OpMs zj3KYW$L(5Kkj%pBA}*ZyE~=*E*&M|YD~EtPeFY$W4(;CC3jxn_*9oXJt%&Qlpe)4? ztq?xIxK4*OfmQ<&O3CQ@Fm)0kQXCGyBjcq_8AIG_&x3CYl1rjpI+}V0jdpWnj-V?8<@1K(I`&`wpbHVD5apH2MyorFO~AI0No3A zc(V746Qq};qosAqY+gEo5-wFUBD0Pc|2u50(V(r#7hAUruVub)Z3QU(`|3UTZq~}! zwo~|)`pa%kJ3H3ofZLg*%Npd!0!>^kTy5r6rp!aYrj%ipP9LZ9UW?PDzQp6j!lf0P z49Wy`-OAUM$ZhX?V`zEJ6`o0As){6zgl5Tjj z#eu7Je&$RBf?o?zlhvKl3w8k|8K0aSSD`TEy6b_O6761AQ*YWQhYRr-Kpoqx(T)++ zCa*93BkES{t*@2-9Et>EZnPjXJdg1al$fmyjRX|I?2DIp#a@7QW(mrz*Gt6kJ(cC| zMwGZZ?k6$n{QE*uzF6K|os6u9+Nxr0rQLvrcdbpjANJ;{^;W?P+|^~Ez?1AH!wcSM z+IfxhwJam=n~KNwBwABqX%1j0_>of+aea-U%?~xX-Ab{~$j{}f$@#QxA#t6nhfFW6 z$uJ0UyIYZL7U+hI?d4pzagP)uL9H=r^5a=?;Lt3(_trBHd26&djYrNLB6CICj8tvi z>QJazpJ0&wJhW~I=>rJMF5L0S zd3*tvpkR-9V3k>3{6TW#bTf&V^+2Oo_rz`6Yx3^^FYTCA&V|p_f?O#WMyI)XHVe3SnkJ zYsz_OjaQU(g=K_SLVR$4Lnyxph^vr^lW0N@4c`@9VISaFnc@|}gC5Z(u#yxVV&LRm zCTo7Yq9ZBE#XNSrwST;Kb+XROD^eV%niF4e+wJ?unj3Ko zkd$;)B1DOl=of_mGuS2K?V z`lEqCWza;3Wv!BhE1ZGVV%@ZzVluo035RrvH3E)DXS^Q`~PF&xneFD{M+J#`2%_wM}0SS zBA>IkSI}s2C<|}upH+e_t(jTtp6Cf)8c!$?A`+-E&T;oS&T|maEg1plL z2GFI zj%be~ZKOmJKw=g0x5@O&Y+`RWSx3Aj^9whg=7PYuV9r29GernSX_%+g_C*!vLkL4B zH@dO+0>+3 zh~`m8AWeo3Z30(Yg@|$Giazn5D_DkGmn*vHQQVwj)3S>Y<+kHrLrkvo2Y}&g>`-Zd zPiOXYw_Wl;nEt+=cFr?y=O0gGKc@s4CHWZaAmN5wh=2rXB(q4dZ96DG>~;x6G=eB(EEI`QD;0LytbSAkA-)boff z#*^tP@#pT^v;Dsu*$S`OQ&4)GzBvsCA1 z7owMEp$VZMsp_cO`gf@gAJ^!`FAM;+Ag`e+d#y-Xh-$xuZDwwslJa=r#w*()M`C^F z0j=DsT^zMBs1Et)5vZI35~0eVC?ls;nlk(JyV9!Yw0hf|P-&NC9wJ58yt@ zrwATO>v|g&(E*LVoe!@kpxB9##@zQC*)*J>z_&yVN-~1G5Bz00=T1dO3CbH@K}2Aqc0hh(T#CHDcR{nk*upil zXuJYzdC=tcQxSBsdd-ElG$5s(S2J5FJ@%P+L-Z*Ai;FcBglh~hU%fhz*R6k7qPQD< zDRK$LMOSUxKfE`LJA8XsZCDrYlH>BboR)Zb@Dm(**_^g=)n#W77xJ+s6!@LhzK@-sET1Y@F!rx|$w(lq0@RYX8= z)hzQ0ALmRj1dI?HWjleI8a2)rr)@sOx<=l-fjTcQ%Jgyo)aoA69npTKG3M44mGxQz zU-{)sgE@SQx$R^A1$ul>0S+sGlWA@nuSN+7}3fhKu8CmNT_E==}P zNQ!NCtc^FM5C1qt`a??3swKYxlR60m^M`&#$7j#7c|BZhkeN>`8frGVwZgaYtPa!7 zYk&}dFP1x`6IY8Uho9?uy-+7?sIEShyL^+5MDSj^-x96(Wb)xuhbx*&A7f&S8_x%i zVmd4Tse8TwPSS!79xP{I#}Vo{daD@-8|@0W@u0;GTv7PXEEXNHSu$B*1XWD~{Z6hQ zK>V0xKerJS%0Z}zM)y|zMtd`3{drrYC zsG_bf7C*Vsfkbaxx+E>Ja-DysTx2hEf36AyJ|ng2+_tSt>|BBq9<*FTgzv7ADK-(8 z7xDapw6YrYd3pe1cxl$l7M(-5rvm*Mp`2Ff`RmYrJsMLYt^;$rIxtCFw@!^KGbYQL zKh+$cLH$dJ>D@lw&qwpx4ZOzG6hN1@dDU`YA_gBv#nSs4U20rD1DD@I8!FW=tK-rl zvM^^*hX^!Y3=0B-pGLMLRp)4G78&};S$hOWqKgxz;6&b%!k@hgwIG7AH5Om%Za`w_ zEYRTb zs3;!Qni=S1s5o`Vb*v6DvEk(9YvXd^6j##vQ8iYij%ZT5C|TAcIZ57?Op&x4TWl;c z{#mkxTtXc)4gc{#aN zAT?(m&XsTJ%o**hn7##XbaWa#@BK<@NsC)Znt4}rjO*@jk~os; zhm*<*8k}GOuX{+tPS4h3o7|jhgz+h7haPLP;WTx3o!1q7W0U%g9CFVZpR4vN4J&Zr zxk~;LjbvchAt*F3nv3BKN$Fj^#bH0nBV(e`FBmBljE>mF}%(qwE zDMFpN-8REgxc2HzVdnYxuX3LP)`9*$6+&1vgxvB@{xeWY#m8?o(vm-+x3GYCoc#{) z4sS46^i2q}YS``ZcU!p~Nm?|bX+R*7>otzQRHCmRzn`qmz&%|+xizs3t%*fF&&--+ zVOl}bX~3713DoxzZ6j$)(J8gcXe?fT<5oX7e=!1SM$JTu$&+m|Y}=Q_SiWp@H$hmi z`DGY<@O_C%FNOAq&d~1{y$WS7$m$l@8Ud*EnVsF%j1w~J@I(V$i$QU*O^em9WI^qL zlH&Y^Iq{~I-^VyR4}TqAEp1AZsT4FFS6nq4ae~h0#~4p}YbDzYTkMiB~|c(h9Q#FHiKfaRoBXbzh03rz*OeNh__*b_&g% zlb>Q?)x~!xd8-A@E$APZZ}}g=d5>ycrW4^VzCh(6-fsLXGah~1xXPy$`wIn=NFx-v z5sN9`mz0xG>K-ZzmT%-2-;ol8IJ3MKQ*~7ysP5#=yQ=$S@FcdggLzcjXqAu3~=3orDJ(l(b;7PHYcKlPL+@xo$=Sjx%CxpJA^zCfAAKUVS?xqLC(qk}BHP+Gt6<3J-& z?PAlut(MZYhSA|S=wLn)l!SDp1S9EtM78sw-r5WE{A32zljQhpa%MVog$6?b>zI~T zzu+{;nul=K(Wj2K>(~}=*B-WgzFAR+@5(m&Y@w-t!umYK9Y#{osBHdw;0&Fl`Sy7m z5eKq*Q;=V1xW3L+DnujPD(+R2@3#RmltkUBdvL*oFVou|sU%H2ewvS?kT9A*51k;= z5QkIz9C}xZ{d=F6k{@tmW$uz5X(_%8ppY0N9t^cpcUFY?Cm}{fOP>>x%t@a47JMyo zCB0ON~#(C8!=;sC}=d-&xT>KDwnKsE^H1d&O}9 z+Oe*nZjS}p`YFIJy6j~g{PUjB8?~Nw_85&ipXL!K_N3RvdFfi?XrcS0Ltf8TDxrAM zV37%lEELoyUlGG@U0&Lw!r1Hi7aW0b%$3`0X|c~r$kn^r2m6)%O?;lBhx%VHiB@td zc~Bp8N7e?qxH;I*%-wHY5BPm3C~doQoCgi(+3J=~!k)GEEr7ovf+^Nnq-WI0>T zeK!gD>+-GgM~zqgjntuH3{HOnyS#o#d*Hh6YtN=f#zuju<^yb&u4j3jd%)Wf|Q$n0X! zp+fwOx2AcfBsJrlUsRZ=UTNOv?;v?gJfp&M^g(w!C z9{zf&zS4pjAA%-;Rh_cUs!Is(4B4bQ_mD(<1M}c~{`mKA8-5p3FKG#z-;ts~Bj6or z0yYN8Yu;`Xk~0z$gE<+8lFmeuk;1kzUjlreq`9{Pa-9!U500vyx8_y0R-pd^t>S_4 zsxh^fvRnPUcX{Iq)o9s*S>i0vG`}&vtD#U zi7Fbwuf?y|MM(;Hm}2}iZ#{}*7}dl*1#O9};_shdTq|HnKWEeZXeva(rAy%y!97SG z==9X)rTKJ79xup=XSI}W6tmO$K6^IWy1lB$k5g$dq3veUAh1?>RfQ0}^9kKDXlY;i6;k^bMAtH%v!df;(>=PNouc zQOtd@0CK<$DV;H-R1blml}ejo7>hUEuXq7l6|XjyKG&`qrnl0DPubI$9!2#%v5Tr4 z;6Z-iJKEY4Y#?on=`xn>$bHfF1oMP?Fr#mwRempjDC%_Cf*#iwi*C22%ax%YQulUE z1u>l_9qK0%dhtE%u1SSsI$8T$dHKL|kifL!X8%1BxcPa9O8;y<4|dQre(TJNa^>?a z!C#Y>u}DM`HVZ4QFv-vUX;ZKQQ_FVu+GfW)SBpt|2qwtUi)egQ2`gW{45)$@o4D zy>aRzkaN&PS#wCm=;T;^+2k%q*8Yy&Zoh@p01bxUq$hjs$V(IOW7?#U-A{Po!pzLf ziq_ENjP8>gTn~9YNT1gEdU9pR&L@wbvmD-QJ>)iUcYa}ihDAs108%^;nK>eBtwXif z)l$hU?@j7Cup-j{)#@D)O{@`X(2^DdhA)m1cD!INLihsScKDnuDSzgQjD~t`#{#v6 zmX#mqeu7?pzP3T;-3d9Kuh($S2|gU9=(P?jwBqy41ugnBUazm?CPYCVlkU#kN-HPD zS#7n_chDn!!+Hd9m`u=*2ZGn=)PA0|I^((wF~;ydw8kl9CzPOXULgBqq*r1d?CILf zh5XB(xb!+x z%K!UrZSZalbmF`1Oe;MbR~Q5m3Wo6^HL1qrMefNNG>mb5=#2p(!pN=4J+60bQG!yk zHBZ#H9ic#!O%8{{pq6Sk!L-vr3SUclJCqOEb8j(Ve1KRoGi}U^cy@n6M>nT>-dv=g z7(sI+EJliL!LIfF8RH-Z9+k3ay{i}#lXTZ6E(X!8^553p69TTth|c%MC$sfWFRqX! zKGlqe61LmV1T(QCnC@(Telq%NRMxxrOQ>TfD8*A#!Q@gHNRTn^Q8iJUI&6L~DRV!# z?Zt~1*A1s*1#GoS}6qH@B^7u0f$F zCyIFwbeYt!3R4RU{~C-Dcm1<4;V8iPa2PbWdfW1X`dbYL5@roOn>h$i$(x0+^vP(tR}api7_HHDj{%O_&(Rmcmt*2{AGNX2U3i0{o>8GepI{@&1Cfb@@dCufizGQ zZz^hW*H%S^pD*=_0aMmg86($kSf*i7E7xun!5DTkpl2nR!=T{-kqDmCeIOh&xG#eg z#&-W(o5^oA$i^@`LhNAmzMH@ij@Ln-^tv$Ho{6HjBh7h$;dG2kkgaJ@Ps2%aa1LM~r*fi4K%QF|#7I>JAi+!l@%9Ttl4jBHdEonn3#_S=1Y1a?o+g_UDVqxN z9l9y3$$cQ@8h)NiD611W8mRY?$oAd|bu|{(=y$WgCD9^yi$`M@vH(*(fuv+`lDW5A zN$ZX(LxQ>3pf>3Z#gN?UI+Bp4kp1oxhO|&S z;BQ>M0wh}`LhSMX&mqwB5K42du9C%}3{BjcEM*mH3;Ocx){zjT<#yob4AW%^HH-sD zpI(pvf#O^6;^l$$r=?m4t#MCPMkXhz2%Am1t)b82km%iT+RA44%~o+wh~stYpvrE7-l} zk=dkHxE_^^jbBhaoBi*a*EtwdFMjwey8LE)Z-y?J-1XGG;`#ggvfkWmKj6LyaP#Mq zcAg9Oig1p|cz3lkEl*3yRiKgSh?Dm$NFz}ne5Qm^?n`!!c+X9ydnk)RE`p+u^ShsH zb1b7$HVqfc@p{_yK`-cU={#J6BHpFfN@0Ox?B9pF{{1^NBE>J>VBc1Qp)7waCH1{C zkKwRAPopks>I1EUjN3e@eL#=pTTN8VP*3*8a_i$qH#fJ2$|7^{k&YbHof?9Kgq^qa zfj(}6C{*2}thft@k8l=<~Jy-y*8>etP=!!9#=ZS`hRt%L}pXt#@}ZU8H|MsF$J8*3tRU z7Ps=qn7QbZK2;c5%0Bw31=bG8le~v*U@@1$xb=An4f92UBcp;>iY;K|PNAjT7Njvm zhWid0opb_8ZP2&9tTr`vYz{4GLOlQZ_Ly?SdtzeZsFore?)&!$cj$xN!dTg+{caV1 z>7CaH42bNtsjWHl*vIUJdZCtBbPV^wtzK!_qn9kd)%Ze1*zWt{;?l38CjqCZ|jey*WxH7?XygO;qdf^AfKf1T|eDW)1krV3C>dx>(6=E64*=DVAW%Ang z%^-oO}lr2X_af^_PH^%rsG+E6iKM0|K8F8?0nfwl&&_byJ9b0BM$datQ zjdEJ`b9-wrNeImrPPk24v)vkyWIwr${&0J4$#B)WKRS#x;(x1RM|gR=6o^aBR^0*M zi0se%vaKgENYdyX(y-)R!enB+tn#NkBvP!btg4}Xc9cf}V*pCDmTskWFVq+CyaOEf zAHD>HcRcORtNO(zy5*;H;VQx|890f2k}Mk(8R@Aiv5|?kC!xVAOf913llE#pP}sXY z1#&U3jEKT9*7nC@rIRqe8fK-v$9?%IxP48ZTe7*t2}8q<^(q{mCSrS0G(~bJ9|6`% zLr+5}|7qochxdfMcQE`FZZR(I;Z{Lz?rk?3C>f>U*?kcK{U-ic|8j}^RcrI*&R9B_ zqzSE|4d;h#6OqumjTK2_oBDsRt|5a@?W~SaQq-~=as#OCUnZVrMF$8Vx4uS$ z3y$FUAydMJhK6oI<%?Ykg&%$dS0Pf9B5*w=L)afad@1_6bzg>~GmzNmDLuoKl!Tp- z>y9CRK1DzcIRj!D?wt~bgnED(4-LQ#{QTJz#IKpW+{htlgHH#aUtgN%Iu$du3jxt(EMiFY$|i zS|l9_6-SIPcTpvnXxWxN{t($1JckH-O%q}-B4DC#dV77M(@1l7CE2Sm{Mo_aeOa#Q ziM~`xp{=w~v2f7kG*`H))r~u7T&Na41i3CJfgC$fEt|bNv=0ThWpNd6p%RAE=irSe z_Q{tQ$bdEYAR$z>;crpAr;*aUM6ifVFMDIp7dwzzXK_-ye1-Y+0i6^I&@NBW0FN)^@x8F{D7FLB%vdNPN^}YlX;KEGfa9u4+$Lm(T>PwGM(=j;m zQSi2uHpeC3g7nH_V%>6fQ2xfTj^Nd=ibOIoMH0i__X0dDYN~Y+>tQy;BPpDGgF1at zf475E%mCMg6|yf_Ty)I>!2VyKdwA?_>-g5|SBsU`v~BPpXHv%sK)#Rjd?x63cE6kb zp*`(^&bmd-TUO-OUc3D$LxM96O7SXON&&Xxn%aVB%41=4QxpZv;$U#mPe?8YAxCPevU`Rk`dIba`10lc9w#3MVjXu+m5=!W%8#S7$ks`ol zIHlnI7zI1ud8zTWX9)7&4svL#hRH5#iDm4E^pysC_HD%X>&ZJDy$Uv6^0SMT#B{_T zgg^Cp^rJu}gj`AfSI5!@tZtYyL&qm&q{(XIe;@K=(%o%yRP-0O zek2yxW@i$uJKYEDz$()_6cSau*-m4(p6BANBY2XAL#WsRkTbxUrQ^6e|J;&Hut7z@ zGwf#R6})ropn(Q!hhwh&ZxS@jP!#C7(~J_Rgj{7p?_kk6nr{1JFtRHj9h{QcOLryE z1G%+6@SI{08(BiE3o~ra?-LnHe6o!}Qk^;^sq5doHgzFb2U?7jaWXhwDsnR?ID;fV z>3l0BzO_!Zo}^E^VA~Xz$m4PP_9DEB7$8iMI9yFh-cs#I;l|BPYuCi9yV2dqr&7t80X+P89)C?Yn zb2j-4r-3{2g0ea~@t&4bC^%k)`rrFh9(acTHOZD1r=mUwX9!i7O~+Y392Smf-sHF2 z#Ku?wq*>x8&v;OsahzYkyzItTX3N6?2?#Vg?B(Q6sRSK`7fJ8R_0%7Y{AyPE+Xgok z>X^QG)_JbiUPpWl&l#scrrc zyRQA_WCFX9_af=Z>BXt#$Z>1Fvx-=aqUXsHsi-G|?rPS zEdfHn>CyuR(2gha{k+QBo#SbU*71w~JGIr_$X}P7iTiKFLQlVUD5B?6Ow{-816<%+mnJ!KjBU&0|$R3>23` zG$LdC!Nj;e_VBye^4Y3UkY!}-tnRSgf+e4*NrAA(u(ml48I+glocZ?kuqGek0V z2I8B>sK{sD?fV%vuGA}NM1+jlyo^ojuc-H|_OPK;aMoMFUEg2t&il&0-}NBj&}KWq z?OKPf&rHGoXRdK)VgRf5w*2Dan9{*4EM8D9YN=M%MI?>s_Nez~Cn7*AeA`u>^Rm z5GpyDa-^fnWjL*z-8YVJir+xQ+7fy=?kLL>r=9VwhUbg&Rs;|=Uo)6eojol>E94b- zfUgA?te+ybBV_*P({0c4#W`f17Z`Z!l(m=HA_kK6!a<|H_2P$gShu!zRg?81xJyWL zp|s9O&QD>q0fyUM7L1iHaDDE6$v?Ik297LOR zv>ACFJSr4*VVBf|FaNLEUKsV@*G72S!=fT`AIZ`t&@X|s@!lTp_pSPu!cI%9m2Mkl zA|9u&LQSxC`6?_NK@_4-SrN^|Zop~g1Eo9?+H#)UuQ)=Se)(j4C#RMbU+zkFTgFN+ z5Qin_OXqJY#8u$LywmPU1)eT`A!{E=<+?(|hd)`3Z^(U=#ByDK9Sv7w`}MeM)$Vl% zP^SPKDq$@Z7C%`l$B*e2_+@C{BOv4TqJdD-Bd#s8$$^S;$Iy)<(G0uKXR3c(_g?SS z;M*DIfA;mmj0hD9mcI_>|Eyw!ikxNkKj)b})*@xeaIQ5FTocpQ6Tjq3f`at$FGuia z4-x+V{<|G-ET{baQNA-Jt=GaK;OC6XKaRuo{dLNYyj8j%o^D~P8LnS4WfqSKe&@UM zFMF5wHc~OkV_f4`U}uRQ37IOVlDz&vuRng$&1n>abfk4u%(%z=8T{8zrSvWg{B=SM$jg78d-czA zXCBlepTXge{{MaxXQ~&)pZs!=Z-eKR3Fd=H=AWmoppoIlaQQbSgAeIP`}YBVGY{Ix zaesbl{?`Uso7~o+{uNCk(t*zX(c~z-CoYYr z$gL{fY(-{zM~eKhSFFvbiwuWuCCiN06*sDLTUkR%V4)#RY*H82r#vk;`LnasArqVIMttHYY8rbT3_J z7bFVDD)vzy7FdTL^waH~zAwNUVVzv)WnZh_`*x6g5W+B2Ci9mCAB0!asWT|ij5DK3 zE2#yRAhbLf96ZX|N5F#q@aT(LzX?&$C= z*7A$Teezj5Gb~E%I>j6|(>!lsl`gHAw0LX%@;EZytf`py&B-#m8_1+yBJ}0}qVeXu z(M;=Vj@!+t1=0@70Uc$$qhftFL`_ccz_Lo-80m|08E^clDCC9l!J27jf2H+eX1a80 z#ROs2lgOcPzAbixL&fV;Eu5k?Pq;^Q1kO1lwsR;Gs#9JUeQTrq=WO^J*+T2EX`ODZ zS|VT*8aFofppR%)|1qb$-1FcFCCjfE;^M<5iV`vy-~8Y1TC z!);xVzCzs5iP-)%u)=u&Te2)Vtj`UOH&fQH#fcFy(0_amM?40*S1|hKo(xDpO2c3* z{q`)=e!8G7#`g>JsK=}%`76!&lex;(9GX!+*}V;^_EH^}1K1p^=kXkdWvqZCj~`BS z(qs3%1=Ng)Du$cGfsh|rz1?0vtmbSEOSBN&{M zTp7H0|_l&t(!)QB1ygc5z%k*BhY%d{hEL z)PN`?lhT#`c3a4A8KCe-?zAl1QBlr@fU*9rp7*|sL%x0-B}H~9lKsYap&e$_Hy%3V zu=;wm)By!*Mv(XMSenPQ_@Z(Nxs9b zN&SeG(*WX_YzRyy>W|5UBh>+5cu^pD#N@ez43%XPm+Z7=QI`tsR1pY<7$4U?*_x?? z8AP#vIVT|@mLES*+j9w2bQM5G-w?Eo`cXw9AFC{9>GCIPzzRd6t!8?dq4s@qFbk3( zgBd^%uIi}Ydvv*Y+UsTso8lIUoe84Sbqo_fN=s$O5=!aXbCQZ46-=QWuMK2{YiM)8 z1(Qo7u)&Bi{wymdmP0bXG$YjYZD2crM1s+?0p_)m^`Z>@)`|0~mv642t~WNk>ABjM zp1bW|7GEi8CJx)7;0)K5tD%P^T0SI|d$@`BXXo7^ob~A%FgtBcgt&t>^|{6{4lBr` zKvV^#s-SzmL>yuRJpeuG+tX#g>ND2liQ&sCB7BD|O=P|8XKLBW$Apu{Lq4m>P5n9D zJ(awRkJZL!bx*b$LjODxxHYox>}vA@jSx_m(9R@Yjk<{J27;~0RKoUz%Hi;x_0R<) zHY_e?%9orT>a@iX_!$uos&3BIT`0;Vbm3upp=Ixagv6)7pw~9x)0B$ZAhzyI=FtJ2 zII|3-UV*fCf&s%-fvpA?`OsHYQ6uMZO`_T7*2D5wIe~0T-~tWcxY@$zNk~W5I`qN& zQEQHLbj{Dz3w0mnl#FPC$FWh1>T~GVWP``O8o~MMWivq@9<+Y55VrYPJJPhY(GjPP zQF|#7{m=d!EMTPES1#>^_=t)x1oQVn9d%gaFnJV+sO9~K+Cgu}gI}LM2X!i(G_AAJ zwPz6Rr9g=z$Oa_dq`|F`>vv$o#GtixPUdjX*0eY1{41*-nZ6x5@1+K0}hl44@XA zAY@vHC(W*h4xpR0(`x9IF%&2UI9XU&Y(x|D+eJOjh=BZwnQw+l@Zg`Jx+2<8<^0=| zm#Kd+6e^CH^ch*&*bH*AT1~RK0Egl8U&amIbPcTk{U)#x3@GcK>C=we$m%BYz>I2a zrmEEH^0OUFz^yUJ`u7TaCAzLOcsFSypOAe7Y0MM9Y|8D)FnA7^d*4U&yV>D&0_qBd z^1%HUY4J;-1Hq};^WgX;#fKUCe-0#1{GZ zY*0D`1f;t}x}-KKAR!GBl1fNQsHAjCiFAm7bV#$M;XfNV&++Fu@A>-f&-cD(?wK`f zX0ElaYfU4j#`uq^5l9a+s3R2E{1s0HS8zb%?@m*due-c#ogn5Kllb3r-KfM4GiP;& zw|^sLE~Hu*OEskaAJ5y%`Mr$`GcY%kUnv#RuCQ~Sj5YM@8@M#xPu^GI^8FdCQDu<% zbUDfaA3m6FEZ{PIO?gd{AyxA1f6U|QRt@@Lu)sZ-I%loH`IoBy^_i7+KkhQn>#}tg z65|z6GXcKbuo&09D(#I|s;eabHvaoy^Sv_uo^D$^%NV*WCfO*dU{e~rh`DpeCSztB zO7^T2-C3c{(eVGzseW816ZN{-4 zb?1Li4<`X*D-~i<>Td#s`4-4*(C|H<4P!S0e4P)VgtP~!iT+z@^XFd{lZ$FB9s)(o zP|<#d_wU#9^YdSTmd^qpn67RLh!y4M$1o|UF9*9{|JxiWfcgAF7_V7f$B2}`^x5gL zIVhL6x(Q-H1Cx_QXFhSG$0ZtKRaBsk4?(@Xo`021jmT3YHxDlQ0{u^>gbpq?kwz_96H1GR8(S8 zMLbk}KQB4_<(J}sU9^ehVc z`@20naW?30^Tz?6?l)q0`2g~C*uQFOY2_^<1J3HbRzMY94h~!ZwOXvyS3v#VF5oMN zQFNa8o@~pUMkoF4bpQD{rD4xs@GxgyKE8?M-NKKc{Bk_fT9G9%HK^u&?+Gt93*mH39(Ww3WT#L{&^Z(SME8a2l}L z;g!!=E<2L6pAr{my7Hbk{0Si|Hz=?Xo5F8j06PO9`YOBde;GqimQ)tDH5n5hAD=F_ z>aZ?-yV7!y@zh+VNrMxnEjbH@Z%@sIS?gve5UD?1`}zjwGS04~qONXUJ=!Ckg5(Pd z52+d&(u$k`e2&Y!>qhy+Gm$F``g30a##csUi-J?O!-_Irj2>FzsF)=kF<|I=gRwCb ztLy$nv~uAFSZp70)cL3s`_4f5AqEh(oUgxsdJhEKe8oI=vVCNGuFS6g_sV5N53$97 z;R1I}fXH=&lJpW@Ah4fZTPrf}4XULND=!GyF*dH_J}=JC$6_XCmCtVIV5wU`f9NLx z>?c&Sj(>4$LZ6T_%m~O|egQsac+y0ViD(AUzw?*UbZok>TrZ(*##JH?)cvLAEgK9d zvUKxz8jxy*??D@+;Mxj;YL6e$tf4@uL8jBm)=vo+#VS4~txr!?WM97tzkO{Kv2ac; zuXXh-cX|TdzB4DSa4vi|gPF%DH?6g2g-z-$HHj5<2dtMER>X`W_fpD1?E;99{wBMA zO?HJi*BKiplRK!Ri0}=k@2NqWTw&1T03Z=v=uAN`vgM_w7I(NFNuUU}U(3vpKU}^b zHD-r+HWo+YH;XxM@L9u#8qfJ*VTFZ-5?{3+KhFM5Y0It+TYFz=_9dE59&J--*@M>r zu&l#jc1`}*Ryj4Actz=jg@q650f8PZN(QCcbKg$Eo~?L00ADm3oIKH|so2sO8mE1Z zZMTwd1?8j*I!jA~n%7fNtt~TQ?Df$gb!YHHDIsj8bcQg}8voTHC`vwj*xLbgqPqa4 zVF5%b@jQHGusF`Vr6X@rs7}c~!xg%#AX1XJ0GK`jO9-(toyV4-BBd%QGalIj?H`^I z+B?esZti3A_G=HPn;d3aLsi&}UL0dfylFT) zT8l;_&)|Ie?G`WDnB{F|aA;HH`P+~}d63%+N*Dd8up%h-7U?_&0vtakspa8gNpIQK zidv5pMUOTn^IH3tm?GcvlT;C_90viM{Czq>Nv<~ycBCz0S#qR3&k~T<>K5ChRp-iL zKyij`kPrH;M3Jl`Z677 zR(Va0kaoG1`j=y98WL?aeF^(G1G9v|sh05wHr7^^Ry=jKoYYhlS0kbOUSLXNcsq8j2|70@gRDW6eiFCW;;y7p@oja-cDP0#jR(9~Mw`>riPunzD&>gl&oqkY(^Og_E%K^r0@ z&JJdH9q<3Rp=-@bZ5FKfU6v1w8g6PX%Lb`-v>#k^s{1$JNh{yp0meVBoZX+NFB z-r{TRN;~~6-VLRG*Cb0SUhd+t?I2e=Yk4@|qS>?cetS1`4ky7!+M9F7@uZ7=g+}$Rx_ywtwuG7y3H*T`OERqC@JmTpW^KA8j$RaG6 zvk!OJkbA7!*H=`t2b3hfv{N!NUiu{95#3I_5 z$ox&`OnND0zurtu!j@te#)nBp9`-Uo>cW6JmkE)`IP`TAJ_E+r(_WPRG?(?&Q0BMw z`T=h7VB41B4P8_{HMKh7KawD7lKtva;k1lZCD@9IYMpB=Ze z$K#0Q_AX~A0S6jw z6ky8dz>>=3KM_CE02ZAb^5+WP#td91vP22@NM9WG>8&dV`S#aEqlSIT%p)T`WV!sU zP9+r70S+n>dFZ_ZANhvn-SR{L&MTxup!TWti{}z`@Qit zO7uupe-|8)hD?WaC)Jex%g*b;Oc2R~jJ|l-xR`EG1hG+y1Rv#5Wi6BO{Jm%R61){# zwyNIkQG85}Sjv=u|C2v6BH(7#y`XB08rGnc-aqbYv*i@(c4}f`(eigBy zbPkS~5<3a)SVYK2?V*j0VH|+N7ag{?@tyZE1S{sDB>uf#JDp7H3Fes$jb$(UK47oL zanh2V*B=@M?#dm(Va`6( z5l^7;rcdQW11Xe^?JdM<0V&NvkPZNmVR=x85(R0Sn4605F&V&6iZ&H(+&3l#;YX{( zg%as9n+(OBwM%R~oBC9^N{~h>emfkGzHka|GjQCy+bJoSWH=ayzbijV(8*PwQSUbb z5Ws+rKm|W!(ZQ2s&P1}LC6g8h^U!`I#Hp>#V*DCCNB9TH%z|^OHIpw++2mQ=rAzES zT1_0TlZEXKY^!`4@$LBcWQ)@pFRwsqxw4VI;lZ>FDl^5_9Pn zF`f)6i@pAOP+-9_d0RmXtxKd<2jHINQ64Px67j*uvLc!uw>{Z#7!HUNWuBHAuMX`@+i_%OPbO zUULyUW$)>=NCD@?hhJo|Ka!L9c^8)X^3s4Z$9iIuq*wRsKcEZdg!^Nb^(Q;s(WfSy ziQG5I(`|f8%1UD?Qt?CSU{TRyAQkE1wLWk;9-5{^ld$c_4x@Q;Gr+X}bJ#q=sHxW! z$k*DF7~aye`3yn^%Q3APu-mYOE)rf=s=EGmUWzcqHWVTZXOE;fIlqp@QO=$!=oYk$ z=#$Em$Pd6W11NyUDIcmxu0oF~)n zHcuZiagY|jL@&ZDwzqttLfd}#o6_w6g&%UY5P#+d*W^~6O-`>LwRiMnTzL`=F+%NR z!mQ}ZTq#pEFJa6-PmX$GblGc{Xs79mtw@+(nHZ0TM${8y{+{*WI0FeUJW_6F%D?f7 zf8ef8Sd`S@9!dSWf7@vELsQJXR6*w<4lBqvggVHoBcBF}8`0l?{^wfxtgd6K@hJ5N zNS{;c9dDEW7ULgZOV{YrAPXx?=4tHu1(;|24L=CM#oN&HSN$s+F*e2B1iAGCIaRgk zI}GLv3F$ETQ!^F0{Kq3WaUjP;mESr2Mv2%hsA#Qa-8u0iFW*1ntUg5l0B8W7@j1$$ z2-C%9#YOmRK)H0BYtPRn`|fX?&KWrk6tldJ*DF;YKZXEO#StCn+>`3#{$F(&3UlG( zs$05;rc|6ZpMz}kC%&J(BoKQ(yx2ct6a#%&*0>juIZKpZWFK3^u-R=L+)z0bD*11i z^UuqFp9OS8TYE6n+e!%A6uaTwZN0BaKfKg_Q!R;;HqY3W+tnz50u(=BpF(O zY^z3}{lq7^>({T(1BB_w*!aQB;gsiQXAmyc&>Xl8T=Pt{wz3-fW|a%jhEtQ?pi@hX&BIWqNt(vJcV*H0VxX`9#KCM+pn<WaSO}2fpBG4(48E<0M>QxquV(8RNMnrSDT=5tbqo$x za0wx_CPtzuP*pn5!{f;|g%X%R&x!jxx?<)*$-$=}_+6KTx$^ssVmBLW>=afk8utam zZL=xv->}Na@ilDHyj?cn)DRhgq-D*fEr3U`3NRXQ{7}``r*^8irW0&wb@qI>v zI70(@y%MG+Eu=|MiUEuBX7WFs{&U_#xPnA!g{>?czWqgYPGu}lxZdF+(pvcg8m=%nrP`nA2P87G%PWCV2m&vJZR$4^?*TxtQu zs%QcFac0Zk-X0A)0;jg03AgW!1F*(gn>gkQQW0JRTdaZbv+t_8R~dwe*N`9(@D>p5 z$cIIxM}vaJot>RAASHB-U8WZxS?VBb5Zv=}ssIk@+1mq6%&E!_)TJgg39z(RN_?PD zsk<6)_z(8}C%#y=!%Z}kkU)Ak6M#Ptc>Q!N{L`P^l*J&d0yM{`vqxQBU5NWXU9nb~ zxESOKa26%n{N5FaSOrM)lSNAv^EK#_;nDi@{r|>Ii}tldu$HiwfCB8EL^BuI(z>^w zRAVLt(0pckA=RdGdjA1nIu@x#gUmjtn_>9~JMEvF^1o1NfC17#8(_-s#{rrGOynB0 z-Q8CJ0f5jFaU%?*=ew&;a&q$C?5#k_CZOuYaIWu*7x3gF>-GW(x&m^wHhnw5h{u*Y zCw9J5Ou2X-S-J`*L$pJ+;pEo8Y~WuAb!*m?>78{zZ1g`y{!Xpq2{a1Ff%mc*E2V2S z?@s6hC>*@910X>g48aj85R0#={bBS^>?W(|xGj2Png_0{S8Q`|J4e4#$B)BJsm$}v z!&Lqe7;+Sqs2d z>1V(3n$nxwJtpFu{nDIeZ3DbO!8n*>YimD9-+mzIFx4B}4c~PUh13GX z`7Uz3_pvL7Wu4dI2Y|gYDyJ7kg6P((fdSS0qM}y-!BIPfva>70@q|IxX@en5zIz^k zZ7|2N#S|)j2zT_!*-Bc=NFZ4oU6~CF$ee%>8#J8KpwuBp+1M&05T6Ek$(oO(}92*MA*3VM1T zL*YuCi@kg+oUU&f9=NplrhYJ>OzBunl$4!(p!=dGdz2lKZ4R=~{}1x5zL^2_eE0Rm zdYGRe^4WF=?XWeVTXE42(3O{$TP-|wSRG<^IRUbB!*ig9Xg9!FdhFdmnCgdukPALJ z&fPmIE$Q(2A>8w-S%0%9_#^D01e#mcRLz$y@p%O)VLS*V@VuPdm@N}`t{@>QtS!J7 zQJ)R#8?SLG1cUpeC+Te@0^VPRw|UI~=s3T+b=mv@(XR_1_-uy;X@%nTPKSCTTW7xR zJB$qy@fwtd3cAbiqIHYK@-qU-e;{*&`j=Ob?Bj?RMI7v7%S{nzkpA)UaRd|#cL(qn z-BP>Nw+qKD$Fzy;x4R;jpCWYXjTw3ayt2yfiiro_`sK~@ix>|gcyhCf*LozcbvU)i z1ewK#kcdr7eRwP#WA~*+Z!!arN`36?UpD(`5Zo6G20A^F0GIR!$)7#^*SmncI}dQG9l)^_sA*Ic0sCTI_TWAevC)A3_MgLtLy5VKei=j9>q!^&*1=UVsI*J_ zHCe3&WDav0-s=OhWmaTZD}b3+gD+QPT0KS*QryN%!|w!_Mqd7t+w9x@i>MSLK9J<( z?Oi97tKD;L3UKJoT(PwdwZ-^S!A3odA4PF7w;uRNsmjXoKZaH|iLt>ZU42u;{o5?? z2Sc)00l7d}(~w(i^9FFknoI2=;aXL$>pELPoC{6Z2pC_#FNR)bFHx_f#1^wy_bSph zu|)x6etIig_$nObuP#8lh-8{_+a{F9q&387U}9Xv2yk+}{Tw45q+=h+gdtW{S}g_= z@yxV#m4oqC`bckYuezrh>WRCVNL2Nv61T^Wnac(sFE6u8wE?H*KN|YJuNFxoaYW=JI}!F~-l@R`MBNQ?X^Jl()1U=L(N2h~=9zQY9p8x-5aKxPwJny40p zIt~x!qJ}kOw-M*1SfwAR=mHt(uWIrKr?tzu^fSw8Lvkl$b+-?I^EX)sQe-{t7I)jtNw zRN01G78bz<_STAYq%2Y6R;|s@qr4Nq#t5SL4ydn^ zzyGrqX|u!BRP+q6YllqrFjjwnW83idm`@RXrko$GsMj5k%%UE;5AL(gXCW_WdpOQ% zJ-tka-F{Kqp_@P7tTgSwb14SMIU@hVza&_}50FGNNoQMlvvQ#{65TE*6G1aIS6rPE z+>E!0{^)>zfH32cV~|KM@8eT{y`UG!jCYnzaZgfUj(~e_VUQ{rDJeRt{TIQ;uPgIV zBnMF_vx{18gEA|)`9{!&Z&4EQ0IV(5Xs%3mPu;OtX5#0F{p83OR@)Jfo)gHy?{?h+YuBaqQE0wXPbeKr7pJuaa?vI;hKO%e<*n>-fT6*33>UG zwCW`U`3z{!ntyxqU=<3oHZ`EVvd^~3Lks&oBdlATWnZBQvbrE4kyXk`K6tG14#;`N zBj|EVrPKUlRgPJ(a6&o(yt*&46Wce7$(FIUfSy8jUY;zp(rG~+ewZZxyaUv6D+m@b znBo5OvtwuuqUOs@_t|j63?)%z5jg`mnIIyquCCrRQ^idfy?Sf89tJOTf?enQXxwVc1^GUeD&uI)FC=pUV0{YEen@UawR!i<^_bW86K>x_-k zAHr0K&;>~8Ik&tlupAT=6t)C<@cBfE<;*-KW0BakO7GW+S`K7KFO|7dr+3H_egh&R zH1u-32LRCZ7vnL~M>fslJ418-DsT>H?ZeJ6lv=Qv!^v$!UIR7Q{H2&*9oVUcC5Gwz z1eNb{a?h{Q6fMUC3_*r{_e9l!B+q1Eba6-TtuAmCZGA9Z!k@al-v5Bh2yK)AYcmjw z__266T?D4jT=85wNb)<4x%5ZbRIl3-6&Op0FcwFHqyJXw@CY=G}OKbK;%SO7$P_Gb`S6d-zlnL zvBIJw`tTD0H<)i~Fnu{9l1!k%prQ8O?8v7F+I@Q$?6}*!4F?{5b)&2iV4_+SFDI(@ z@n(canDlIzpSE%;F5#Zg#i0f46c?*(P^&>%gviJnuT%*@HKcxA_cxi-0l_p&(*>j^ zH0>Kfq=4oGto|b!IrdkI8Rv+;a0X=QJ{E&fvN{Q6_H=$&P=pyNwmQYkM1+%cUlr#w ziiKcU)M;FVHD)3bD0*WEZFjKpJ~wv<5*9N{g1DlZC58rMj@CL#Q_7dCDn{q=S=Cac zLPC2RJ5U3u@(>FoaJx_4S}TH}2BO(0`O(FFcH2x4bY9GRb5nww5J`sEX7&3m7swJ= zR%-ix$p12|&<=uS0NQ^-UFL0{^FIEazds|GWdUf({oOA+k}9oPxh!yygSH>~ZF?L= zp+3dn*d!|%9UU!%kj^NZDsO?37lljGQ8c#I=sm{m&Ig1zg%5zrbMPx<4x$wYEDnuN z?WTL=R2lfuC$~fSNXK4c9O%{gc<8sj&p(vfznX%aqN89$_Aez_LsCK-@n{BI_2t(L@h3qSWIrP@33lAftp9;1J^XeeHnL-31)! zk*WWEz5s-6fSXu4aI%D`M{yRQ`10Ea1O^gg(Z?!6V0m%Un+0VvaVa2PE82P#f(Q8s zKv!%*+TUz^8a6>s6{3Gpc_L@YjOn4v7CH7F2*W}-Wwez2kYQO7BssX8o0Oih9mHOd zSF2?9W#8q#bi2CdXG(n`zAj>lfE*94{(+u-AA*6Y9*rG5ir_U$Hj-kuH>+!nNoP=n z&fL(Yr#SX9@w~x4*D*7UWm<>v?4KP<(|_tAtN)9@572vhRc~Mdy}I1ScEvG|L1z8b zp51;Ehh@=h_;6awb61pgnyGyNtFEwPbR$7`rA=-!%LY15xffAR)A-m(0C)5XPq6Uv z&>%-IeZBy{_8qa1Z;pxRCu|%&y69piOp>wZ(@>u~g~A8FK(bD@-`hBqw$~d8o_gWI zqH4ow12mb!7iga1Gd6BGt_ctIYc7GvBn7Sg8m*8{UaU)YlfYb|9718gOdbtq(7qpaFS4Uqg!=iLo0~0XX$I|4 zO@+kmPWYcNh>hF?VS7&XHJXKckiTqnFA5=ygGc{DtD41(y;g~_|6KR zP#Omp@&+Xb8M%ZHy|zPSD+VBh$l=ZzzRSg5y$bcAN-G|I9{ON~Fi`}l%QC=uVI+VS z&X-0oi$X4)8lRGT!Q3@>RkCwXA9?!VO~i{5f<5DOeNQ7XHJr9q&mP^^?h(iPn+ic; zh4!9uxy0>z;SUez3c3?ohj6#654^x71hkvq7O#vtdb)XORhfBt%f50{ui%9#O@)Id zG+RB2!k2AoeR;BVaFKAltI%ztLouwSNXk;aF;T7nG-$HY&%wcMh>xkgK1b^UfNR#CAUfRKNTQ6ctPA!gikG$ zO}olm8ZWQ2POfnH3?og+FIX9-01MZDDyQ^=AVV0Iyk9waJ2Z5f$f&h2-{Zypo)(~D zXCju7&a~UUHYLTh`!5<&iD>FvvElD7AfLQxDov1N_#*rJCR-X8iy1bFun{J&2CRKn z7`793*n~h0@V9TSRocX#5ier@9w^Bpvasr^1kB(pzD*M$|xBK8^tyR zT=wHsMrkEZRdi_J%Yj@EmK<|o!f>$C^13h`1n*5S&u6E+Lg4ff@>)(#&Ircbk+}=i zw}74rHtuFP?W8w z$0<21SVvQ1?x^_Q>&S9H?RTl$v}h+eZ>Q?LA8iF?abBDc(+JoDgm|8%`c4qG=7WWk ze#>syR2$^OCTG8mFx=O#7#$cp=mq4MY_IEN#B`AKo+MrwRh{pw@IJ+lQ`+qIl`efX zkX#A69oBO2{&zi_CJf+nO8;wtD%i-%TUi`8xlINbkG zXuW_qmo5zGdwI{v<=fi~garYnDeiNjA^SmQfHxI{sHt67kXhwqr3)Opo$oQ5q4tu6 zH6ns`y?o7Cj_po@I))%^5&@H-$Lc4XyR0fM4@=zijeqy#?2LT3UrT)b&FH{G#j<)& z+q+ej=0}(5KG4t70uC;hjF1k3^2lxHc0UOWS%tf3#hRH?^e`GsQawrS0EUaBx{#i_4YhaFhRPbqySR&Msi|m z&f%%JiSbf;9amo7kQ$(;oX2BiR~3P9+)s?t55$p(TieVm*Gkzrm4e_i8Q?4+hiwcy z0F+phk^(5~&};wuKORsIti@|ec_pg~nHA7{25{%Y+J26oV%dwNCr1-8!U;hgB_^M5 z^kC6&l`Hz3yeO0)Cc-7R6V9=XG}FLSbCCn5JI8O?;{#mv>TG!Nd2bRu9SrJMuk|LcQYXk~mt*tBf;1p8x}tk)`DIkRJHb znb6Ke*qWZhoHMM-V^!;Q&}ngWjujYGsa^r*0+ zE_hNrf%r|jET7p1x$aKim0UygEJCPM3Ht<%Y+xb8UNVk#vPOaC?8}?bXZ8d)Y{%ix zUybt1No&U!(C4whPPc0iVo~8LaptXG`knSS^iQpiuH%ULx*a-ovN6Yrru1pmv*7Z2 zv~yWnQKUQ)^C6gpASiUjYXrsQTB$NLjT4AdCCwi)-8E!H~~uRlNY^Z;ec&dxFD>e2UkHrCne55#}d zh_G^#HL#eMN5pap$i(WrD-`;f%f>kYAM;)TL}D|t8(kyR!d{6409938-_?Dr!&q&a z>)2E%i+l1pmsdINXtC@U7NaDpOD5eT{sB~%R{P^%dKyKx_d59<1%U*!ck*$G&xClH z&PUTR?6fE|C2v6W>-f`SK{Ay2Op;}+o_g*gcCLlF0t_aUgJmi`ll~QF^1J1*%c?;K zG|F>2s;(|AbXbx5f{mNqj>^@q~zCgL>=dDwZxs1~(rpXbROp91y|s}HEjy{00UEb9)2Zu7%bcMl9!plxcJ7=EnlkCL2 zTlVIT_rP3BN`m#f-|N-;$&m7|3BUYrYk%|zn*?EyK733$S%;DmTm+Fhjex3oHM$m%A$jszhs&&lCIz?}J&JEUhxYy0 zlg=4pzR+wy%8c*WP{@d2$~hluiya!fYdH*$ReAhmB`tSId6uTVF5HuHpKrslp?B9- zxi!sF@UTx+P3>GKN)hN6Z9s-2en3mjgK`|fydY?@mKc1>zOQve%-+((5s8PeKoa9m z3x`f-bAfO|DbJ&ehh7OW%=UgtJ)u4Ny?)HTm-LE2T+h^Ku9Xn%N?6tE*8x=-B+|hM zE#4)liz#%n$&Q+;h+{gY&C`{SP9>Fp9new%O##AnVFzjt zG5FbS@SRp3YzYx0(_@W`isTU$A{*+CZ}fdtCYn!3lD z`BEfeaQzS82mG!&l^?=1xD!_2*VT2D2oUalqJZuzZ*QVsUY)+Wi5wdvt z&cfnc208-X)%SK0H4*2qyyI*w0X+gSlCK6FZu>lB&Y!O}6c1^TrQv)c;iR6n2k?`D z(#w_#!m2nuzJx3>x5a; zF9|&6^}yMAS4^R}ph3W)Ck03o9ghTEaLiu~+)qv7flYLOn%!v$p@m;OEO8-(>m=1% ziK?^2frTJvGv~ZTpOt70&3}&X2B8Z)h)i8w^o@XM!O@%rcrF@#@x01mW|`kt zk&X3pQexu1CJYA*M!ggqO>@|=HPz@|_{FeJ)JcgXJ^p^eRi3?b$I}Ok4l6X7SdV)L zQzLkzlP&fR;$5F}LQvyaWNKx>%Ipzr2#gNI(1jRh7}+=#^M_n?h{sXtB-qkktSBY*qF)+Os??{OI~ zJ{JByWK*>(xE}Nn1ft^M>~O7U0^rhs_EQ`Ob#=OT78gKk;4mcxd-_qylrHPmGQZT-(a^r{NkWEnOf1 zu^cXOP%h~WF$w|KcYBj=y#sj{Hlw;3T7|oySvWRoz}mzq{#vz%iHQyxGdfS-)D>`N$mO zOIyRE45=h=qdSwQNB}%A=j{HKYZoc4Q15gT( zf!Pb$+7?}WUcN1%A>CJbu>HSQ<=caGZN+5m?ChM$NA+UT1y;2-8uy-y=D&6Ewo++R zW^N-HXmF|6)2~QmkyS%1L22e$rdE}ZT-BFd2s8Fh;<1^Z{gfJeE)mrP{{M`VIg9Z` zUv)6Mty6-k;UVZCSB_U6P7;su(Y|WSEy-{$AmcCp6(U*y>HsPL{;Z`%r_Uj^-Ns)nss5u1b=KcA_^Fr5Tf-Jvkdgj>`E41ZN{>_S TGpor#5beuJDM=QK8~Oh~ozZRS literal 0 HcmV?d00001 diff --git a/nav2_route/package.xml b/nav2_route/package.xml new file mode 100644 index 00000000000..d33cb7e942b --- /dev/null +++ b/nav2_route/package.xml @@ -0,0 +1,40 @@ + + + + nav2_route + 1.1.0 + A Route Graph planner to compliment the Planner Server + Steve Macenski + Apache-2.0 + + ament_cmake + nav2_common + + rclcpp + rclcpp_lifecycle + std_msgs + geometry_msgs + nav2_costmap_2d + pluginlib + visualization_msgs + nav2_msgs + nav_msgs + tf2_ros + nav2_util + nav2_core + angles + libnanoflann-dev + nlohmann-json-dev + + ament_lint_common + ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest + launch + launch_testing + + + ament_cmake + + + diff --git a/nav2_route/plugins.xml b/nav2_route/plugins.xml new file mode 100644 index 00000000000..89d6b745c3b --- /dev/null +++ b/nav2_route/plugins.xml @@ -0,0 +1,80 @@ + + + + Cost function for penalizing edge distance proportionally to potential speed limits + + + + + Cost function for rejecting edge that are closed due to non-traversability or setting remote dynamic costs (as opposed to static costs in the metadata from the Penalty Scorer) + + + + + Cost function for adding a cost based on metadata stored in the navigation graph + + + + + Cost function for adding a cost based on the costmap values in the edge + + + + + Cost function for adding costs to edges based on arbitrary semantic data + + + + + Cost function for adding costs to edges time to complete the edge based on previous runs and/or estimation with absolute speed limits. + + + + + Cost function for checking if the orientation of route matches orientation of the goal + + + + + Cost function for checking if the orientation of robot matches with the orientation of the edge + + + + + + Route tracking operation to adjust speed limit based on graph metadata + + + + + Exposes a route operation to replan upon external service request + + + + + Route operation templated by service types to call arbitrary services on node or edge events + + + + + + Parse the geojson graph file into the graph data type + + + + + Save a route graph to a geojson graph file + + + + + + Route operation to periodically check a costmap for localized viability of the route + + + + + Route operation to add actual times of traversal to edge's metadata for use in later more accurate planning by the TimeScorer + + + diff --git a/nav2_route/src/edge_scorer.cpp b/nav2_route/src/edge_scorer.cpp new file mode 100644 index 00000000000..d98ddf46973 --- /dev/null +++ b/nav2_route/src/edge_scorer.cpp @@ -0,0 +1,94 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 + +#include "nav2_route/edge_scorer.hpp" + +namespace nav2_route +{ + +EdgeScorer::EdgeScorer( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + const std::shared_ptr costmap_subscriber) +: plugin_loader_("nav2_route", "nav2_route::EdgeCostFunction") +{ + // load plugins with a default of the DistanceScorer + const std::vector default_plugin_ids({"DistanceScorer", "DynamicEdgesScorer"}); + const std::vector default_plugin_types( + {"nav2_route::DistanceScorer", "nav2_route::DynamicEdgesScorer"}); + + nav2_util::declare_parameter_if_not_declared( + node, "edge_cost_functions", rclcpp::ParameterValue(default_plugin_ids)); + auto edge_cost_function_ids = node->get_parameter("edge_cost_functions").as_string_array(); + + if (edge_cost_function_ids == default_plugin_ids) { + for (unsigned int i = 0; i != edge_cost_function_ids.size(); i++) { + nav2_util::declare_parameter_if_not_declared( + node, default_plugin_ids[i] + ".plugin", rclcpp::ParameterValue(default_plugin_types[i])); + } + } + + for (size_t i = 0; i != edge_cost_function_ids.size(); i++) { + try { + std::string type = nav2_util::get_plugin_type_param( + node, edge_cost_function_ids[i]); + EdgeCostFunction::Ptr scorer = plugin_loader_.createUniqueInstance(type); + RCLCPP_INFO( + node->get_logger(), "Created edge cost function plugin %s of type %s", + edge_cost_function_ids[i].c_str(), type.c_str()); + scorer->configure(node, tf_buffer, costmap_subscriber, edge_cost_function_ids[i]); + plugins_.push_back(std::move(scorer)); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + node->get_logger(), + "Failed to create edge cost function. Exception: %s", ex.what()); + throw ex; + } + } +} + +bool EdgeScorer::score( + const EdgePtr edge, const RouteRequest & route_request, + const EdgeType & edge_type, float & total_score) +{ + total_score = 0.0; + float curr_score = 0.0; + + for (auto & plugin : plugins_) { + plugin->prepare(); + } + + for (auto & plugin : plugins_) { + curr_score = 0.0; + if (plugin->score(edge, route_request, edge_type, curr_score)) { + total_score += curr_score; + } else { + return false; + } + } + + return true; +} + +int EdgeScorer::numPlugins() const +{ + return plugins_.size(); +} + +} // namespace nav2_route diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp new file mode 100644 index 00000000000..210e8911975 --- /dev/null +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -0,0 +1,315 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 + +#include "nav2_route/goal_intent_extractor.hpp" + +namespace nav2_route +{ + +static float EPSILON = 1e-6; + +void GoalIntentExtractor::configure( + nav2_util::LifecycleNode::SharedPtr node, + Graph & graph, + GraphToIDMap * id_to_graph_map, + std::shared_ptr tf, + std::shared_ptr costmap_subscriber, + const std::string & route_frame, + const std::string & base_frame) +{ + logger_ = node->get_logger(); + id_to_graph_map_ = id_to_graph_map; + graph_ = &graph; + tf_ = tf; + costmap_subscriber_ = costmap_subscriber; + route_frame_ = route_frame; + base_frame_ = base_frame; + node_spatial_tree_ = std::make_shared(); + node_spatial_tree_->computeTree(graph); + + nav2_util::declare_parameter_if_not_declared( + node, "prune_goal", rclcpp::ParameterValue(true)); + prune_goal_ = node->get_parameter("prune_goal").as_bool(); + + nav2_util::declare_parameter_if_not_declared( + node, "max_prune_dist_from_edge", rclcpp::ParameterValue(8.0)); + max_dist_from_edge_ = static_cast( + node->get_parameter("max_prune_dist_from_edge").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "min_prune_dist_from_goal", rclcpp::ParameterValue(0.15)); + min_dist_from_goal_ = static_cast( + node->get_parameter("min_prune_dist_from_goal").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "min_prune_dist_from_start", rclcpp::ParameterValue(0.10)); + min_dist_from_start_ = static_cast( + node->get_parameter("min_prune_dist_from_start").as_double()); + + nav2_util::declare_parameter_if_not_declared( + node, "enable_nn_search", rclcpp::ParameterValue(true)); + enable_search_ = node->get_parameter("enable_nn_search").as_bool(); + nav2_util::declare_parameter_if_not_declared( + node, "max_nn_search_iterations", rclcpp::ParameterValue(10000)); + max_nn_search_iterations_ = node->get_parameter("max_nn_search_iterations").as_int(); + + nav2_util::declare_parameter_if_not_declared( + node, "num_nearest_nodes", rclcpp::ParameterValue(5)); + int num_of_nearest_nodes = node->get_parameter("num_nearest_nodes").as_int(); + node_spatial_tree_->setNumOfNearestNodes(num_of_nearest_nodes); +} + +void GoalIntentExtractor::setGraph(Graph & graph, GraphToIDMap * id_to_graph_map) +{ + id_to_graph_map_ = id_to_graph_map; + graph_ = &graph; + node_spatial_tree_->computeTree(graph); +} + +geometry_msgs::msg::PoseStamped GoalIntentExtractor::transformPose( + geometry_msgs::msg::PoseStamped & pose, + const std::string & target_frame) +{ + if (pose.header.frame_id != target_frame) { + RCLCPP_INFO( + logger_, + "Request pose in %s frame. Converting to route server frame: %s.", + pose.header.frame_id.c_str(), target_frame.c_str()); + if (!nav2_util::transformPoseInTargetFrame(pose, pose, *tf_, target_frame)) { + throw nav2_core::RouteTFError("Failed to transform starting pose to: " + target_frame); + } + } + return pose; +} + +void GoalIntentExtractor::overrideStart(const geometry_msgs::msg::PoseStamped & start_pose) +{ + // Override the start pose when rerouting is requested, using the current pose + start_ = start_pose; +} + +template +NodeExtents +GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) +{ + // If not using the poses, then use the requests Node IDs to establish start and goal + if (!goal->use_poses) { + unsigned int start_idx = id_to_graph_map_->at(goal->start_id); + unsigned int goal_idx = id_to_graph_map_->at(goal->goal_id); + const Coordinates & start_coords = graph_->at(start_idx).coords; + const Coordinates & goal_coords = graph_->at(goal_idx).coords; + start_.pose.position.x = start_coords.x; + start_.pose.position.y = start_coords.y; + goal_.pose.position.x = goal_coords.x; + goal_.pose.position.y = goal_coords.y; + return {start_idx, goal_idx}; + } + + // Find request start pose + geometry_msgs::msg::PoseStamped start_pose, goal_pose = goal->goal; + if (goal->use_start) { + start_pose = goal->start; + } else { + if (!nav2_util::getCurrentPose(start_pose, *tf_, route_frame_, base_frame_)) { + throw nav2_core::RouteTFError("Failed to obtain starting pose in: " + route_frame_); + } + } + + // transform to route_frame + start_ = transformPose(start_pose, route_frame_); + goal_ = transformPose(goal_pose, route_frame_); + + // Find closest route graph nodes to start and goal to plan between. + // Note that these are the location indices in the graph + std::vector start_route, end_route; + if (!node_spatial_tree_->findNearestGraphNodesToPose(start_, start_route) || + !node_spatial_tree_->findNearestGraphNodesToPose(goal_, end_route)) + { + throw nav2_core::IndeterminantNodesOnGraph( + "Could not determine node closest to start or goal pose requested!"); + } + + unsigned int start_route_loc = start_route.front(); + unsigned int end_route_loc = end_route.front(); + + // If given cost information, check which of the nearest graph nodes is nearest by + // traversability, not just Euclidean distance, in case of obstacles, walls, etc. + // However, if the closest node has Line of Sight to the goal, then use that node + // skipping the search as we know it is the closest and now optimally traversible node. + std::shared_ptr costmap = nullptr; + std::string costmap_frame_id; + bool enable_search = enable_search_; + if (enable_search) { + try { + costmap = costmap_subscriber_->getCostmap(); + costmap_frame_id = costmap_subscriber_->getFrameID(); + } catch (const std::exception & ex) { + enable_search = false; + RCLCPP_WARN( + logger_, + "Failed to get costmap for goal intent extractor: %s. " + "Falling back to closest euclidean route node instead.", ex.what()); + } + } + + if (enable_search && start_route.size() > 1u) { + // Convert the nearest node candidates to the costmap frame for search + std::vector candidate_nodes; + candidate_nodes.reserve(start_route.size()); + for (const auto & node : start_route) { + auto & node_data = graph_->at(node); + geometry_msgs::msg::PoseStamped node_pose; + node_pose.pose.position.x = node_data.coords.x; + node_pose.pose.position.y = node_data.coords.y; + node_pose.header.frame_id = node_data.coords.frame_id; + candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id)); + } + + auto transformed_start = transformPose(start_, costmap_frame_id); + GoalIntentSearch::LoSCollisionChecker los_checker(costmap); + if (los_checker.worldToMap( + candidate_nodes.front().pose.position, transformed_start.pose.position)) + { + if (los_checker.isInCollision()) { + GoalIntentSearch::BreadthFirstSearch bfs(costmap); + if (bfs.search(transformed_start, candidate_nodes, max_nn_search_iterations_)) { + start_route_loc = start_route[bfs.getClosestNodeIdx()]; + } + } + } + } + + if (enable_search && end_route.size() > 1u) { + // Convert the nearest node candidates to the costmap frame for search + std::vector candidate_nodes; + candidate_nodes.reserve(end_route.size()); + for (const auto & node : end_route) { + auto & node_data = graph_->at(node); + geometry_msgs::msg::PoseStamped node_pose; + node_pose.pose.position.x = node_data.coords.x; + node_pose.pose.position.y = node_data.coords.y; + node_pose.header.frame_id = node_data.coords.frame_id; + candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id)); + } + + auto transformed_end = transformPose(goal_, costmap_frame_id); + GoalIntentSearch::LoSCollisionChecker los_checker(costmap); + if (los_checker.worldToMap( + candidate_nodes.front().pose.position, transformed_end.pose.position)) + { + if (los_checker.isInCollision()) { + GoalIntentSearch::BreadthFirstSearch bfs(costmap); + if (bfs.search(transformed_end, candidate_nodes)) { + end_route_loc = end_route[bfs.getClosestNodeIdx()]; + } + } + } + } + + return {start_route_loc, end_route_loc}; +} + +template +Route GoalIntentExtractor::pruneStartandGoal( + const Route & input_route, + const std::shared_ptr goal, + ReroutingState & rerouting_info) +{ + Route pruned_route = input_route; + + // Grab and update the rerouting state + EdgePtr last_curr_edge = rerouting_info.curr_edge; + rerouting_info.curr_edge = nullptr; + bool first_time = rerouting_info.first_time; + rerouting_info.first_time = false; + + // Cannot prune if no edges to prune or if using nodeIDs in the initial request (no effect) + if (input_route.edges.empty() || (!goal->use_poses && first_time)) { + return pruned_route; + } + + // Check on pruning the start node + NodePtr first = pruned_route.start_node; + NodePtr next = pruned_route.edges[0]->end; + float vrx = next->coords.x - first->coords.x; + float vry = next->coords.y - first->coords.y; + float vpx = start_.pose.position.x - first->coords.x; + float vpy = start_.pose.position.y - first->coords.y; + float dot_prod = utils::normalizedDot(vrx, vry, vpx, vpy); + Coordinates closest_pt_on_edge = utils::findClosestPoint(start_, first->coords, next->coords); + if (dot_prod > EPSILON && // A projection exists + hypotf(vpx, vpy) > min_dist_from_start_ && // We're not on the node to prune entire edge + utils::distance(closest_pt_on_edge, start_) <= max_dist_from_edge_) // Close enough to edge + { + // Record the pruned edge information if its the same edge as previously routed so that + // the tracker can seed this information into its state to proceed with its task losslessly + if (last_curr_edge && last_curr_edge->edgeid == pruned_route.edges.front()->edgeid) { + rerouting_info.closest_pt_on_edge = closest_pt_on_edge; + rerouting_info.curr_edge = pruned_route.edges.front(); + } + + pruned_route.start_node = next; + pruned_route.route_cost -= pruned_route.edges.front()->end->search_state.traversal_cost; + pruned_route.edges.erase(pruned_route.edges.begin()); + } + + // Don't prune the goal if requested, if given a known goal_id (no effect), or now empty + if (!prune_goal_ || !goal->use_poses || pruned_route.edges.empty()) { + return pruned_route; + } + + // Check on pruning the goal node + next = pruned_route.edges.back()->start; + NodePtr last = pruned_route.edges.back()->end; + vrx = last->coords.x - next->coords.x; + vry = last->coords.y - next->coords.y; + vpx = goal_.pose.position.x - last->coords.x; + vpy = goal_.pose.position.y - last->coords.y; + + dot_prod = utils::normalizedDot(vrx, vry, vpx, vpy); + closest_pt_on_edge = utils::findClosestPoint(goal_, next->coords, last->coords); + if (dot_prod < -EPSILON && // A projection exists + hypotf(vpx, vpy) > min_dist_from_goal_ && // We're not on the node to prune entire edge + utils::distance(closest_pt_on_edge, goal_) <= max_dist_from_edge_) // Close enough to edge + { + pruned_route.route_cost -= pruned_route.edges.back()->end->search_state.traversal_cost; + pruned_route.edges.pop_back(); + } + + return pruned_route; +} + +geometry_msgs::msg::PoseStamped GoalIntentExtractor::getStart() +{ + return start_; +} + +template Route GoalIntentExtractor::pruneStartandGoal( + const Route & input_route, + const std::shared_ptr goal, + ReroutingState & rerouting_info); +template +Route GoalIntentExtractor::pruneStartandGoal( + const Route & input_route, + const std::shared_ptr goal, + ReroutingState & rerouting_info); +template NodeExtents GoalIntentExtractor::findStartandGoal( + const std::shared_ptr goal); +template +NodeExtents GoalIntentExtractor::findStartandGoal( + const std::shared_ptr goal); + +} // namespace nav2_route diff --git a/nav2_route/src/graph_loader.cpp b/nav2_route/src/graph_loader.cpp new file mode 100644 index 00000000000..268c04760af --- /dev/null +++ b/nav2_route/src/graph_loader.cpp @@ -0,0 +1,157 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "nav2_route/graph_loader.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +namespace nav2_route +{ + +GraphLoader::GraphLoader( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf, + const std::string frame) +: plugin_loader_("nav2_route", "nav2_route::GraphFileLoader"), + default_plugin_id_("GeoJsonGraphFileLoader") +{ + logger_ = node->get_logger(); + tf_ = tf; + route_frame_ = frame; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue(std::string(""))); + graph_filepath_ = node->get_parameter("graph_filepath").as_string(); + + // Default Graph Parser + nav2_util::declare_parameter_if_not_declared( + node, "graph_file_loader", rclcpp::ParameterValue(default_plugin_id_)); + auto graph_file_loader_id = node->get_parameter("graph_file_loader").as_string(); + if (graph_file_loader_id == default_plugin_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_plugin_id_ + ".plugin", + rclcpp::ParameterValue("nav2_route::GeoJsonGraphFileLoader")); + } + + // Create graph file loader plugin + try { + plugin_type_ = nav2_util::get_plugin_type_param(node, graph_file_loader_id); + graph_file_loader_ = plugin_loader_.createSharedInstance((plugin_type_)); + RCLCPP_INFO( + logger_, "Created GraphFileLoader %s of type %s", + graph_file_loader_id.c_str(), plugin_type_.c_str()); + graph_file_loader_->configure(node); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + logger_, + "Failed to create GraphFileLoader. Exception: %s", ex.what()); + throw ex; + } +} + +bool GraphLoader::loadGraphFromFile( + Graph & graph, + GraphToIDMap & graph_to_id_map, + const std::string & filepath) +{ + if (filepath.empty()) { + RCLCPP_ERROR( + logger_, "The graph filepath was not provided."); + return false; + } + + RCLCPP_INFO( + logger_, + "Loading graph file from %s, by parser %s", filepath.c_str(), plugin_type_.c_str()); + + if (!graph_file_loader_->loadGraphFromFile(graph, graph_to_id_map, filepath)) { + return false; + } + + if (!transformGraph(graph)) { + RCLCPP_WARN( + logger_, + "Failed to transform nodes graph file (%s) to %s!", filepath.c_str(), route_frame_.c_str()); + return false; + } + + return true; +} + +bool GraphLoader::loadGraphFromParameter( + Graph & graph, + GraphToIDMap & graph_to_id_map) +{ + if (graph_filepath_.empty()) { + RCLCPP_INFO(logger_, "No graph file provided to load yet."); + return true; + } + + RCLCPP_INFO( + logger_, + "Loading graph file from %s, by parser %s", graph_filepath_.c_str(), plugin_type_.c_str()); + + if (!graph_file_loader_->loadGraphFromFile(graph, graph_to_id_map, graph_filepath_)) { + return false; + } + + if (!transformGraph(graph)) { + RCLCPP_WARN( + logger_, + "Failed to transform nodes graph file (%s) to %s!", + graph_filepath_.c_str(), route_frame_.c_str()); + return false; + } + + return true; +} + +bool GraphLoader::transformGraph(Graph & graph) +{ + std::unordered_map cached_transforms; + for (auto & node : graph) { + std::string node_frame = node.coords.frame_id; + if (node_frame.empty() || node_frame == route_frame_) { + continue; + } + + if (cached_transforms.find(node_frame) == cached_transforms.end()) { + tf2::Transform tf_transform; + bool got_transform = nav2_util::getTransform( + node_frame, route_frame_, tf2::durationFromSec(0.1), tf_, tf_transform); + + if (!got_transform) { + return false; + } + + cached_transforms.insert({node_frame, tf_transform}); + } + + tf2::Vector3 graph_coord( + node.coords.x, + node.coords.y, + 0.0); + + tf2::Vector3 new_coord = cached_transforms[node_frame] * graph_coord; + + node.coords.x = static_cast(new_coord.x()); + node.coords.y = static_cast(new_coord.y()); + node.coords.frame_id = route_frame_; + } + + return true; +} + +} // namespace nav2_route diff --git a/nav2_route/src/graph_saver.cpp b/nav2_route/src/graph_saver.cpp new file mode 100644 index 00000000000..be0386ce883 --- /dev/null +++ b/nav2_route/src/graph_saver.cpp @@ -0,0 +1,142 @@ +// Copyright (c) 2024 John Chrosniak +// +// 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 "nav2_route/graph_saver.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +namespace nav2_route +{ + +GraphSaver::GraphSaver( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf, + const std::string frame) +: plugin_loader_("nav2_route", "nav2_route::GraphFileSaver"), + default_plugin_id_("GeoJsonGraphFileSaver") +{ + logger_ = node->get_logger(); + tf_ = tf; + route_frame_ = frame; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue(std::string(""))); + graph_filepath_ = node->get_parameter("graph_filepath").as_string(); + + // Default Graph Parser + const std::string default_plugin_type = "nav2_route::GeoJsonGraphFileSaver"; + nav2_util::declare_parameter_if_not_declared( + node, "graph_file_saver", rclcpp::ParameterValue(default_plugin_id_)); + auto graph_file_saver_id = node->get_parameter("graph_file_saver").as_string(); + if (graph_file_saver_id == default_plugin_id_) { + nav2_util::declare_parameter_if_not_declared( + node, default_plugin_id_ + ".plugin", rclcpp::ParameterValue(default_plugin_type)); + } + + // Create graph file saver plugin + try { + plugin_type_ = nav2_util::get_plugin_type_param(node, graph_file_saver_id); + graph_file_saver_ = plugin_loader_.createSharedInstance((plugin_type_)); + RCLCPP_INFO( + logger_, "Created GraphFileSaver %s of type %s", + graph_file_saver_id.c_str(), plugin_type_.c_str()); + graph_file_saver_->configure(node); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + logger_, + "Failed to create GraphFileSaver. Exception: %s", ex.what()); + throw ex; + } +} + +bool GraphSaver::saveGraphToFile( + Graph & graph, + std::string filepath) +{ + if (filepath.empty() && !graph_filepath_.empty()) { + RCLCPP_DEBUG( + logger_, "The graph filepath was not provided. " + "Setting to %s", graph_filepath_.c_str()); + filepath = graph_filepath_; + } else if (filepath.empty() && graph_filepath_.empty()) { + // No graph to try to save + RCLCPP_WARN( + logger_, + "The graph filepath was not provided and no default was specified. " + "Failed to save the route graph."); + return false; + } + + RCLCPP_INFO( + logger_, + "Saving graph file from %s, by parser %s", filepath.c_str(), plugin_type_.c_str()); + + if (!graph_file_saver_->saveGraphToFile(graph, filepath)) { + return false; + } + + if (!transformGraph(graph)) { + RCLCPP_WARN( + logger_, + "Failed to transform nodes graph file (%s) to %s!", filepath.c_str(), route_frame_.c_str()); + return false; + } + + return true; +} + +bool GraphSaver::transformGraph(Graph & graph) +{ + std::unordered_map cached_transforms; + for (auto & node : graph) { + std::string node_frame = node.coords.frame_id; + if (node_frame.empty() || node_frame == route_frame_) { + // If there is no frame provided or the frame of the node is the same as the route graph + // then no transform is required + continue; + } + // Find the transform to convert node from node frame to route frame + if (cached_transforms.find(node_frame) == cached_transforms.end()) { + tf2::Transform tf_transform; + bool got_transform = nav2_util::getTransform( + node_frame, route_frame_, tf2::durationFromSec(0.1), tf_, tf_transform); + + if (!got_transform) { + RCLCPP_WARN( + logger_, + "Could not get transform from node frame %s to route frame %s", + node_frame.c_str(), route_frame_.c_str()); + return false; + } + + cached_transforms.insert({node_frame, tf_transform}); + } + // Apply the transform + tf2::Vector3 graph_coord( + node.coords.x, + node.coords.y, + 0.0); + + tf2::Vector3 new_coord = cached_transforms[node_frame] * graph_coord; + + node.coords.x = static_cast(new_coord.x()); + node.coords.y = static_cast(new_coord.y()); + node.coords.frame_id = route_frame_; + } + + return true; +} + +} // namespace nav2_route diff --git a/nav2_route/src/main.cpp b/nav2_route/src/main.cpp new file mode 100644 index 00000000000..ca98729213f --- /dev/null +++ b/nav2_route/src/main.cpp @@ -0,0 +1,28 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 "nav2_route/route_server.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_route/src/node_spatial_tree.cpp b/nav2_route/src/node_spatial_tree.cpp new file mode 100644 index 00000000000..19f37ddacf1 --- /dev/null +++ b/nav2_route/src/node_spatial_tree.cpp @@ -0,0 +1,79 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 + +#include "nav2_route/node_spatial_tree.hpp" + +namespace nav2_route +{ + +NodeSpatialTree::~NodeSpatialTree() +{ + if (kdtree_) { + delete kdtree_; + kdtree_ = nullptr; + } + + if (adaptor_) { + delete adaptor_; + adaptor_ = nullptr; + } +} + +void NodeSpatialTree::setNumOfNearestNodes(int num_of_nearest_nodes) +{ + num_of_nearest_nodes_ = num_of_nearest_nodes; +} + +void NodeSpatialTree::computeTree(Graph & graph) +{ + if (kdtree_) { + delete kdtree_; + kdtree_ = nullptr; + } + + if (adaptor_) { + delete adaptor_; + adaptor_ = nullptr; + } + + adaptor_ = new GraphAdaptor(graph); + kdtree_ = new kd_tree_t(DIMENSION, *adaptor_, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + kdtree_->buildIndex(); + graph_ = &graph; +} + +bool NodeSpatialTree::findNearestGraphNodesToPose( + const geometry_msgs::msg::PoseStamped & pose_in, std::vector & node_ids) +{ + size_t num_results = static_cast(num_of_nearest_nodes_); + std::vector ret_index(num_results); + std::vector out_dist_sqr(num_results); + const double query_pt[2] = {pose_in.pose.position.x, pose_in.pose.position.y}; + num_results = kdtree_->knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]); + + if (num_results == 0) { + return false; + } + + for (int i = 0; i < static_cast(ret_index.size()); ++i) { + node_ids.push_back(ret_index[i]); + } + return true; +} + +} // namespace nav2_route diff --git a/nav2_route/src/operations_manager.cpp b/nav2_route/src/operations_manager.cpp new file mode 100644 index 00000000000..ce883de929e --- /dev/null +++ b/nav2_route/src/operations_manager.cpp @@ -0,0 +1,173 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/interfaces/route_operation.hpp" +#include "nav2_route/operations_manager.hpp" + +namespace nav2_route +{ + +OperationsManager::OperationsManager( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber) +: plugin_loader_("nav2_route", "nav2_route::RouteOperation") +{ + logger_ = node->get_logger(); + + // Have some default operations + const std::vector default_plugin_ids( + {"AdjustSpeedLimit", "ReroutingService"}); + const std::vector default_plugin_types( + {"nav2_route::AdjustSpeedLimit", "nav2_route::ReroutingService"}); + + nav2_util::declare_parameter_if_not_declared( + node, "operations", rclcpp::ParameterValue(default_plugin_ids)); + auto operation_ids = node->get_parameter("operations").as_string_array(); + + if (operation_ids == default_plugin_ids) { + for (unsigned int i = 0; i != operation_ids.size(); i++) { + nav2_util::declare_parameter_if_not_declared( + node, default_plugin_ids[i] + ".plugin", rclcpp::ParameterValue(default_plugin_types[i])); + } + } + + // Create plugins and sort them into On Query, Status Change, and Graph-calling Operations + for (size_t i = 0; i != operation_ids.size(); i++) { + try { + std::string type = nav2_util::get_plugin_type_param(node, operation_ids[i]); + RouteOperation::Ptr operation = plugin_loader_.createSharedInstance(type); + RCLCPP_INFO( + node->get_logger(), "Created route operation %s of type %s", + operation_ids[i].c_str(), type.c_str()); + operation->configure(node, costmap_subscriber, operation_ids[i]); + RouteOperationType process_type = operation->processType(); + if (process_type == RouteOperationType::ON_QUERY) { + query_operations_.push_back(std::move(operation)); + } else if (process_type == RouteOperationType::ON_STATUS_CHANGE) { + change_operations_.push_back(std::move(operation)); + } else { + graph_operations_[operation->getName()] = operation; + } + } catch (const pluginlib::PluginlibException & ex) { + throw ex; + } + } +} + +template +void OperationsManager::findGraphOperationsToProcess( + T & obj, const OperationTrigger & trigger, + OperationPtrs & operations) +{ + if (!obj) { + return; + } + + Operations & op_vec = obj->operations; + for (Operations::iterator it = op_vec.begin(); it != op_vec.end(); ++it) { + if (it->trigger == trigger) { + operations.push_back(&(*it)); + } + } +} + +OperationPtrs OperationsManager::findGraphOperations( + const NodePtr node, const EdgePtr edge_enter, const EdgePtr edge_exit) +{ + OperationPtrs ops; + findGraphOperationsToProcess(node, OperationTrigger::NODE, ops); + findGraphOperationsToProcess(edge_enter, OperationTrigger::ON_ENTER, ops); + findGraphOperationsToProcess(edge_exit, OperationTrigger::ON_EXIT, ops); + return ops; +} + +void OperationsManager::updateResult( + const std::string & name, const OperationResult & op_result, OperationsResult & result) +{ + result.reroute = result.reroute || op_result.reroute; + result.blocked_ids.insert( + result.blocked_ids.end(), op_result.blocked_ids.begin(), op_result.blocked_ids.end()); + result.operations_triggered.push_back(name); +} + +void OperationsManager::processOperationsPluginVector( + const std::vector & route_operations, + OperationsResult & result, + const NodePtr node, + const EdgePtr edge_entered, + const EdgePtr edge_exited, + const Route & route, + const geometry_msgs::msg::PoseStamped & pose) +{ + for (OperationsIter it = route_operations.begin(); it != route_operations.end(); ++it) { + const RouteOperation::Ptr & plugin = *it; + OperationResult op_result = plugin->perform(node, edge_entered, edge_exited, route, pose); + updateResult(plugin->getName(), op_result, result); + } +} + +OperationsResult OperationsManager::process( + const bool status_change, + const RouteTrackingState & state, + const Route & route, + const geometry_msgs::msg::PoseStamped & pose, + const ReroutingState & rerouting_info) +{ + // Get important state information + OperationsResult result; + NodePtr node = state.last_node; + EdgePtr edge_entered = state.current_edge; + EdgePtr edge_exited = + state.route_edges_idx > 0 ? route.edges[state.route_edges_idx - 1] : nullptr; + + // If we have rerouting_info.curr_edge, then after the first node is achieved, + // the robot is exiting the partial previous edge. + if (state.route_edges_idx == 0 && rerouting_info.curr_edge) { + edge_exited = rerouting_info.curr_edge; + } + + if (status_change) { + // Process operations defined in the navigation graph at node or edge + OperationPtrs operations = findGraphOperations(node, edge_entered, edge_exited); + for (unsigned int i = 0; i != operations.size(); i++) { + auto op = graph_operations_.find(operations[i]->type); + if (op != graph_operations_.end()) { + OperationResult op_result = op->second->perform( + node, edge_entered, edge_exited, route, pose, &operations[i]->metadata); + updateResult(op->second->getName(), op_result, result); + } else { + throw nav2_core::OperationFailed( + "Operation " + operations[i]->type + + " does not exist in route operations loaded!"); + } + } + + // Process operations which trigger on any status changes + processOperationsPluginVector( + change_operations_, result, node, edge_entered, edge_exited, route, pose); + } + + // Process operations which trigger regardless of status change or nodes / edges + processOperationsPluginVector( + query_operations_, result, node, edge_entered /*edge_curr*/, edge_exited, route, pose); + return result; +} + +} // namespace nav2_route diff --git a/nav2_route/src/path_converter.cpp b/nav2_route/src/path_converter.cpp new file mode 100644 index 00000000000..38adea20c25 --- /dev/null +++ b/nav2_route/src/path_converter.cpp @@ -0,0 +1,124 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 +#include +#include +#include + +#include "nav2_route/path_converter.hpp" + +namespace nav2_route +{ + +void PathConverter::configure(nav2_util::LifecycleNode::SharedPtr node) +{ + // Density to make path points + nav2_util::declare_parameter_if_not_declared( + node, "path_density", rclcpp::ParameterValue(0.05)); + density_ = static_cast(node->get_parameter("path_density").as_double()); + + path_pub_ = node->create_publisher("plan", 1); + path_pub_->on_activate(); +} + +nav_msgs::msg::Path PathConverter::densify( + const Route & route, + const ReroutingState & rerouting_info, + const std::string & frame, + const rclcpp::Time & now) +{ + nav_msgs::msg::Path path; + path.header.stamp = now; + path.header.frame_id = frame; + + // If we're rerouting and covering the same previous edge to start, + // the path should contain the relevant partial information along edge + // to avoid unnecessary free-space planning where state is retained + if (rerouting_info.curr_edge) { + const Coordinates & start = rerouting_info.closest_pt_on_edge; + const Coordinates & end = rerouting_info.curr_edge->end->coords; + interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + } + + // Fill in path via route edges + for (unsigned int i = 0; i != route.edges.size(); i++) { + const EdgePtr edge = route.edges[i]; + const Coordinates & start = edge->start->coords; + const Coordinates & end = edge->end->coords; + interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + } + + if (route.edges.empty()) { + path.poses.push_back(utils::toMsg(route.start_node->coords.x, route.start_node->coords.y)); + } else { + path.poses.push_back( + utils::toMsg(route.edges.back()->end->coords.x, route.edges.back()->end->coords.y)); + } + + // Set path poses orientations for each point + for (size_t i = 0; i < path.poses.size() - 1; ++i) { + const auto & pose = path.poses[i]; + const auto & next_pose = path.poses[i + 1]; + const double dx = next_pose.pose.position.x - pose.pose.position.x; + const double dy = next_pose.pose.position.y - pose.pose.position.y; + const double yaw = atan2(dy, dx); + path.poses[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw); + } + + // Set the last pose orientation to the last edge + if (!route.edges.empty()) { + const auto & last_edge = route.edges.back(); + const double dx = last_edge->end->coords.x - last_edge->start->coords.x; + const double dy = last_edge->end->coords.y - last_edge->start->coords.y; + path.poses.back().pose.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(atan2(dy, dx)); + } + + // publish path similar to planner server + path_pub_->publish(std::make_unique(path)); + + return path; +} + +void PathConverter::interpolateEdge( + float x0, float y0, float x1, float y1, + std::vector & poses) +{ + // Find number of points to populate by given density + const float mag = hypotf(x1 - x0, y1 - y0); + const unsigned int num_pts = ceil(mag / density_); + const float iterpolated_dist = mag / num_pts; + + // Find unit vector direction + float ux = (x1 - x0) / mag; + float uy = (y1 - y0) / mag; + + // March along it until dist + float x = x0; + float y = y0; + poses.push_back(utils::toMsg(x, y)); + + unsigned int pt_ctr = 0; + while (pt_ctr < num_pts - 1) { + x += ux * iterpolated_dist; + y += uy * iterpolated_dist; + pt_ctr++; + poses.push_back(utils::toMsg(x, y)); + } +} + +} // namespace nav2_route diff --git a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp new file mode 100644 index 00000000000..445f2572328 --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp @@ -0,0 +1,152 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 "nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp" + +namespace nav2_route +{ + +void CostmapScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr costmap_subscriber, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring costmap scorer."); + name_ = name; + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + // Find whether to use average or maximum cost values + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".use_maximum", rclcpp::ParameterValue(true)); + use_max_ = static_cast(node->get_parameter(getName() + ".use_maximum").as_bool()); + + // Edge is invalid if its in collision + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".invalid_on_collision", rclcpp::ParameterValue(true)); + invalid_on_collision_ = + static_cast(node->get_parameter(getName() + ".invalid_on_collision").as_bool()); + + // Edge is invalid if edge is off the costmap + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".invalid_off_map", rclcpp::ParameterValue(true)); + invalid_off_map_ = + static_cast(node->get_parameter(getName() + ".invalid_off_map").as_bool()); + + // Max cost to be considered valid + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".max_cost", rclcpp::ParameterValue(253.0)); + max_cost_ = static_cast(node->get_parameter(getName() + ".max_cost").as_double()); + + // Resolution to check the costmap over (1=every cell, 2=every other cell, etc.) + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".check_resolution", rclcpp::ParameterValue(2)); + check_resolution_ = static_cast( + node->get_parameter(getName() + ".check_resolution").as_int()); + + // Create costmap subscriber if not the same as the server costmap + std::string server_costmap_topic = node->get_parameter("costmap_topic").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".costmap_topic", + rclcpp::ParameterValue(std::string("global_costmap/costmap_raw"))); + std::string costmap_topic = + node->get_parameter(getName() + ".costmap_topic").as_string(); + if (costmap_topic != server_costmap_topic) { + costmap_subscriber_ = std::make_shared( + node, costmap_topic); + RCLCPP_INFO(node->get_logger(), + "Using costmap topic: %s instead of server costmap topic: %s for CostmapScorer.", + costmap_topic.c_str(), server_costmap_topic.c_str()); + } else { + costmap_subscriber_ = costmap_subscriber; + } + + // Find the proportional weight to apply, if multiple cost functions + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".weight", rclcpp::ParameterValue(1.0)); + weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); +} + +void CostmapScorer::prepare() +{ + try { + costmap_ = costmap_subscriber_->getCostmap(); + } catch (...) { + costmap_.reset(); + } +} + +bool CostmapScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + if (!costmap_) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000, "No costmap yet received!"); + return false; + } + + float largest_cost = 0.0, running_cost = 0.0, point_cost = 0.0; + unsigned int x0, y0, x1, y1, idx = 0; + if (!costmap_->worldToMap(edge->start->coords.x, edge->start->coords.y, x0, y0) || + !costmap_->worldToMap(edge->end->coords.x, edge->end->coords.y, x1, y1)) + { + if (invalid_off_map_) { + // Edge is invalid if it is off the costmap + return false; + } + return true; + } + + for (nav2_util::LineIterator iter(x0, y0, x1, y1); iter.isValid(); ) { + point_cost = static_cast(costmap_->getCost(iter.getX(), iter.getY())); + if (point_cost >= max_cost_ && max_cost_ != 255.0f /*Unknown*/ && invalid_on_collision_) { + // Edge is invalid if it is in collision or higher than max allowed cost + return false; + } + + idx++; + running_cost += point_cost; + if (largest_cost < point_cost && point_cost != 255.0) { + largest_cost = point_cost; + } + + // Advance the iterator by the check resolution on the edge, pruning to a coarse resolution + for (unsigned int i = 0; i < check_resolution_; i++) { + iter.advance(); + } + } + + if (use_max_) { + cost = weight_ * largest_cost / max_cost_; + } else { + cost = weight_ * running_cost / (static_cast(idx) * max_cost_); + } + + return true; +} + +std::string CostmapScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::CostmapScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp new file mode 100644 index 00000000000..28e313db8cb --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/distance_scorer.cpp @@ -0,0 +1,65 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "nav2_route/plugins/edge_cost_functions/distance_scorer.hpp" + +namespace nav2_route +{ + +void DistanceScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring distance scorer."); + name_ = name; + + // Find the tag at high the speed limit information is stored + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".speed_tag", rclcpp::ParameterValue("speed_limit")); + speed_tag_ = node->get_parameter(getName() + ".speed_tag").as_string(); + + // Find the proportional weight to apply, if multiple cost functions + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".weight", rclcpp::ParameterValue(1.0)); + weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); +} + +bool DistanceScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + // Get the speed limit, if set for an edge + float speed_val = 1.0f; + speed_val = edge->metadata.getValue(speed_tag_, speed_val); + cost = weight_ * hypotf( + edge->end->coords.x - edge->start->coords.x, + edge->end->coords.y - edge->start->coords.y) / speed_val; + return true; +} + +std::string DistanceScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::DistanceScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp new file mode 100644 index 00000000000..bd0a9a3c4ba --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/dynamic_edges_scorer.cpp @@ -0,0 +1,94 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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 "nav2_route/plugins/edge_cost_functions/dynamic_edges_scorer.hpp" + +namespace nav2_route +{ + +void DynamicEdgesScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring adjust edges scorer."); + name_ = name; + logger_ = node->get_logger(); + service_ = + node->create_service( + std::string(node->get_name()) + "/" + getName() + "/adjust_edges", std::bind( + &DynamicEdgesScorer::closedEdgesCb, this, + std::placeholders::_1, std::placeholders::_2)); + + dynamic_penalties_.clear(); + closed_edges_.clear(); +} + +void DynamicEdgesScorer::closedEdgesCb( + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO(logger_, "Edge closure and cost adjustment in progress!"); + + // Add new closed edges + for (unsigned int edge : request->closed_edges) { + closed_edges_.insert(edge); + } + + // Removed now opened edges, if stored + for (unsigned int edge : request->opened_edges) { + if (closed_edges_.find(edge) != closed_edges_.end()) { + closed_edges_.erase(edge); + } + } + + // Add dynamic costs from application system for edges + for (auto & edge : request->adjust_edges) { + dynamic_penalties_[edge.edgeid] = edge.cost; + } + + response->success = true; +} + +bool DynamicEdgesScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + // Find if this edge is in the closed set of edges + if (closed_edges_.find(edge->edgeid) != closed_edges_.end()) { + return false; + } + + const auto & dyn_pen = dynamic_penalties_.find(edge->edgeid); + if (dyn_pen != dynamic_penalties_.end()) { + cost = dyn_pen->second; + } + + return true; +} + +std::string DynamicEdgesScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::DynamicEdgesScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp new file mode 100644 index 00000000000..cfd0d82f0c0 --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/goal_orientation_scorer.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2024, Polymath Robotics 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 "nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp" + +namespace nav2_route +{ + +void GoalOrientationScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring goal orientation scorer."); + name_ = name; + logger_ = node->get_logger(); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".orientation_tolerance", rclcpp::ParameterValue(M_PI / 2.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".orientation_weight", rclcpp::ParameterValue(1.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".use_orientation_threshold", rclcpp::ParameterValue(false)); + + orientation_tolerance_ = node->get_parameter(getName() + ".orientation_tolerance").as_double(); + orientation_weight_ = + static_cast(node->get_parameter(getName() + ".orientation_weight").as_double()); + use_orientation_threshold_ = + node->get_parameter(getName() + ".use_orientation_threshold").as_bool(); +} + +bool GoalOrientationScorer::score( + const EdgePtr edge, + const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) +{ + if (!route_request.use_poses) { + throw nav2_core::InvalidEdgeScorerUse( + "Cannot use goal orientation scorer without goal pose specified!"); + } + + if (edge_type == EdgeType::END) { + double edge_orientation = std::atan2( + edge->end->coords.y - edge->start->coords.y, + edge->end->coords.x - edge->start->coords.x); + double goal_orientation = tf2::getYaw(route_request.goal_pose.pose.orientation); + double d_yaw = std::abs(angles::shortest_angular_distance(edge_orientation, goal_orientation)); + + if (use_orientation_threshold_) { + if (d_yaw > orientation_tolerance_) { + return false; + } + } + + cost = orientation_weight_ * static_cast(d_yaw); + } + return true; +} + +std::string GoalOrientationScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::GoalOrientationScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp new file mode 100644 index 00000000000..409054fff75 --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/penalty_scorer.cpp @@ -0,0 +1,63 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "nav2_route/plugins/edge_cost_functions/penalty_scorer.hpp" + +namespace nav2_route +{ + +void PenaltyScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring penalty scorer."); + name_ = name; + + // Find the tag at high the speed limit information is stored + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".penalty_tag", rclcpp::ParameterValue("penalty")); + penalty_tag_ = node->get_parameter(getName() + ".penalty_tag").as_string(); + + // Find the proportional weight to apply, if multiple cost functions + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".weight", rclcpp::ParameterValue(1.0)); + weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); +} + +bool PenaltyScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + // Get the speed limit, if set for an edge + float penalty_val = 0.0f; + penalty_val = edge->metadata.getValue(penalty_tag_, penalty_val); + cost = weight_ * penalty_val; + return true; +} + +std::string PenaltyScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::PenaltyScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp new file mode 100644 index 00000000000..f44fbf18f47 --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/semantic_scorer.cpp @@ -0,0 +1,105 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "nav2_route/plugins/edge_cost_functions/semantic_scorer.hpp" + +namespace nav2_route +{ + +void SemanticScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring semantic scorer."); + name_ = name; + + // Find the semantic data + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".semantic_classes", rclcpp::ParameterValue(std::vector{})); + std::vector classes = + node->get_parameter(getName() + ".semantic_classes").as_string_array(); + for (auto & cl : classes) { + nav2_util::declare_parameter_if_not_declared( + node, getName() + "." + cl, rclcpp::ParameterType::PARAMETER_DOUBLE); + const double cost = node->get_parameter(getName() + "." + cl).as_double(); + semantic_info_[cl] = static_cast(cost); + } + + // Find the key to look for semantic data for within the metadata. If set to empty string, + // will search instead for any key in the metadata. + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".semantic_key", rclcpp::ParameterValue(std::string("class"))); + key_ = node->get_parameter(getName() + ".semantic_key").as_string(); + + // Find the proportional weight to apply, if multiple cost functions + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".weight", rclcpp::ParameterValue(1.0)); + weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); +} + +void SemanticScorer::metadataKeyScorer(Metadata & mdata, float & score) +{ + for (auto it = mdata.data.cbegin(); it != mdata.data.cend(); ++it) { + if (auto sem = semantic_info_.find(it->first); sem != semantic_info_.end()) { + score += sem->second; + } + } +} + +void SemanticScorer::metadataValueScorer(Metadata & mdata, float & score) +{ + std::string cl; + cl = mdata.getValue(key_, cl); + if (auto sem = semantic_info_.find(cl); sem != semantic_info_.end()) { + score += sem->second; + } +} + +bool SemanticScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + float score = 0.0; + Metadata & node_mdata = edge->end->metadata; + Metadata & edge_mdata = edge->metadata; + + // If a particular key is known to have semantic info, use it, else search + // each metadata key field to see if it matches + if (key_.empty()) { + metadataKeyScorer(node_mdata, score); + metadataKeyScorer(edge_mdata, score); + } else { + metadataValueScorer(node_mdata, score); + metadataValueScorer(edge_mdata, score); + } + + cost = weight_ * score; + return true; +} + +std::string SemanticScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::SemanticScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp new file mode 100644 index 00000000000..13fd5762a3e --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/start_pose_orientation_scorer.cpp @@ -0,0 +1,88 @@ +// Copyright (c) 2025, Polymath Robotics 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 "nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp" + +namespace nav2_route +{ + +void StartPoseOrientationScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring start pose orientation scorer."); + name_ = name; + logger_ = node->get_logger(); + + nav2_util::declare_parameter_if_not_declared( + node, + getName() + ".orientation_tolerance", rclcpp::ParameterValue(M_PI / 2.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".orientation_weight", rclcpp::ParameterValue(1.0)); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".use_orientation_threshold", rclcpp::ParameterValue(false)); + + orientation_tolerance_ = node->get_parameter(getName() + ".orientation_tolerance").as_double(); + orientation_weight_ = + static_cast(node->get_parameter(getName() + ".orientation_weight").as_double()); + use_orientation_threshold_ = + node->get_parameter(getName() + ".use_orientation_threshold").as_bool(); + + tf_buffer_ = tf_buffer; +} + +bool StartPoseOrientationScorer::score( + const EdgePtr edge, + const RouteRequest & route_request, + const EdgeType & edge_type, float & cost) +{ + if (!route_request.use_poses) { + throw nav2_core::InvalidEdgeScorerUse( + "Cannot use start pose orientation scorer without start pose specified!"); + } + + if (edge_type == EdgeType::START) { + double edge_orientation = std::atan2( + edge->end->coords.y - edge->start->coords.y, + edge->end->coords.x - edge->start->coords.x); + double start_orientation = tf2::getYaw(route_request.start_pose.pose.orientation); + double d_yaw = std::abs(angles::shortest_angular_distance(edge_orientation, start_orientation)); + + if (use_orientation_threshold_) { + if (d_yaw > orientation_tolerance_) { + return false; + } + } + + cost = orientation_weight_ * static_cast(d_yaw); + } + return true; +} + +std::string StartPoseOrientationScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::StartPoseOrientationScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp new file mode 100644 index 00000000000..a334ed6e18d --- /dev/null +++ b/nav2_route/src/plugins/edge_cost_functions/time_scorer.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "nav2_route/plugins/edge_cost_functions/time_scorer.hpp" + +namespace nav2_route +{ + +void TimeScorer::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + const std::shared_ptr/* tf_buffer */, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring time scorer."); + name_ = name; + + // Find the tag at high the speed limit information is stored + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".speed_tag", rclcpp::ParameterValue("abs_speed_limit")); + speed_tag_ = node->get_parameter(getName() + ".speed_tag").as_string(); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".time_tag", rclcpp::ParameterValue("abs_time_taken")); + prev_time_tag_ = node->get_parameter(getName() + ".time_tag").as_string(); + + // Find the proportional weight to apply, if multiple cost functions + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".weight", rclcpp::ParameterValue(1.0)); + weight_ = static_cast(node->get_parameter(getName() + ".weight").as_double()); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".max_vel", rclcpp::ParameterValue(0.5)); + max_vel_ = static_cast(node->get_parameter(getName() + ".max_vel").as_double()); +} + +bool TimeScorer::score( + const EdgePtr edge, + const RouteRequest & /* route_request */, + const EdgeType & /* edge_type */, float & cost) +{ + // We have a previous actual time to utilize for a refined estimate + float time = 0.0; + time = edge->metadata.getValue(prev_time_tag_, time); + if (time > 0.0f) { + cost = weight_ * time; + return true; + } + + // Else: Get the speed limit, if set for an edge, else use max velocity + float velocity = 0.0f; + velocity = edge->metadata.getValue(speed_tag_, velocity); + if (velocity <= 0.0f) { + velocity = max_vel_; + } + + cost = weight_ * hypotf( + edge->end->coords.x - edge->start->coords.x, + edge->end->coords.y - edge->start->coords.y) / velocity; + return true; +} + +std::string TimeScorer::getName() +{ + return name_; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::TimeScorer, nav2_route::EdgeCostFunction) diff --git a/nav2_route/src/plugins/graph_file_loaders/geojson_graph_file_loader.cpp b/nav2_route/src/plugins/graph_file_loaders/geojson_graph_file_loader.cpp new file mode 100644 index 00000000000..51d0fe9bea2 --- /dev/null +++ b/nav2_route/src/plugins/graph_file_loaders/geojson_graph_file_loader.cpp @@ -0,0 +1,241 @@ +// Copyright (c) 2025 Joshua Wallace +// +// 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 +#include + +#include "nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp" + +namespace nav2_route +{ + +void GeoJsonGraphFileLoader::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) +{ + RCLCPP_INFO(node->get_logger(), "Configuring geojson graph file loader"); + logger_ = node->get_logger(); +} + +bool GeoJsonGraphFileLoader::loadGraphFromFile( + Graph & graph, GraphToIDMap & graph_to_id_map, std::string filepath) +{ + if (!doesFileExist(filepath)) { + RCLCPP_ERROR(logger_, "The filepath %s does not exist", filepath.c_str()); + return false; + } + + std::ifstream graph_file(filepath); + Json json_graph; + + try { + json_graph = Json::parse(graph_file); + } catch (Json::parse_error & ex) { + RCLCPP_ERROR(logger_, "Failed to parse %s: %s", filepath.c_str(), ex.what()); + return false; + } + + auto features = json_graph["features"]; + std::vector nodes, edges; + getGraphElements(features, nodes, edges); + + if (nodes.empty() || edges.empty()) { + RCLCPP_ERROR( + logger_, "The graph is malformed. Is does not contain nodes or edges. Please check %s", + filepath.c_str()); + return false; + } + + graph.resize(nodes.size()); + addNodesToGraph(graph, graph_to_id_map, nodes); + addEdgesToGraph(graph, graph_to_id_map, edges); + return true; +} + +bool GeoJsonGraphFileLoader::doesFileExist(const std::string & filepath) +{ + return std::filesystem::exists(filepath); +} + +void GeoJsonGraphFileLoader::getGraphElements( + const Json & features, std::vector & nodes, std::vector & edges) +{ + for (const auto & feature : features) { + if (feature["geometry"]["type"] == "Point") { + nodes.emplace_back(feature); + } else if ( // NOLINT + feature["geometry"]["type"] == "MultiLineString" || + feature["geometry"]["type"] == "LineString") + { + edges.emplace_back(feature); + } + } +} + +void GeoJsonGraphFileLoader::addNodesToGraph( + Graph & graph, GraphToIDMap & graph_to_id_map, std::vector & nodes) +{ + int idx = 0; + for (const auto & node : nodes) { + const auto properties = node["properties"]; + graph[idx].nodeid = properties["id"]; + graph_to_id_map[graph[idx].nodeid] = idx; + graph[idx].coords = convertCoordinatesFromJson(node); + graph[idx].operations = convertOperationsFromJson(properties); + graph[idx].metadata = convertMetaDataFromJson(properties); + idx++; + } +} + +void GeoJsonGraphFileLoader::addEdgesToGraph( + Graph & graph, GraphToIDMap & graph_to_id_map, std::vector & edges) +{ + for (const auto & edge : edges) { + // Required data + const auto properties = edge["properties"]; + unsigned int id = properties["id"]; + unsigned int start_id = properties["startid"]; + unsigned int end_id = properties["endid"]; + + if (graph_to_id_map.find(start_id) == graph_to_id_map.end()) { + RCLCPP_ERROR(logger_, "Start id %u does not exist for edge id %u", start_id, id); + throw nav2_core::NoValidGraph("Start id does not exist"); + } + + if (graph_to_id_map.find(end_id) == graph_to_id_map.end()) { + RCLCPP_ERROR(logger_, "End id of %u does not exist for edge id %u", end_id, id); + throw nav2_core::NoValidGraph("End id does not exist"); + } + + EdgeCost edge_cost = convertEdgeCostFromJson(properties); + Operations operations = convertOperationsFromJson(properties); + Metadata metadata = convertMetaDataFromJson(properties); + + graph[graph_to_id_map[start_id]].addEdge( + edge_cost, &graph[graph_to_id_map[end_id]], id, + metadata, operations); + } +} + +Coordinates GeoJsonGraphFileLoader::convertCoordinatesFromJson(const Json & node) +{ + Coordinates coords; + const auto & properties = node["properties"]; + if (properties.contains("frame")) { + coords.frame_id = properties["frame"]; + } + + const auto & coordinates = node["geometry"]["coordinates"]; + coords.x = coordinates[0]; + coords.y = coordinates[1]; + + return coords; +} + +Metadata GeoJsonGraphFileLoader::convertMetaDataFromJson( + const Json & properties, + const std::string & key) +{ + Metadata metadata; + if (!properties.contains(key)) {return metadata;} + + for (const auto & data : properties[key].items()) { + if (data.value().is_object() ) { + Metadata new_metadata = convertMetaDataFromJson(properties[key], data.key()); + metadata.setValue(data.key(), new_metadata); + continue; + } + + const auto setPrimitiveType = [&](const auto & value) -> std::any + { + if (value.is_number()) { + if (value.is_number_unsigned()) { + return static_cast(value); + } else if (value.is_number_integer()) { + return static_cast(value); + } else { + return static_cast(value); + } + } + + if (value.is_boolean()) { + return static_cast(value); + } + + if (value.is_string()) { + return static_cast(value); + } + RCLCPP_ERROR( + logger_, "Failed to convert the key: %s to a value", data.key().c_str()); + throw std::runtime_error("Failed to convert"); + }; + + if (data.value().is_array()) { + std::vector array; + for (const auto & el : data.value()) { + auto value = setPrimitiveType(el); + array.push_back(value); + } + metadata.setValue(data.key(), array); + continue; + } + + auto value = setPrimitiveType(data.value()); + metadata.setValue(data.key(), value); + } + + return metadata; +} + +Operation GeoJsonGraphFileLoader::convertOperationFromJson(const Json & json_operation) +{ + Operation operation; + json_operation.at("type").get_to(operation.type); + Json trigger = json_operation.at("trigger"); + operation.trigger = trigger.get(); + Metadata metadata = convertMetaDataFromJson(json_operation); + operation.metadata = metadata; + + return operation; +} + +Operations GeoJsonGraphFileLoader::convertOperationsFromJson(const Json & properties) +{ + Operations operations; + if (properties.contains("operations")) { + for (const auto & json_operation : properties["operations"]) { + operations.push_back(convertOperationFromJson(json_operation)); + } + } + return operations; +} + +EdgeCost GeoJsonGraphFileLoader::convertEdgeCostFromJson(const Json & properties) +{ + EdgeCost edge_cost; + if (properties.contains("cost")) { + edge_cost.cost = properties["cost"]; + } + + if (properties.contains("overridable")) { + edge_cost.overridable = properties["overridable"]; + } + return edge_cost; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::GeoJsonGraphFileLoader, nav2_route::GraphFileLoader) diff --git a/nav2_route/src/plugins/graph_file_savers/geojson_graph_file_saver.cpp b/nav2_route/src/plugins/graph_file_savers/geojson_graph_file_saver.cpp new file mode 100644 index 00000000000..64d618cba44 --- /dev/null +++ b/nav2_route/src/plugins/graph_file_savers/geojson_graph_file_saver.cpp @@ -0,0 +1,177 @@ +// Copyright (c) 2024 John Chrosniak +// +// 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 +#include + +#include "nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp" + +namespace nav2_route +{ + +void GeoJsonGraphFileSaver::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node) +{ + RCLCPP_INFO(node->get_logger(), "Configuring geojson graph file saver"); + logger_ = node->get_logger(); +} + +bool GeoJsonGraphFileSaver::saveGraphToFile( + Graph & graph, std::string filepath) +{ + if (filepath.empty()) { + RCLCPP_ERROR(logger_, "File path is empty"); + return false; + } + Json json_graph, json_crs, json_properties; + json_graph["type"] = "FeatureCollection"; + json_graph["name"] = "graph"; + json_properties["name"] = "urn:ogc:def:crs:EPSG::3857"; + json_crs["type"] = "name"; + json_crs["properties"] = json_properties; + json_graph["crs"] = json_crs; + std::vector json_features; + try { + loadNodesFromGraph(graph, json_features); + loadEdgesFromGraph(graph, json_features); + json_graph["features"] = json_features; + std::ofstream file(filepath); + file << json_graph.dump(4) << std::endl; + file.close(); + } catch (const std::exception & e) { + RCLCPP_ERROR(logger_, "An error occurred: %s", e.what()); + return false; + } + return true; +} + +void GeoJsonGraphFileSaver::loadNodesFromGraph( + Graph & graph, std::vector & json_features) +{ + for (const auto & node : graph) { + if (node.nodeid == std::numeric_limits::max()) { // Skip "deleted" nodes + continue; + } + Json json_feature, json_properties, json_geometry, json_metadata, json_operations; + json_geometry["type"] = "Point"; + json_geometry["coordinates"] = std::vector{node.coords.x, node.coords.y}; + json_feature["geometry"] = json_geometry; + json_properties["id"] = node.nodeid; + json_properties["frame"] = node.coords.frame_id; + convertMetaDataToJson(node.metadata, json_metadata); + if (json_metadata.size() > 0) { + json_properties["metadata"] = json_metadata; + } + convertOperationsToJson(node.operations, json_operations); + if (json_operations.size() > 0) { + json_properties["operations"] = json_operations; + } + json_feature["properties"] = json_properties; + json_feature["type"] = "Feature"; + json_features.push_back(json_feature); + } +} + +void GeoJsonGraphFileSaver::loadEdgesFromGraph( + Graph & graph, std::vector & json_edges) +{ + for (const auto & node : graph) { + for (const auto & edge : node.neighbors) { + Json json_edge, json_properties, json_geometry, json_metadata, json_operations; + json_geometry["type"] = "MultiLineString"; + json_edge["geometry"] = json_geometry; + json_properties["id"] = edge.edgeid; + json_properties["startid"] = edge.start->nodeid; + json_properties["endid"] = edge.end->nodeid; + convertMetaDataToJson(edge.metadata, json_metadata); + if (json_metadata.size() > 0) { + json_properties["metadata"] = json_metadata; + } + convertOperationsToJson(edge.operations, json_operations); + if (json_operations.size() > 0) { + json_properties["operations"] = json_operations; + } + json_properties["cost"] = edge.edge_cost.cost; + json_properties["overridable"] = edge.edge_cost.overridable; + json_edge["properties"] = json_properties; + json_edge["type"] = "Feature"; + json_edges.push_back(json_edge); + } + } +} + +void GeoJsonGraphFileSaver::convertMetaDataToJson( + const Metadata & metadata, Json & json_metadata) +{ + /* Function partially created using GPT */ + for (auto itr = metadata.data.begin(); itr != metadata.data.end(); itr++) { + if (itr->second.type() == typeid(std::string)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(int)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(unsigned int)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(float)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(bool)) { + json_metadata[itr->first] = std::any_cast(itr->second); + } else if (itr->second.type() == typeid(Metadata)) { + // If the itr->second is another Metadata, recursively convert it to JSON + Json nested_metadata_json; + convertMetaDataToJson(std::any_cast(itr->second), nested_metadata_json); + json_metadata[itr->first] = nested_metadata_json; + } else if (itr->second.type() == typeid(std::vector)) { + // If the itr->second is a vector, convert each element + std::vector arrayJson; + for (const auto & element : std::any_cast>(itr->second)) { + if (element.type() == typeid(std::string)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(int)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(unsigned int)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(float)) { + arrayJson.push_back(std::any_cast(element)); + } else if (element.type() == typeid(bool)) { + arrayJson.push_back(std::any_cast(element)); + } + } + json_metadata[itr->first] = arrayJson; + } else { + // If we have an unknown type, handle as needed + json_metadata[itr->first] = itr->second.type().name(); + } + } +} + +void GeoJsonGraphFileSaver::convertOperationsToJson( + const Operations & operations, Json & json_operations) +{ + for (const auto & operation : operations) { + Json json_operation, json_metadata; + json_operation["type"] = operation.type; + json_operation["trigger"] = operation.trigger; + convertMetaDataToJson(operation.metadata, json_metadata); + if (json_metadata.size()) { + json_operation["metadata"] = json_metadata; + } + json_operations[operation.type] = json_operation; + } +} +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::GeoJsonGraphFileSaver, nav2_route::GraphFileSaver) diff --git a/nav2_route/src/plugins/route_operations/adjust_speed_limit.cpp b/nav2_route/src/plugins/route_operations/adjust_speed_limit.cpp new file mode 100644 index 00000000000..46802194e29 --- /dev/null +++ b/nav2_route/src/plugins/route_operations/adjust_speed_limit.cpp @@ -0,0 +1,72 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "nav2_route/plugins/route_operations/adjust_speed_limit.hpp" + +namespace nav2_route +{ + +void AdjustSpeedLimit::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring Adjust speed limit operation."); + name_ = name; + logger_ = node->get_logger(); + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".speed_tag", rclcpp::ParameterValue("speed_limit")); + speed_tag_ = node->get_parameter(getName() + ".speed_tag").as_string(); + + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".speed_limit_topic", rclcpp::ParameterValue("speed_limit")); + std::string topic = node->get_parameter(getName() + ".speed_tag").as_string(); + + speed_limit_pub_ = node->create_publisher(topic, 1); + speed_limit_pub_->on_activate(); +} + +OperationResult AdjustSpeedLimit::perform( + NodePtr /*node_achieved*/, + EdgePtr edge_entered, + EdgePtr /*edge_exited*/, + const Route & /*route*/, + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, + const Metadata * /*mdata*/) +{ + OperationResult result; + if (!edge_entered) { + return result; + } + + float speed_limit = 100.0; + speed_limit = edge_entered->metadata.getValue(speed_tag_, speed_limit); + RCLCPP_DEBUG(logger_, "Setting speed limit to %.2f%% of maximum.", speed_limit); + + auto msg = std::make_unique(); + msg->percentage = true; + msg->speed_limit = speed_limit; + speed_limit_pub_->publish(std::move(msg)); + return result; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::AdjustSpeedLimit, nav2_route::RouteOperation) diff --git a/nav2_route/src/plugins/route_operations/collision_monitor.cpp b/nav2_route/src/plugins/route_operations/collision_monitor.cpp new file mode 100644 index 00000000000..9bc7c1c2d7e --- /dev/null +++ b/nav2_route/src/plugins/route_operations/collision_monitor.cpp @@ -0,0 +1,241 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 + +#include "nav2_route/plugins/route_operations/collision_monitor.hpp" + +namespace nav2_route +{ + +void CollisionMonitor::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_subscriber, + const std::string & name) +{ + name_ = name; + clock_ = node->get_clock(); + logger_ = node->get_logger(); + last_check_time_ = clock_->now(); + + std::string server_costmap_topic = node->get_parameter("costmap_topic").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".costmap_topic", rclcpp::ParameterValue("local_costmap/costmap_raw")); + std::string costmap_topic = node->get_parameter(getName() + ".costmap_topic").as_string(); + if (costmap_topic != server_costmap_topic) { + RCLCPP_INFO(node->get_logger(), + "Using costmap topic: %s instead of server costmap topic: %s for CollisionMonitor.", + costmap_topic.c_str(), server_costmap_topic.c_str()); + costmap_subscriber_ = std::make_shared(node, costmap_topic); + topic_ = costmap_topic; + } else { + costmap_subscriber_ = costmap_subscriber; + topic_ = server_costmap_topic; + } + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".rate", rclcpp::ParameterValue(1.0)); + double checking_rate = node->get_parameter(getName() + ".rate").as_double(); + checking_duration_ = rclcpp::Duration::from_seconds(1.0 / checking_rate); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".reroute_on_collision", rclcpp::ParameterValue(true)); + reroute_on_collision_ = node->get_parameter(getName() + ".reroute_on_collision").as_bool(); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".max_cost", rclcpp::ParameterValue(253.0)); + max_cost_ = static_cast(node->get_parameter(getName() + ".max_cost").as_double()); + + // Resolution to check the costmap over (1=every cell, 2=every other cell, etc.) + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".check_resolution", rclcpp::ParameterValue(1)); + check_resolution_ = static_cast( + node->get_parameter(getName() + ".check_resolution").as_int()); + + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".max_collision_dist", rclcpp::ParameterValue(5.0)); + max_collision_dist_ = static_cast( + node->get_parameter(getName() + ".max_collision_dist").as_double()); + if (max_collision_dist_ <= 0.0) { + RCLCPP_INFO( + logger_, "Max collision distance to evaluate is zero or negative, checking the full route."); + max_collision_dist_ = std::numeric_limits::max(); + } +} + +void CollisionMonitor::getCostmap() +{ + try { + costmap_ = costmap_subscriber_->getCostmap(); + } catch (...) { + throw nav2_core::OperationFailed( + "Collision Monitor could not obtain a costmap from topic: " + topic_); + } +} + +OperationResult CollisionMonitor::perform( + NodePtr /*node*/, + EdgePtr curr_edge, + EdgePtr /*edge_exited*/, + const Route & route, + const geometry_msgs::msg::PoseStamped & curr_pose, + const Metadata * /*mdata*/) +{ + // Not time yet to check or before getting to first route edge + auto now = clock_->now(); + if (now - last_check_time_ < checking_duration_ || !curr_edge) { + return OperationResult(); + } + last_check_time_ = now; + + OperationResult result; + getCostmap(); + + float dist_checked = 0.0; + Coordinates end = curr_edge->end->coords; + Coordinates start = utils::findClosestPoint( + curr_pose, curr_edge->start->coords, end); + unsigned int curr_edge_id = curr_edge->edgeid; + + bool final_edge = false; + while (!final_edge) { + // Track how far we've checked and should check for collisions + const float edge_dist = hypotf(end.x - start.x, end.y - start.y); + if (dist_checked + edge_dist >= max_collision_dist_) { + float dist_to_eval = max_collision_dist_ - dist_checked; + end = backoutValidEndPoint(start, end, dist_to_eval); + final_edge = true; + } + dist_checked += edge_dist; + + // Find the valid edge grid coords, in case the edge is partially off the grid + LineSegment line; + if (!lineToMap(start, end, line)) { + final_edge = true; + if (!backoutValidEndPoint(start, line)) { + break; + } + } + + // Collision check edge on grid within max distance and + // report blocked edges for rerouting or exit task + if (isInCollision(line)) { + RCLCPP_INFO( + logger_, "Collision has been detected within %0.2fm of robot pose!", max_collision_dist_); + + if (reroute_on_collision_) { + result.reroute = true; + result.blocked_ids.push_back(curr_edge_id); + return result; + } + + throw nav2_core::OperationFailed( + "Collision detected, but rerouting is not enabled, canceling tracking task."); + } + + // Restart loop for next edge until complete + start = end; + if (!final_edge) { + auto isCurrEdge = [&](const EdgePtr & edge) {return edge->edgeid == curr_edge_id;}; + auto iter = std::find_if(route.edges.begin(), route.edges.end(), isCurrEdge); + if (iter != route.edges.end() && ++iter != route.edges.end()) { + // If we found the edge and the next edge is also valid + curr_edge_id = (*iter)->edgeid; + end = (*iter)->end->coords; + } else { + final_edge = true; + } + } + } + + return result; +} + +Coordinates CollisionMonitor::backoutValidEndPoint( + const Coordinates & start, const Coordinates & end, const float dist) +{ + Coordinates new_end; + const float dx = end.x - start.x; + const float dy = end.y - start.y; + const float mag = hypotf(dx, dy); + if (mag < 1e-6) { + return start; + } + new_end.x = (dx / mag) * dist + start.x; + new_end.y = (dy / mag) * dist + start.y; + return new_end; +} + +bool CollisionMonitor::backoutValidEndPoint( + const Coordinates & start, LineSegment & line) +{ + // Check if any part of this edge is potentially valid + if (!costmap_->worldToMap(start.x, start.y, line.x0, line.y0)) { + return false; + } + + // Since worldToMap will populate the out-of-bounds (x1, y1), we can + // iterate through the partially valid line until we hit invalid coords + unsigned int last_end_x = line.x0, last_end_y = line.y0; + nav2_util::LineIterator iter(line.x0, line.y0, line.x1, line.y1); + int size_x = static_cast(costmap_->getSizeInCellsX()); + int size_y = static_cast(costmap_->getSizeInCellsY()); + for (; iter.isValid(); iter.advance()) { + if (iter.getX() >= size_x || iter.getY() >= size_y) { + line.x1 = last_end_x; + line.y1 = last_end_y; + return true; + } + last_end_x = iter.getX(); + last_end_y = iter.getY(); + } + + return false; +} + +bool CollisionMonitor::lineToMap( + const Coordinates & start, const Coordinates & end, LineSegment & line) +{ + if (!costmap_->worldToMap(start.x, start.y, line.x0, line.y0) || + !costmap_->worldToMap(end.x, end.y, line.x1, line.y1)) + { + return false; + } + return true; +} + +bool CollisionMonitor::isInCollision(const LineSegment & line) +{ + nav2_util::LineIterator iter(line.x0, line.y0, line.x1, line.y1); + for (; iter.isValid(); ) { + float cost = static_cast(costmap_->getCost(iter.getX(), iter.getY())); + if (cost >= max_cost_ && cost != 255.0 /*unknown*/) { + return true; + } + + // Advance the iterator by the check resolution on the edge, pruning to a coarse resolution + for (unsigned int i = 0; i < check_resolution_; i++) { + iter.advance(); + } + } + return false; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::CollisionMonitor, nav2_route::RouteOperation) diff --git a/nav2_route/src/plugins/route_operations/rerouting_service.cpp b/nav2_route/src/plugins/route_operations/rerouting_service.cpp new file mode 100644 index 00000000000..44cfb4d44a9 --- /dev/null +++ b/nav2_route/src/plugins/route_operations/rerouting_service.cpp @@ -0,0 +1,70 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "nav2_route/plugins/route_operations/rerouting_service.hpp" + +namespace nav2_route +{ + +void ReroutingService::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring Rerouting service operation."); + name_ = name; + logger_ = node->get_logger(); + reroute_.store(false); + service_ = node->create_service( + std::string(node->get_name()) + "/" + getName() + "/reroute", + std::bind( + &ReroutingService::serviceCb, this, + std::placeholders::_1, std::placeholders::_2)); +} + +void ReroutingService::serviceCb( + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + RCLCPP_INFO(logger_, "A reroute has been requested!"); + reroute_.store(true); + response->success = true; +} + +OperationResult ReroutingService::perform( + NodePtr /*node*/, + EdgePtr /*edge_entered*/, + EdgePtr /*edge_exited*/, + const Route & /*route*/, + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, + const Metadata * /*mdata*/) +{ + OperationResult result; + if (reroute_.load()) { + reroute_.store(false); + result.reroute = true; + return result; + } + + return result; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::ReroutingService, nav2_route::RouteOperation) diff --git a/nav2_route/src/plugins/route_operations/time_marker.cpp b/nav2_route/src/plugins/route_operations/time_marker.cpp new file mode 100644 index 00000000000..4da0cbd2c19 --- /dev/null +++ b/nav2_route/src/plugins/route_operations/time_marker.cpp @@ -0,0 +1,67 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "nav2_route/plugins/route_operations/time_marker.hpp" + +namespace nav2_route +{ + +void TimeMarker::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr/* costmap_subscriber */, + const std::string & name) +{ + RCLCPP_INFO(node->get_logger(), "Configuring Adjust speed limit operation."); + name_ = name; + nav2_util::declare_parameter_if_not_declared( + node, getName() + ".time_tag", rclcpp::ParameterValue("abs_time_taken")); + time_tag_ = node->get_parameter(getName() + ".time_tag").as_string(); + clock_ = node->get_clock(); + edge_start_time_ = rclcpp::Time(0.0); +} + +OperationResult TimeMarker::perform( + NodePtr /*node_achieved*/, + EdgePtr edge_entered, + EdgePtr edge_exited, + const Route & /*route*/, + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, + const Metadata * /*mdata*/) +{ + OperationResult result; + rclcpp::Time now = clock_->now(); + if (!edge_exited || edge_exited->edgeid != curr_edge_) { + edge_start_time_ = now; + curr_edge_ = edge_entered ? edge_entered->edgeid : 0; + return result; + } + + float dur = static_cast((now - edge_start_time_).seconds()); + if (edge_start_time_.seconds() > 1e-3) { + edge_exited->metadata.setValue(time_tag_, dur); + } + + edge_start_time_ = now; + curr_edge_ = edge_entered ? edge_entered->edgeid : 0; + return result; +} + +} // namespace nav2_route + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::TimeMarker, nav2_route::RouteOperation) diff --git a/nav2_route/src/plugins/route_operations/trigger_event.cpp b/nav2_route/src/plugins/route_operations/trigger_event.cpp new file mode 100644 index 00000000000..91ba6882537 --- /dev/null +++ b/nav2_route/src/plugins/route_operations/trigger_event.cpp @@ -0,0 +1,17 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "nav2_route/plugins/route_operations/trigger_event.hpp" +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_route::TriggerEvent, nav2_route::RouteOperation) diff --git a/nav2_route/src/route_planner.cpp b/nav2_route/src/route_planner.cpp new file mode 100644 index 00000000000..1fc005837c2 --- /dev/null +++ b/nav2_route/src/route_planner.cpp @@ -0,0 +1,218 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 +#include +#include +#include + +#include "nav2_route/route_planner.hpp" + +namespace nav2_route +{ + +void RoutePlanner::configure( + nav2_util::LifecycleNode::SharedPtr node, + const std::shared_ptr tf_buffer, + const std::shared_ptr costmap_subscriber) +{ + nav2_util::declare_parameter_if_not_declared( + node, "max_iterations", rclcpp::ParameterValue(0)); + max_iterations_ = node->get_parameter("max_iterations").as_int(); + + if (max_iterations_ <= 0) { + max_iterations_ = std::numeric_limits::max(); + } + + edge_scorer_ = std::make_unique(node, tf_buffer, costmap_subscriber); +} + +Route RoutePlanner::findRoute( + Graph & graph, unsigned int start_index, unsigned int goal_index, + const std::vector & blocked_ids, + const RouteRequest & route_request) +{ + if (graph.empty()) { + throw nav2_core::NoValidGraph("Graph is invalid for routing!"); + } + + // Find the start and goal pointers, it is important in this function + // that the start node is the underlying pointer, so that the address + // is valid when this function goes out of scope + const NodePtr & start_node = &graph.at(start_index); + const NodePtr & goal_node = &graph.at(goal_index); + findShortestGraphTraversal(graph, start_node, goal_node, blocked_ids, route_request); + + EdgePtr & parent_edge = goal_node->search_state.parent_edge; + if (!parent_edge) { + throw nav2_core::NoValidRouteCouldBeFound("Could not find a route to the requested goal!"); + } + + // Convert graph traversal into a meaningful route + Route route; + while (parent_edge) { + route.edges.push_back(parent_edge); + parent_edge = parent_edge->start->search_state.parent_edge; + } + + std::reverse(route.edges.begin(), route.edges.end()); + route.start_node = start_node; + route.route_cost = goal_node->search_state.integrated_cost; + return route; +} + +void RoutePlanner::resetSearchStates(Graph & graph) +{ + // For graphs < 75,000 nodes, iterating through one time on initialization to reset the state + // is neglibably different to allocating & deallocating the complimentary blocks of memory + for (unsigned int i = 0; i != graph.size(); i++) { + graph[i].search_state.reset(); + } +} + +void RoutePlanner::findShortestGraphTraversal( + Graph & graph, const NodePtr start_node, const NodePtr goal_node, + const std::vector & blocked_ids, + const RouteRequest & route_request) +{ + // Setup the Dijkstra's search + resetSearchStates(graph); + start_id_ = start_node->nodeid; + goal_id_ = goal_node->nodeid; + start_node->search_state.integrated_cost = 0.0; + addNode(0.0, start_node); + + NodePtr neighbor{nullptr}; + EdgePtr edge{nullptr}; + float potential_cost = 0.0, traversal_cost = 0.0; + int iterations = 0; + while (!queue_.empty() && iterations < max_iterations_) { + iterations++; + + // Get the next lowest cost node + auto [curr_cost, node] = getNextNode(); + + // This has been visited, thus already lowest cost + if (curr_cost != node->search_state.integrated_cost) { + continue; + } + + // We have the shortest path + if (isGoal(node)) { + // Reset states + clearQueue(); + return; + } + + // Expand to connected nodes + EdgeVector & edges = getEdges(node); + for (unsigned int edge_num = 0; edge_num != edges.size(); edge_num++) { + edge = &edges[edge_num]; + neighbor = edge->end; + + // If edge is invalid (lane closed, occupied, etc), don't expand + if (!getTraversalCost(edge, traversal_cost, blocked_ids, route_request)) { + continue; + } + + potential_cost = curr_cost + traversal_cost; + if (potential_cost < neighbor->search_state.integrated_cost) { + neighbor->search_state.parent_edge = edge; + neighbor->search_state.integrated_cost = potential_cost; + neighbor->search_state.traversal_cost = traversal_cost; + addNode(potential_cost, neighbor); + } + } + } + + if (iterations == max_iterations_) { + // Reset states + clearQueue(); + throw nav2_core::TimedOut("Maximum iterations was exceeded!"); + } +} + +bool RoutePlanner::getTraversalCost( + const EdgePtr edge, float & score, const std::vector & blocked_ids, + const RouteRequest & route_request) +{ + // If edge or node is in the blocked list, don't expand + auto is_blocked = std::find_if( + blocked_ids.begin(), blocked_ids.end(), + [&](unsigned int id) {return id == edge->edgeid || id == edge->end->nodeid;}); + if (is_blocked != blocked_ids.end()) { + return false; + } + + // If an edge's cost is marked as not to be overridden by scoring plugins + // Or there are no scoring plugins, use the edge's cost, if it is valid (positive) + if (!edge->edge_cost.overridable || edge_scorer_->numPlugins() == 0) { + if (edge->edge_cost.cost <= 0.0) { + throw nav2_core::NoValidGraph( + "Edge " + std::to_string(edge->edgeid) + + " doesn't contain and cannot compute a valid edge cost!"); + } + score = edge->edge_cost.cost; + return true; + } + + return edge_scorer_->score(edge, route_request, classifyEdge(edge), score); +} + +NodeElement RoutePlanner::getNextNode() +{ + NodeElement data = queue_.top(); + queue_.pop(); + return data; +} + +void RoutePlanner::addNode(const float cost, const NodePtr node) +{ + queue_.emplace(cost, node); +} + +EdgeVector & RoutePlanner::getEdges(const NodePtr node) +{ + return node->neighbors; +} + +void RoutePlanner::clearQueue() +{ + NodeQueue q; + std::swap(queue_, q); +} + +bool RoutePlanner::isGoal(const NodePtr node) +{ + return node->nodeid == goal_id_; +} + +bool RoutePlanner::isStart(const NodePtr node) +{ + return node->nodeid == start_id_; +} + +nav2_route::EdgeType RoutePlanner::classifyEdge(const EdgePtr edge) +{ + if (isStart(edge->start)) { + return EdgeType::START; + } else if (isGoal(edge->end)) { + return EdgeType::END; + } + return nav2_route::EdgeType::NONE; +} + +} // namespace nav2_route diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp new file mode 100644 index 00000000000..aad64e16ec6 --- /dev/null +++ b/nav2_route/src/route_server.cpp @@ -0,0 +1,408 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include "nav2_route/route_server.hpp" + +using nav2_util::declare_parameter_if_not_declared; +using std::placeholders::_1; +using std::placeholders::_2; + +namespace nav2_route +{ + +RouteServer::RouteServer(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("route_server", "", options) +{} + +nav2_util::CallbackReturn +RouteServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + tf_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + get_node_base_interface(), + get_node_timers_interface()); + tf_->setCreateTimerInterface(timer_interface); + transform_listener_ = std::make_shared(*tf_); + + auto node = shared_from_this(); + graph_vis_publisher_ = + node->create_publisher( + "route_graph", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + compute_route_server_ = std::make_shared( + node, "compute_route", + std::bind(&RouteServer::computeRoute, this), + nullptr, std::chrono::milliseconds(500), true); + + compute_and_track_route_server_ = std::make_shared( + node, "compute_and_track_route", + std::bind(&RouteServer::computeAndTrackRoute, this), + nullptr, std::chrono::milliseconds(500), true); + + set_graph_service_ = std::make_shared>>( + std::string(node->get_name()) + "/set_route_graph", + node, + std::bind( + &RouteServer::setRouteGraph, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + declare_parameter_if_not_declared( + node, "route_frame", rclcpp::ParameterValue(std::string("map"))); + declare_parameter_if_not_declared( + node, "base_frame", rclcpp::ParameterValue(std::string("base_link"))); + declare_parameter_if_not_declared( + node, "max_planning_time", rclcpp::ParameterValue(2.0)); + + route_frame_ = node->get_parameter("route_frame").as_string(); + base_frame_ = node->get_parameter("base_frame").as_string(); + max_planning_time_ = node->get_parameter("max_planning_time").as_double(); + + // Create costmap subscriber + nav2_util::declare_parameter_if_not_declared( + node, "costmap_topic", + rclcpp::ParameterValue(std::string("global_costmap/costmap_raw"))); + std::string costmap_topic = node->get_parameter("costmap_topic").as_string(); + costmap_subscriber_ = std::make_shared(node, costmap_topic); + + try { + graph_loader_ = std::make_shared(node, tf_, route_frame_); + if (!graph_loader_->loadGraphFromParameter(graph_, id_to_graph_map_)) { + return nav2_util::CallbackReturn::FAILURE; + } + + goal_intent_extractor_ = std::make_shared(); + goal_intent_extractor_->configure( + node, graph_, &id_to_graph_map_, tf_, costmap_subscriber_, route_frame_, base_frame_); + + route_planner_ = std::make_shared(); + route_planner_->configure(node, tf_, costmap_subscriber_); + + route_tracker_ = std::make_shared(); + route_tracker_->configure( + node, tf_, costmap_subscriber_, compute_and_track_route_server_, route_frame_, base_frame_); + + path_converter_ = std::make_shared(); + path_converter_->configure(node); + } catch (std::exception & e) { + RCLCPP_FATAL(get_logger(), "Failed to configure route server: %s", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RouteServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + compute_route_server_->activate(); + compute_and_track_route_server_->activate(); + graph_vis_publisher_->on_activate(); + graph_vis_publisher_->publish(utils::toMsg(graph_, route_frame_, this->now())); + createBond(); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RouteServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + compute_route_server_->deactivate(); + compute_and_track_route_server_->deactivate(); + graph_vis_publisher_->on_deactivate(); + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RouteServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + compute_route_server_.reset(); + compute_and_track_route_server_.reset(); + set_graph_service_.reset(); + graph_loader_.reset(); + route_planner_.reset(); + route_tracker_.reset(); + path_converter_.reset(); + goal_intent_extractor_.reset(); + graph_vis_publisher_.reset(); + transform_listener_.reset(); + tf_.reset(); + graph_.clear(); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +RouteServer::on_shutdown(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +rclcpp::Duration +RouteServer::findPlanningDuration(const rclcpp::Time & start_time) +{ + auto cycle_duration = this->now() - start_time; + if (max_planning_time_ > 0.0 && cycle_duration.seconds() > max_planning_time_) { + RCLCPP_WARN( + get_logger(), + "Route planner missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", + 1 / max_planning_time_, 1 / cycle_duration.seconds()); + } + + return cycle_duration; +} + +template +bool +RouteServer::isRequestValid( + std::shared_ptr> & action_server) +{ + if (!action_server || !action_server->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); + return false; + } + + if (action_server->is_cancel_requested()) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling route planning action."); + action_server->terminate_all(); + return false; + } + + if (graph_.empty()) { + RCLCPP_INFO(get_logger(), "No graph set! Aborting request."); + action_server->terminate_current(); + return false; + } + + return true; +} + +void RouteServer::populateActionResult( + std::shared_ptr result, + const Route & route, + const nav_msgs::msg::Path & path, + const rclcpp::Duration & planning_duration) +{ + result->route = utils::toMsg(route, route_frame_, this->now()); + result->path = path; + result->planning_time = planning_duration; +} + +void RouteServer::populateActionResult( + std::shared_ptr result, + const Route &, + const nav_msgs::msg::Path &, + const rclcpp::Duration & execution_duration) +{ + result->execution_duration = execution_duration; +} + +template +Route RouteServer::findRoute( + const std::shared_ptr goal, + ReroutingState & rerouting_info) +{ + // Find the search boundaries + auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal); + + // If we're rerouting, use the rerouting start node and pose as the new start + if (rerouting_info.rerouting_start_id != std::numeric_limits::max()) { + start_route = id_to_graph_map_.at(rerouting_info.rerouting_start_id); + goal_intent_extractor_->overrideStart(rerouting_info.rerouting_start_pose); + } + + Route route; + if (start_route == end_route) { + // Succeed with a single-point route + route.route_cost = 0.0; + route.start_node = &graph_.at(start_route); + } else { + // Populate request data (start & goal id, start & goal pose, if set) for routing + RouteRequest route_request; + route_request.start_nodeid = start_route; + route_request.goal_nodeid = end_route; + route_request.start_pose = goal_intent_extractor_->getStart(); + route_request.goal_pose = goal->goal; + route_request.use_poses = goal->use_poses; + + // Compute the route via graph-search, returns a node-edge sequence + route = route_planner_->findRoute( + graph_, start_route, end_route, rerouting_info.blocked_ids, route_request); + } + + return goal_intent_extractor_->pruneStartandGoal(route, goal, rerouting_info); +} + +template +void +RouteServer::processRouteRequest( + std::shared_ptr> & action_server) +{ + auto goal = action_server->get_current_goal(); + auto result = std::make_shared(); + ReroutingState rerouting_info; + auto start_time = this->now(); + + try { + while (rclcpp::ok()) { + if (!isRequestValid(action_server)) { + return; + } + + if (action_server->is_preempt_requested()) { + RCLCPP_INFO(get_logger(), "Computing new preempted route to goal."); + goal = action_server->accept_pending_goal(); + rerouting_info.reset(); + } + + // Find the route + Route route = findRoute(goal, rerouting_info); + RCLCPP_INFO(get_logger(), "Route found with %zu nodes and %zu edges", + route.edges.size() + 1u, route.edges.size()); + auto path = path_converter_->densify(route, rerouting_info, route_frame_, this->now()); + + if (std::is_same::value) { + // blocks until re-route requested or task completion, publishes feedback + switch (route_tracker_->trackRoute(route, path, rerouting_info)) { + case TrackerResult::COMPLETED: + populateActionResult(result, route, path, this->now() - start_time); + action_server->succeeded_current(result); + return; + case TrackerResult::INTERRUPTED: + // Reroute, cancel, or preempt requested + break; + case TrackerResult::EXITED: + // rclcpp::ok() is false, so just return + return; + } + } else { + // Return route if not tracking + populateActionResult(result, route, path, findPlanningDuration(start_time)); + action_server->succeeded_current(result); + return; + } + } + } catch (nav2_core::NoValidRouteCouldBeFound & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::NO_VALID_ROUTE; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::TimedOut & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::TIMEOUT; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::RouteTFError & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::TF_ERROR; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::NoValidGraph & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::NO_VALID_GRAPH; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::IndeterminantNodesOnGraph & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::INDETERMINANT_NODES_ON_GRAPH; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::InvalidEdgeScorerUse & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::INVALID_EDGE_SCORER_USE; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::OperationFailed & ex) { + // A special case since Operation Failed is only in Compute & Track + // actions, specifying it to allow otherwise fully shared code + exceptionWarning(goal, ex); + result->error_code = ComputeAndTrackRoute::Result::OPERATION_FAILED; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (nav2_core::RouteException & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::UNKNOWN; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } catch (std::exception & ex) { + exceptionWarning(goal, ex); + result->error_code = ActionT::Result::UNKNOWN; + result->error_msg = ex.what(); + action_server->terminate_current(result); + } +} + +void +RouteServer::computeRoute() +{ + RCLCPP_INFO(get_logger(), "Computing route to goal."); + processRouteRequest(compute_route_server_); +} + +void +RouteServer::computeAndTrackRoute() +{ + RCLCPP_INFO(get_logger(), "Computing and tracking route to goal."); + processRouteRequest(compute_and_track_route_server_); +} + +void RouteServer::setRouteGraph( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO(get_logger(), "Setting new route graph: %s.", request->graph_filepath.c_str()); + graph_.clear(); + id_to_graph_map_.clear(); + try { + if (graph_loader_->loadGraphFromFile(graph_, id_to_graph_map_, request->graph_filepath)) { + goal_intent_extractor_->setGraph(graph_, &id_to_graph_map_); + graph_vis_publisher_->publish(utils::toMsg(graph_, route_frame_, this->now())); + response->success = true; + return; + } + } catch (std::exception & ex) { + } + + RCLCPP_WARN( + get_logger(), + "Failed to set new route graph: %s!", request->graph_filepath.c_str()); + response->success = false; +} + +template +void RouteServer::exceptionWarning( + const std::shared_ptr goal, + const std::exception & ex) +{ + RCLCPP_WARN( + get_logger(), + "Route server failed on request: Start: [(%0.2f, %0.2f) / %i] Goal: [(%0.2f, %0.2f) / %i]:" + " \"%s\"", goal->start.pose.position.x, goal->start.pose.position.y, goal->start_id, + goal->goal.pose.position.x, goal->goal.pose.position.y, goal->goal_id, ex.what()); +} + +} // namespace nav2_route + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_route::RouteServer) diff --git a/nav2_route/src/route_tracker.cpp b/nav2_route/src/route_tracker.cpp new file mode 100644 index 00000000000..7c083a35b3d --- /dev/null +++ b/nav2_route/src/route_tracker.cpp @@ -0,0 +1,246 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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 "nav2_route/route_tracker.hpp" + +namespace nav2_route +{ + +void RouteTracker::configure( + nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr tf_buffer, + std::shared_ptr costmap_subscriber, + std::shared_ptr action_server, + const std::string & route_frame, + const std::string & base_frame) +{ + clock_ = node->get_clock(); + logger_ = node->get_logger(); + route_frame_ = route_frame; + base_frame_ = base_frame; + action_server_ = action_server; + tf_buffer_ = tf_buffer; + + nav2_util::declare_parameter_if_not_declared( + node, "radius_to_achieve_node", rclcpp::ParameterValue(2.0)); + radius_threshold_ = node->get_parameter("radius_to_achieve_node").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, "boundary_radius_to_achieve_node", rclcpp::ParameterValue(1.0)); + boundary_radius_threshold_ = node->get_parameter("boundary_radius_to_achieve_node").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, "tracker_update_rate", rclcpp::ParameterValue(50.0)); + tracker_update_rate_ = node->get_parameter("tracker_update_rate").as_double(); + nav2_util::declare_parameter_if_not_declared( + node, "aggregate_blocked_ids", rclcpp::ParameterValue(false)); + aggregate_blocked_ids_ = node->get_parameter("aggregate_blocked_ids").as_bool(); + + operations_manager_ = std::make_unique(node, costmap_subscriber); +} + +geometry_msgs::msg::PoseStamped RouteTracker::getRobotPose() +{ + geometry_msgs::msg::PoseStamped pose; + if (!nav2_util::getCurrentPose(pose, *tf_buffer_, route_frame_, base_frame_)) { + throw nav2_core::RouteTFError("Unable to get robot pose in route frame: " + route_frame_); + } + return pose; +} + +bool RouteTracker::nodeAchieved( + const geometry_msgs::msg::PoseStamped & pose, + RouteTrackingState & state, + const Route & route) +{ + // check if inside a *generous* radius window + const double dx = state.next_node->coords.x - pose.pose.position.x; + const double dy = state.next_node->coords.y - pose.pose.position.y; + const double dist_mag = std::sqrt(dx * dx + dy * dy); + const bool is_boundary_node = isStartOrEndNode(state, route); + const bool in_radius = + (dist_mag <= (is_boundary_node ? boundary_radius_threshold_ : radius_threshold_)); + + // Within 0.1mm is achieved or within radius and now not, consider node achieved + if (dist_mag < 1e-4 || (!in_radius && state.within_radius)) { + return true; + } + + // Update the state for the next iteration + state.within_radius = in_radius; + + // If start or end node, use the radius check only since the final node may not pass + // threshold depending on the configurations. The start node has no last_node for + // computing the vector bisector. If this is an issue, please file a ticket to discuss. + if (is_boundary_node) { + return state.within_radius; + } + + // We can evaluate the unit distance vector from the node w.r.t. the unit vector bisecting + // the last and current edges to find the average whose orthogonal is an imaginary + // line representing the migration from one edge's spatial domain to the other. + // When the dot product is negative, it means that there exists a projection between + // the vectors and that the robot position has passed this imaginary orthogonal line. + // This enables a more refined definition of when a node is considered achieved while + // enabling the use of dynamic behavior that may deviate from the path non-trivially + if (state.within_radius) { + NodePtr last_node = state.current_edge->start; + const double nx = state.next_node->coords.x - last_node->coords.x; + const double ny = state.next_node->coords.y - last_node->coords.y; + const double n_mag = std::sqrt(nx * nx + ny * ny); + + NodePtr future_next_node = route.edges[state.route_edges_idx + 1]->end; + const double mx = future_next_node->coords.x - state.next_node->coords.x; + const double my = future_next_node->coords.y - state.next_node->coords.y; + const double m_mag = std::sqrt(mx * mx + my * my); + + // If nodes overlap so there is no vector, use radius check only (divide by zero) + if (n_mag < 1e-6 || m_mag < 1e-6) { + return true; + } + + // Unnormalized Bisector = |n|*m + |m|*n + const double bx = nx * m_mag + mx * n_mag; + const double by = ny * m_mag + my * n_mag; + return utils::normalizedDot(bx, by, dx, dy) <= 0; + } + + return false; +} + +bool RouteTracker::isStartOrEndNode(RouteTrackingState & state, const Route & route) +{ + // Check if current_edge is nullptr in case we have a rerouted previous + // edge to use for the refined node achievement vectorized estimate + return + (state.route_edges_idx == static_cast(route.edges.size() - 1)) || + (state.route_edges_idx == -1 && !state.current_edge); +} + +void RouteTracker::publishFeedback( + const bool rereouted, + const unsigned int next_node_id, + const unsigned int last_node_id, + const unsigned int edge_id, + const std::vector & operations) +{ + auto feedback = std::make_unique(); + feedback->route = route_msg_; + feedback->path = path_; + feedback->rerouted = rereouted; + feedback->next_node_id = next_node_id; + feedback->last_node_id = last_node_id; + feedback->current_edge_id = edge_id; + feedback->operations_triggered = operations; + action_server_->publish_feedback(std::move(feedback)); +} + +TrackerResult RouteTracker::trackRoute( + const Route & route, const nav_msgs::msg::Path & path, + ReroutingState & rerouting_info) +{ + route_msg_ = utils::toMsg(route, route_frame_, clock_->now()); + path_ = path; + RouteTrackingState state; + state.next_node = route.start_node; + + // If we're rerouted but still covering the same previous edge to + // start, retain the state so we can continue as previously set with + // refined node achievement logic and performing edge operations on exit + if (rerouting_info.curr_edge) { + // state.next_node is not updated since the first edge is removed from route when rerouted + // along the same edge in the goal intent extractor. Thus, state.next_node is still the + // future node to reach in this case and we add in the state.last_node and state.current_edge + // to represent the 'currently' progressing edge that is omitted from the route (and its start) + state.current_edge = rerouting_info.curr_edge; + state.last_node = state.current_edge->start; + publishFeedback( + true, route.start_node->nodeid, state.last_node->nodeid, state.current_edge->edgeid, {}); + } else { + publishFeedback(true, route.start_node->nodeid, 0, 0, {}); + } + + rclcpp::Rate r(tracker_update_rate_); + while (rclcpp::ok()) { + bool status_change = false, completed = false; + + // Check if OK to keep processing + if (action_server_->is_cancel_requested()) { + return TrackerResult::INTERRUPTED; + } else if (action_server_->is_preempt_requested()) { + return TrackerResult::INTERRUPTED; + } + + // Update the tracking state + geometry_msgs::msg::PoseStamped robot_pose = getRobotPose(); + if (nodeAchieved(robot_pose, state, route)) { + status_change = true; + state.within_radius = false; + state.last_node = state.next_node; + if (++state.route_edges_idx < static_cast(route.edges.size())) { + state.current_edge = route.edges[state.route_edges_idx]; + state.next_node = state.current_edge->end; + } else { // At achieved the last node in the route + state.current_edge = nullptr; + state.next_node = nullptr; + completed = true; + } + } + + // Process any operations necessary + OperationsResult ops_result = + operations_manager_->process(status_change, state, route, robot_pose, rerouting_info); + + if (completed) { + RCLCPP_INFO(logger_, "Routing to goal completed!"); + // Publishing last feedback + publishFeedback(false, 0, state.last_node->nodeid, 0, ops_result.operations_triggered); + return TrackerResult::COMPLETED; + } + + if ((status_change || !ops_result.operations_triggered.empty()) && state.current_edge) { + publishFeedback( + false, // No rerouting occurred + state.next_node->nodeid, state.last_node->nodeid, + state.current_edge->edgeid, ops_result.operations_triggered); + } + + if (ops_result.reroute) { + if (!aggregate_blocked_ids_) { + rerouting_info.blocked_ids = ops_result.blocked_ids; + } else { + rerouting_info.blocked_ids.insert( + rerouting_info.blocked_ids.end(), + ops_result.blocked_ids.begin(), ops_result.blocked_ids.end()); + } + + if (state.last_node) { + rerouting_info.rerouting_start_id = state.last_node->nodeid; + rerouting_info.rerouting_start_pose = robot_pose; + } else { + rerouting_info.rerouting_start_id = std::numeric_limits::max(); + rerouting_info.rerouting_start_pose = geometry_msgs::msg::PoseStamped(); + } + + // Update so during rerouting we can check if we are continuing on the same edge + rerouting_info.curr_edge = state.current_edge; + RCLCPP_INFO(logger_, "Rerouting requested by route tracking operations!"); + return TrackerResult::INTERRUPTED; + } + + r.sleep(); + } + + return TrackerResult::EXITED; +} + +} // namespace nav2_route diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt new file mode 100644 index 00000000000..186c93dbfca --- /dev/null +++ b/nav2_route/test/CMakeLists.txt @@ -0,0 +1,129 @@ +# Route planner benchmarking script +add_executable(performance_benchmarking performance_benchmarking.cpp) +target_link_libraries(performance_benchmarking + ${library_name} +) +install(TARGETS + performance_benchmarking + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +# Test utilities and basic types +ament_add_gtest(test_utils_and_types + test_utils_and_types.cpp +) +target_link_libraries(test_utils_and_types + ${library_name} +) + +# Test edge scorer + plugins +ament_add_gtest(test_edge_scorers + test_edge_scorers.cpp +) +target_link_libraries(test_edge_scorers + ${library_name} edge_scorers +) + +# Test path converter +ament_add_gtest(test_path_converter + test_path_converter.cpp +) +target_link_libraries(test_path_converter + ${library_name} +) + +# Test node spatial tree +ament_add_gtest(test_spatial_tree + test_spatial_tree.cpp +) +target_link_libraries(test_spatial_tree + ${library_name} +) + +# Test operation manager + plugins +ament_add_gtest(test_operations + test_operations.cpp +) +target_link_libraries(test_operations + ${library_name} route_operations +) + +# Test graph loader +ament_add_gtest(test_graph_loader + test_graph_loader.cpp +) +target_link_libraries(test_graph_loader + ${library_name} +) + +# Test graph saver +ament_add_gtest(test_graph_saver + test_graph_saver.cpp +) +target_link_libraries(test_graph_saver + ${library_name} +) + +# Test geojson parser +ament_add_gtest(test_geojson_graph_file_loader + test_geojson_graph_file_loader.cpp +) +target_link_libraries(test_geojson_graph_file_loader + ${library_name} graph_file_loaders +) + +# Test geojson saver +ament_add_gtest(test_geojson_graph_file_saver + test_geojson_graph_file_saver.cpp +) +target_link_libraries(test_geojson_graph_file_saver + ${library_name} graph_file_loaders graph_file_savers +) + +# Test collision monitor separately due to relative complexity +ament_add_gtest(test_collision_operation + test_collision_operation.cpp +) +target_link_libraries(test_collision_operation + ${library_name} route_operations +) + +# Test route planner +ament_add_gtest(test_route_planner + test_route_planner.cpp +) +target_link_libraries(test_route_planner + ${library_name} edge_scorers +) + +# Test goal intent extractor +ament_add_gtest(test_goal_intent_extractor + test_goal_intent_extractor.cpp +) +target_link_libraries(test_goal_intent_extractor + ${library_name} +) + +# Test route tracker +ament_add_gtest(test_route_tracker + test_route_tracker.cpp +) +target_link_libraries(test_route_tracker + ${library_name} +) + +# Test route server +ament_add_gtest(test_route_server + test_route_server.cpp +) +target_link_libraries(test_route_server + ${library_name} +) + +# Test goal intent search +ament_add_gtest(test_goal_intent_search + test_goal_intent_search.cpp +) +target_link_libraries(test_goal_intent_search + ${library_name} +) diff --git a/nav2_route/test/performance_benchmarking.cpp b/nav2_route/test/performance_benchmarking.cpp new file mode 100644 index 00000000000..9773a8838cc --- /dev/null +++ b/nav2_route/test/performance_benchmarking.cpp @@ -0,0 +1,144 @@ +// Copyright (c) 2025 Samsung Research +// +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/route_planner.hpp" +#include "nav2_route/node_spatial_tree.hpp" + +using namespace nav2_route; // NOLINT + +// This is a script to generate a regularized but arbitrarily sized graph for testing +// the route planner's performance across massive spaces + +// Size of the benchmarking side length (e.g. 1000 x 1000 = 1,000,000 nodes) +const unsigned int DIM = 300; +// Number of tests to average results over +const unsigned int NUM_TESTS = 100; + +inline Graph createGraph() +{ + Graph graph; + graph.resize(DIM * DIM); + + EdgeCost e_cost; + unsigned int curr_edge_idx = DIM * DIM + 1; + + unsigned int curr_graph_idx = 0; + for (unsigned int j = 0; j != DIM; j++) { + for (unsigned int i = 0; i != DIM; i++) { + Node & node = graph[curr_graph_idx]; + node.nodeid = curr_graph_idx + 1; + node.coords.x = i; + node.coords.y = j; + + if (i > 0) { + // (i - 1, j) + node.addEdge(e_cost, &graph[curr_graph_idx - 1], curr_edge_idx++); + graph[curr_graph_idx - 1].addEdge(e_cost, &node, curr_edge_idx++); + } + if (j > 0) { + // (i, j - 1) + node.addEdge(e_cost, &graph[curr_graph_idx - DIM], curr_edge_idx++); + graph[curr_graph_idx - DIM].addEdge(e_cost, &node, curr_edge_idx++); + } + + curr_graph_idx++; + } + } + + return graph; +} + +int main(int argc, char const * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("route_benchmarking2"); + std::shared_ptr tf_buffer; + + Graph graph = createGraph(); + + // To visualize for smaller graphs reasonable for rviz to handle (DIM < 50) + // auto graph_vis_pub = node->create_publisher( + // "route_graph", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + // graph_vis_pub->publish(utils::toMsg(graph, "map", node->now())); + + std::shared_ptr costmap_subscriber; + RoutePlanner planner; + planner.configure(node, tf_buffer, costmap_subscriber); + std::vector blocked_ids; + Route route; + + // // First test: Plan clear across the maximum diagonal + // auto start = node->now(); + // for (unsigned int i = 0; i != NUM_TESTS; i++) { + // route = planner.findRoute(graph, 0u, static_cast(DIM * DIM - 1), blocked_ids); + // } + // auto end = node->now(); + // RCLCPP_INFO( + // node->get_logger(), + // "Across map took %0.5f milliseconds.", + // (end - start).seconds() * 1000.0 / static_cast(NUM_TESTS)); + // RCLCPP_INFO( + // node->get_logger(), + // "Route size: %li", route.edges.size()); + + // // Second test: Random start and goal poses + // srand (time(NULL)); + // unsigned int route_legs = 0; + // start = node->now(); + // for (unsigned int i = 0; i != NUM_TESTS; i++) { + // unsigned int start = rand() % DIM; + // unsigned int goal = rand() % DIM; + // while (start == goal) { + // goal = rand() % DIM; + // } + // route = planner.findRoute(graph, start, goal, blocked_ids); + // route_legs += route.edges.size(); + // } + // end = node->now(); + // RCLCPP_INFO( + // node->get_logger(), + // "Random planning took %0.5f milliseconds.", + // (end - start).seconds() * 1000.0 / static_cast(NUM_TESTS)); + // RCLCPP_INFO( + // node->get_logger(), + // "Averaged route size: %f", static_cast(route_legs) / static_cast(NUM_TESTS)) + + // Third test: + // Check how much time it takes to do random lookups in the Kd-tree of various graph sizes + std::shared_ptr kd_tree = std::make_shared(); + kd_tree->computeTree(graph); + + auto start = node->now(); + unsigned int seed = 1u; + for (unsigned int i = 0; i != NUM_TESTS; i++) { + std::vector kd_tree_idxs; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = static_cast(rand_r(&seed) % DIM); + pose.pose.position.y = static_cast(rand_r(&seed) % DIM); + kd_tree->findNearestGraphNodesToPose(pose, kd_tree_idxs); + } + auto end = node->now(); + RCLCPP_INFO( + node->get_logger(), + "Finding the nodes in the K-d tree took %0.5f milliseconds.", + (end - start).seconds() * 1000.0 / static_cast(NUM_TESTS)); + + return 0; +} diff --git a/nav2_route/test/test_collision_operation.cpp b/nav2_route/test/test_collision_operation.cpp new file mode 100644 index 00000000000..bdb10de1c6f --- /dev/null +++ b/nav2_route/test/test_collision_operation.cpp @@ -0,0 +1,333 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "nav2_route/operations_manager.hpp" +#include "nav2_route/types.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/plugins/route_operations/collision_monitor.hpp" +#include "nav2_costmap_2d/costmap_2d_publisher.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +class CollisionMonitorWrapper : public CollisionMonitor +{ +public: + CollisionMonitorWrapper() = default; + + Coordinates backoutValidEndPointWrapper( + const Coordinates & start, const Coordinates & end, const float dist) + { + return backoutValidEndPoint(start, end, dist); + } + + bool lineToMapWrapper( + const Coordinates & start, const Coordinates & end, LineSegment & line) + { + return lineToMap(start, end, line); + } + + bool backoutValidEndPointWrapper( + const Coordinates & start, LineSegment & line) + { + return backoutValidEndPoint(start, line); + } + + bool isInCollisionWrapper(const LineSegment & line) + { + return isInCollision(line); + } + + void getCostmapWrapper() + { + getCostmap(); + } +}; + +TEST(TestCollisionMonitor, test_lifecycle) +{ + auto node = std::make_shared("test"); + node->declare_parameter("costmap_topic", rclcpp::ParameterValue("dummy_topic")); + CollisionMonitor monitor; + std::shared_ptr costmap_subscriber; + monitor.configure(node, costmap_subscriber, "name"); + EXPECT_EQ(monitor.getName(), std::string("name")); + EXPECT_EQ(monitor.processType(), RouteOperationType::ON_QUERY); +} + +TEST(TestCollisionMonitor, test_geometric_backout_vector) +{ + auto node = std::make_shared("test"); + node->declare_parameter("costmap_topic", rclcpp::ParameterValue("local_costmap/costmap_raw")); + node->declare_parameter("name.max_collision_dist", rclcpp::ParameterValue(-1.0)); + std::shared_ptr costmap_subscriber; + CollisionMonitorWrapper monitor; + monitor.configure(node, costmap_subscriber, "name"); + + geometry_msgs::msg::PoseStamped pose; + Coordinates start, end; + start.x = 0.0; + start.y = 0.0; + end.x = 10.0; + end.y = 0.0; + float dist = 2.0; + + // Test with straight X / Y changes + Coordinates rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 2.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + dist = 5.0; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 5.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + start.x = 0.0; + start.y = 0.0; + end.x = 0.0; + end.y = 1.0; + + dist = 0.1; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 0.1, 0.01); + + dist = 15.0; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 15.0, 0.01); + + // Now that we know the basis work, let's try a none vector + start.x = 0.0; + start.y = 10.0; + end.x = 0.0; + end.y = 10.0; + dist = 5.0; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 10.0, 0.01); + + // Now an x=y vector + start.x = 0.0; + start.y = 0.0; + end.x = 10.0; + end.y = 10.0; + dist = 5.0; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 3.53, 0.01); + EXPECT_NEAR(rtn.y, 3.53, 0.01); + + // Let's try a random vector that I solved by hand + start.x = 4.0; + start.y = 10.0; + end.x = 50.0; + end.y = 100.0; + dist = 45.0; + rtn = monitor.backoutValidEndPointWrapper(start, end, dist); + EXPECT_NEAR(rtn.x, 24.479, 0.01); + EXPECT_NEAR(rtn.y, 50.069, 0.01); +} + + +TEST(TestCollisionMonitor, test_costmap_apis) +{ + auto node = std::make_shared("test"); + node->declare_parameter("costmap_topic", rclcpp::ParameterValue("dummy_topic")); + auto node_thread = std::make_unique(node); + std::shared_ptr costmap_subscriber; + CollisionMonitorWrapper monitor; + monitor.configure(node, costmap_subscriber, "name"); + + // No costmap received yet + EXPECT_THROW(monitor.getCostmapWrapper(), nav2_core::OperationFailed); + + // Create a demo costmap: * = 100, - = 0, / = 254 + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + nav2_costmap_2d::Costmap2D * costmap = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 0; i < 40; ++i) { + for (unsigned int j = 0; j < 100; ++j) { + costmap->setCost(i, j, 100); + } + } + + nav2_costmap_2d::Costmap2DPublisher publisher( + node, costmap, "map", "local_costmap/costmap", true); + publisher.on_activate(); + publisher.publishCostmap(); + + // Give it a moment to receive the costmap + rclcpp::Rate r(10); + r.sleep(); + monitor.getCostmapWrapper(); // Since would otherwise be called in `perform` + + Coordinates start, end; + start.x = 1.0; + start.y = 1.0; + end.x = 10.0; + end.y = 10.0; + CollisionMonitor::LineSegment line; + EXPECT_TRUE(monitor.lineToMapWrapper(start, end, line)); + EXPECT_EQ(line.x0, 9u); + EXPECT_EQ(line.y0, 9u); + EXPECT_EQ(line.x1, 99u); + EXPECT_EQ(line.y1, 99u); + + end.x = 11.0; + end.y = 11.0; + EXPECT_FALSE(monitor.lineToMapWrapper(start, end, line)); + EXPECT_EQ(line.x0, 9u); + EXPECT_EQ(line.y0, 9u); + EXPECT_EQ(line.x1, 109u); + EXPECT_EQ(line.y1, 109u); + + // Should backout to the costmap edge + EXPECT_TRUE(monitor.backoutValidEndPointWrapper(start, line)); + EXPECT_EQ(line.x1, 99u); + EXPECT_EQ(line.y1, 99u); + + // In this case, even starting point is invalid, reject + start.x = -1.0; + start.y = -1.0; + EXPECT_FALSE(monitor.backoutValidEndPointWrapper(start, line)); + + // Check collision checker + start.x = 1.0; + start.y = 1.0; + end.x = 9.0; + end.y = 1.0; + EXPECT_TRUE(monitor.lineToMapWrapper(start, end, line)); + EXPECT_FALSE(monitor.isInCollisionWrapper(line)); + + start.x = 1.0; + start.y = 1.0; + end.x = 1.0; + end.y = 9.0; + EXPECT_TRUE(monitor.lineToMapWrapper(start, end, line)); + EXPECT_FALSE(monitor.isInCollisionWrapper(line)); + + start.x = 1.0; + start.y = 1.0; + end.x = 9.0; + end.y = 9.0; + EXPECT_TRUE(monitor.lineToMapWrapper(start, end, line)); + EXPECT_TRUE(monitor.isInCollisionWrapper(line)); + + // Sanity check via a realistic graph route + Node node1, node2, node3; + node1.coords.x = 1.0; // lower left corner + node1.coords.y = 1.0; + node2.coords.x = 1.0; // upper left corner + node2.coords.y = 9.0; + node3.coords.x = 11.0; // lower right corner, through center (collision), off edge + node3.coords.y = -1.0; + DirectionalEdge edge1, edge2; + edge1.start = &node1; + edge1.end = &node2; + edge2.start = &node2; + edge2.end = &node3; + edge1.edgeid = 1u; + edge2.edgeid = 2u; + + Route route; + route.start_node = &node1; + route.edges.push_back(&edge1); + route.edges.push_back(&edge2); + + geometry_msgs::msg::PoseStamped pose; + EdgePtr curr_edge = &edge1; + pose.pose.position.x = 1.0; + pose.pose.position.y = 2.0; + + // We are along the first edge, outside of 5m check from costmap lethal block + // Need to wait long enough between calls due to collision monitor throttling + OperationResult result = monitor.perform( + &node1 /*unused*/, curr_edge, curr_edge /*unused*/, route, pose, nullptr); + EXPECT_FALSE(result.reroute); + rclcpp::Rate r1(0.8); + r1.sleep(); + + // Still along first bit of the route, 5m ahead still not in lethal block + pose.pose.position.x = 1.0; + pose.pose.position.y = 5.0; + result = monitor.perform( + &node1 /*unused*/, curr_edge, curr_edge /*unused*/, route, pose, nullptr); + EXPECT_FALSE(result.reroute); + rclcpp::Rate r2(0.8); + r2.sleep(); + + // Now should be within range of lethal block + pose.pose.position.x = 1.0; + pose.pose.position.y = 9.0; + result = monitor.perform( + &node1 /*unused*/, curr_edge, curr_edge /*unused*/, route, pose, nullptr); + EXPECT_TRUE(result.reroute); + EXPECT_EQ(result.blocked_ids.size(), 1u); + EXPECT_EQ(result.blocked_ids[0], 2u); + + // But should be not reporting now if we immediately try again due + // to not meeting the timer needs, even if we give it a better edge to use + pose.pose.position.x = 1.0; + pose.pose.position.y = 9.0; + curr_edge = &edge2; + result = monitor.perform( + &node1 /*unused*/, curr_edge, curr_edge /*unused*/, route, pose, nullptr); + EXPECT_FALSE(result.reroute); + EXPECT_EQ(result.blocked_ids.size(), 0u); + + rclcpp::Rate r3(0.8); + r3.sleep(); + + // Finally let's try once more after the collision block to make sure handles terminal situations + pose.pose.position.x = 8.0; + pose.pose.position.y = 2.0; + curr_edge = &edge2; + result = monitor.perform( + &node1 /*unused*/, curr_edge, curr_edge /*unused*/, route, pose, nullptr); + EXPECT_FALSE(result.reroute); + EXPECT_EQ(result.blocked_ids.size(), 0u); +} diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp new file mode 100644 index 00000000000..03abe64b788 --- /dev/null +++ b/nav2_route/test/test_edge_scorers.cpp @@ -0,0 +1,959 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_route/edge_scorer.hpp" +#include "nav2_msgs/srv/dynamic_edges.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "tf2_ros/static_transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(EdgeScorersTest, test_lifecycle) +{ + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); +} + +TEST(EdgeScorersTest, test_api) +{ + // Tests basic API and default behavior. Also covers the DistanceScorer plugin. + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 2); // default DistanceScorer, AdjustEdgesScorer + + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // Because nodes coords are 0/0 + + n1.coords.x = 1.0; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 1.0); // Distance is now 1m + + // For full coverage, add in a speed limit tag to make sure it is applied appropriately + float speed_limit = 0.8f; + edge.metadata.setValue("speed_limit", speed_limit); + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 1.25); // 1m / 0.8 = 1.25 +} + +TEST(EdgeScorersTest, test_failed_api) +{ + // Expect failure since plugin does not exist + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"FakeScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "FakeScorer.plugin", rclcpp::ParameterValue(std::string{"FakePluginPath"})); + + std::shared_ptr costmap_subscriber; + EXPECT_THROW( + EdgeScorer scorer(node, tf_buffer, costmap_subscriber), pluginlib::PluginlibException); +} + +TEST(EdgeScorersTest, test_invalid_edge_scoring) +{ + // Test API for the edge scorer to maintain proper state when a plugin + // rejects and edge. Also covers the DynamicEdgesScorer plugin to demonstrate. + auto node = std::make_shared("route_server"); + auto node_thread = std::make_unique(node); + auto node2 = std::make_shared("my_node2"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"DynamicEdgesScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "DynamicEdgesScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::DynamicEdgesScorer"})); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // AdjustEdgesScorer + + // Send service to set an edge as invalid + auto srv_client = + nav2_util::ServiceClient( + "route_server/DynamicEdgesScorer/adjust_edges", node2); + auto req = std::make_shared(); + req->closed_edges.push_back(10u); + req->adjust_edges.resize(1); + req->adjust_edges[0].edgeid = 11u; + req->adjust_edges[0].cost = 42.0; + auto resp = srv_client.invoke(req, std::chrono::nanoseconds(1000000000)); + EXPECT_TRUE(resp->success); + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // The score function should return false since closed + float traversal_cost = -1; + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + + // The score function should return true since no longer the problematic edge ID + // and edgeid 42 as the dynamic cost of 42 assigned to it + traversal_cost = -1; + edge.edgeid = 11; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 42.0); + + // Try to re-open this edge + auto req2 = std::make_shared(); + req2->opened_edges.push_back(10u); + auto resp2 = srv_client.invoke(req2, std::chrono::nanoseconds(1000000000)); + EXPECT_TRUE(resp2->success); + + // The score function should return true since now opened up + traversal_cost = -1; + edge.edgeid = 10; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + + node_thread.reset(); +} + +TEST(EdgeScorersTest, test_penalty_scoring) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"PenaltyScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "PenaltyScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::PenaltyScorer"})); + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // PenaltyScorer + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + float penalty = 10.0f; + edge.metadata.setValue("penalty", penalty); + + // The score function should return 10.0 from penalty value + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 10.0); +} + +TEST(EdgeScorersTest, test_costmap_scoring) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + node->declare_parameter("costmap_topic", "dummy_topic"); + auto node_thread = std::make_unique(node); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"CostmapScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "CostmapScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::CostmapScorer"})); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // CostmapScorer + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // The score function should return false because no costmap given + float traversal_cost = -1; + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + + // Create a demo costmap: * = 100, - = 0, / = 254 + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + nav2_costmap_2d::Costmap2D * costmap = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 0; i < 40; ++i) { + for (unsigned int j = 0; j < 100; ++j) { + costmap->setCost(i, j, 100); + } + } + + nav2_costmap_2d::Costmap2DPublisher publisher( + node, costmap, "map", "global_costmap/costmap", true); + publisher.on_activate(); + publisher.publishCostmap(); + + // Give it a moment to receive the costmap + rclcpp::Rate r(10); + r.sleep(); + + n1.coords.x = 5.0; + n1.coords.y = 8.0; + n2.coords.x = 8.0; + n2.coords.y = 8.0; + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + // Segment in freespace + EXPECT_EQ(traversal_cost, 0.0); + + n1.coords.x = 2.0; + n1.coords.y = 2.0; + n2.coords.x = 2.0; + n2.coords.y = 8.0; + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + // Segment in 100 space + EXPECT_NEAR(traversal_cost, 100.0 / 254.0, 0.01); + + n1.coords.x = 4.1; + n1.coords.y = 4.1; + n2.coords.x = 5.9; + n2.coords.y = 5.9; + traversal_cost = -1; + // Segment in lethal space, won't fill in + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + + n1.coords.x = 1.0; + n1.coords.y = 1.0; + n2.coords.x = 6.0; + n2.coords.y = 1.0; + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + // Segment in 0 and 100 space, use_max so 100 (normalized) + EXPECT_NEAR(traversal_cost, 100.0 / 254.0, 0.01); + + n1.coords.x = -1.0; + n1.coords.y = -1.0; + n2.coords.x = 11.0; + n2.coords.y = 11.0; + traversal_cost = -1; + // Off map, so invalid + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + + node_thread.reset(); +} + +TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + node->declare_parameter("costmap_topic", "dummy_costmap/costmap_raw"); + auto node_thread = std::make_unique(node); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"CostmapScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "CostmapScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::CostmapScorer"})); + node->declare_parameter( + "CostmapScorer.use_maximum", rclcpp::ParameterValue(false)); + node->declare_parameter( + "CostmapScorer.invalid_on_collision", rclcpp::ParameterValue(false)); + node->declare_parameter( + "CostmapScorer.invalid_off_map", rclcpp::ParameterValue(false)); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // CostmapScorer + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + // Create a demo costmap: * = 100, - = 0, / = 254 + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + nav2_costmap_2d::Costmap2D * costmap = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 0; i < 40; ++i) { + for (unsigned int j = 0; j < 100; ++j) { + costmap->setCost(i, j, 100); + } + } + + nav2_costmap_2d::Costmap2DPublisher publisher( + node, costmap, "map", "global_costmap/costmap", true); + publisher.on_activate(); + publisher.publishCostmap(); + + // Give it a moment to receive the costmap + rclcpp::Rate r(1); + r.sleep(); + + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // Off map + n1.coords.x = -1.0; + n1.coords.y = -1.0; + n2.coords.x = 11.0; + n2.coords.y = 11.0; + float traversal_cost = -1; + // Off map, so cannot score + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + n1.coords.x = 4.1; + n1.coords.y = 4.1; + n2.coords.x = 5.9; + n2.coords.y = 5.9; + traversal_cost = -1; + // Segment in lethal space, so score is maximum (1) + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_NEAR(traversal_cost, 1.0, 0.01); + + n1.coords.x = 1.0; + n1.coords.y = 1.0; + n2.coords.x = 6.0; + n2.coords.y = 1.0; + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + // Segment in 0 and 100 space, 3m @ 100, 2m @ 0, averaged is 60 + EXPECT_NEAR(traversal_cost, 60.0 / 254.0, 0.01); + + node_thread.reset(); +} + +TEST(EdgeScorersTest, test_time_scoring) +{ + // Test Time scorer plugin loading + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"TimeScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "TimeScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::TimeScorer"})); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // TimeScorer + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + float time_taken = 10.0f; + edge.metadata.setValue("abs_time_taken", time_taken); + + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // The score function should return 10.0 from time taken + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 10.0); // 10.0 * 1.0 weight + + // Without time taken or abs speed limit set, uses default max speed of 0.5 m/s + edge.metadata.data.clear(); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 2.0); // 1.0 m / 0.5 m/s * 1.0 weight + + // Use speed limit if set + float speed_limit = 0.85; + edge.metadata.setValue("abs_speed_limit", speed_limit); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_NEAR(traversal_cost, 1.1764, 0.001); // 1.0 m / 0.85 m/s * 1.0 weight + + // Still use time taken measurements if given first + edge.metadata.setValue("abs_time_taken", time_taken); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 10.0); // 10.0 * 1.0 weight +} + +TEST(EdgeScorersTest, test_semantic_scoring_key) +{ + // Test Time scorer plugin loading + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"SemanticScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::SemanticScorer"})); + + std::vector classes; + classes.push_back("Test"); + classes.push_back("Test1"); + classes.push_back("Test2"); + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer.semantic_classes", + rclcpp::ParameterValue(classes)); + + for (unsigned int i = 0; i != classes.size(); i++) { + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer." + classes[i], + rclcpp::ParameterValue(static_cast(i))); + } + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // SemanticScorer + + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + // Should fail, since both nothing under key `class` nor metadata set at all + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // nothing is set in semantics + + // Should be valid under the right key + std::string test_n = "Test1"; + edge.metadata.setValue("class", test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 1.0); // 1.0 * 1.0 weight + + test_n = "Test2"; + edge.metadata.setValue("class", test_n); + n2.metadata.setValue("class", test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight + + // Cannot find, doesn't exist + test_n = "Test4"; + edge.metadata.setValue("class", test_n); + n2.metadata.setValue("class", test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight +} + +TEST(EdgeScorersTest, test_semantic_scoring_keys) +{ + // Test Time scorer plugin loading + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", rclcpp::ParameterValue(std::vector{"SemanticScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::SemanticScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer.semantic_key", + rclcpp::ParameterValue(std::string{""})); + + std::vector classes; + classes.push_back("Test"); + classes.push_back("Test1"); + classes.push_back("Test2"); + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer.semantic_classes", + rclcpp::ParameterValue(classes)); + + for (unsigned int i = 0; i != classes.size(); i++) { + nav2_util::declare_parameter_if_not_declared( + node, "SemanticScorer." + classes[i], + rclcpp::ParameterValue(static_cast(i))); + } + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // SemanticScorer + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 1.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + const geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + EdgeType edge_type = EdgeType::NONE; + + // Should fail, since both nothing under key `class` nor metadata set at all + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // nothing is set in semantics + + // Should fail, since under the class key when the semantic key is empty string + // so it will look for the keys themselves + std::string test_n = "Test1"; + edge.metadata.setValue("class", test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight + + // Should succeed, since now actual class is a key, not a value of the `class` key + test_n = "Test2"; + edge.metadata.setValue(test_n, test_n); + n2.metadata.setValue(test_n, test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 4.0); // (2.0 + 2.0) * 1.0 weight + + // Cannot find, doesn't exist + edge.metadata.data.clear(); + n2.metadata.data.clear(); + test_n = "Test4"; + edge.metadata.setValue(test_n, test_n); + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); // 0.0 * 1.0 weight +} + +TEST(EdgeScorersTest, test_goal_orientation_threshold) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + node->declare_parameter( + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"GoalOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::GoalOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.use_orientation_threshold", + rclcpp::ParameterValue(true)); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + goal_pose.pose.orientation.x = 0.0; + goal_pose.pose.orientation.y = 0.0; + goal_pose.pose.orientation.z = 0.0; + goal_pose.pose.orientation.w = 1.0; + + RouteRequest route_request; + route_request.goal_pose = goal_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::END; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 0.0; + n1.coords.y = 0.0; + n2.coords.x = 1.0; + n2.coords.y = 0.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + + traversal_cost = -1; + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + route_request.use_poses = false; + + EXPECT_THROW( + scorer.score( + &edge, route_request, edge_type, + traversal_cost), nav2_core::InvalidEdgeScorerUse); +} + +TEST(EdgeScorersTest, test_goal_orientation_scoring) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer; + + double orientation_weight = 100.0; + + node->declare_parameter( + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"GoalOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::GoalOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.use_orientation_thershold", + rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, "GoalOrientationScorer.orientation_weight", + rclcpp::ParameterValue(orientation_weight)); + + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + goal_pose.pose.orientation.x = 0.0; + goal_pose.pose.orientation.y = 0.0; + goal_pose.pose.orientation.z = 0.0; + goal_pose.pose.orientation.w = 1.0; + + RouteRequest route_request; + route_request.goal_pose = goal_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::END; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 0.0; + n1.coords.y = 0.0; + n2.coords.x = 1.0; + n2.coords.y = 0.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + + traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_NEAR(traversal_cost, orientation_weight * M_PI, 0.001); + + route_request.use_poses = false; + + EXPECT_THROW( + scorer.score( + &edge, route_request, edge_type, + traversal_cost), nav2_core::InvalidEdgeScorerUse); +} + +TEST(EdgeScorersTest, test_start_pose_orientation_threshold) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer = std::make_shared(node->get_clock()); + std::shared_ptr tf_listener = + std::make_shared(*tf_buffer); + + node->declare_parameter( + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.use_orientation_threshold", + rclcpp::ParameterValue(true)); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + double yaw = 0.0; + + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + goal_pose.pose.orientation.x = 0.0; + goal_pose.pose.orientation.y = 0.0; + goal_pose.pose.orientation.z = 0.0; + goal_pose.pose.orientation.w = 1.0; + + start_pose.header.frame_id = "map"; + start_pose.header.stamp = node->get_clock()->now(); + + start_pose.pose.position.x = 0.0; + start_pose.pose.position.x = 0.0; + start_pose.pose.position.z = 0.0; + + start_pose.pose.orientation.x = q.getX(); + start_pose.pose.orientation.y = q.getY(); + start_pose.pose.orientation.z = q.getZ(); + start_pose.pose.orientation.w = q.getW(); + + RouteRequest route_request; + route_request.start_pose = start_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::START; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 0.0; + n1.coords.y = 0.0; + n2.coords.x = 1.0; + n2.coords.y = 0.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + EXPECT_FALSE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + route_request.use_poses = false; + + EXPECT_THROW( + scorer.score( + &edge, route_request, edge_type, + traversal_cost), nav2_core::InvalidEdgeScorerUse); +} + +TEST(EdgeScorersTest, test_start_pose_orientation_scoring) +{ + // Test Penalty scorer plugin loading + penalizing on metadata values + auto node = std::make_shared("edge_scorer_test"); + std::shared_ptr tf_buffer = std::make_shared(node->get_clock()); + std::shared_ptr tf_listener = + std::make_shared(*tf_buffer); + + double orientation_weight = 100.0; + + node->declare_parameter( + "edge_cost_functions", + rclcpp::ParameterValue(std::vector{"StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::StartPoseOrientationScorer"})); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.orientation_tolerance", + rclcpp::ParameterValue(1.57)); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.use_orientation_thershold", + rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, "StartPoseOrientationScorer.orientation_weight", + rclcpp::ParameterValue(orientation_weight)); + + std::shared_ptr costmap_subscriber; + EdgeScorer scorer(node, tf_buffer, costmap_subscriber); + EXPECT_EQ(scorer.numPlugins(), 1); // GoalOrientationScorer + + double yaw = 0.0; + + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + goal_pose.pose.orientation.x = 0.0; + goal_pose.pose.orientation.y = 0.0; + goal_pose.pose.orientation.z = 0.0; + goal_pose.pose.orientation.w = 1.0; + + start_pose.header.frame_id = "map"; + start_pose.header.stamp = node->get_clock()->now(); + + start_pose.pose.position.x = 0.0; + start_pose.pose.position.x = 0.0; + start_pose.pose.position.z = 0.0; + + start_pose.pose.orientation.x = q.getX(); + start_pose.pose.orientation.y = q.getY(); + start_pose.pose.orientation.z = q.getZ(); + start_pose.pose.orientation.w = q.getW(); + + + RouteRequest route_request; + route_request.start_pose = start_pose; + route_request.use_poses = true; + + EdgeType edge_type = EdgeType::START; + + // Create edge to score + Node n1, n2; + n1.nodeid = 1; + n2.nodeid = 2; + n1.coords.x = 0.0; + n1.coords.y = 0.0; + n2.coords.x = 1.0; + n2.coords.y = 0.0; + + DirectionalEdge edge; + edge.edgeid = 10; + edge.start = &n1; + edge.end = &n2; + + float traversal_cost = -1; + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_EQ(traversal_cost, 0.0); + + edge.start = &n2; + edge.end = &n1; + + EXPECT_TRUE(scorer.score(&edge, route_request, edge_type, traversal_cost)); + EXPECT_NEAR(traversal_cost, orientation_weight * M_PI, 0.001); + + route_request.use_poses = false; + + EXPECT_THROW( + scorer.score( + &edge, route_request, edge_type, + traversal_cost), nav2_core::InvalidEdgeScorerUse); +} diff --git a/nav2_route/test/test_geojson_graph_file_loader.cpp b/nav2_route/test/test_geojson_graph_file_loader.cpp new file mode 100644 index 00000000000..ae7c3cc3ac5 --- /dev/null +++ b/nav2_route/test/test_geojson_graph_file_loader.cpp @@ -0,0 +1,388 @@ +// Copyright (c) 2025 Joshua Wallace +// +// 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 +#include +#include + +#include "nav2_util/node_utils.hpp" +#include "nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT +using Json = nlohmann::json; + +Json g_simple_graph = nlohmann::json::parse( + R"( + { + "features": [ + { + "type": "Feature", + "properties": + { + "id": 0, + "frame": "map" + }, + "geometry": + { "type": "Point", + "coordinates": [ 0.0, 0.0 ] + } + }, + { + "type": "Feature", + "properties": + { + "id": 1, + "frame": "map" + }, + "geometry": + { + "type":"Point", + "coordinates": [ 1.0, 0.0 ] + } + }, + { + "type": "Feature", + "properties": + { + "id": 2, + "startid": 0, + "endid": 1 + }, + "geometry": + { + "type": "MultiLineString", + "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] + } + } + ] + } + )"); + +void writeGraphToFile(const Json & graph, const std::string & file_path = "graph.geojson") +{ + std::ofstream missing_id_file(file_path); + missing_id_file << graph; + missing_id_file.close(); +} + +TEST(GeoJsonGraphFileLoader, test_file_does_not_exist) +{ + Graph graph; + GraphToIDMap graph_to_id_map; + std::string file_path; + + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_FALSE(result); +} + +TEST(GeoJsonGraphFileLoader, test_no_nodes_in_graph) +{ + Json json_graph = g_simple_graph; + + auto & features = json_graph["features"]; + features.erase(features.begin(), features.begin() + 2); + + std::string file_path = "no_nodes.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_FALSE(result); + std::filesystem::remove(file_path); +} + +TEST(GeoJsonGraphFileLoader, test_no_edges_in_graph) +{ + Json json_graph = g_simple_graph; + auto & features = json_graph["features"]; + features.erase(2); + + std::string file_path = "no_edges.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_FALSE(result); + std::filesystem::remove(file_path); +} + +TEST(GeoJsonGraphFileLoader, test_missing_node_id) +{ + Json json_graph = g_simple_graph; + auto & properties = json_graph["features"][0]["properties"]; + properties.erase("id"); + + std::string file_path = "missing_id.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + EXPECT_THROW( + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path), + Json::exception); + std::filesystem::remove(file_path); +} + +TEST(GeoJsonGraphFileLoader, test_invalid_start_id_for_edge) +{ + Json json_graph = g_simple_graph; + auto & properties = json_graph["features"][2]["properties"]; + properties["startid"] = 100; + + std::string file_path = "invalid_start_id.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + EXPECT_THROW( + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path), + std::runtime_error); +} + +TEST(GeoJsonGraphFileLoader, test_invalid_goal_id_for_edge) +{ + Json json_graph = g_simple_graph; + auto & properties = json_graph["features"][2]["properties"]; + properties["endid"] = 1000; + + std::string file_path = "invalid_goal_id.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + EXPECT_THROW( + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path), + std::runtime_error); +} + +TEST(GeoJsonGraphFileLoader, test_node_metadata) { + Json json_graph = g_simple_graph; + + Json node_metadata = nlohmann::json::parse( + R"( + { + "metadata": + { + "person": + { + "name": "josh", + "can_drive": true, + "top_speed": 0.85, + "age": 29 + }, + "array": [0.0, 1.0, 2.0] + } + } +)"); + Json edge_metadata = nlohmann::json::parse( + R"( + { + "metadata": + { + "color": "green" + } + } +)"); + + json_graph["features"][0]["properties"].insert(node_metadata.begin(), node_metadata.end()); + json_graph["features"][2]["properties"].insert(edge_metadata.begin(), edge_metadata.end()); + + std::string file_path = "metadata.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_TRUE(result); + std::filesystem::remove(file_path); + + // Check node metadata + Metadata metadata; + metadata = graph[0].metadata.getValue("person", metadata); + + std::string name; + name = metadata.getValue("name", name); + EXPECT_EQ(name, node_metadata["metadata"]["person"]["name"]); + + bool can_drive = false; + can_drive = metadata.getValue("can_drive", can_drive); + EXPECT_EQ(can_drive, node_metadata["metadata"]["person"]["can_drive"]); + + float top_speed = 0.0f; + top_speed = metadata.getValue("top_speed", top_speed); + EXPECT_NEAR(top_speed, node_metadata["metadata"]["person"]["top_speed"], 1e-6); + + unsigned int age = 0; + age = metadata.getValue("age", age); + EXPECT_EQ(age, node_metadata["metadata"]["person"]["age"]); + + std::vector array; + array = graph[0].metadata.getValue("array", array); + EXPECT_EQ(array.size(), 3u); + + EXPECT_EQ(std::any_cast(array[0]), 0.0f); + EXPECT_EQ(std::any_cast(array[1]), 1.0f); + EXPECT_EQ(std::any_cast(array[2]), 2.0f); + + // Check edge metadata + std::string color; + color = graph[0].neighbors[0].metadata.getValue("color", color); + EXPECT_EQ(color, edge_metadata["metadata"]["color"]); +} + +TEST(GeoJsonGraphFileLoader, operations) +{ + Json json_graph = g_simple_graph; + + Json json_operation = nlohmann::json::parse( + R"( + { + "operations": + { + "open_door": + { + "type": "open_door", + "trigger": "ON_EXIT", + "metadata": + { + "color": "green" + } + } + } + } +)"); + + json_graph["features"][0]["properties"].insert(json_operation.begin(), json_operation.end()); + + std::string file_path = "operations.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_TRUE(result); + std::filesystem::remove(file_path); + + // Check node operations + Operations node_operations = graph[0].operations; + + EXPECT_EQ(node_operations.size(), 1u); + + EXPECT_EQ(node_operations[0].type, "open_door"); + EXPECT_EQ(node_operations[0].trigger, OperationTrigger::ON_EXIT); + + std::string color; + color = node_operations[0].metadata.getValue("color", color); + EXPECT_EQ(color, "green"); +} + +TEST(GeoJsonGraphFileLoader, simple_graph) +{ + std::string file_path = "simple_graph.geojson"; + + writeGraphToFile(g_simple_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_TRUE(result); + std::filesystem::remove(file_path); + + EXPECT_EQ(graph.size(), 2u); + + // Node 1 + EXPECT_EQ(graph[0].nodeid, 0u); + EXPECT_EQ(graph[0].coords.x, 0.0); + EXPECT_EQ(graph[0].coords.x, 0.0); + EXPECT_EQ(graph[0].coords.frame_id, "map"); + EXPECT_EQ(graph[0].neighbors.size(), 1u); + EXPECT_EQ(graph_to_id_map[0], graph[0].nodeid); + + // Node 2 + EXPECT_EQ(graph[1].nodeid, 1u); + EXPECT_EQ(graph[1].coords.x, 1.0); + EXPECT_EQ(graph[1].coords.y, 0.0); + EXPECT_EQ(graph[1].coords.frame_id, "map"); + EXPECT_EQ(graph[1].neighbors.size(), 0u); + EXPECT_EQ(graph_to_id_map[1], graph[1].nodeid); + + // Edge from node 1 -> 2 + EXPECT_EQ(graph[0].neighbors[0].edgeid, 2u); + EXPECT_EQ(graph[0].neighbors[0].start, &graph[0]); + EXPECT_EQ(graph[0].neighbors[0].end, &graph[1]); + EXPECT_TRUE(graph[0].neighbors[0].edge_cost.overridable); + EXPECT_EQ(graph[0].neighbors[0].edge_cost.cost, 0.0f); +} + +TEST(GeoJsonGraphFileLoader, sample_graph) +{ + auto file_path = ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/sample_graph.geojson"; + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + bool result = graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + EXPECT_TRUE(result); + + Metadata region; + region = graph[0].metadata.getValue("region", region); + EXPECT_EQ(region.data.size(), 3u); + + std::vector x_values; + x_values = region.getValue("x_values", x_values); + EXPECT_EQ(x_values.size(), 4u); + + EXPECT_EQ(graph[0].neighbors[0].edge_cost.cost, 10.0f); + EXPECT_EQ(graph[0].neighbors[0].edge_cost.overridable, false); + + auto & operations = graph[0].neighbors[0].operations; + + EXPECT_EQ(operations[0].type, "open_door"); + + std::string type; + type = operations[1].metadata.getValue("type", type); + EXPECT_EQ(type, "jpg"); +} + +TEST(GeoJsonGraphFileLoader, invalid_file) +{ + auto file_path = ament_index_cpp::get_package_share_directory("nav2_route") + + "/test/test_graphs/invalid.json"; + GeoJsonGraphFileLoader graph_file_loader; + Graph graph; + GraphToIDMap graph_to_id_map; + EXPECT_FALSE(graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path)); +} diff --git a/nav2_route/test/test_geojson_graph_file_saver.cpp b/nav2_route/test/test_geojson_graph_file_saver.cpp new file mode 100644 index 00000000000..249dbea2ab2 --- /dev/null +++ b/nav2_route/test/test_geojson_graph_file_saver.cpp @@ -0,0 +1,354 @@ +// Copyright (c) 2025 John Chrosniak +// +// 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 +#include +#include + +#include "nav2_util/node_utils.hpp" +#include "nav2_route/plugins/graph_file_loaders/geojson_graph_file_loader.hpp" +#include "nav2_route/plugins/graph_file_savers/geojson_graph_file_saver.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT +using Json = nlohmann::json; + +Json g_simple_graph = nlohmann::json::parse( + R"( + { + "features": [ + { + "type": "Feature", + "properties": + { + "id": 0, + "frame": "map" + }, + "geometry": + { "type": "Point", + "coordinates": [ 0.0, 0.0 ] + } + }, + { + "type": "Feature", + "properties": + { + "id": 1, + "frame": "map" + }, + "geometry": + { + "type":"Point", + "coordinates": [ 1.0, 0.0 ] + } + }, + { + "type": "Feature", + "properties": + { + "id": 2, + "startid": 0, + "endid": 1 + }, + "geometry": + { + "type": "MultiLineString", + "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] + } + } + ] + } + )"); + +void writeGraphToFile(const Json & graph, const std::string & file_path = "graph.geojson") +{ + std::ofstream missing_id_file(file_path); + missing_id_file << graph; + missing_id_file.close(); +} + +TEST(GeoJsonGraphFileSaver, test_file_empty) +{ + Graph graph; + std::string file_path; + + GeoJsonGraphFileSaver graph_file_saver; + bool result = graph_file_saver.saveGraphToFile(graph, file_path); + EXPECT_FALSE(result); +} + +TEST(GeoJsonGraphFileSaver, test_node_metadata) { + Json json_graph = g_simple_graph; + + Json node_metadata = nlohmann::json::parse( + R"( + { + "metadata": + { + "person": + { + "name": "josh", + "can_drive": true, + "top_speed": 0.85, + "age": 29, + "altitude": -10 + }, + "double_array": [0.0, 1.0, 2.0], + "int_array": [0, 1, 2], + "string_array": ["a", "b", "c"], + "bool_array": [true, false, true] + } + } +)"); + Json edge_metadata = nlohmann::json::parse( + R"( + { + "metadata": + { + "color": "green" + } + } +)"); + + json_graph["features"][0]["properties"].insert(node_metadata.begin(), node_metadata.end()); + json_graph["features"][2]["properties"].insert(edge_metadata.begin(), edge_metadata.end()); + + std::string file_path = "metadata.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + GeoJsonGraphFileSaver graph_file_saver; + bool result = graph_file_saver.saveGraphToFile(graph, file_path); + EXPECT_TRUE(result); + Graph graph2; + GraphToIDMap graph_to_id_map2; + graph_file_loader.loadGraphFromFile(graph2, graph_to_id_map2, file_path); + EXPECT_EQ(graph.size(), graph2.size()); + for (size_t i = 0; i < graph.size(); ++i) { + EXPECT_EQ(graph[i].nodeid, graph2[i].nodeid); + EXPECT_EQ(graph[i].coords.x, graph2[i].coords.x); + EXPECT_EQ(graph[i].coords.y, graph2[i].coords.y); + EXPECT_EQ(graph[i].coords.frame_id, graph2[i].coords.frame_id); + EXPECT_EQ(graph[i].neighbors.size(), graph2[i].neighbors.size()); + for (size_t j = 0; j < graph[i].neighbors.size(); ++j) { + EXPECT_EQ(graph[i].neighbors[j].edgeid, graph2[i].neighbors[j].edgeid); + EXPECT_EQ(graph[i].neighbors[j].start->nodeid, graph2[i].neighbors[j].start->nodeid); + EXPECT_EQ(graph[i].neighbors[j].end->nodeid, graph2[i].neighbors[j].end->nodeid); + EXPECT_EQ(graph[i].neighbors[j].edge_cost.cost, graph2[i].neighbors[j].edge_cost.cost); + EXPECT_EQ( + graph[i].neighbors[j].edge_cost.overridable, + graph2[i].neighbors[j].edge_cost.overridable); + } + } + EXPECT_EQ(graph_to_id_map.size(), graph_to_id_map2.size()); + for (const auto & pair : graph_to_id_map) { + EXPECT_EQ(pair.first, graph_to_id_map2[pair.second]); + } + // Check node metadata + Metadata metadata; + metadata = graph2[0].metadata.getValue("person", metadata); + + std::string name; + name = metadata.getValue("name", name); + EXPECT_EQ(name, node_metadata["metadata"]["person"]["name"]); + + bool can_drive = false; + can_drive = metadata.getValue("can_drive", can_drive); + EXPECT_EQ(can_drive, node_metadata["metadata"]["person"]["can_drive"]); + + float top_speed = 0.0f; + top_speed = metadata.getValue("top_speed", top_speed); + EXPECT_NEAR(top_speed, node_metadata["metadata"]["person"]["top_speed"], 1e-6); + + unsigned int age = 0; + age = metadata.getValue("age", age); + EXPECT_EQ(age, node_metadata["metadata"]["person"]["age"]); + + std::vector array; + array = graph2[0].metadata.getValue("double_array", array); + EXPECT_EQ(array.size(), 3u); + + EXPECT_EQ(std::any_cast(array[0]), 0.0f); + EXPECT_EQ(std::any_cast(array[1]), 1.0f); + EXPECT_EQ(std::any_cast(array[2]), 2.0f); + + // Check edge metadata + std::string color; + color = graph2[0].neighbors[0].metadata.getValue("color", color); + EXPECT_EQ(color, edge_metadata["metadata"]["color"]); + std::filesystem::remove(file_path); +} + +TEST(GeoJsonGraphFileSaver, operations) +{ + Json json_graph = g_simple_graph; + + Json json_operation = nlohmann::json::parse( + R"( + { + "operations": + { + "open_door": + { + "type": "open_door", + "trigger": "ON_EXIT", + "metadata": + { + "color": "green" + } + } + } + } +)"); + + json_graph["features"][0]["properties"].insert(json_operation.begin(), json_operation.end()); + + std::string file_path = "operations.geojson"; + writeGraphToFile(json_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + GeoJsonGraphFileSaver graph_file_saver; + Graph graph2; + GraphToIDMap graph_to_id_map2; + bool result = graph_file_saver.saveGraphToFile(graph, file_path); + EXPECT_TRUE(result); + graph_file_loader.loadGraphFromFile(graph2, graph_to_id_map2, file_path); + EXPECT_EQ(graph.size(), graph2.size()); + for (size_t i = 0; i < graph.size(); ++i) { + EXPECT_EQ(graph[i].nodeid, graph2[i].nodeid); + EXPECT_EQ(graph[i].coords.x, graph2[i].coords.x); + EXPECT_EQ(graph[i].coords.y, graph2[i].coords.y); + EXPECT_EQ(graph[i].coords.frame_id, graph2[i].coords.frame_id); + EXPECT_EQ(graph[i].neighbors.size(), graph2[i].neighbors.size()); + for (size_t j = 0; j < graph[i].neighbors.size(); ++j) { + EXPECT_EQ(graph[i].neighbors[j].edgeid, graph2[i].neighbors[j].edgeid); + EXPECT_EQ(graph[i].neighbors[j].start->nodeid, graph2[i].neighbors[j].start->nodeid); + EXPECT_EQ(graph[i].neighbors[j].end->nodeid, graph2[i].neighbors[j].end->nodeid); + EXPECT_EQ(graph[i].neighbors[j].edge_cost.cost, graph2[i].neighbors[j].edge_cost.cost); + EXPECT_EQ( + graph[i].neighbors[j].edge_cost.overridable, + graph2[i].neighbors[j].edge_cost.overridable); + } + } + EXPECT_EQ(graph_to_id_map.size(), graph_to_id_map2.size()); + for (const auto & pair : graph_to_id_map) { + EXPECT_EQ(pair.first, graph_to_id_map2[pair.second]); + } + std::filesystem::remove(file_path); + + // Check node operations + Operations node_operations = graph2[0].operations; + + EXPECT_EQ(node_operations.size(), 1u); + + EXPECT_EQ(node_operations[0].type, "open_door"); + EXPECT_EQ(node_operations[0].trigger, OperationTrigger::ON_EXIT); + + std::string color; + color = node_operations[0].metadata.getValue("color", color); + EXPECT_EQ(color, "green"); +} + +TEST(GeoJsonGraphFileSaver, simple_graph) +{ + std::string file_path = "simple_graph.geojson"; + + writeGraphToFile(g_simple_graph, file_path); + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + GeoJsonGraphFileSaver graph_file_saver; + bool result = graph_file_saver.saveGraphToFile(graph, file_path); + EXPECT_TRUE(result); + Graph graph2; + GraphToIDMap graph_to_id_map2; + graph_file_loader.loadGraphFromFile(graph2, graph_to_id_map2, file_path); + std::filesystem::remove(file_path); + + // Node 1 + EXPECT_EQ(graph2[0].nodeid, 0u); + EXPECT_EQ(graph2[0].coords.x, 0.0); + EXPECT_EQ(graph2[0].coords.x, 0.0); + EXPECT_EQ(graph2[0].coords.frame_id, "map"); + EXPECT_EQ(graph2[0].neighbors.size(), 1u); + EXPECT_EQ(graph_to_id_map2[0], graph2[0].nodeid); + + // Node 2 + EXPECT_EQ(graph2[1].nodeid, 1u); + EXPECT_EQ(graph2[1].coords.x, 1.0); + EXPECT_EQ(graph2[1].coords.y, 0.0); + EXPECT_EQ(graph2[1].coords.frame_id, "map"); + EXPECT_EQ(graph2[1].neighbors.size(), 0u); + EXPECT_EQ(graph_to_id_map2[1], graph2[1].nodeid); + + // Edge from node 1 -> 2 + EXPECT_EQ(graph2[0].neighbors[0].edgeid, 2u); + EXPECT_EQ(graph2[0].neighbors[0].start, &graph2[0]); + EXPECT_EQ(graph2[0].neighbors[0].end, &graph2[1]); + EXPECT_TRUE(graph2[0].neighbors[0].edge_cost.overridable); + EXPECT_EQ(graph2[0].neighbors[0].edge_cost.cost, 0.0f); +} + +TEST(GeoJsonGraphFileSaver, sample_graph) +{ + auto file_path = ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/sample_graph.geojson"; + + Graph graph; + GraphToIDMap graph_to_id_map; + GeoJsonGraphFileLoader graph_file_loader; + graph_file_loader.loadGraphFromFile(graph, graph_to_id_map, file_path); + GeoJsonGraphFileSaver graph_file_saver; + bool result = graph_file_saver.saveGraphToFile(graph, file_path); + EXPECT_TRUE(result); + Graph graph2; + GraphToIDMap graph_to_id_map2; + graph_file_loader.loadGraphFromFile(graph2, graph_to_id_map2, file_path); + + Metadata region; + region = graph2[0].metadata.getValue("region", region); + EXPECT_EQ(region.data.size(), 3u); + + std::vector x_values; + x_values = region.getValue("x_values", x_values); + EXPECT_EQ(x_values.size(), 4u); + + EXPECT_EQ(graph2[0].neighbors[0].edge_cost.cost, 10.0f); + EXPECT_EQ(graph2[0].neighbors[0].edge_cost.overridable, false); + + auto & operations = graph2[0].neighbors[0].operations; + + EXPECT_EQ(operations[0].type, "open_door"); + + std::string type; + type = operations[1].metadata.getValue("type", type); + EXPECT_EQ(type, "jpg"); +} diff --git a/nav2_route/test/test_goal_intent_extractor.cpp b/nav2_route/test/test_goal_intent_extractor.cpp new file mode 100644 index 00000000000..0ed6827391f --- /dev/null +++ b/nav2_route/test/test_goal_intent_extractor.cpp @@ -0,0 +1,381 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_route/goal_intent_extractor.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + + +class GoalIntentExtractorWrapper : public GoalIntentExtractor +{ +public: + GoalIntentExtractorWrapper() = default; + + // API to set the start/goal poses that would be set normally + // via `findStartandGoal` when called sequentially, so we can test + // `pruneStartandGoal` independent for many cases easily + void setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) + { + start_ = start; + goal_ = goal; + } + + geometry_msgs::msg::PoseStamped getStart() + { + return start_; + } +}; + +TEST(GoalIntentExtractorTest, test_obj_lifecycle) +{ + auto node = std::make_shared("goal_intent_extractor_test"); + GoalIntentExtractor extractor; + Graph graph; + GraphToIDMap id_map; + std::shared_ptr costmap_subscriber = nullptr; + extractor.configure(node, graph, &id_map, nullptr, costmap_subscriber, "map", "base_link"); +} + +TEST(GoalIntentExtractorTest, test_transform_pose) +{ + auto node = std::make_shared("goal_intent_extractor_test"); + auto node_thread = std::make_unique(node); + GoalIntentExtractor extractor; + Graph graph; + GraphToIDMap id_map; + auto tf = std::make_shared(node->get_clock()); + auto timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); + tf->setCreateTimerInterface(timer_interface); + auto transform_listener = std::make_shared(*tf); + tf2_ros::TransformBroadcaster broadcaster(node); + std::shared_ptr costmap_subscriber = nullptr; + extractor.configure(node, graph, &id_map, tf, costmap_subscriber, "map", "base_link"); + + // Test transformations same frame, should pass + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "map"; + EXPECT_NO_THROW(extractor.transformPose(pose, "map")); + + // Test transformations when nothing on TF buffer of different frames + pose.header.frame_id = "gps"; + EXPECT_THROW(extractor.transformPose(pose, "map"), nav2_core::RouteTFError); + + // Now transforms are available, should work + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "map"; + transform.header.stamp = node->now(); + transform.child_frame_id = "gps"; + broadcaster.sendTransform(transform); + EXPECT_NO_THROW(extractor.transformPose(pose, "map")); +} + +TEST(GoalIntentExtractorTest, test_start_goal_finder) +{ + auto node = std::make_shared("goal_intent_extractor_test"); + node->declare_parameter("enable_nn_search", rclcpp::ParameterValue(false)); + auto node_thread = std::make_unique(node); + GoalIntentExtractorWrapper extractor; + Graph graph; + GraphToIDMap id_map; + auto tf = std::make_shared(node->get_clock()); + auto timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); + tf->setCreateTimerInterface(timer_interface); + + // Make a 3x3 graph of points 0,0 -> 2,2 (ROS logo) + graph.resize(9); + unsigned int idx = 0; + unsigned int ids = 1; + for (unsigned int i = 0; i != 3; i++) { + for (unsigned int j = 0; j != 3; j++) { + graph[idx].nodeid = ids; + graph[idx].coords.x = i; + graph[idx].coords.y = j; + id_map[graph[idx].nodeid] = idx; + idx++; + ids++; + } + } + std::shared_ptr costmap_subscriber = nullptr; + extractor.configure(node, graph, &id_map, tf, costmap_subscriber, "map", "base_link"); + + // Test sending goal and start IDs to search + nav2_msgs::action::ComputeRoute::Goal raw_goal; + raw_goal.start_id = 1; + raw_goal.goal_id = 9; + raw_goal.use_poses = false; + auto goal = std::make_shared(raw_goal); + auto [start1, goal1] = extractor.findStartandGoal(goal); + EXPECT_EQ(start1, 0u); + EXPECT_EQ(goal1, 8u); + + // Set and check reset + geometry_msgs::msg::PoseStamped p1; + p1.pose.position.x = 45.0; + auto start = extractor.getStart(); + EXPECT_EQ(start.pose.position.x, 0.0); + EXPECT_EQ(start.pose.position.y, 0.0); + extractor.overrideStart(p1); + start = extractor.getStart(); + EXPECT_EQ(start.pose.position.x, 45.0); + + // Test sending a goal with start/goal poses to find closest nodes to + raw_goal.start.header.frame_id = "map"; + raw_goal.start.pose.position.x = -1.0; + raw_goal.start.pose.position.y = -1.0; // Closest on graph should be (0,0) -> 0 + raw_goal.goal.header.frame_id = "map"; + raw_goal.goal.pose.position.x = 1.0; + raw_goal.goal.pose.position.y = 1.0; // Closest on graph should be (1,1) -> 5 + raw_goal.use_start = true; + raw_goal.use_poses = true; + goal = std::make_shared(raw_goal); + auto [start2, goal2] = extractor.findStartandGoal(goal); + EXPECT_EQ(start2, 0u); + EXPECT_EQ(goal2, 4u); + + // Test sending a goal without using start (TF lookup failure) + raw_goal.use_start = false; + raw_goal.use_poses = true; + goal = std::make_shared(raw_goal); + EXPECT_THROW(extractor.findStartandGoal(goal), nav2_core::RouteTFError); + + // Try with an empty graph, should fail to find nodes + Graph empty_graph; + GraphToIDMap empty_id_map; + extractor.setGraph(empty_graph, &empty_id_map); + raw_goal.use_start = true; + raw_goal.use_poses = true; + goal = std::make_shared(raw_goal); + EXPECT_THROW(extractor.findStartandGoal(goal), nav2_core::IndeterminantNodesOnGraph); +} + +TEST(GoalIntentExtractorTest, test_pruning) +{ + auto node = std::make_shared("goal_intent_extractor_test"); + GoalIntentExtractorWrapper extractor; + Graph graph; + GraphToIDMap id_map; + std::shared_ptr costmap_subscriber = nullptr; + extractor.configure(node, graph, &id_map, nullptr, costmap_subscriber, "map", "base_link"); + + // Setup goal to use (only uses the use_poses field) + nav2_msgs::action::ComputeRoute::Goal raw_goal; + raw_goal.use_poses = false; + auto no_poses_goal = std::make_shared(raw_goal); + raw_goal.use_poses = true; + auto poses_goal = std::make_shared(raw_goal); + + // Test that we return identical things if not using poses or route is empty of edges + Route routeA; + routeA.edges.resize(10); + ReroutingState rerouting_info; + rerouting_info.first_time = true; + EXPECT_EQ(extractor.pruneStartandGoal(routeA, no_poses_goal, rerouting_info).edges.size(), 10u); + routeA.edges.clear(); + EXPECT_EQ(extractor.pruneStartandGoal(routeA, poses_goal, rerouting_info).edges.size(), 0u); + EXPECT_EQ(extractor.pruneStartandGoal(routeA, no_poses_goal, rerouting_info).edges.size(), 0u); + + // Create a sample route to test pruning upon with different start/goal pose requests + Node node1, node2, node3, node4; + node1.nodeid = 1; + node1.coords.x = 0.0; + node1.coords.y = 0.0; + node1.search_state.traversal_cost = 0.0; + node2.nodeid = 2; + node2.coords.x = 1.0; + node2.coords.y = 0.0; + node2.search_state.traversal_cost = 1.0; + node3.nodeid = 3; + node3.coords.x = 2.0; + node3.coords.y = 0.0; + node3.search_state.traversal_cost = 1.0; + node4.nodeid = 4; + node4.coords.x = 3.0; + node4.coords.y = 0.0; + node4.search_state.traversal_cost = 1.0; + + DirectionalEdge edge1, edge2, edge3; + edge1.edgeid = 5; + edge1.start = &node1; + edge1.end = &node2; + edge2.edgeid = 6; + edge2.start = &node2; + edge2.end = &node3; + edge3.edgeid = 7; + edge3.start = &node3; + edge3.end = &node4; + + Route route; + route.route_cost = 3.0; + route.start_node = &node1; + route.edges.push_back(&edge1); + route.edges.push_back(&edge2); + route.edges.push_back(&edge3); + + // Test overlapping goal/start poses to their respective terminal points on route + geometry_msgs::msg::PoseStamped start, goal; + goal.pose.position.x = 3.0; + extractor.setStartAndGoal(start, goal); + EXPECT_EQ(extractor.pruneStartandGoal(route, poses_goal, rerouting_info).edges.size(), 3u); + + // Test orthogonally misaligned, so should still have all the same points + start.pose.position.y = -10.0; + goal.pose.position.y = 10.0; + extractor.setStartAndGoal(start, goal); + EXPECT_EQ(extractor.pruneStartandGoal(route, poses_goal, rerouting_info).edges.size(), 3u); + + // Test start node before start pose and end node before end pose (no pruning) + start.pose.position.x = -1.0; + goal.pose.position.x = 4.0; + start.pose.position.y = 0.0; + goal.pose.position.y = 0.0; + extractor.setStartAndGoal(start, goal); + auto rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 3u); + + // Test start, and only start is after the start node along edge1, should be pruned + start.pose.position.x = 0.2; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 2u); + EXPECT_EQ(rtn.edges[0]->edgeid, 6u); + EXPECT_EQ(rtn.start_node->nodeid, 2u); + EXPECT_EQ(rtn.route_cost, 2.0); + + // Test start, and only start is after the start node along edge1 + // should not be pruned, within min distance + start.pose.position.x = 0.09; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 3u); + EXPECT_EQ(rtn.edges[0]->edgeid, 5u); + EXPECT_EQ(rtn.start_node->nodeid, 1u); + EXPECT_EQ(rtn.route_cost, 3.0); + + // Test but now with no_poses_goal to test if we trigger with !first_time condition + // when we should be able to prune due to a secondary trigger as the result of rerouting + start.pose.position.x = 0.2; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, no_poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 2u); + EXPECT_EQ(rtn.edges[0]->edgeid, 6u); + EXPECT_EQ(rtn.start_node->nodeid, 2u); + EXPECT_EQ(rtn.route_cost, 2.0); + // But now if we have first_time, doesn't prune! + rerouting_info.first_time = true; + rtn = extractor.pruneStartandGoal(route, no_poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 3u); + + // Test end, and only end is before the end node along edge3, should be pruned + start.pose.position.x = 0.0; + goal.pose.position.x = 2.5; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 2u); + EXPECT_EQ(rtn.edges.back()->edgeid, 6u); + EXPECT_EQ(rtn.edges.back()->end->nodeid, 3u); + EXPECT_EQ(rtn.route_cost, 2.0); + + // Test end, and only end is before the end node along edge3, should not be pruned + // As within the min distance + start.pose.position.x = 0.0; + goal.pose.position.x = 2.9; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 3u); + EXPECT_EQ(rtn.edges.back()->edgeid, 7u); + EXPECT_EQ(rtn.edges.back()->end->nodeid, 4u); + EXPECT_EQ(rtn.route_cost, 3.0); + + // Test both together can be pruned with realistic tracking offsets from route + // Also, Check route info for resetting to nullptrs since not the same last edge + start.pose.position.x = 0.6; + start.pose.position.y = 0.4; + goal.pose.position.x = 2.6; + goal.pose.position.y = -0.4; + rerouting_info.curr_edge = &edge3; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 1u); + EXPECT_EQ(rtn.edges[0]->edgeid, 6u); + EXPECT_EQ(rtn.edges.back()->start->nodeid, 2u); + EXPECT_EQ(rtn.edges.back()->end->nodeid, 3u); + EXPECT_EQ(rtn.route_cost, 1.0); + EXPECT_EQ(rerouting_info.curr_edge, nullptr); + + // Test pruning with route information with same situation as above + // But now check route info is retained because it IS the same edge as last time + // & stores closest point to use + rerouting_info.curr_edge = &edge1; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 1u); + EXPECT_EQ(rtn.edges[0]->edgeid, 6u); + EXPECT_EQ(rtn.edges.back()->start->nodeid, 2u); + EXPECT_EQ(rtn.edges.back()->end->nodeid, 3u); + EXPECT_EQ(rtn.route_cost, 1.0); + EXPECT_EQ(rerouting_info.curr_edge->edgeid, 5u); + EXPECT_NEAR(rerouting_info.closest_pt_on_edge.x, 0.6, 0.01); + EXPECT_NEAR(rerouting_info.closest_pt_on_edge.y, 0.0, 0.01); + + // Test both together can be pruned but won't be due to huge offsets > max_dist_from_edge (8m) + start.pose.position.x = 0.6; + start.pose.position.y = 8.1; + goal.pose.position.x = 2.6; + goal.pose.position.y = -8.1; + extractor.setStartAndGoal(start, goal); + rtn = extractor.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges[0]->start->nodeid, 1u); + EXPECT_EQ(rtn.edges.back()->end->nodeid, 4u); + EXPECT_EQ(rtn.edges.size(), 3u); + + // Test the both pruned condition but goal is not pruned because prune_goal = false + node->set_parameter(rclcpp::Parameter("prune_goal", rclcpp::ParameterValue(false))); + start.pose.position.x = 0.6; + start.pose.position.y = 0.4; + goal.pose.position.x = 2.6; + goal.pose.position.y = -0.4; + GoalIntentExtractorWrapper extractor2; + std::shared_ptr costmap_subscriber2 = nullptr; + extractor2.configure(node, graph, &id_map, nullptr, costmap_subscriber2, "map", "base_link"); + extractor2.setStartAndGoal(start, goal); + rtn = extractor2.pruneStartandGoal(route, poses_goal, rerouting_info); + EXPECT_EQ(rtn.edges.size(), 2u); +} diff --git a/nav2_route/test/test_goal_intent_search.cpp b/nav2_route/test/test_goal_intent_search.cpp new file mode 100644 index 00000000000..722273b76f5 --- /dev/null +++ b/nav2_route/test/test_goal_intent_search.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_route/goal_intent_extractor.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(GoalIntentSearchTest, test_obj_lifecycle) +{ + std::shared_ptr costmap = nullptr; + GoalIntentSearch::BreadthFirstSearch bfs(costmap); + EXPECT_EQ(bfs.getClosestNodeIdx(), 0u); + GoalIntentSearch::LoSCollisionChecker los_checker(costmap); +} + +TEST(GoalIntentSearchTest, test_los_checker) +{ + // Create a demo costmap: * = 100, - = 0, / = 254 + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + auto costmap = + std::make_shared(100, 100, 0.1, 0.0, 0.0, 0); + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 0; i < 40; ++i) { + for (unsigned int j = 0; j < 100; ++j) { + costmap->setCost(i, j, 100); + } + } + + GoalIntentSearch::LoSCollisionChecker los_checker(costmap); + + // Valid request + geometry_msgs::msg::Point start, end; + start.x = 1.0; + start.y = 1.0; + end.x = 1.0; + end.y = 9.0; + EXPECT_TRUE(los_checker.worldToMap(start, end)); + EXPECT_FALSE(los_checker.isInCollision()); + + // In collision request + start.x = 1.0; + start.y = 1.0; + end.x = 9.0; + end.y = 9.0; + EXPECT_TRUE(los_checker.worldToMap(start, end)); + EXPECT_TRUE(los_checker.isInCollision()); + + // Off of costmap request + end.x = 11.0; + end.y = 11.0; + EXPECT_FALSE(los_checker.worldToMap(start, end)); +} + +TEST(GoalIntentSearchTest, test_breadth_first_search) +{ + // Create a demo costmap: * = 100, - = 0, / = 254 + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * / / / / - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + // * * * * - - - - - - - - + auto costmap = + std::make_shared(100, 100, 0.1, 0.0, 0.0, 0); + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap->setCost(i, j, 254); + } + } + for (unsigned int i = 0; i < 40; ++i) { + for (unsigned int j = 0; j < 100; ++j) { + costmap->setCost(i, j, 100); + } + } + + GoalIntentSearch::BreadthFirstSearch bfs(costmap); + + std::vector candidate_nodes; + geometry_msgs::msg::PoseStamped target_node; + target_node.header.frame_id = "map"; + target_node.pose.position.x = 1.0; + target_node.pose.position.y = 1.0; + + geometry_msgs::msg::PoseStamped node; + node.header.frame_id = "map"; + node.pose.position.x = 9.0; + node.pose.position.y = 9.0; + candidate_nodes.push_back(node); // First is far + + node.pose.position.x = 5.0; + node.pose.position.y = 5.0; + candidate_nodes.push_back(node); // Second is close but in collision + + node.pose.position.x = 1.0; + node.pose.position.y = 9.0; + candidate_nodes.push_back(node); // Third is closest valid + + // Test successful search + EXPECT_TRUE(bfs.search(target_node, candidate_nodes)); + EXPECT_EQ(bfs.getClosestNodeIdx(), 2u); // Should find the third + + // Test max iterations + int max_it = 1; + EXPECT_FALSE(bfs.search(target_node, candidate_nodes, max_it)); + EXPECT_EQ(bfs.getClosestNodeIdx(), 0u); + + // Test reference off of the map + target_node.pose.position.x = 10000.0; + EXPECT_FALSE(bfs.search(target_node, candidate_nodes)); + + // Test candidates off of the map + target_node.pose.position.x = 1.0; + for (auto & node_c : candidate_nodes) { + node_c.pose.position.x = 10000.0; + } + EXPECT_FALSE(bfs.search(target_node, candidate_nodes)); + + // Test empty candidates + candidate_nodes.clear(); + EXPECT_FALSE(bfs.search(target_node, candidate_nodes)); +} diff --git a/nav2_route/test/test_graph_loader.cpp b/nav2_route/test/test_graph_loader.cpp new file mode 100644 index 00000000000..145d84aea3a --- /dev/null +++ b/nav2_route/test/test_graph_loader.cpp @@ -0,0 +1,151 @@ +// Copyright (c) 2025, Joshua Wallace +// +// 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. Reserved. + +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_route/graph_loader.hpp" +#include "tf2_ros/static_transform_broadcaster.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; //NOLINT + +TEST(GraphLoader, test_invalid_plugin) +{ + auto node = std::make_shared("graph_loader_test"); + auto tf = std::make_shared(node->get_clock()); + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + // Set dummy parameter + std::string default_plugin = "nav2_route::Dummy"; + nav2_util::declare_parameter_if_not_declared( + node, "graph_file_loader", rclcpp::ParameterValue(default_plugin)); + + EXPECT_THROW(GraphLoader graph_loader(node, tf, frame), pluginlib::PluginlibException); +} + +TEST(GraphLoader, test_api) +{ + auto node = std::make_shared("graph_loader_test"); + auto tf = std::make_shared(node->get_clock()); + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + GraphLoader graph_loader(node, tf, frame); + + Graph graph; + GraphToIDMap graph_to_id_map; + std::string filepath; + EXPECT_TRUE(graph_loader.loadGraphFromParameter(graph, graph_to_id_map)); + EXPECT_FALSE(graph_loader.loadGraphFromFile(graph, graph_to_id_map, filepath)); +} + +TEST(GraphLoader, test_transformation_api) +{ + auto node = std::make_shared("graph_loader_test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); + auto tf_listener = std::make_shared(*tf); + auto tf_broadcaster = std::make_shared(node); + + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + GraphLoader graph_loader(node, tf, frame); + + // Test with a file that now works + Graph graph; + GraphToIDMap graph_to_id_map; + std::string filepath; + filepath = + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson"; + EXPECT_TRUE(graph_loader.loadGraphFromFile(graph, graph_to_id_map, filepath)); + + // Test with another frame, should transform + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "map"; + transform.child_frame_id = "map_test"; + transform.header.stamp = node->now(); + transform.transform.rotation.w = 1.0; + transform.transform.translation.x = 1.0; + tf_broadcaster->sendTransform(transform); + rclcpp::Rate(1).sleep(); + tf_broadcaster->sendTransform(transform); + rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(1)); + rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + + graph[0].coords.frame_id = "map_test"; + EXPECT_EQ(graph[0].coords.frame_id, "map_test"); + double or_coord = graph[0].coords.x; + EXPECT_TRUE(graph_loader.transformGraph(graph)); + EXPECT_EQ(graph[0].coords.frame_id, "map"); + EXPECT_NE(graph[0].coords.x, or_coord); + + // Test failing to do so because no frame exists + graph[0].coords.frame_id = "map_test2"; + EXPECT_EQ(graph[0].coords.frame_id, "map_test2"); + or_coord = graph[0].coords.x; + EXPECT_FALSE(graph_loader.transformGraph(graph)); + EXPECT_EQ(graph[0].coords.frame_id, "map_test2"); + EXPECT_EQ(graph[0].coords.x, or_coord); +} + +TEST(GraphLoader, test_transformation_api2) +{ + auto node = std::make_shared("graph_loader_test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); + + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/test/test_graphs/no_frame.json")); + + GraphLoader graph_loader(node, tf, frame); + + // Test with a file that has unknown frames that cannot be transformed + Graph graph; + GraphToIDMap graph_to_id_map; + EXPECT_FALSE(graph_loader.loadGraphFromParameter(graph, graph_to_id_map)); + std::string filepath = ament_index_cpp::get_package_share_directory("nav2_route") + + "/test/test_graphs/no_frame.json"; + EXPECT_FALSE(graph_loader.loadGraphFromFile(graph, graph_to_id_map, filepath)); +} diff --git a/nav2_route/test/test_graph_saver.cpp b/nav2_route/test/test_graph_saver.cpp new file mode 100644 index 00000000000..e6a3913f51a --- /dev/null +++ b/nav2_route/test/test_graph_saver.cpp @@ -0,0 +1,166 @@ +// Copyright (c) 2025 John Chrosniak +// +// 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. Reserved. + +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_route/graph_loader.hpp" +#include "nav2_route/graph_saver.hpp" +#include "tf2_ros/static_transform_broadcaster.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; //NOLINT + +TEST(GraphSaver, test_invalid_plugin) +{ + auto node = std::make_shared("graph_saver_test"); + auto tf = std::make_shared(node->get_clock()); + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + // Set dummy parameter + std::string default_plugin = "nav2_route::Dummy"; + nav2_util::declare_parameter_if_not_declared( + node, "graph_file_saver", rclcpp::ParameterValue(default_plugin)); + + EXPECT_THROW(GraphSaver graph_saver(node, tf, frame), std::runtime_error); +} + +TEST(GraphSaver, test_empty_filename) +{ + auto node = std::make_shared("graph_saver_test"); + auto tf = std::make_shared(node->get_clock()); + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + GraphLoader graph_loader(node, tf, frame); + GraphSaver graph_saver(node, tf, frame); + + Graph first_graph, second_graph; + GraphToIDMap first_graph_to_id_map, second_graph_to_id_map; + std::string file_path = ""; + graph_loader.loadGraphFromParameter(first_graph, first_graph_to_id_map); + EXPECT_TRUE(graph_saver.saveGraphToFile(first_graph, file_path)); +} + +TEST(GraphSaver, test_api) +{ + auto node = std::make_shared("graph_saver_test"); + auto tf = std::make_shared(node->get_clock()); + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + GraphLoader graph_loader(node, tf, frame); + GraphSaver graph_saver(node, tf, frame); + + Graph first_graph, second_graph; + GraphToIDMap first_graph_to_id_map, second_graph_to_id_map; + std::string file_path = "test.geojson"; + graph_loader.loadGraphFromParameter(first_graph, first_graph_to_id_map); + EXPECT_TRUE(graph_saver.saveGraphToFile(first_graph, file_path)); + graph_loader.loadGraphFromFile(second_graph, second_graph_to_id_map, file_path); + EXPECT_EQ(first_graph.size(), second_graph.size()); + EXPECT_EQ(first_graph_to_id_map.size(), second_graph_to_id_map.size()); + for (size_t i = 0; i < first_graph.size(); ++i) { + EXPECT_EQ(first_graph[i].nodeid, second_graph[i].nodeid); + EXPECT_EQ(first_graph[i].coords.x, second_graph[i].coords.x); + EXPECT_EQ(first_graph[i].coords.y, second_graph[i].coords.y); + } + for (size_t i = 0; i < first_graph_to_id_map.size(); ++i) { + EXPECT_EQ(first_graph_to_id_map[i], second_graph_to_id_map[i]); + } + std::filesystem::remove(file_path); +} + +TEST(GraphSaver, test_transformation_api) +{ + auto node = std::make_shared("graph_saver_test"); + auto tf = std::make_shared(node->get_clock()); + tf->setUsingDedicatedThread(true); + auto tf_listener = std::make_shared(*tf); + auto tf_broadcaster = std::make_shared(node); + + std::string frame = "map"; + + nav2_util::declare_parameter_if_not_declared( + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); + + GraphLoader graph_loader(node, tf, frame); + + // Test with a file that now works + Graph graph; + GraphToIDMap graph_to_id_map; + std::string filepath; + filepath = + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson"; + graph_loader.loadGraphFromFile(graph, graph_to_id_map, filepath); + + // Test with another frame, should transform + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "map"; + transform.child_frame_id = "map_test"; + transform.header.stamp = node->now(); + transform.transform.rotation.w = 1.0; + transform.transform.translation.x = 1.0; + tf_broadcaster->sendTransform(transform); + rclcpp::Rate(1).sleep(); + tf_broadcaster->sendTransform(transform); + rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(1)); + rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + + GraphSaver graph_saver(node, tf, frame); + std::string file_path = "test.geojson"; + graph[0].coords.frame_id = "map_test"; + EXPECT_EQ(graph[0].coords.frame_id, "map_test"); + double or_coord = graph[0].coords.x; + EXPECT_TRUE(graph_saver.saveGraphToFile(graph, file_path)); + EXPECT_EQ(graph[0].coords.frame_id, "map"); + EXPECT_NE(graph[0].coords.x, or_coord); + std::filesystem::remove(file_path); + + // Test failing to do so because no frame exists + graph[0].coords.frame_id = "map_test2"; + EXPECT_EQ(graph[0].coords.frame_id, "map_test2"); + or_coord = graph[0].coords.x; + EXPECT_FALSE(graph_saver.saveGraphToFile(graph, file_path)); + EXPECT_EQ(graph[0].coords.frame_id, "map_test2"); + EXPECT_EQ(graph[0].coords.x, or_coord); +} diff --git a/nav2_route/test/test_graphs/error_codes.geojson b/nav2_route/test/test_graphs/error_codes.geojson new file mode 100644 index 00000000000..6712c506d04 --- /dev/null +++ b/nav2_route/test/test_graphs/error_codes.geojson @@ -0,0 +1,21 @@ +{ + "type": "FeatureCollection", + "name": "turtlebot3_graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "features": [ + { "type": "Feature", "properties": { "id": 1 }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 399 }, "geometry": { "type": "Point", "coordinates": [ 399.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 400 }, "geometry": { "type": "Point", "coordinates": [ 400.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 401 }, "geometry": { "type": "Point", "coordinates": [ 401.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 402 }, "geometry": { "type": "Point", "coordinates": [ 402.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 403 }, "geometry": { "type": "Point", "coordinates": [ 403.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 404 }, "geometry": { "type": "Point", "coordinates": [ 404.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 405 }, "geometry": { "type": "Point", "coordinates": [ 405.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 406 }, "geometry": { "type": "Point", "coordinates": [ 406.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 407 }, "geometry": { "type": "Point", "coordinates": [ 407.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 408 }, "geometry": { "type": "Point", "coordinates": [ 408.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 409 }, "geometry": { "type": "Point", "coordinates": [ 409.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 410 }, "geometry": { "type": "Point", "coordinates": [ 410.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 399 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 399.0, 0.0 ] ] ] } } + ] +} diff --git a/nav2_route/test/test_graphs/invalid.json b/nav2_route/test/test_graphs/invalid.json new file mode 100644 index 00000000000..41af33f4603 --- /dev/null +++ b/nav2_route/test/test_graphs/invalid.json @@ -0,0 +1,133 @@ +// This file has parsing errors, it is not valid +{ + "type": "FeatureCollection", + "name": "graph" + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "date_generated": "Wed Feb 22 05:41:45 PM EST 2025", + "features": + { "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, + { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, + { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, + { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, + { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, + { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, + { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, + { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, + { "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, + { "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, + { "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, + { "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, + { "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, + { "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, + { "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, + { "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, + { "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, + { "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, + { "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, + { "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, + { "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, + { "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, + { "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } + ] + } diff --git a/nav2_route/test/test_graphs/no_frame.json b/nav2_route/test/test_graphs/no_frame.json new file mode 100644 index 00000000000..6c60c370465 --- /dev/null +++ b/nav2_route/test/test_graphs/no_frame.json @@ -0,0 +1,132 @@ +{ + "type": "FeatureCollection", + "name": "graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "date_generated": "Wed Feb 22 05:41:45 PM EST 2025", + "features": [ + { "type": "Feature", "properties": { "id": 0, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, + { "type": "Feature", "properties": { "id": 1, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, + { "type": "Feature", "properties": { "id": 2, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, + { "type": "Feature", "properties": { "id": 3, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, + { "type": "Feature", "properties": { "id": 4, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, + { "type": "Feature", "properties": { "id": 5, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 6, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, + { "type": "Feature", "properties": { "id": 7, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, + { "type": "Feature", "properties": { "id": 8, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 9, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 10, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 11, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 12, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 13, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 14, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, + { "type": "Feature", "properties": { "id": 15, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, + { "type": "Feature", "properties": { "id": 16, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, + { "type": "Feature", "properties": { "id": 17, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, + { "type": "Feature", "properties": { "id": 18, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, + { "type": "Feature", "properties": { "id": 19, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, + { "type": "Feature", "properties": { "id": 20, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, + { "type": "Feature", "properties": { "id": 21, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 22, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, + { "type": "Feature", "properties": { "id": 23, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, + { "type": "Feature", "properties": { "id": 24, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, + { "type": "Feature", "properties": { "id": 25, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 26, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 27, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 28, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, + { "type": "Feature", "properties": { "id": 29, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, + { "type": "Feature", "properties": { "id": 30, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 31, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 32, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, + { "type": "Feature", "properties": { "id": 33, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, + { "type": "Feature", "properties": { "id": 34, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 35, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, + { "type": "Feature", "properties": { "id": 36, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 37, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, + { "type": "Feature", "properties": { "id": 38, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 39, "frame": "map_none" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, + { "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, + { "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, + { "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, + { "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, + { "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, + { "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, + { "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, + { "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, + { "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, + { "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } + ] + } diff --git a/nav2_route/test/test_operations.cpp b/nav2_route/test/test_operations.cpp new file mode 100644 index 00000000000..a8e7301625e --- /dev/null +++ b/nav2_route/test/test_operations.cpp @@ -0,0 +1,571 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_msgs/msg/speed_limit.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "nav2_route/operations_manager.hpp" +#include "nav2_route/types.hpp" +#include "nav2_core/route_exceptions.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(OperationsManagerTest, test_lifecycle) +{ + auto node = std::make_shared("operations_manager_test"); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); +} + +TEST(OperationsManagerTest, test_failed_plugins) +{ + // This plugin does not exist + auto node = std::make_shared("operations_manager_test"); + node->declare_parameter("operations", rclcpp::ParameterValue(std::vector{"hi"})); + std::shared_ptr costmap_subscriber; + EXPECT_THROW(OperationsManager manager(node, costmap_subscriber), std::runtime_error); +} + +TEST(OperationsManagerTest, test_find_operations) +{ + auto node = std::make_shared("operations_manager_test"); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + EdgePtr ent{nullptr}, exit{nullptr}; + + // No valid nodes or edges, should fail to find anything but not crash + EXPECT_EQ(manager.findGraphOperations(&node2, ent, exit).size(), 0u); + + // Still shouldn't find anything, but without nullptrs so can evaluate + DirectionalEdge enter, exit2; + EXPECT_EQ(manager.findGraphOperations(&node2, &enter, &enter).size(), 0u); + + // Try again with some operations in the node and edge (2x-ed) + Operation op, op2; + op.type = "test"; + op.trigger = OperationTrigger::ON_ENTER; + enter.operations.push_back(op); + op.trigger = OperationTrigger::NODE; + node2.operations.push_back(op); + op2.type = "test2"; + op2.trigger = OperationTrigger::ON_EXIT; + exit2.operations.push_back(op2); + EXPECT_EQ(manager.findGraphOperations(&node2, &enter, &exit2).size(), 3u); + + // Again, but now the triggers are inverted, so shouldn't be returned except node2 + enter.operations[0].trigger = OperationTrigger::ON_EXIT; + exit2.operations[0].trigger = OperationTrigger::ON_ENTER; + EXPECT_EQ(manager.findGraphOperations(&node2, &enter, &exit2).size(), 1u); + + // Now, should be empty + node2.operations[0].trigger = OperationTrigger::ON_ENTER; + EXPECT_EQ(manager.findGraphOperations(&node2, &enter, &exit2).size(), 0u); +} + +TEST(OperationsManagerTest, test_find_operations_failure2) +{ + // This plugin does not exist + auto node = std::make_shared("operations_manager_test"); + node->declare_parameter("operations", rclcpp::ParameterValue(std::vector{"hi"})); + std::shared_ptr costmap_subscriber; + EXPECT_THROW(OperationsManager manager(node, costmap_subscriber), std::runtime_error); +} + +TEST(OperationsManagerTest, test_processing_fail) +{ + auto node = std::make_shared("operations_manager_test"); + + // No operation plugins to trigger + node->declare_parameter("operations", rclcpp::ParameterValue(std::vector{})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + DirectionalEdge enter; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + geometry_msgs::msg::PoseStamped pose; + Route route; + ReroutingState info; + + // Should trigger nothing + auto result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 0u); +} + +TEST(OperationsManagerTest, test_processing_speed_on_status) +{ + auto node = std::make_shared("operations_manager_test"); + auto node_thread = std::make_unique(node); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + bool got_msg = false; + nav2_msgs::msg::SpeedLimit my_msg; + auto sub = node->create_subscription( + "speed_limit", + rclcpp::QoS(1), + [&, this](nav2_msgs::msg::SpeedLimit msg) {got_msg = true; my_msg = msg;}); + + Node node2; + DirectionalEdge enter; + float limit = 0.5f; + enter.metadata.setValue("speed_limit", limit); + geometry_msgs::msg::PoseStamped pose; + Route route; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + ReroutingState info; + + // No status change, shouldn't do anything + OperationsResult result = manager.process(false, state, route, pose, info); + EXPECT_FALSE(result.reroute); + EXPECT_EQ(result.operations_triggered.size(), 1u); // ReroutingService + EXPECT_EQ(result.operations_triggered[0], std::string("ReroutingService")); + + // Status change, may now trigger the only plugin + result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 2u); + EXPECT_EQ(result.operations_triggered[0], std::string("AdjustSpeedLimit")); + EXPECT_EQ(result.operations_triggered[1], std::string("ReroutingService")); + rclcpp::Rate r(10); + r.sleep(); + + // Check values are correct + EXPECT_TRUE(got_msg); + EXPECT_TRUE(my_msg.percentage); + EXPECT_NEAR(my_msg.speed_limit, 0.5f, 0.001f); +} + +TEST(OperationsManagerTest, test_rerouting_service_on_query) +{ + auto node = std::make_shared("route_server"); + auto node_thread = std::make_unique(node); + auto node_int = std::make_shared("my_node2"); + + // Enable rerouting service, which conducts on query (not status change) + node->declare_parameter( + "operations", rclcpp::ParameterValue(std::vector{"ReroutingService"})); + nav2_util::declare_parameter_if_not_declared( + node, "ReroutingService.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::ReroutingService"})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + DirectionalEdge enter; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + geometry_msgs::msg::PoseStamped pose; + Route route; + ReroutingState info; + + // Should trigger, either way! + auto result = manager.process(false, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); + result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); + + auto srv_client = + nav2_util::ServiceClient( + "route_server/ReroutingService/reroute", node_int); + auto req = std::make_shared(); + auto resp = srv_client.invoke(req, std::chrono::nanoseconds(1000000000)); + EXPECT_TRUE(resp->success); + + // Check values are correct after service call + result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_TRUE(result.reroute); + + // and resets + result = manager.process(false, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); +} + +TEST(OperationsManagerTest, test_trigger_event_on_graph) +{ + auto node = std::make_shared("operations_manager_test"); + auto node_thread = std::make_unique(node); + auto node_int = std::make_shared("my_node2"); + auto node_thread_int = std::make_unique(node_int); + + // Enable trigger event operation, which conducts on node or edge change + // when a graph object contains the request for opening a door only. + // This tests the trigger event plugin, ON_GRAPH actions in the + // Operations Manager as well as the route operations client. + node->declare_parameter( + "operations", rclcpp::ParameterValue(std::vector{"OpenDoor"})); + nav2_util::declare_parameter_if_not_declared( + node, "OpenDoor.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::TriggerEvent"})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + DirectionalEdge enter; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + geometry_msgs::msg::PoseStamped pose; + Route route; + Metadata mdata; + ReroutingState info; + + // Setup some test operations + Operation op, op2, op3; + op.type = "test"; + op.trigger = OperationTrigger::NODE; + + op2.type = "OpenDoor"; + op2.trigger = OperationTrigger::NODE; + + op3.type = "OpenDoor"; + op3.trigger = OperationTrigger::NODE; + std::string service_name = "open_door"; + std::string key = "service_name"; + op3.metadata.setValue(key, service_name); + + // Should do nothing in the operations manager, throw + node2.operations.push_back(op); + EXPECT_THROW(manager.process(true, state, route, pose, info), nav2_core::OperationFailed); + + // Now, let's try a node that should make it through the operations manager but fail + // because the proper service_name was provided neither in the parameter nor operation + // metadata + node2.operations.clear(); + node2.operations.push_back(op2); + EXPECT_THROW(manager.process(true, state, route, pose, info), nav2_core::OperationFailed); + + // Now let's test what should actually work with a real service in the metadata + node2.operations.clear(); + node2.operations.push_back(op3); + + // This should throw because this service is not yet available on wait_for_service + EXPECT_THROW(manager.process(true, state, route, pose, info), nav2_core::OperationFailed); + + // Now, let's test with a real server that is really available for use + bool got_srv = false; + auto callback = + [&]( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response + ) -> void + { + got_srv = true; + response->success = true; + }; + + auto service = node_int->create_service(service_name, callback); + + auto result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); + EXPECT_TRUE(got_srv); +} + +TEST(OperationsManagerTest, test_trigger_event_on_graph_global_service) +{ + auto node = std::make_shared("operations_manager_test"); + auto node_thread = std::make_unique(node); + auto node_int = std::make_shared("my_node2"); + auto node_thread_int = std::make_unique(node_int); + + // Set the global service to use instead of file settings for conflict testing + node->declare_parameter( + "operations", rclcpp::ParameterValue(std::vector{"OpenDoor"})); + nav2_util::declare_parameter_if_not_declared( + node, "OpenDoor.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::TriggerEvent"})); + nav2_util::declare_parameter_if_not_declared( + node, "OpenDoor.service_name", + rclcpp::ParameterValue(std::string{"hello_world"})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + DirectionalEdge enter; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + geometry_msgs::msg::PoseStamped pose; + Route route; + Metadata mdata; + ReroutingState info; + + // Setup working case + Operation op3; + op3.type = "OpenDoor"; + op3.trigger = OperationTrigger::NODE; + std::string service_name = "open_door"; + std::string key = "service_name"; + op3.metadata.setValue(key, service_name); + node2.operations.push_back(op3); + + // Setup a server for the node's metadata, to create conflict with the global setting + // If there's a conflict, the file version wins due to more specificity + bool got_srv = false; + auto callback = + [&]( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response + ) -> void + { + got_srv = true; + response->success = true; + }; + + auto service = node_int->create_service(service_name, callback); + + auto result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); + EXPECT_TRUE(got_srv); + + // Now, let's reset without the metadata and see that the global version is now called + node2.operations.clear(); + Operation op4; + op4.type = "OpenDoor"; + op4.trigger = OperationTrigger::NODE; + node2.operations.push_back(op4); + + bool got_srv2 = false; + auto callback2 = + [&]( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response + ) -> void + { + got_srv2 = true; + response->success = true; + }; + + auto service2 = node_int->create_service("hello_world", callback2); + + OperationsManager manager2(node, costmap_subscriber); + result = manager2.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + EXPECT_FALSE(result.reroute); + EXPECT_TRUE(got_srv2); +} + +TEST(OperationsManagerTest, test_trigger_event_on_graph_failures) +{ + auto node = std::make_shared("operations_manager_test"); + auto node_thread = std::make_unique(node); + auto node_int = std::make_shared("my_node2"); + + // Enable trigger event operation, which conducts on node or edge change + // when a graph object contains the request for opening a door only + node->declare_parameter( + "operations", rclcpp::ParameterValue(std::vector{"OpenDoor"})); + nav2_util::declare_parameter_if_not_declared( + node, "OpenDoor.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::TriggerEvent"})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node2; + DirectionalEdge enter; + RouteTrackingState state; + state.last_node = &node2; + state.next_node = &node2; + state.current_edge = &enter; + geometry_msgs::msg::PoseStamped pose; + Route route; + ReroutingState info; + + // No operations, nothing should trigger even though status changed + auto result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 0u); + EXPECT_FALSE(result.reroute); + + // Setup some test operations + Operation op, op2; + op.type = "test"; + op2.type = "TriggerEvent"; + op.trigger = OperationTrigger::NODE; + op2.trigger = OperationTrigger::NODE; + + // Should also do nothing, this type isn't a plugin type supported + node2.operations.push_back(op); + EXPECT_THROW(manager.process(true, state, route, pose, info), nav2_core::OperationFailed); + + // Make sure its using the provided plugin name NOT its type + node2.operations.clear(); + node2.operations.push_back(op2); + EXPECT_THROW(manager.process(true, state, route, pose, info), nav2_core::OperationFailed); +} + + +TEST(OperationsManagerTest, test_time_marker) +{ + auto node = std::make_shared("operations_manager_test"); + auto node_thread = std::make_unique(node); + node->declare_parameter( + "operations", rclcpp::ParameterValue(std::vector{"TimeMarker"})); + nav2_util::declare_parameter_if_not_declared( + node, "TimeMarker.plugin", + rclcpp::ParameterValue(std::string{"nav2_route::TimeMarker"})); + std::shared_ptr costmap_subscriber; + OperationsManager manager(node, costmap_subscriber); + + Node node1, node2, node3, node4; + DirectionalEdge exit, enter, last; + exit.start = &node1; + exit.end = &node2; + enter.start = &node2; + enter.end = &node3; + last.start = &node3; + last.end = &node4; + geometry_msgs::msg::PoseStamped pose; + + Route route; + route.start_node = &node1; + route.edges.push_back(&exit); + route.edges.push_back(&enter); + route.edges.push_back(&last); + + ReroutingState info; + RouteTrackingState state; + state.last_node = &node1; + state.next_node = &node2; + state.current_edge = &exit; + state.route_edges_idx = 0; + + // No status change, shouldn't do anything ... even after some time + OperationsResult result = manager.process(false, state, route, pose, info); + EXPECT_FALSE(result.reroute); + EXPECT_EQ(result.operations_triggered.size(), 0u); + rclcpp::Rate r(1); + r.sleep(); + result = manager.process(false, state, route, pose, info); + EXPECT_FALSE(result.reroute); + EXPECT_EQ(result.operations_triggered.size(), 0u); + + // Status change, may now trigger but state doesn't match + // (new edge) so it won't update times on the first call + result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + + float time = 0.0f; + time = enter.metadata.getValue("abs_time_taken", time); + EXPECT_EQ(time, 0.0f); + time = 0.0f; + time = exit.metadata.getValue("abs_time_taken", time); + EXPECT_EQ(time, 0.0f); + + rclcpp::Rate r2(1); + r2.sleep(); + + // The second time around after switching edges, should update the last edge's time + state.last_node = &node2; + state.next_node = &node3; + state.current_edge = &enter; + state.route_edges_idx = 1; + result = manager.process(true, state, route, pose, info); + EXPECT_EQ(result.operations_triggered.size(), 1u); + time = 0.0f; + time = exit.metadata.getValue("abs_time_taken", time); + EXPECT_GT(time, 0.5f); + time = 0.0f; + time = enter.metadata.getValue("abs_time_taken", time); + EXPECT_EQ(time, 0.0f); + + // Immediately call again on new edge, should also work with an edge change + // But the last edge not completed should still remain empty + state.last_node = &node3; + state.next_node = &node4; + state.current_edge = &last; + state.route_edges_idx = 2; + result = manager.process(true, state, route, pose, info); + time = 0.0f; + time = enter.metadata.getValue("abs_time_taken", time); + EXPECT_GT(time, 1e-6f); + time = 0.0f; + time = last.metadata.getValue("abs_time_taken", time); + EXPECT_EQ(time, 0.0f); + + // Check on terminal conditions + rclcpp::Rate r3(1); + r3.sleep(); + state.last_node = &node4; + state.next_node = nullptr; + state.current_edge = nullptr; + state.route_edges_idx = 3; + result = manager.process(true, state, route, pose, info); + time = 0.0f; + time = last.metadata.getValue("abs_time_taken", time); + EXPECT_GT(time, 0.5f); +} + +// A test operation that does nothing to test `processType()` +class TestRouteOperations : public nav2_route::RouteOperation +{ +public: + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr, + std::shared_ptr, + const std::string &) override + { + } + + std::string getName() override {return "TestRouteOperation";} + OperationResult perform( + NodePtr, + EdgePtr, + EdgePtr, + const Route &, + const geometry_msgs::msg::PoseStamped &, + const Metadata *) override {return OperationResult();} +}; + +TEST(OperationsTest, test_interface) +{ + TestRouteOperations op; + EXPECT_EQ(op.processType(), nav2_route::RouteOperationType::ON_GRAPH); +} diff --git a/nav2_route/test/test_path_converter.cpp b/nav2_route/test/test_path_converter.cpp new file mode 100644 index 00000000000..cadea6ac0c5 --- /dev/null +++ b/nav2_route/test/test_path_converter.cpp @@ -0,0 +1,165 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/path_converter.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(PathConverterTest, test_path_converter_api) +{ + auto node = std::make_shared("edge_scorer_test"); + auto node_thread = std::make_unique(node); + + nav_msgs::msg::Path path_msg; + auto sub = node->create_subscription( + "plan", rclcpp::QoS(1), [&, this](nav_msgs::msg::Path msg) {path_msg = msg;}); + + PathConverter converter; + converter.configure(node); + + std::string frame = "fake_frame"; + rclcpp::Time time(1000); + Route route; + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 10; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 11; + test_node2.coords.x = 10.0; + test_node2.coords.y = 10.0; + test_node3.nodeid = 12; + test_node3.coords.x = 20.0; + test_node3.coords.y = 20.0; + + DirectionalEdge test_edge1, test_edge2; + test_edge1.edgeid = 13; + test_edge1.start = &test_node1; + test_edge1.end = &test_node2; + test_edge2.edgeid = 14; + test_edge2.start = &test_node2; + test_edge2.end = &test_node3; + + route.start_node = &test_node1; + route.route_cost = 50.0; + route.edges.push_back(&test_edge1); + route.edges.push_back(&test_edge2); + ReroutingState info; + + auto path = converter.densify(route, info, frame, time); + EXPECT_EQ(path.header.frame_id, frame); + EXPECT_EQ(path.header.stamp.nanosec, time.nanoseconds()); + + // 2 * sqrt(200) * 20 (0.05 density/m) + 1 (for starting node) + EXPECT_EQ(path.poses.size(), 567u); + EXPECT_NEAR(path.poses[0].pose.position.x, 0.0, 0.01); + EXPECT_NEAR(path.poses[0].pose.position.y, 0.0, 0.01); + EXPECT_NEAR(path.poses.back().pose.position.x, 20.0, 0.01); + EXPECT_NEAR(path.poses.back().pose.position.y, 20.0, 0.01); + + rclcpp::Rate r(10); + r.sleep(); + + // Checks the same as returned and actually was published + EXPECT_EQ(path_msg.poses.size(), path.poses.size()); + node_thread.reset(); +} + +TEST(PathConverterTest, test_path_single_pt_path) +{ + auto node = std::make_shared("edge_scorer_test"); + PathConverter converter; + converter.configure(node); + + std::string frame = "fake_frame"; + rclcpp::Time time(1000); + + Node test_node; + test_node.nodeid = 17; + test_node.coords.x = 10.0; + test_node.coords.y = 40.0; + + Route route; + route.start_node = &test_node; + ReroutingState info; + + auto path = converter.densify(route, info, frame, time); + EXPECT_EQ(path.poses.size(), 1u); + EXPECT_NEAR(path.poses[0].pose.position.x, 10.0, 0.01); + EXPECT_NEAR(path.poses[0].pose.position.y, 40.0, 0.01); +} + +TEST(PathConverterTest, test_prev_info_path) +{ + auto node = std::make_shared("edge_scorer_test"); + PathConverter converter; + converter.configure(node); + + std::string frame = "fake_frame"; + rclcpp::Time time(1000); + + Node test_node; + test_node.nodeid = 17; + test_node.coords.x = 1.0; + test_node.coords.y = 0.0; + + Route route; + route.start_node = &test_node; + + DirectionalEdge edge; + edge.end = &test_node; + + ReroutingState info; + info.closest_pt_on_edge.x = 0.0; + info.closest_pt_on_edge.y = 0.0; + info.curr_edge = &edge; + + auto path = converter.densify(route, info, frame, time); + EXPECT_EQ(path.poses.size(), 21u); // 20 for density + 1 for single node point +} + +TEST(PathConverterTest, test_path_converter_interpolation) +{ + auto node = std::make_shared("edge_scorer_test"); + PathConverter converter; + converter.configure(node); + + float x0 = 10.0, y0 = 10.0, x1 = 20.0, y1 = 20.0; + std::vector poses; + converter.interpolateEdge(x0, y0, x1, y1, poses); + + EXPECT_EQ(poses.size(), 283u); // regular density + edges + for (unsigned int i = 0; i != poses.size() - 1; i++) { + // Check its always closer than the requested density + EXPECT_LT( + hypotf( + poses[i].pose.position.x - poses[i + 1].pose.position.x, + poses[i].pose.position.y - poses[i + 1].pose.position.y), 0.05); + } +} diff --git a/nav2_route/test/test_route_planner.cpp b/nav2_route/test/test_route_planner.cpp new file mode 100644 index 00000000000..4c3d53b2153 --- /dev/null +++ b/nav2_route/test/test_route_planner.cpp @@ -0,0 +1,226 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/route_planner.hpp" +#include "nav2_msgs/action/compute_route.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +inline Graph create4x4Graph() +{ + // * - * - * > * 12 13 14 15 + // | | | ^ + // * - * - * - * 8 9 10 11 + // | | | | + // * - * - * - * 4 5 6 7 + // | | | | + // * - * - * - * 0 1 2 3 + // where `-` and `|` are bidirectional edges and > are single direction edges + + EdgeCost default_cost; + Graph graph; + graph.resize(16); + unsigned int idx = 1; + for (unsigned int j = 0; j != 4; j++) { + for (unsigned int i = 0; i != 4; i++) { + Node & node = graph[idx - 1]; + node.coords.x = i; + node.coords.y = j; + node.nodeid = idx; + idx++; + } + } + + // Bottom row + graph[0].addEdge(default_cost, &graph[1], idx++); + graph[1].addEdge(default_cost, &graph[0], idx++); + graph[0].addEdge(default_cost, &graph[4], idx++); + graph[4].addEdge(default_cost, &graph[0], idx++); + + graph[1].addEdge(default_cost, &graph[2], idx++); + graph[2].addEdge(default_cost, &graph[1], idx++); + graph[1].addEdge(default_cost, &graph[5], idx++); + graph[5].addEdge(default_cost, &graph[1], idx++); + + graph[2].addEdge(default_cost, &graph[3], idx++); + graph[3].addEdge(default_cost, &graph[2], idx++); + graph[2].addEdge(default_cost, &graph[6], idx++); + graph[6].addEdge(default_cost, &graph[2], idx++); + + graph[7].addEdge(default_cost, &graph[3], idx++); + graph[3].addEdge(default_cost, &graph[7], idx++); + + // Second row + graph[4].addEdge(default_cost, &graph[5], idx++); + graph[5].addEdge(default_cost, &graph[4], idx++); + graph[4].addEdge(default_cost, &graph[8], idx++); + graph[8].addEdge(default_cost, &graph[4], idx++); + + graph[5].addEdge(default_cost, &graph[6], idx++); + graph[6].addEdge(default_cost, &graph[5], idx++); + graph[5].addEdge(default_cost, &graph[9], idx++); + graph[9].addEdge(default_cost, &graph[5], idx++); + + graph[6].addEdge(default_cost, &graph[7], idx++); + graph[7].addEdge(default_cost, &graph[6], idx++); + graph[6].addEdge(default_cost, &graph[10], idx++); + graph[10].addEdge(default_cost, &graph[6], idx++); + + graph[7].addEdge(default_cost, &graph[11], idx++); + graph[11].addEdge(default_cost, &graph[7], idx++); + + // third row + graph[8].addEdge(default_cost, &graph[9], idx++); + graph[9].addEdge(default_cost, &graph[8], idx++); + graph[8].addEdge(default_cost, &graph[12], idx++); + graph[12].addEdge(default_cost, &graph[8], idx++); + + graph[9].addEdge(default_cost, &graph[10], idx++); + graph[10].addEdge(default_cost, &graph[9], idx++); + graph[9].addEdge(default_cost, &graph[13], idx++); + graph[13].addEdge(default_cost, &graph[9], idx++); + + graph[10].addEdge(default_cost, &graph[11], idx++); + graph[11].addEdge(default_cost, &graph[10], idx++); + graph[10].addEdge(default_cost, &graph[14], idx++); + graph[14].addEdge(default_cost, &graph[10], idx++); + + graph[11].addEdge(default_cost, &graph[15], idx++); // one direction + + // Top row + graph[12].addEdge(default_cost, &graph[13], idx++); + graph[13].addEdge(default_cost, &graph[12], idx++); + + graph[13].addEdge(default_cost, &graph[14], idx++); + graph[14].addEdge(default_cost, &graph[13], idx++); + + graph[14].addEdge(default_cost, &graph[15], idx++); // one direction + return graph; +} + +TEST(RoutePlannerTest, test_route_planner_positive) +{ + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + + auto node = std::make_shared("router_test"); + std::shared_ptr tf_buffer; + std::shared_ptr collision_checker; + RoutePlanner planner; + planner.configure(node, tf_buffer, collision_checker); + std::vector blocked_ids; + unsigned int start, goal; + + // Create a graph to test routing upon. + Graph graph = create4x4Graph(); + + // Plan across diagonal, should be length 6 + start = 0u; + goal = 15u; + Route route = planner.findRoute(graph, start, goal, blocked_ids, route_request); + EXPECT_NEAR(route.route_cost, 6.0, 0.001); + EXPECT_EQ(route.edges.size(), 6u); + + // If we try in reverse, it should fail though since Node 16 is only + // achievable from the other direction + start = 15; + goal = 0; + EXPECT_THROW( + planner.findRoute( + graph, start, goal, blocked_ids, + route_request), nav2_core::NoValidRouteCouldBeFound); + + // Let's find a simple plan and then mess with the planner with blocking edges + start = 0; + goal = 12; + route = planner.findRoute(graph, start, goal, blocked_ids, route_request); + EXPECT_NEAR(route.route_cost, 3.0, 0.001); + EXPECT_EQ(route.edges.size(), 3u); + + // Now block an edge as closed along the chain, should find the next best path + unsigned int key_edgeid = 19u; + blocked_ids.push_back(key_edgeid); // Edge between node 0-4 in the 0-4-9-12 chain + route = planner.findRoute(graph, start, goal, blocked_ids, route_request); + EXPECT_NEAR(route.route_cost, 5.0, 0.001); + EXPECT_EQ(route.edges.size(), 5u); + for (auto & edge : route.edges) { + EXPECT_NE(edge->edgeid, key_edgeid); + } + + // Now don't block, but instead just increase the cost so that it goes elsewhere + // this should produce the same results + blocked_ids.clear(); + graph[0].neighbors[1].edge_cost.cost = 1e6; + graph[0].neighbors[1].edge_cost.overridable = false; + route = planner.findRoute(graph, start, goal, blocked_ids, route_request); + EXPECT_NEAR(route.route_cost, 5.0, 0.001); + EXPECT_EQ(route.edges.size(), 5u); +} + +TEST(RoutePlannerTest, test_route_planner_negative) +{ + geometry_msgs::msg::PoseStamped start_pose, goal_pose; + RouteRequest route_request; + + auto node = std::make_shared("router_test"); + std::shared_ptr tf_buffer; + node->declare_parameter("max_iterations", rclcpp::ParameterValue(5)); + std::shared_ptr collision_checker; + RoutePlanner planner; + planner.configure(node, tf_buffer, collision_checker); + std::vector blocked_ids; + unsigned int start = 0; + unsigned int goal = 15; + Graph graph; + + // No graph yet, should fail + EXPECT_THROW( + planner.findRoute( + graph, start, goal, blocked_ids, + route_request), nav2_core::NoValidGraph); + + // Create a graph to test routing upon. + graph = create4x4Graph(); + + // Try with a stupidly low number of iterations + graph[0].neighbors[1].edge_cost.overridable = true; + EXPECT_THROW( + planner.findRoute( + graph, start, goal, blocked_ids, + route_request), nav2_core::TimedOut); + + // If we try to plan but we both cannot override and has 0 cost, will throw + graph[0].neighbors[1].edge_cost.overridable = false; + graph[0].neighbors[1].edge_cost.cost = 0.0; + EXPECT_THROW( + planner.findRoute( + graph, start, goal, blocked_ids, + route_request), nav2_core::NoValidGraph); +} diff --git a/nav2_route/test/test_route_server.cpp b/nav2_route/test/test_route_server.cpp new file mode 100644 index 00000000000..17aa3b87a4f --- /dev/null +++ b/nav2_route/test/test_route_server.cpp @@ -0,0 +1,402 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/lifecycle_node.hpp" +#include "std_srvs/srv/trigger.hpp" +#include "nav2_util/service_client.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/route_tracker.hpp" +#include "nav2_route/route_server.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +class RoutePlannerErrorTester : public RoutePlanner +{ +public: + RoutePlannerErrorTester() = default; + + Route findRoute( + Graph & graph, unsigned int start_index, unsigned int goal_index, + const std::vector & blocked_ids, + const RouteRequest & route_request) override + { + const NodePtr & start_node = &graph.at(start_index); + + // Special inputs to trigger specific exceptions for server testing + if (start_node->nodeid == 399u) { + throw std::runtime_error("runtime_error"); + } else if (start_node->nodeid == 400u) { + throw nav2_core::RouteException("RouteException"); + } else if (start_node->nodeid == 401u) { + throw nav2_core::RouteTFError("RouteTFError"); + } else if (start_node->nodeid == 402u) { + throw nav2_core::NoValidGraph("NoValidGraph"); + } else if (start_node->nodeid == 403u) { + throw nav2_core::IndeterminantNodesOnGraph("IndeterminantNodesOnGraph"); + } else if (start_node->nodeid == 404u) { + throw nav2_core::TimedOut("TimedOut"); + } else if (start_node->nodeid == 405u) { + throw nav2_core::NoValidRouteCouldBeFound("NoValidRouteCouldBeFound"); + } else if (start_node->nodeid == 406u) { + throw nav2_core::OperationFailed("OperationFailed"); + } else if (start_node->nodeid == 407u) { + throw nav2_core::InvalidEdgeScorerUse("InvalidEdgeScorerUse"); + } + + // If none set, just call the base class + return RoutePlanner::findRoute(graph, start_index, goal_index, blocked_ids, route_request); + } +}; + +class RouteServerWrapper : public RouteServer +{ +public: + explicit RouteServerWrapper(const rclcpp::NodeOptions & options) + : RouteServer(options) + {} + + void lifecycleCycle() + { + on_configure(rclcpp_lifecycle::State()); + on_activate(rclcpp_lifecycle::State()); + on_deactivate(rclcpp_lifecycle::State()); + on_cleanup(rclcpp_lifecycle::State()); + on_shutdown(rclcpp_lifecycle::State()); + } + + void startup() + { + on_configure(rclcpp_lifecycle::State()); + on_activate(rclcpp_lifecycle::State()); + } + + void configure() + { + on_configure(rclcpp_lifecycle::State()); + } + + void activate() + { + on_activate(rclcpp_lifecycle::State()); + } + + void shutdown() + { + on_deactivate(rclcpp_lifecycle::State()); + on_cleanup(rclcpp_lifecycle::State()); + } + + void testPrint( + const std::shared_ptr goal, + const std::exception & ex) + { + exceptionWarning(goal, ex); + } + + rclcpp::Duration findPlanningDurationWrapper(const rclcpp::Time & start) + { + return findPlanningDuration(start); + } + + void populateActionResultWrapper( + std::shared_ptr result, + const Route & route, + const nav_msgs::msg::Path & path, + const rclcpp::Duration & planning_duration) + { + return populateActionResult(result, route, path, planning_duration); + } + + bool isRequestValidWrapper() + { + return isRequestValid(compute_and_track_route_server_); + } + + void setNontrivialGraph() + { + graph_.resize(1); + } + + void useErrorCodePlanner() + { + route_planner_ = std::make_shared(); + } +}; + +TEST(RouteServerTest, test_lifecycle) +{ + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + server->lifecycleCycle(); + server.reset(); +} + +TEST(RouteServerTest, test_set_srv) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_route"); + std::string real_filepath = pkg_share_dir + "/graphs/aws_graph.geojson"; + + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + server->declare_parameter("graph_filepath", rclcpp::ParameterValue(real_filepath)); + auto node_thread = std::make_unique(server); + auto node2 = std::make_shared("my_node2"); + + server->startup(); + auto srv_client = + nav2_util::ServiceClient( + "route_server/set_route_graph", node2); + auto req = std::make_shared(); + req->graph_filepath = "non/existent/path.json"; + auto resp = srv_client.invoke(req, std::chrono::nanoseconds(1000000000)); + EXPECT_FALSE(resp->success); + + auto req2 = std::make_shared(); + req2->graph_filepath = real_filepath; + auto resp2 = srv_client.invoke(req2, std::chrono::nanoseconds(1000000000)); + EXPECT_TRUE(resp2->success); + + auto req3 = std::make_shared(); + req3->graph_filepath = ament_index_cpp::get_package_share_directory("nav2_route") + + "/test/test_graphs/invalid.json"; + auto resp3 = srv_client.invoke(req3, std::chrono::nanoseconds(1000000000)); + EXPECT_FALSE(resp3->success); + + server->shutdown(); + node_thread.reset(); + server.reset(); +} + +inline Route getRoute() +{ + static Node node1, node2, node3; + node1.nodeid = 1; + node1.coords.x = 0.0; + node1.coords.y = 0.0; + node2.nodeid = 2; + node2.coords.x = 10.0; + node2.coords.y = 0.0; + node3.nodeid = 3; + node3.coords.x = 20.0; + node3.coords.y = 0.0; + + static DirectionalEdge edge1, edge2; + edge1.edgeid = 5; + edge1.start = &node1; + edge1.end = &node2; + edge2.edgeid = 6; + edge2.start = &node2; + edge2.end = &node3; + + Route route; + route.start_node = &node1; + route.edges.push_back(&edge1); + route.edges.push_back(&edge2); + return route; +} + +TEST(RouteServerTest, test_minor_utils) +{ + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + + // Find planning duration should print and provide duration + rclcpp::Time start(1000, 0, RCL_ROS_TIME); + auto dur = server->findPlanningDurationWrapper(start); + EXPECT_TRUE(dur.seconds() > 1e2); + + // This should print the goal info regarding error + nav2_msgs::action::ComputeRoute::Goal goal_raw; + auto goal = std::make_shared(goal_raw); + std::runtime_error ex("Hi:-)"); + server->testPrint(goal, ex); + + // Populate the result message with content + auto result = std::make_shared(); + Route route = getRoute(); + nav_msgs::msg::Path path; + path.poses.resize(406); + server->populateActionResultWrapper(result, route, path, dur); + EXPECT_EQ(result->path.poses.size(), path.poses.size()); + EXPECT_EQ(result->route.edges.size(), route.edges.size()); + + server.reset(); +} + +TEST(RouteServerTest, test_request_valid) +{ + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + + // Should be nullptr + EXPECT_FALSE(server->isRequestValidWrapper()); + + // Should be inactive + server->configure(); + EXPECT_FALSE(server->isRequestValidWrapper()); + + // Should be active, but graph is empty + server->activate(); + EXPECT_FALSE(server->isRequestValidWrapper()); + + // Should be valid now + server->setNontrivialGraph(); + EXPECT_TRUE(server->isRequestValidWrapper()); + + server.reset(); +} + +TEST(RouteServerTest, test_complete_action_api) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_route"); + std::string real_file = pkg_share_dir + "/graphs/aws_graph.geojson"; + + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + server->declare_parameter("graph_filepath", rclcpp::ParameterValue(real_file)); + auto node_thread = std::make_unique(server); + server->startup(); + + // Compute a simple route action request + auto node2 = std::make_shared("my_node2"); + auto compute_client = + rclcpp_action::create_client(node2, "compute_route"); + + nav2_msgs::action::ComputeRoute::Goal goal; + goal.start_id = 1u; + goal.goal_id = 1u; + goal.use_poses = false; + auto future_goal = compute_client->async_send_goal(goal); + + rclcpp::spin_until_future_complete(node2, future_goal); + auto goal_handle = future_goal.get(); + auto result_future = compute_client->async_get_result(goal_handle); + rclcpp::spin_until_future_complete(node2, result_future); + auto result = result_future.get().result; + EXPECT_EQ(result->route.edges.size(), 0u); + EXPECT_NEAR(result->route.route_cost, 0.0, 1e-6); + EXPECT_EQ(result->route.nodes[0].nodeid, 1u); + + // Compute a route and tracking request + auto track_client = + rclcpp_action::create_client( + node2, "compute_and_track_route"); + + nav2_msgs::action::ComputeAndTrackRoute::Goal goal2; + goal2.start_id = 1u; + goal2.goal_id = 2u; + goal2.use_poses = false; + auto future_goal2 = track_client->async_send_goal(goal2); + + rclcpp::spin_until_future_complete(node2, future_goal2); + auto goal_handle2 = future_goal2.get(); + + // Preempt it + rclcpp::Rate r(1.0); + r.sleep(); + auto future_goal3 = track_client->async_send_goal(goal2); + rclcpp::spin_until_future_complete(node2, future_goal3); + auto goal_handle3 = future_goal3.get(); + + // Request a reroute + auto node_int = std::make_shared("my_node2"); + auto srv_client = + nav2_util::ServiceClient( + "route_server/ReroutingService/reroute", node_int); + auto req = std::make_shared(); + auto resp = srv_client.invoke(req, std::chrono::nanoseconds(1000000000)); + EXPECT_TRUE(resp->success); + + // Cancel them all + track_client->async_cancel_all_goals(); + + auto result_future3 = track_client->async_get_result(goal_handle3); + rclcpp::spin_until_future_complete(node2, result_future3); + auto code = result_future3.get().code; + EXPECT_EQ(code, rclcpp_action::ResultCode::CANCELED); + + // Make sure it still shuts down completely after real work + server->shutdown(); + node_thread.reset(); + server.reset(); +} + +TEST(RouteServerTest, test_error_codes) +{ + std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_route"); + std::string real_file = pkg_share_dir + "/test/test_graphs/error_codes.geojson"; + + rclcpp::NodeOptions options; + auto server = std::make_shared(options); + server->declare_parameter("graph_filepath", rclcpp::ParameterValue(real_file)); + auto node_thread = std::make_unique(server); + server->startup(); + + // This uses the error code planner rather than the built-in planner + // to test throwing error conditions in the action and how that's handled by the server. + server->useErrorCodePlanner(); + + // Compute a simple route action request + using nav2_msgs::action::ComputeAndTrackRoute; + auto node2 = std::make_shared("my_node2"); + auto compute_client = + rclcpp_action::create_client(node2, "compute_and_track_route"); + + // These make use of poses to bypass data structure lookups and `map` to bypass TF + // Our test planner will still use the start ID + nav2_msgs::action::ComputeAndTrackRoute::Goal goal; + goal.use_start = true; + goal.use_poses = true; + goal.start.header.frame_id = "map"; + goal.goal.header.frame_id = "map"; + goal.goal.pose.position.x = 1.0; // On graph, just make sure always different + for (unsigned int i = 399; i != 408; i++) { + goal.start.pose.position.x = static_cast(i); + auto future_goal = compute_client->async_send_goal(goal); + + rclcpp::spin_until_future_complete(node2, future_goal); + auto goal_handle = future_goal.get(); + auto result_future = compute_client->async_get_result(goal_handle); + rclcpp::spin_until_future_complete(node2, result_future); + auto result = result_future.get().result; + if (i == 399u) { + EXPECT_EQ(result->error_code, i + 1u); // Also UNKNOWN, just distinguished differently + } else { + EXPECT_EQ(result->error_code, i); + } + } + + server->shutdown(); + node_thread.reset(); + server.reset(); +} diff --git a/nav2_route/test/test_route_tracker.cpp b/nav2_route/test/test_route_tracker.cpp new file mode 100644 index 00000000000..33b4dfcc173 --- /dev/null +++ b/nav2_route/test/test_route_tracker.cpp @@ -0,0 +1,356 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_core/route_exceptions.hpp" +#include "nav2_route/route_tracker.hpp" +#include "nav2_route/route_server.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + + +class RouteTrackerWrapper : public RouteTracker +{ +public: + RouteTrackerWrapper() = default; + + void setRouteMsgSize(const int & size) + { + nav2_msgs::msg::Route msg; + msg.edges.resize(size); + route_msg_ = msg; + } +}; + +TEST(RouteTrackerTest, test_lifecycle) +{ + auto node = std::make_shared("router_test"); + + RouteTracker tracker; + std::shared_ptr costmap_subscriber; + tracker.configure(node, nullptr, costmap_subscriber, nullptr, "map", "base_link"); +} + +TEST(RouteTrackerTest, test_get_robot_pose) +{ + auto node = std::make_shared("router_test"); + auto tf = std::make_shared(node->get_clock()); + auto timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); + tf->setCreateTimerInterface(timer_interface); + auto transform_listener = std::make_shared(*tf); + tf2_ros::TransformBroadcaster broadcaster(node); + std::shared_ptr costmap_subscriber; + + RouteTracker tracker; + tracker.configure(node, tf, costmap_subscriber, nullptr, "map", "base_link"); + + EXPECT_THROW(tracker.getRobotPose(), nav2_core::RouteTFError); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "map"; + transform.header.stamp = node->now(); + transform.child_frame_id = "base_link"; + broadcaster.sendTransform(transform); + EXPECT_NO_THROW(tracker.getRobotPose()); +} + +TEST(RouteTrackerTest, test_route_start_end) +{ + auto node = std::make_shared("router_test"); + + std::shared_ptr costmap_subscriber; + RouteTrackerWrapper tracker; + tracker.configure(node, nullptr, costmap_subscriber, nullptr, "map", "base_link"); + Route route; + route.edges.resize(7); + DirectionalEdge edge; + RouteTrackingState state; + + state.route_edges_idx = -1; + EXPECT_TRUE(tracker.isStartOrEndNode(state, route)); // Attempting to get to first node + + state.current_edge = &edge; + EXPECT_FALSE(tracker.isStartOrEndNode(state, route)); // with tracking continued + + state.route_edges_idx = 0; + EXPECT_FALSE(tracker.isStartOrEndNode(state, route)); + + state.route_edges_idx = 1; + EXPECT_FALSE(tracker.isStartOrEndNode(state, route)); + + state.route_edges_idx = 3; + EXPECT_FALSE(tracker.isStartOrEndNode(state, route)); + + state.route_edges_idx = 5; + EXPECT_FALSE(tracker.isStartOrEndNode(state, route)); + + state.route_edges_idx = 6; + EXPECT_TRUE(tracker.isStartOrEndNode(state, route)); // Approaching final node +} + +TEST(RouteTrackerTest, test_feedback) +{ + auto node = std::make_shared("router_test"); + std::shared_ptr costmap_subscriber; + RouteTrackerWrapper tracker; + tracker.configure(node, nullptr, costmap_subscriber, nullptr, "map", "base_link"); + + Route route; + tracker.setRouteMsgSize(7); + std::vector ops; + + // This will segfault since there is no action server to publish feedback upon + ASSERT_EXIT( + tracker.publishFeedback(false, 0u, 1u, 2u, ops), ::testing::KilledBySignal(SIGSEGV), ".*"); +} + +TEST(RouteTrackerTest, test_node_achievement_simple) +{ + auto node = std::make_shared("router_test"); + std::shared_ptr costmap_subscriber; + + // Test with straight line to do exact analysis easier. More realistic routes in the next test + RouteTrackerWrapper tracker; + tracker.configure(node, nullptr, costmap_subscriber, nullptr, "map", "base_link"); + + // Create a demo route to test upon + Node node1, node2, node3; + node1.nodeid = 1; + node1.coords.x = 0.0; + node1.coords.y = 0.0; + node2.nodeid = 2; + node2.coords.x = 10.0; + node2.coords.y = 0.0; + node3.nodeid = 3; + node3.coords.x = 20.0; + node3.coords.y = 0.0; + + DirectionalEdge edge1, edge2; + edge1.edgeid = 5; + edge1.start = &node1; + edge1.end = &node2; + edge2.edgeid = 6; + edge2.start = &node2; + edge2.end = &node3; + + Route route; + route.start_node = &node1; + route.edges.push_back(&edge1); + route.edges.push_back(&edge2); + + RouteTrackingState state; + state.last_node = nullptr; + state.next_node = &node1; + state.current_edge = nullptr; + state.route_edges_idx = -1; + state.within_radius = false; + + geometry_msgs::msg::PoseStamped pose; + + // Test a few cases surrounding the line y = 10 where the triggering should occur. + // In a single straight line for a simple test that the mathematical criteria works exactly + // at the boundary. Tests below will test for odd / real angled routes to ensure functionality + // in realistic conditions but without having to find the exact line equations after being proven + state.last_node = &node1; + state.next_node = &node2; + state.current_edge = &edge1; + state.route_edges_idx = 0; + state.within_radius = false; + pose.pose.position.x = 10.0; // exact + pose.pose.position.y = 0.0; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + state.within_radius = false; + + pose.pose.position.x = 9.99; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = 10.01; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + state.within_radius = false; + + // Set some tracking error + pose.pose.position.y = 0.1; + pose.pose.position.x = 9.99; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = 10.01; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + state.within_radius = false; + + // Test symmetry + pose.pose.position.y = -0.1; + pose.pose.position.x = 9.99; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = 10.01; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + state.within_radius = false; +} + +TEST(RouteTrackerTest, test_node_achievement) +{ + auto node = std::make_shared("router_test"); + std::shared_ptr costmap_subscriber; + + // Minimum threshold is 2m by default + RouteTrackerWrapper tracker; + tracker.configure(node, nullptr, costmap_subscriber, nullptr, "map", "base_link"); + + // Create a demo route to test upon + Node node1, node2, node3, node4; + node1.nodeid = 1; + node1.coords.x = 0.0; + node1.coords.y = 0.0; + node2.nodeid = 2; + node2.coords.x = -10.0; + node2.coords.y = 10.0; + node3.nodeid = 3; + node3.coords.x = -10.0; + node3.coords.y = 20.0; + node4.nodeid = 4; + node4.coords.x = -20.0; + node4.coords.y = 10.0; + + DirectionalEdge edge1, edge2, edge3; + edge1.edgeid = 5; + edge1.start = &node1; + edge1.end = &node2; + edge2.edgeid = 6; + edge2.start = &node2; + edge2.end = &node3; + edge3.edgeid = 7; + edge3.start = &node3; + edge3.end = &node4; + + Route route; + route.start_node = &node1; + route.edges.push_back(&edge1); + route.edges.push_back(&edge2); + route.edges.push_back(&edge3); + + RouteTrackingState state; + state.last_node = nullptr; + state.next_node = &node1; + state.current_edge = nullptr; + state.route_edges_idx = -1; + state.within_radius = false; + + geometry_msgs::msg::PoseStamped pose; + + // Test radius for start + pose.pose.position.x = -1.5; + pose.pose.position.y = 1.5; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -0.7; + pose.pose.position.y = 0.7; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + + // Test radius for end + state.last_node = &node3; + state.next_node = &node4; + state.current_edge = &edge3; + state.route_edges_idx = 2; + state.within_radius = false; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -20.7; + pose.pose.position.y = 10.7; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + + // Test exactly on top of a node mid-execution + state.last_node = &node2; + state.next_node = &node3; + state.current_edge = &edge2; + state.route_edges_idx = 1; + state.within_radius = false; + pose.pose.position.x = -5.0; + pose.pose.position.y = 5.0; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -10.0; + pose.pose.position.y = 20.0; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + + // Test within radius in last iteration, and now not + state.within_radius = true; + pose.pose.position.x = -1000.0; + pose.pose.position.y = 1500.0; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + state.within_radius = false; + + // Test approaching in mid-execution with acute angle edge + // A: Just on initial side in various locations + pose.pose.position.x = -9.5; + pose.pose.position.y = 19.5; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -9.9; + pose.pose.position.y = 19.2; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -10.0; + pose.pose.position.y = 19.9; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + // B: Just on other side in various locations + pose.pose.position.x = -10.0; + pose.pose.position.y = 20.01; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -9.9; + pose.pose.position.y = 20.4; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -10.5; + pose.pose.position.y = 20.5; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + + // Repeat similar edges but now with Edge1 with an obtuse angle + state.last_node = &node1; + state.next_node = &node2; + state.current_edge = &edge1; + state.route_edges_idx = 0; + state.within_radius = false; + + // A: Just on initial side + pose.pose.position.x = -9.9; + pose.pose.position.y = 9.9; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -9.7; + pose.pose.position.y = 10.0; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -10.3; + pose.pose.position.y = 9.7; + EXPECT_FALSE(tracker.nodeAchieved(pose, state, route)); + // B: Just on other side + pose.pose.position.x = -10.1; + pose.pose.position.y = 10.1; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -10.3; + pose.pose.position.y = 10.0; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); + pose.pose.position.x = -9.5; + pose.pose.position.y = 10.5; + EXPECT_TRUE(tracker.nodeAchieved(pose, state, route)); +} diff --git a/nav2_route/test/test_spatial_tree.cpp b/nav2_route/test/test_spatial_tree.cpp new file mode 100644 index 00000000000..f3ffb6b298e --- /dev/null +++ b/nav2_route/test/test_spatial_tree.cpp @@ -0,0 +1,71 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_route/node_spatial_tree.hpp" + +using namespace nav2_route; // NOLINT + +TEST(NodeSpatialTreeTest, test_kd_tree) +{ + // Create a large graph of random nodes + unsigned int seed = time(NULL); + Graph graph; + unsigned int graph_size = 10000; + graph.resize(graph_size); + for (unsigned int i = 0; i != graph_size; i++) { + graph[i].nodeid = i; + graph[i].coords.x = static_cast((rand_r(&seed) % 6000000) + 1); + graph[i].coords.y = static_cast((rand_r(&seed) % 6000000) + 1); + } + + // Compute our kd tree for this graph + std::shared_ptr kd_tree = std::make_shared(); + kd_tree->computeTree(graph); + + // Test a bunch of times to find the nearest neighbor to this node + // By checking for the idx in the Kd-tree and then brute force searching + unsigned int num_tests = 50; + for (unsigned int i = 0; i != num_tests; i++) { + std::vector kd_tree_idxs; + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = static_cast((rand_r(&seed) % 6000000) + 1); + pose.pose.position.y = static_cast((rand_r(&seed) % 6000000) + 1); + + if (!kd_tree->findNearestGraphNodesToPose(pose, kd_tree_idxs)) { + EXPECT_TRUE(false); // Unable to find nearest neighbor! + } + + unsigned int closest_via_brute_force = 0u; + float closest_dist = std::numeric_limits::max(); + for (unsigned int j = 0; j != graph_size; j++) { + float dist = hypotf( + pose.pose.position.x - graph[j].coords.x, + pose.pose.position.y - graph[j].coords.y); + if (dist < closest_dist) { + closest_dist = dist; + closest_via_brute_force = j; + } + } + + EXPECT_EQ(kd_tree_idxs[0], closest_via_brute_force); + } +} diff --git a/nav2_route/test/test_utils_and_types.cpp b/nav2_route/test/test_utils_and_types.cpp new file mode 100644 index 00000000000..6586e834f9f --- /dev/null +++ b/nav2_route/test/test_utils_and_types.cpp @@ -0,0 +1,346 @@ +// Copyright (c) 2025, Open Navigation LLC +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/utils.hpp" +#include "nav2_route/types.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(TypesTest, test_metadata) +{ + Metadata mdata; + float flt = 0.8f; + std::string str = "value"; + unsigned int uintv = 17u; + mdata.setValue("key", str); + mdata.setValue("speed_limit", flt); + mdata.setValue("graph_id", uintv); + + float default_flt = 1.0f; + std::string default_str = ""; + unsigned int default_uint = 0u; + EXPECT_EQ(mdata.getValue("key", default_str), str); + EXPECT_EQ(mdata.getValue("speed_limit", default_flt), flt); + EXPECT_EQ(mdata.getValue("graph_id", default_uint), uintv); +} + +TEST(TypesTest, test_search_state) +{ + DirectionalEdge edge; + SearchState state; + state.parent_edge = &edge; + state.integrated_cost = 25; + state.traversal_cost = 250; + + state.reset(); + EXPECT_EQ(state.integrated_cost, std::numeric_limits::max()); + EXPECT_EQ(state.traversal_cost, std::numeric_limits::max()); + EXPECT_EQ(state.parent_edge, nullptr); +} + +TEST(TypesTest, test_node) +{ + Node node1, node2; + node1.nodeid = 50u; + node2.nodeid = 51u; + + EdgeCost cost; + cost.overridable = false; + cost.cost = 100.0; + EXPECT_EQ(node1.neighbors.size(), 0u); + node1.addEdge(cost, &node2, 52u); + + EXPECT_EQ(node1.neighbors.size(), 1u); + EXPECT_EQ(node1.neighbors[0].edgeid, 52u); + EXPECT_EQ(node1.neighbors[0].start->nodeid, node1.nodeid); + EXPECT_EQ(node1.neighbors[0].end->nodeid, node2.nodeid); + EXPECT_EQ(node1.neighbors[0].edge_cost.cost, 100.0); + EXPECT_FALSE(node1.neighbors[0].edge_cost.overridable); +} + +TEST(UtilsTest, test_to_msg_conversions) +{ + // Test conversion of PoseStamped + auto pose_msg = utils::toMsg(50.0, 20.0); + EXPECT_EQ(pose_msg.pose.position.x, 50.0); + EXPECT_EQ(pose_msg.pose.position.y, 20.0); + + // Test conversion of Route + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 10; + test_node2.nodeid = 11; + test_node3.nodeid = 12; + + DirectionalEdge test_edge1, test_edge2; + test_edge1.edgeid = 13; + test_edge1.start = &test_node1; + test_edge1.end = &test_node2; + test_edge2.edgeid = 14; + test_edge2.start = &test_node2; + test_edge2.end = &test_node3; + + Route route; + route.start_node = &test_node1; + route.route_cost = 50.0; + route.edges.push_back(&test_edge1); + route.edges.push_back(&test_edge2); + + std::string frame = "fake_frame"; + rclcpp::Time time(1000); + auto route_msg = utils::toMsg(route, frame, time); + EXPECT_EQ(route_msg.header.frame_id, frame); + EXPECT_EQ(route_msg.header.stamp.nanosec, time.nanoseconds()); + EXPECT_EQ(route_msg.route_cost, 50.0); + + EXPECT_EQ(route_msg.nodes.size(), 3u); + EXPECT_EQ(route_msg.edges.size(), 2u); + + EXPECT_EQ(route_msg.nodes[0].nodeid, test_node1.nodeid); + EXPECT_EQ(route_msg.nodes[1].nodeid, test_node2.nodeid); + EXPECT_EQ(route_msg.nodes[2].nodeid, test_node3.nodeid); + EXPECT_EQ(route_msg.edges[0].edgeid, test_edge1.edgeid); + EXPECT_EQ(route_msg.edges[1].edgeid, test_edge2.edgeid); +} + +TEST(UtilsTest, test_to_visualization_msg_conversion) +{ + // Test conversion of Route Graph as MarkerArray + std::string frame = "fake_frame"; + rclcpp::Time time(1000); + Graph graph; + graph.resize(9); + unsigned int idx = 0; + unsigned int ids = 1; + for (unsigned int i = 0; i != 3; i++) { + for (unsigned int j = 0; j != 3; j++) { + graph[idx].nodeid = ids; + graph[idx].coords.x = i; + graph[idx].coords.y = j; + idx++; + ids++; + } + } + + EdgeCost default_cost; + graph[0].addEdge(default_cost, &graph[1], ids++); + graph[1].addEdge(default_cost, &graph[0], ids++); + graph[4].addEdge(default_cost, &graph[1], ids++); + graph[1].addEdge(default_cost, &graph[4], ids++); + graph[5].addEdge(default_cost, &graph[4], ids++); + graph[4].addEdge(default_cost, &graph[5], ids++); + graph[0].addEdge(default_cost, &graph[3], ids++); + graph[3].addEdge(default_cost, &graph[6], ids++); + + auto graph_msg = utils::toMsg(graph, frame, time); + EXPECT_EQ(graph_msg.markers.size(), 34u); // 9 nodes and 8 edges (with text markers) + for (auto & marker : graph_msg.markers) { + if (marker.ns == "route_graph_ids") { + EXPECT_EQ(marker.type, visualization_msgs::msg::Marker::TEXT_VIEW_FACING); + } else if (marker.ns == "route_graph") { + EXPECT_TRUE( + (marker.type == visualization_msgs::msg::Marker::LINE_LIST) || + (marker.type == visualization_msgs::msg::Marker::SPHERE)); + } + } +} + +TEST(UtilsTest, test_normalized_dot) +{ + // Vectors are orthogonal + float v1x = 0; + float v1y = 1; + float v2x = 1; + float v2y = 0; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 0.0, 1e-4); + + // Vectors are identical + v2x = 0; + v2y = 1; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 1.0, 1e-4); + + // vectors are opposite direction + v2x = 0; + v2y = -1; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), -1.0, 1e-4); + + // One vector is null + v2x = 0; + v2y = 0; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 0.0, 1e-4); + + // Both are null + v1x = 0; + v1y = 0; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 0.0, 1e-4); + + // Try un-normalized overlap / opposite / orthogonal + v1x = 10; + v1y = 0; + v2x = 0; + v2y = 6; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 0.0, 1e-4); + + v2x = 4.5; + v2y = 0; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), 1.0, 1e-4); + + v2x = -4.5; + v2y = 0; + EXPECT_NEAR(utils::normalizedDot(v1x, v1y, v2x, v2y), -1.0, 1e-4); +} + +TEST(UtilsTest, test_find_closest_point) +{ + geometry_msgs::msg::PoseStamped pose; + Coordinates start, end; + start.x = 0.0; + start.y = 0.0; + end.x = 10.0; + end.y = 0.0; + pose.pose.position.x = 0.0; + pose.pose.position.y = 0.0; + + // Test at, far from, and away from initial point + Coordinates rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = -10.0; + pose.pose.position.y = 0.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = -10.0; + pose.pose.position.y = 100.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + // Test at, far from, and away from final point + pose.pose.position.x = 10.0; + pose.pose.position.y = 0.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 10.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = 100.0; + pose.pose.position.y = 0.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 10.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = 100.0; + pose.pose.position.y = -10000.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 10.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = 1000.0; + pose.pose.position.y = 1000.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 10.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + // Test along length of the line + pose.pose.position.x = 5.0; + pose.pose.position.y = 1000.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 5.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + pose.pose.position.x = 0.1; + pose.pose.position.y = -10.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 0.1, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); + + // Let's try a more legit line now that we know the basics work OK + start.x = 0.0; + start.y = 0.0; + end.x = 10.0; + end.y = 10.0; + pose.pose.position.x = 5.0; + pose.pose.position.y = 5.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 5.0, 0.01); + EXPECT_NEAR(rtn.y, 5.0, 0.01); + + pose.pose.position.x = 0.0; + pose.pose.position.y = 10.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 5.0, 0.01); + EXPECT_NEAR(rtn.y, 5.0, 0.01); + + pose.pose.position.x = 10.0; + pose.pose.position.y = 0.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 5.0, 0.01); + EXPECT_NEAR(rtn.y, 5.0, 0.01); + + pose.pose.position.x = 2.0; + pose.pose.position.y = 4.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 3.0, 0.01); + EXPECT_NEAR(rtn.y, 3.0, 0.01); + + pose.pose.position.x = 4.0; + pose.pose.position.y = 10.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 7.0, 0.01); + EXPECT_NEAR(rtn.y, 7.0, 0.01); + + // Try identity to make sure no nan issues + start.x = 0.0; + start.y = 0.0; + end.x = 0.0; + end.y = 0.0; + pose.pose.position.x = 4.0; + pose.pose.position.y = 10.0; + rtn = utils::findClosestPoint(pose, start, end); + EXPECT_NEAR(rtn.x, 0.0, 0.01); + EXPECT_NEAR(rtn.y, 0.0, 0.01); +} + +TEST(UtilsTest, test_routing_state) +{ + ReroutingState state; + state.blocked_ids.resize(10); + state.first_time = false; + state.closest_pt_on_edge.x = 1.0; + state.rerouting_start_id = 10u; + state.rerouting_start_pose.pose.position.x = 1.0; + state.reset(); + EXPECT_EQ(state.blocked_ids.size(), 0u); + EXPECT_EQ(state.first_time, true); + EXPECT_EQ(state.closest_pt_on_edge.x, 0.0); + EXPECT_EQ(state.rerouting_start_id, std::numeric_limits::max()); + EXPECT_EQ(state.rerouting_start_pose.pose.position.x, 0.0); +} diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index bd9059d6989..2b9e6c30c44 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(nav2_common REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(nav2_route REQUIRED) find_package(pluginlib REQUIRED) find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) find_package(rclcpp REQUIRED) @@ -33,13 +35,23 @@ set(nav2_rviz_plugins_headers_to_moc include/nav2_rviz_plugins/goal_pose_updater.hpp include/nav2_rviz_plugins/goal_tool.hpp include/nav2_rviz_plugins/nav2_panel.hpp + include/nav2_rviz_plugins/route_tool.hpp include/nav2_rviz_plugins/selector.hpp include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp ) + foreach(header "${nav2_rviz_plugins_headers_to_moc}") qt5_wrap_cpp(nav2_rviz_plugins_moc_files "${header}") endforeach() +qt5_wrap_ui(route_tool_UIS_H resource/route_tool.ui) + +# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTOUIC ON) +set(CMAKE_AUTORCC ON) + set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED @@ -47,11 +59,13 @@ add_library(${library_name} SHARED src/docking_panel.cpp src/goal_tool.cpp src/nav2_panel.cpp + src/route_tool.cpp src/selector.cpp src/utils.cpp src/particle_cloud_display/flat_weighted_arrows_array.cpp src/particle_cloud_display/particle_cloud_display.cpp ${nav2_rviz_plugins_moc_files} + ${route_tool_UIS_H} ) target_include_directories(${library_name} PUBLIC ${Qt5Widgets_INCLUDE_DIRS} @@ -64,6 +78,7 @@ target_link_libraries(${library_name} PUBLIC nav2_lifecycle_manager::nav2_lifecycle_manager_core ${nav2_msgs_TARGETS} nav2_util::nav2_util_core + nav2_route::route_server_core rclcpp::rclcpp rclcpp_action::rclcpp_action rviz_common::rviz_common @@ -99,6 +114,8 @@ install( DESTINATION "share/${PROJECT_NAME}" ) +install(DIRECTORY rviz launch DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -110,6 +127,7 @@ ament_export_dependencies( geometry_msgs nav2_lifecycle_manager nav2_msgs + nav2_route nav2_util Qt5 rclcpp diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp new file mode 100644 index 00000000000..38c75f33466 --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp @@ -0,0 +1,119 @@ +// Copyright (c) 2024 John Chrosniak +// +// 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 NAV2_RVIZ_PLUGINS__ROUTE_TOOL_HPP_ +#define NAV2_RVIZ_PLUGINS__ROUTE_TOOL_HPP_ + +#include +#include +#include +#include +#include +#include "geometry_msgs/msg/point_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_route/graph_loader.hpp" +#include "nav2_route/graph_saver.hpp" +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rviz_common/panel.hpp" +#include "std_msgs/msg/int16.hpp" +#include "std_msgs/msg/string.hpp" + + +namespace nav2_rviz_plugins +{ +/** + * Here we declare our new subclass of rviz::Panel. Every panel which + * can be added via the Panels/Add_New_Panel menu is a subclass of + * rviz::Panel. + */ + +class RouteTool : public rviz_common::Panel +{ + /** + * This class uses Qt slots and is a subclass of QObject, so it needs + * the Q_OBJECT macro. + */ + Q_OBJECT + +public: + /** + * QWidget subclass constructors usually take a parent widget + * parameter (which usually defaults to 0). At the same time, + * pluginlib::ClassLoader creates instances by calling the default + * constructor (with no arguments). Taking the parameter and giving + * a default of 0 let's the default constructor work and also let's + * someone using the class for something else to pass in a parent + * widget as they normally would with Qt. + */ + explicit RouteTool(QWidget * parent = nullptr); + + void onInitialize() override; + + /** + * Now we declare overrides of rviz_common::Panel functions for saving and + * loading data from the config file. Here the data is the topic name. + */ + virtual void save(rviz_common::Config config) const; + virtual void load(const rviz_common::Config & config); + + + /** + * Here we declare some internal slots. + */ + +private Q_SLOTS: + void on_load_button_clicked(void); + + void on_save_button_clicked(void); + + void on_create_button_clicked(void); + + void on_confirm_button_clicked(void); + + void on_delete_button_clicked(void); + + void on_add_node_button_toggled(void); + + void on_edit_node_button_toggled(void); + + /** + * Finally, we close up with protected member variables + */ + +protected: + // UI pointer + std::unique_ptr ui_; + +private: + void update_route_graph(void); + nav2_util::LifecycleNode::SharedPtr node_; + std::shared_ptr graph_loader_; + std::shared_ptr graph_saver_; + std::shared_ptr tf_; + nav2_route::Graph graph_; + nav2_route::GraphToIDMap graph_to_id_map_; + nav2_route::GraphToIDMap edge_to_node_map_; + nav2_route::GraphToIncomingEdgesMap graph_to_incoming_edges_map_; + + rclcpp::Publisher::SharedPtr + graph_vis_publisher_; + rclcpp::Subscription::SharedPtr + clicked_point_subscription_; + + unsigned int next_node_id_ = 0; +}; +} // namespace nav2_rviz_plugins +#endif // NAV2_RVIZ_PLUGINS__ROUTE_TOOL_HPP_ diff --git a/nav2_rviz_plugins/launch/route_tool.launch.py b/nav2_rviz_plugins/launch/route_tool.launch.py new file mode 100644 index 00000000000..e0914b9d861 --- /dev/null +++ b/nav2_rviz_plugins/launch/route_tool.launch.py @@ -0,0 +1,75 @@ +# Copyright (c) 2025 John Chrosniak +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +import launch_ros.actions + + +def generate_launch_description() -> LaunchDescription: + + # Nodes launching commands + map_file = LaunchConfiguration('yaml_filename') + + declare_map_file_cmd = DeclareLaunchArgument( + 'yaml_filename', + default_value='', + description='Full path to an occupancy grid map yaml file', + ) + + start_route_tool_cmd = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + output='screen', + arguments=[ + '-d' + + os.path.join( + get_package_share_directory('nav2_rviz_plugins'), + 'rviz', + 'route_tool.rviz', + ) + ], + ) + + start_map_server = launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + output='screen', + parameters=[{'yaml_filename': map_file}], + ) + + start_lifecycle_manager_cmd = launch_ros.actions.Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[ + {'use_sim_time': False}, + {'autostart': True}, + {'node_names': ['map_server']}, + ], + ) + + return LaunchDescription( + [ + declare_map_file_cmd, + start_route_tool_cmd, + start_map_server, + start_lifecycle_manager_cmd, + ] + ) diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index c90944df774..0c509957230 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -16,6 +16,8 @@ nav2_lifecycle_manager nav2_msgs nav2_util + nav2_route + nav_msgs pluginlib rclcpp rclcpp_action diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index a28b1d9992a..920f9424c8b 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -36,4 +36,10 @@ The Particle Cloud rviz display. + + A tool used to create route graphs. + + diff --git a/nav2_rviz_plugins/resource/route_tool.ui b/nav2_rviz_plugins/resource/route_tool.ui new file mode 100644 index 00000000000..0db029c71d1 --- /dev/null +++ b/nav2_rviz_plugins/resource/route_tool.ui @@ -0,0 +1,386 @@ + + + + + + route_tool + + + + 0 + 0 + 394 + 461 + + + + MainWindow + + + + + + + + + + 0 + + + + + 0 + 0 + + + + Add + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + Text: + + + + + + + + + + + + + + 16777215 + 50 + + + + Field 1: + + + + + + + + 500 + 30 + + + + + + + + + + + + + 16777215 + 50 + + + + Field 2: + + + + + + + + 16777215 + 30 + + + + + + + + + + + + Create + + + + + + + + + + Edit + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + ID: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Text: + + + + + + + + + + + + + Field 1: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Field 2: + + + + + + + + 16777215 + 50 + + + + + + + + + + + + Confirm + + + + + + + + + + + + + Remove + + + + + + + + + + Node + + + + 16 + 16 + + + + Del + + + + + + + Edge + + + + + + + + + + + ID: + + + + + + + + 16777215 + 50 + + + + + + + + + + Delete + + + + + + + + + + + + + + + + + + + + Load + + + + + + + Save + + + + + + + + + + + + + + + diff --git a/nav2_rviz_plugins/rviz/route_tool.rviz b/nav2_rviz_plugins/rviz/route_tool.rviz new file mode 100644 index 00000000000..244d39685d3 --- /dev/null +++ b/nav2_rviz_plugins/rviz/route_tool.rviz @@ -0,0 +1,150 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Route Graph1/Topic1 + - /Map1 + - /Map1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 305 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: nav2_rviz_plugins/Route Tool + Name: Route Tool +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Route Graph + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /route_graph + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 22.106815338134766 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1003 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000034dfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001bc000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120072006f0075007400650054006f006f006c00000001a4000001e60000006e00fffffffb000000140052006f00750074006500200054006f006f006c01000001ff0000018b0000018b00ffffff000000010000010f0000034dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000034d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000034d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Route Tool: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1850 + X: 70 + Y: 27 + routeTool: + collapsed: false diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 96a4661878d..47539607ebb 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -54,14 +54,14 @@ void CostmapCostTool::onInitialize() rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); local_cost_client_ = std::make_shared>( - "/local_costmap/get_cost_local_costmap", - node, - false /* Does not create and spin an internal executor*/); + "/local_costmap/get_cost_local_costmap", + node, + false /* Does not create and spin an internal executor*/); global_cost_client_ = std::make_shared>( - "/global_costmap/get_cost_global_costmap", - node, - false /* Does not create and spin an internal executor*/); + "/global_costmap/get_cost_global_costmap", + node, + false /* Does not create and spin an internal executor*/); } void CostmapCostTool::activate() {} diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 0b5ee1055ff..c8a977730ee 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -252,7 +252,7 @@ DockingPanel::DockingPanel(QWidget * parent) if (!plugins_loaded_) { RCLCPP_INFO(client_node_->get_logger(), "Loading dock plugins"); nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); + client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); plugins_loaded_ = true; } }); diff --git a/nav2_rviz_plugins/src/route_tool.cpp b/nav2_rviz_plugins/src/route_tool.cpp new file mode 100644 index 00000000000..767d682887e --- /dev/null +++ b/nav2_rviz_plugins/src/route_tool.cpp @@ -0,0 +1,269 @@ +// Copyright (c) 2024 John Chrosniak +// +// 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 "nav2_rviz_plugins/route_tool.hpp" +#include +#include +#include +#include +#include +#include "rviz_common/display_context.hpp" + + +namespace nav2_rviz_plugins +{ +RouteTool::RouteTool(QWidget * parent) +: rviz_common::Panel(parent), + ui_(std::make_unique()) +{ + // Extend the widget with all attributes and children from UI file + ui_->setupUi(this); + node_ = std::make_shared("route_tool_node", "", rclcpp::NodeOptions()); + node_->configure(); + graph_vis_publisher_ = node_->create_publisher( + "route_graph", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + node_->activate(); + tf_ = std::make_shared(node_->get_clock()); + graph_loader_ = std::make_shared(node_, tf_, "map"); + graph_saver_ = std::make_shared(node_, tf_, "map"); + ui_->add_node_button->setChecked(true); + ui_->edit_node_button->setChecked(true); + ui_->remove_node_button->setChecked(true); + // Needed to prevent memory addresses moving from resizing + // when adding nodes and edges + graph_.reserve(1000); +} + +void RouteTool::onInitialize(void) +{ + auto ros_node_abstraction = getDisplayContext()->getRosNodeAbstraction().lock(); + if (!ros_node_abstraction) { + RCLCPP_ERROR( + node_->get_logger(), "Unable to get ROS node abstraction"); + return; + } + auto node = ros_node_abstraction->get_raw_node(); + + clicked_point_subscription_ = node->create_subscription( + "clicked_point", 1, [this](const geometry_msgs::msg::PointStamped::SharedPtr msg) { + ui_->add_field_1->setText(std::to_string(msg->point.x).c_str()); + ui_->add_field_2->setText(std::to_string(msg->point.y).c_str()); + ui_->edit_field_1->setText(std::to_string(msg->point.x).c_str()); + ui_->edit_field_2->setText(std::to_string(msg->point.y).c_str()); + }); +} + +void RouteTool::on_load_button_clicked(void) +{ + graph_to_id_map_.clear(); + edge_to_node_map_.clear(); + graph_to_incoming_edges_map_.clear(); + graph_.clear(); + QString filename = QFileDialog::getOpenFileName( + this, + tr("Open Address Book"), "", + tr("Address Book (*.geojson);;All Files (*)")); + graph_loader_->loadGraphFromFile(graph_, graph_to_id_map_, filename.toStdString()); + unsigned int max_node_id = 0; + for (const auto & node : graph_) { + max_node_id = std::max(node.nodeid, max_node_id); + for (const auto & edge : node.neighbors) { + max_node_id = std::max(edge.edgeid, max_node_id); + edge_to_node_map_[edge.edgeid] = node.nodeid; + if (graph_to_incoming_edges_map_.find(edge.end->nodeid) != + graph_to_incoming_edges_map_.end()) + { + graph_to_incoming_edges_map_[edge.end->nodeid].push_back(edge.edgeid); + } else { + graph_to_incoming_edges_map_[edge.end->nodeid] = std::vector {edge.edgeid}; + } + } + } + next_node_id_ = max_node_id + 1; + update_route_graph(); +} + +void RouteTool::on_save_button_clicked(void) +{ + QString filename = QFileDialog::getSaveFileName( + this, + tr("Open Address Book"), "", + tr("Address Book (*.geojson);;All Files (*)")); + RCLCPP_INFO(node_->get_logger(), "Save graph to: %s", filename.toStdString().c_str()); + graph_saver_->saveGraphToFile(graph_, filename.toStdString()); +} + +void RouteTool::on_create_button_clicked(void) +{ + if (ui_->add_field_1->toPlainText() == "" || ui_->add_field_2->toPlainText() == "") {return;} + if (ui_->add_node_button->isChecked()) { + auto longitude = ui_->add_field_1->toPlainText().toFloat(); + auto latitude = ui_->add_field_2->toPlainText().toFloat(); + nav2_route::Node new_node; + new_node.nodeid = next_node_id_; + new_node.coords.x = longitude; + new_node.coords.y = latitude; + graph_.push_back(new_node); + graph_to_id_map_[next_node_id_++] = graph_.size() - 1; + RCLCPP_INFO(node_->get_logger(), "Adding node at: (%f, %f)", longitude, latitude); + update_route_graph(); + } else if (ui_->add_edge_button->isChecked()) { + auto start_node = ui_->add_field_1->toPlainText().toInt(); + auto end_node = ui_->add_field_2->toPlainText().toInt(); + nav2_route::EdgeCost edge_cost; + graph_[graph_to_id_map_[start_node]].addEdge( + edge_cost, &(graph_[graph_to_id_map_[end_node]]), + next_node_id_); + if (graph_to_incoming_edges_map_.find(end_node) != graph_to_incoming_edges_map_.end()) { + graph_to_incoming_edges_map_[end_node].push_back(next_node_id_); + } else { + graph_to_incoming_edges_map_[end_node] = std::vector {next_node_id_}; + } + edge_to_node_map_[next_node_id_++] = start_node; + RCLCPP_INFO(node_->get_logger(), "Adding edge from %d to %d", start_node, end_node); + update_route_graph(); + } + ui_->add_field_1->setText(""); + ui_->add_field_2->setText(""); +} + +void RouteTool::on_confirm_button_clicked(void) +{ + if (ui_->edit_id->toPlainText() == "" || ui_->edit_field_1->toPlainText() == "" || + ui_->edit_field_2->toPlainText() == "") {return;} + if (ui_->edit_node_button->isChecked()) { + auto node_id = ui_->edit_id->toPlainText().toInt(); + auto new_longitude = ui_->edit_field_1->toPlainText().toFloat(); + auto new_latitude = ui_->edit_field_2->toPlainText().toFloat(); + if (graph_to_id_map_.find(node_id) != graph_to_id_map_.end()) { + graph_[graph_to_id_map_[node_id]].coords.x = new_longitude; + graph_[graph_to_id_map_[node_id]].coords.y = new_latitude; + update_route_graph(); + } + } else if (ui_->edit_edge_button->isChecked()) { + auto edge_id = (unsigned int) ui_->edit_id->toPlainText().toInt(); + auto new_start = ui_->edit_field_1->toPlainText().toInt(); + auto new_end = ui_->edit_field_2->toPlainText().toInt(); + // Find and remove current edge + auto current_start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = current_start_node->neighbors.begin(); + itr != current_start_node->neighbors.end(); itr++) + { + if (itr->edgeid == edge_id) { + current_start_node->neighbors.erase(itr); + break; + } + } + // Create new edge with same ID using new start and stop nodes + nav2_route::EdgeCost edge_cost; + graph_[graph_to_id_map_[new_start]].addEdge( + edge_cost, &(graph_[graph_to_id_map_[new_end]]), + edge_id); + edge_to_node_map_[edge_id] = new_start; + if (graph_to_incoming_edges_map_.find(new_end) != graph_to_incoming_edges_map_.end()) { + graph_to_incoming_edges_map_[new_end].push_back(edge_id); + } else { + graph_to_incoming_edges_map_[new_end] = std::vector {edge_id}; + } + update_route_graph(); + } + ui_->edit_id->setText(""); + ui_->edit_field_1->setText(""); + ui_->edit_field_2->setText(""); +} + +void RouteTool::on_delete_button_clicked(void) +{ + if (ui_->remove_id->toPlainText() == "") {return;} + if (ui_->remove_node_button->isChecked()) { + unsigned int node_id = ui_->remove_id->toPlainText().toInt(); + // Remove edges pointing to the removed node + for (auto edge_id : graph_to_incoming_edges_map_[node_id]) { + auto start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = start_node->neighbors.begin(); itr != start_node->neighbors.end(); itr++) { + if (itr->edgeid == edge_id) { + start_node->neighbors.erase(itr); + edge_to_node_map_.erase(edge_id); + break; + } + } + } + if (graph_[graph_to_id_map_[node_id]].nodeid == node_id) { + // Use max int to mark the node as deleted + graph_[graph_to_id_map_[node_id]].nodeid = std::numeric_limits::max(); + graph_to_id_map_.erase(node_id); + graph_to_incoming_edges_map_.erase(node_id); + RCLCPP_INFO(node_->get_logger(), "Removed node %d", node_id); + } + update_route_graph(); + } else if (ui_->remove_edge_button->isChecked()) { + auto edge_id = (unsigned int) ui_->remove_id->toPlainText().toInt(); + auto start_node = &graph_[graph_to_id_map_[edge_to_node_map_[edge_id]]]; + for (auto itr = start_node->neighbors.begin(); itr != start_node->neighbors.end(); itr++) { + if (itr->edgeid == edge_id) { + RCLCPP_INFO(node_->get_logger(), "Removed edge %d", edge_id); + start_node->neighbors.erase(itr); + edge_to_node_map_.erase(edge_id); + break; + } + } + update_route_graph(); + } + ui_->remove_id->setText(""); +} + +void RouteTool::on_add_node_button_toggled(void) +{ + if (ui_->add_node_button->isChecked()) { + ui_->add_text->setText("Position:"); + ui_->add_label_1->setText("X:"); + ui_->add_label_2->setText("Y:"); + } else { + ui_->add_text->setText("Connections:"); + ui_->add_label_1->setText("Start Node ID:"); + ui_->add_label_2->setText("End Node ID:"); + } +} + +void RouteTool::on_edit_node_button_toggled(void) +{ + if (ui_->edit_node_button->isChecked()) { + ui_->edit_text->setText("Position:"); + ui_->edit_label_1->setText("X:"); + ui_->edit_label_2->setText("Y:"); + } else { + ui_->edit_text->setText("Connections:"); + ui_->edit_label_1->setText("Start Node ID:"); + ui_->edit_label_2->setText("End Node ID:"); + } +} + +void RouteTool::update_route_graph(void) +{ + graph_vis_publisher_->publish(nav2_route::utils::toMsg(graph_, "map", node_->now())); +} + +void RouteTool::save(rviz_common::Config config) const +{ + rviz_common::Panel::save(config); +} + +void RouteTool::load(const rviz_common::Config & config) +{ + rviz_common::Panel::load(config); +} +} // namespace nav2_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::RouteTool, rviz_common::Panel) diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index 086554573d4..1fffd8ff425 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -149,35 +149,36 @@ void Selector::setProgressChecker() void Selector::loadPlugins() { - load_plugins_thread_ = std::thread([this]() { - rclcpp::Rate rate(0.2); - while (rclcpp::ok() && !plugins_loaded_) { - RCLCPP_INFO(client_node_->get_logger(), "Trying to load plugins..."); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "controller_server", "controller_plugins", controller_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "planner_server", "planner_plugins", planner_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "controller_server", "goal_checker_plugins", - goal_checker_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "smoother_server", "smoother_plugins", smoother_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "controller_server", "progress_checker_plugins", - progress_checker_); - if (controller_->count() > 0 && - planner_->count() > 0 && - goal_checker_->count() > 0 && - smoother_->count() > 0 && - progress_checker_->count() > 0) - { - plugins_loaded_ = true; - } else { - RCLCPP_INFO(client_node_->get_logger(), "Failed to load plugins. Retrying..."); - } - rate.sleep(); + load_plugins_thread_ = std::thread( + [this]() { + rclcpp::Rate rate(0.2); + while (rclcpp::ok() && !plugins_loaded_) { + RCLCPP_INFO(client_node_->get_logger(), "Trying to load plugins..."); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "controller_server", "controller_plugins", controller_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "planner_server", "planner_plugins", planner_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "controller_server", "goal_checker_plugins", + goal_checker_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "smoother_server", "smoother_plugins", smoother_); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "controller_server", "progress_checker_plugins", + progress_checker_); + if (controller_->count() > 0 && + planner_->count() > 0 && + goal_checker_->count() > 0 && + smoother_->count() > 0 && + progress_checker_->count() > 0) + { + plugins_loaded_ = true; + } else { + RCLCPP_INFO(client_node_->get_logger(), "Failed to load plugins. Retrying..."); } - }); + rate.sleep(); + } + }); } } // namespace nav2_rviz_plugins diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index f13cf8491bf..e84c9f4e57c 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -26,11 +26,13 @@ New as of September 2023: the simple navigator constructor will accept a `namesp | spin(spin_dist=1.57, time_allowance=10, disable_collision_checks=False) | Requests the robot to performs an in-place rotation by a given angle. | | backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10, disable_collision_checks=False) | Requests the robot to back up by a given distance. | | cancelTask() | Cancel an ongoing task request.| -| isTaskComplete() | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | -| getFeedback() | Gets feedback from task, returns action server feedback object. | +| isTaskComplete(task=RunningTask.None) | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. Provide the running task return. | +| getFeedback(task=RunningTask.None) | Gets feedback from task, returns action server feedback object. Provide the running task return. | | getResult() | Gets final result of task, to be called after `isTaskComplete` returns `True`. Returns action server result object. | | getPath(start, goal, planner_id='', use_start=False) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | | getPathThroughPoses(start, goals, planner_id='', use_start=False) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | +| getRoute(start, goal, use_start=False) | Gets a sparse route and a dense path from start to goal using the route server. The start and goal may be either `PoseStamped` or `int` NodeIDs. Returns `[nav_msgs/Path, nav_msgs/Route]`. | +| getandTrackRoute(start, goal, use_start=False) | Obtains a route and path (sent to client via feedback) and tracks progress along it to trigger internal route graph operations and rerouting mechanics. The start and goal may be either `PoseStamped` or `int` NodeIDs. | | smoothPath(path, smoother_id='', max_duration=2.0, check_for_collision=False) | Smooths a given `nav_msgs/msg/Path` path. | | changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. | | clearAllCostmaps() | Clears both the global and local costmaps. | @@ -60,9 +62,9 @@ nav.waitUntilNav2Active() # if autostarted, else use `lifecycleStartup()` path = nav.getPath(init_pose, goal_pose) smoothed_path = nav.smoothPath(path) ... -nav.goToPose(goal_pose) -while not nav.isTaskComplete(): - feedback = nav.getFeedback() +go_to_pose_task = nav.goToPose(goal_pose) +while not nav.isTaskComplete(task=go_to_pose_task): + feedback = nav.getFeedback(task=go_to_pose_task) if feedback.navigation_duration > 600: nav.cancelTask() ... @@ -114,6 +116,8 @@ The `nav2_simple_commander` has a few examples to highlight the API functions av - `example_nav_through_poses.py` - Demonstrates the navigate through poses capabilities of the navigator, as well as a number of auxiliary methods. - `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods. - `example_follow_path.py` - Demonstrates the path following capabilities of the navigator, as well as a number of auxiliary methods such as path smoothing. +- `example_route.py` - Demonstrates the route server's capabilities of the navigator, as well as a number of methods of how to use it, such as via waypoint following or control server path tracking (among other options). Make sure to set the `aws_graph.geojson` in your nav2_params.yaml file +- `example_assisted_teleop.py` - Demonstrates the assisted teleop's capabilities of the navigator. ## Demos The `nav2_simple_commander` has a few demonstrations to highlight a couple of simple autonomy applications you can build using the `nav2_simple_commander` API: diff --git a/nav2_simple_commander/launch/route_example_launch.py b/nav2_simple_commander/launch/route_example_launch.py new file mode 100644 index 00000000000..8490f70556a --- /dev/null +++ b/nav2_simple_commander/launch/route_example_launch.py @@ -0,0 +1,136 @@ +# Copyright (c) 2025 Open Navigation LLC +# +# 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. + +import os +from pathlib import Path +import tempfile + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler) +from launch.conditions import IfCondition +from launch.event_handlers import OnShutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + sim_dir = get_package_share_directory('nav2_minimal_tb4_sim') + desc_dir = get_package_share_directory('nav2_minimal_tb4_description') + + robot_sdf = os.path.join(desc_dir, 'urdf', 'standard', 'turtlebot4.urdf.xacro') + world = os.path.join(sim_dir, 'worlds', 'depot.sdf') + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'depot.yaml') + graph_filepath = os.path.join(nav2_bringup_dir, 'graphs', 'turtlebot4_graph.geojson') + + # Launch configuration variables + headless = LaunchConfiguration('headless') + + # Declare the launch arguments + declare_headless_cmd = DeclareLaunchArgument( + 'headless', default_value='False', description='Whether to execute gzclient)' + ) + + # start the simulation + world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') + world_sdf_xacro = ExecuteProcess( + cmd=['xacro', '-o', world_sdf, ['headless:=', headless], world]) + start_gazebo_server_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', + 'gz_sim.launch.py')), + launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items()) + + remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown( + on_shutdown=[ + OpaqueFunction(function=lambda _: os.remove(world_sdf)) + ])) + + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(sim_dir, 'worlds')) + start_gazebo_client_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py') + ), + condition=IfCondition(PythonExpression( + ['not ', headless])), + launch_arguments={'gz_args': ['-v4 -g ']}.items(), + ) + + spawn_robot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb4.launch.py')), + launch_arguments={'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-8.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0'}.items()) + + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + {'use_sim_time': True, 'robot_description': Command(['xacro', ' ', robot_sdf])} + ] + ) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file, 'graph': graph_filepath}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_route', + emulate_tty=True, + output='screen') + + set_env_vars_resources2 = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(desc_dir)).parent.resolve())) + + ld = LaunchDescription() + ld.add_action(declare_headless_cmd) + ld.add_action(set_env_vars_resources) + ld.add_action(world_sdf_xacro) + ld.add_action(remove_temp_sdf_file) + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(spawn_robot_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + ld.add_action(set_env_vars_resources2) + return ld diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index f2acdfe8cb1..87e1be6aefd 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -76,14 +76,14 @@ def main() -> None: inspection_pose.pose.orientation.w = 0.707 inspection_points.append(deepcopy(inspection_pose)) - navigator.followWaypoints(inspection_points) + wpf_task = navigator.followWaypoints(inspection_points) # Do something during our route (e.x. AI to analyze stock information or upload to the cloud) # Simply the current waypoint ID for the demonstration i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=wpf_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=wpf_task) if feedback and i % 5 == 0: print( 'Executing current waypoint: ' @@ -104,8 +104,8 @@ def main() -> None: # go back to start initial_pose.header.stamp = navigator.get_clock().now().to_msg() - navigator.goToPose(initial_pose) - while not navigator.isTaskComplete(): + go_to_pose_task = navigator.goToPose(initial_pose) + while not navigator.isTaskComplete(task=go_to_pose_task): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index c8fe7301272..d9ec76fb971 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -80,15 +80,15 @@ def main() -> None: shelf_item_pose.pose.orientation.z = -0.707 shelf_item_pose.pose.orientation.w = 0.707 print(f'Received request for item picking at {request_item_location}.') - navigator.goToPose(shelf_item_pose) + go_to_pose_task = navigator.goToPose(shelf_item_pose) # Do something during our route # (e.x. queue up future tasks or detect person for fine-tuned positioning) # Simply print information for workers on the robot's ETA for the demonstration i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=go_to_pose_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=go_to_pose_task) if feedback and i % 5 == 0: print( 'Estimated time of arrival at ' @@ -121,7 +121,7 @@ def main() -> None: ][1] shipping_destination.pose.orientation.z = 1.0 shipping_destination.pose.orientation.w = 0.0 - navigator.goToPose(shipping_destination) + go_to_pose_task = navigator.goToPose(shipping_destination) elif result == TaskResult.CANCELED: print( @@ -136,7 +136,7 @@ def main() -> None: f'{error_code}:{error_msg}') exit(-1) - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=go_to_pose_task): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py index dcc3904ee4f..2cbc5be99ec 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py @@ -50,12 +50,12 @@ def main() -> None: goal_pose.pose.position.y = 1.3 goal_pose.pose.orientation.w = 1.0 - navigator.goToPose(goal_pose) + go_to_pose_task = navigator.goToPose(goal_pose) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=go_to_pose_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=go_to_pose_task) if feedback and i % 5 == 0: print( f'Estimated time of arrival to destination is: \ @@ -64,23 +64,23 @@ def main() -> None: # Robot hit a dead end, back it up print("Robot hit a dead end (let's pretend), backing up...") - navigator.backup(backup_dist=0.5, backup_speed=0.1) + backup_task = navigator.backup(backup_dist=0.5, backup_speed=0.1) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=backup_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=backup_task) if feedback and i % 5 == 0: print(f'Distance traveled: {feedback.distance_traveled}') # Turn it around print('Spinning robot around...') - navigator.spin(spin_dist=3.14) + spin_task = navigator.spin(spin_dist=3.14) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=spin_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=spin_task) if feedback and i % 5 == 0: print(f'Spin angle traveled: {feedback.angular_distance_traveled}') @@ -95,8 +95,8 @@ def main() -> None: print('Returning to start...') initial_pose.header.stamp = navigator.get_clock().now().to_msg() - navigator.goToPose(initial_pose) - while not navigator.isTaskComplete(): + go_to_pose_task = navigator.goToPose(initial_pose) + while not navigator.isTaskComplete(task=go_to_pose_task): pass exit(0) diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index fa4be0761e8..9e4bff5f592 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -67,14 +67,14 @@ def main() -> None: pose.pose.position.x = pt[0] pose.pose.position.y = pt[1] route_poses.append(deepcopy(pose)) - navigator.goThroughPoses(route_poses) + go_through_poses_task = navigator.goThroughPoses(route_poses) # Do something during our route (e.x. AI detection on camera images for anomalies) # Simply print ETA for the demonstration i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=go_through_poses_task): i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=go_through_poses_task) if feedback and i % 5 == 0: print( 'Estimated time to complete current route: ' diff --git a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py index be80705a85a..b04c4b9d369 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py +++ b/nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py @@ -42,8 +42,8 @@ def main() -> None: # Wait for navigation to fully activate, since autostarting nav2 navigator.waitUntilNav2Active() - navigator.assistedTeleop(time_allowance=20) - while not navigator.isTaskComplete(): + teleop_task = navigator.assistedTeleop(time_allowance=20) + while not navigator.isTaskComplete(task=teleop_task): # Publish twist commands to be filtered by the assisted teleop action sleep(0.2) pass diff --git a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py index 9b4439e52f6..c423f3a23dd 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -53,10 +53,10 @@ def main() -> None: smoothed_path = navigator.smoothPath(path) # Follow path - navigator.followPath(smoothed_path) + follow_path_task = navigator.followPath(smoothed_path) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=follow_path_task): ################################################ # # Implement some code here for your application! @@ -65,7 +65,7 @@ def main() -> None: # Do something with the feedback i += 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=follow_path_task) if feedback and i % 5 == 0: print( 'Estimated distance remaining to goal position: ' diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py index 7585093fd74..45bf3fd4e77 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -86,10 +86,10 @@ def main() -> None: # sanity check a valid path exists # path = navigator.getPathThroughPoses(initial_pose, goal_poses) - navigator.goThroughPoses(goal_poses) + nav_through_poses_task = navigator.goThroughPoses(goal_poses) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=nav_through_poses_task): ################################################ # # Implement some code here for your application! @@ -98,7 +98,7 @@ def main() -> None: # Do something with the feedback i = i + 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=nav_through_poses_task) if feedback and i % 5 == 0: print( 'Estimated time of arrival: ' diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py index 985230915a3..76855435e8d 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -66,10 +66,10 @@ def main() -> None: # sanity check a valid path exists # path = navigator.getPath(initial_pose, goal_pose) - navigator.goToPose(goal_pose) + go_to_pose_task = navigator.goToPose(goal_pose) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=go_to_pose_task): ################################################ # # Implement some code here for your application! @@ -78,7 +78,7 @@ def main() -> None: # Do something with the feedback i = i + 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=go_to_pose_task) if feedback and i % 5 == 0: print( 'Estimated time of arrival: ' @@ -97,7 +97,7 @@ def main() -> None: if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0): goal_pose.pose.position.x = 0.0 goal_pose.pose.position.y = 0.0 - navigator.goToPose(goal_pose) + go_to_pose_task = navigator.goToPose(goal_pose) # Do something depending on the return code result = navigator.getResult() diff --git a/nav2_simple_commander/nav2_simple_commander/example_route.py b/nav2_simple_commander/nav2_simple_commander/example_route.py new file mode 100644 index 00000000000..472d8b28e6f --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_route.py @@ -0,0 +1,149 @@ +#! /usr/bin/env python3 +# Copyright 2025 Open Navigation LLC +# +# 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. + +from geometry_msgs.msg import Pose, PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, RunningTask, TaskResult +import rclpy +from std_msgs.msg import Header + +""" +Basic navigation demo to using the route server. +""" + + +def toPoseStamped(pt: Pose, header: Header) -> PoseStamped: + pose = PoseStamped() + pose.pose.position.x = pt.x + pose.pose.position.y = pt.y + pose.header = header + return pose + + +def main() -> None: + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 7.5 + initial_pose.pose.position.y = 7.5 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 + navigator.setInitialPose(initial_pose) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # Go to our demos first goal pose + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = 20.12 + goal_pose.pose.position.y = 11.83 + goal_pose.pose.orientation.w = 1.0 + + # Sanity check a valid route exists using PoseStamped. + # May also use NodeIDs on the graph if they are known by passing them instead as `int` + # [path, route] = navigator.getRoute(initial_pose, goal_pose) + + # May also use NodeIDs on the graph if they are known by passing them instead as `int` + route_tracking_task = navigator.getAndTrackRoute(initial_pose, goal_pose) + + # Note for the route server, we have a special route argument in the API b/c it may be + # providing feedback messages simultaneously to others (e.g. controller or WPF as below) + task_canceled = False + last_feedback = None + follow_path_task = RunningTask.NONE + while not navigator.isTaskComplete(task=route_tracking_task): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback, which contains the route / path if tracking + feedback = navigator.getFeedback(task=route_tracking_task) + while feedback is not None: + if not last_feedback or \ + (feedback.last_node_id != last_feedback.last_node_id or + feedback.next_node_id != last_feedback.next_node_id): + print('Passed node ' + str(feedback.last_node_id) + + ' to next node ' + str(feedback.next_node_id) + + ' along edge ' + str(feedback.current_edge_id) + '.') + + last_feedback = feedback + + if feedback.rerouted: # or follow_path_task == RunningTask.None + # Follow the path from the route server using the controller server + print('Passing new route to controller!') + follow_path_task = navigator.followPath(feedback.path) + + # May instead use the waypoint follower + # (or nav through poses) and use the route's sparse nodes! + # print("Passing route to waypoint follower!") + # nodes = + # [toPoseStamped(x.position, feedback.route.header) for x in feedback.route.nodes] + # navigator.followWaypoints(nodes) + # Or navigator.navigateThroughPoses(nodes) + # Consider sending only the first few and iterating + + feedback = navigator.getFeedback(task=route_tracking_task) + + # Check if followPath or WPF task is done (or failed), + # will cancel all current tasks, including route + if navigator.isTaskComplete(task=follow_path_task): + print('Controller or waypoint follower server completed its task!') + navigator.cancelTask() + task_canceled = True + + # Route server will return completed status before the controller / WPF server + # so wait for the actual robot task processing server to complete + while not navigator.isTaskComplete(task=follow_path_task) and not task_canceled: + pass + + # Do something depending on the return code + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Goal succeeded!') + elif result == TaskResult.CANCELED: + print('Goal was canceled!') + elif result == TaskResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py index a70929b1cd9..de739e82d89 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -87,10 +87,10 @@ def main() -> None: # path = navigator.getPath(initial_pose, goal_pose1) nav_start = navigator.get_clock().now() - navigator.followWaypoints(goal_poses) + follow_waypoints_task = navigator.followWaypoints(goal_poses) i = 0 - while not navigator.isTaskComplete(): + while not navigator.isTaskComplete(task=follow_waypoints_task): ################################################ # # Implement some code here for your application! @@ -99,7 +99,7 @@ def main() -> None: # Do something with the feedback i = i + 1 - feedback = navigator.getFeedback() + feedback = navigator.getFeedback(task=follow_waypoints_task) if feedback and i % 5 == 0: print( 'Executing current waypoint: ' @@ -124,7 +124,7 @@ def main() -> None: goal_pose4.pose.orientation.z = 0.0 goal_poses = [goal_pose4] nav_start = now - navigator.followWaypoints(goal_poses) + follow_waypoints_task = navigator.followWaypoints(goal_poses) # Do something depending on the return code result = navigator.getResult() diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index b65dd7449de..50fd706ec57 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 # Copyright 2021 Samsung Research America +# Copyright 2025 Open Navigation LLC # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -16,17 +17,18 @@ from enum import Enum import time -from typing import Any, Optional +from typing import Any, Optional, Union from action_msgs.msg import GoalStatus from builtin_interfaces.msg import Duration from geographic_msgs.msg import GeoPose from geometry_msgs.msg import Point, PoseStamped, PoseWithCovarianceStamped from lifecycle_msgs.srv import GetState -from nav2_msgs.action import (AssistedTeleop, BackUp, ComputePathThroughPoses, ComputePathToPose, - DockRobot, DriveOnHeading, FollowGPSWaypoints, FollowPath, - FollowWaypoints, NavigateThroughPoses, NavigateToPose, SmoothPath, - Spin, UndockRobot) +from nav2_msgs.action import (AssistedTeleop, BackUp, ComputeAndTrackRoute, + ComputePathThroughPoses, ComputePathToPose, ComputeRoute, DockRobot, + DriveOnHeading, FollowGPSWaypoints, FollowPath, FollowWaypoints, + NavigateThroughPoses, NavigateToPose, SmoothPath, Spin, UndockRobot) +from nav2_msgs.msg import Route from nav2_msgs.srv import (ClearCostmapAroundRobot, ClearCostmapExceptRegion, ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes) from nav_msgs.msg import Goals, OccupancyGrid, Path @@ -40,6 +42,7 @@ from rclpy.type_support import GetResultServiceResponse +# Task Result enum for the result of the task being executed class TaskResult(Enum): UNKNOWN = 0 SUCCEEDED = 1 @@ -47,17 +50,48 @@ class TaskResult(Enum): FAILED = 3 +# Task enum for the task being executed, if its a long-running task to be able to obtain +# necessary contextual information in `isTaskComplete` and `getFeedback` regarding the task +# which is running. +class RunningTask(Enum): + NONE = 0 + NAVIGATE_TO_POSE = 1 + NAVIGATE_THROUGH_POSES = 2 + FOLLOW_PATH = 3 + FOLLOW_WAYPOINTS = 4 + FOLLOW_GPS_WAYPOINTS = 5 + SPIN = 6 + BACKUP = 7 + DRIVE_ON_HEADING = 8 + ASSISTED_TELEOP = 9 + DOCK_ROBOT = 10 + UNDOCK_ROBOT = 11 + COMPUTE_AND_TRACK_ROUTE = 12 + + class BasicNavigator(Node): def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> None: super().__init__(node_name=node_name, namespace=namespace) self.initial_pose = PoseStamped() self.initial_pose.header.frame_id = 'map' + self.goal_handle: Optional[ClientGoalHandle[Any, Any, Any]] = None self.result_future: \ Optional[Future[GetResultServiceResponse[Any]]] = None - self.feedback: str = '' + self.feedback: Any = None self.status: Optional[int] = None + + # Since the route server's compute and track action server is likely + # to be running simultaneously with another (e.g. controller, WPF) server, + # we must track its futures and feedback separately. Additionally, the + # route tracking feedback is uniquely important to be complete and ordered + self.route_goal_handle: Optional[ClientGoalHandle[Any, Any, Any]] = None + self.route_result_future: \ + Optional[Future[GetResultServiceResponse[Any]]] = None + self.route_feedback: list[Any] = [] + + # Error code and messages from servers self.last_action_error_code = 0 self.last_action_error_msg = '' @@ -87,6 +121,9 @@ def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> N self, ComputePathThroughPoses, 'compute_path_through_poses' ) self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path') + self.compute_route_client = ActionClient(self, ComputeRoute, 'compute_route') + self.compute_and_track_route_client = ActionClient(self, ComputeAndTrackRoute, + 'compute_and_track_route') self.spin_client = ActionClient(self, Spin, 'spin') self.backup_client = ActionClient(self, BackUp, 'backup') self.drive_on_heading_client = ActionClient( @@ -136,6 +173,8 @@ def destroy_node(self) -> None: self.follow_path_client.destroy() self.compute_path_to_pose_client.destroy() self.compute_path_through_poses_client.destroy() + self.compute_and_track_route_client.destroy() + self.compute_route_client.destroy() self.smoother_client.destroy() self.spin_client.destroy() self.backup_client.destroy() @@ -152,7 +191,7 @@ def setInitialPose(self, initial_pose: PoseStamped) -> None: self.initial_pose = initial_pose self._setInitialPose() - def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> bool: + def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> Optional[RunningTask]: """Send a `NavThroughPoses` action request.""" self.clearTaskError() self.debug("Waiting for 'NavigateThroughPoses' action server") @@ -174,12 +213,12 @@ def goThroughPoses(self, poses: Goals, behavior_tree: str = '') -> bool: msg = f'NavigateThroughPoses request with {len(poses)} was rejected!' self.setTaskError(NavigateThroughPoses.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.NAVIGATE_THROUGH_POSES - def goToPose(self, pose: PoseStamped, behavior_tree: str = '') -> bool: + def goToPose(self, pose: PoseStamped, behavior_tree: str = '') -> Optional[RunningTask]: """Send a `NavToPose` action request.""" self.clearTaskError() self.debug("Waiting for 'NavigateToPose' action server") @@ -213,12 +252,12 @@ def goToPose(self, pose: PoseStamped, behavior_tree: str = '') -> bool: ) self.setTaskError(NavigateToPose.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.NAVIGATE_TO_POSE - def followWaypoints(self, poses: list[PoseStamped]) -> bool: + def followWaypoints(self, poses: list[PoseStamped]) -> Optional[RunningTask]: """Send a `FollowWaypoints` action request.""" self.clearTaskError() self.debug("Waiting for 'FollowWaypoints' action server") @@ -239,12 +278,12 @@ def followWaypoints(self, poses: list[PoseStamped]) -> bool: msg = f'Following {len(poses)} waypoints request was rejected!' self.setTaskError(FollowWaypoints.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.FOLLOW_WAYPOINTS - def followGpsWaypoints(self, gps_poses: list[GeoPose]) -> bool: + def followGpsWaypoints(self, gps_poses: list[GeoPose]) -> Optional[RunningTask]: """Send a `FollowGPSWaypoints` action request.""" self.clearTaskError() self.debug("Waiting for 'FollowWaypoints' action server") @@ -265,14 +304,14 @@ def followGpsWaypoints(self, gps_poses: list[GeoPose]) -> bool: msg = f'Following {len(gps_poses)} gps waypoints request was rejected!' self.setTaskError(FollowGPSWaypoints.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.FOLLOW_GPS_WAYPOINTS def spin( self, spin_dist: float = 1.57, time_allowance: int = 10, - disable_collision_checks: bool = False) -> bool: + disable_collision_checks: bool = False) -> Optional[RunningTask]: self.clearTaskError() self.debug("Waiting for 'Spin' action server") while not self.spin_client.wait_for_server(timeout_sec=1.0): @@ -293,14 +332,15 @@ def spin( msg = 'Spin request was rejected!' self.setTaskError(Spin.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.SPIN def backup( self, backup_dist: float = 0.15, backup_speed: float = 0.025, - time_allowance: int = 10, disable_collision_checks: bool = False) -> bool: + time_allowance: int = 10, + disable_collision_checks: bool = False) -> Optional[RunningTask]: self.clearTaskError() self.debug("Waiting for 'Backup' action server") while not self.backup_client.wait_for_server(timeout_sec=1.0): @@ -322,14 +362,15 @@ def backup( msg = 'Backup request was rejected!' self.setTaskError(BackUp.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.BACKUP def driveOnHeading( self, dist: float = 0.15, speed: float = 0.025, - time_allowance: int = 10, disable_collision_checks: bool = False) -> bool: + time_allowance: int = 10, + disable_collision_checks: bool = False) -> Optional[RunningTask]: self.clearTaskError() self.debug("Waiting for 'DriveOnHeading' action server") while not self.backup_client.wait_for_server(timeout_sec=1.0): @@ -351,12 +392,12 @@ def driveOnHeading( msg = 'Drive On Heading request was rejected!' self.setTaskError(DriveOnHeading.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.DRIVE_ON_HEADING - def assistedTeleop(self, time_allowance: int = 30) -> bool: + def assistedTeleop(self, time_allowance: int = 30) -> Optional[RunningTask]: self.clearTaskError() self.debug("Wanting for 'assisted_teleop' action server") @@ -377,13 +418,13 @@ def assistedTeleop(self, time_allowance: int = 30) -> bool: msg = 'Assisted Teleop request was rejected!' self.setTaskError(AssistedTeleop.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.ASSISTED_TELEOP def followPath(self, path: Path, controller_id: str = '', - goal_checker_id: str = '') -> bool: + goal_checker_id: str = '') -> Optional[RunningTask]: self.clearTaskError() """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") @@ -406,13 +447,13 @@ def followPath(self, path: Path, controller_id: str = '', msg = 'FollowPath goal was rejected!' self.setTaskError(FollowPath.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.FOLLOW_PATH def dockRobotByPose(self, dock_pose: PoseStamped, - dock_type: str = '', nav_to_dock: bool = True) -> bool: + dock_type: str = '', nav_to_dock: bool = True) -> Optional[RunningTask]: self.clearTaskError() """Send a `DockRobot` action request.""" self.info("Waiting for 'DockRobot' action server") @@ -435,12 +476,12 @@ def dockRobotByPose(self, dock_pose: PoseStamped, msg = 'DockRobot request was rejected!' self.setTaskError(DockRobot.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.DOCK_ROBOT - def dockRobotByID(self, dock_id: str, nav_to_dock: bool = True) -> bool: + def dockRobotByID(self, dock_id: str, nav_to_dock: bool = True) -> Optional[RunningTask]: """Send a `DockRobot` action request.""" self.clearTaskError() self.info("Waiting for 'DockRobot' action server") @@ -462,12 +503,12 @@ def dockRobotByID(self, dock_id: str, nav_to_dock: bool = True) -> bool: msg = 'DockRobot request was rejected!' self.setTaskError(DockRobot.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.DOCK_ROBOT - def undockRobot(self, dock_type: str = '') -> bool: + def undockRobot(self, dock_type: str = '') -> Optional[RunningTask]: """Send a `UndockRobot` action request.""" self.clearTaskError() self.info("Waiting for 'UndockRobot' action server") @@ -487,10 +528,10 @@ def undockRobot(self, dock_type: str = '') -> bool: msg = 'UndockRobot request was rejected!' self.setTaskError(UndockRobot.UNKNOWN, msg) self.error(msg) - return False + return None self.result_future = self.goal_handle.get_result_async() - return True + return RunningTask.UNDOCK_ROBOT def cancelTask(self) -> None: """Cancel pending task request of any type.""" @@ -500,17 +541,39 @@ def cancelTask(self) -> None: future = self.goal_handle.cancel_goal_async() rclpy.spin_until_future_complete(self, future) else: - self.error('No goal handle to cancel!') + self.error('Cancel task failed, goal handle is None') + self.setTaskError(0, 'Cancel task failed, goal handle is None') + return + if self.route_result_future: + if self.route_goal_handle is not None: + future = self.route_goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, future) + else: + self.error('Cancel route task failed, goal handle is None') + self.setTaskError(0, 'Cancel route task failed, goal handle is None') + return self.clearTaskError() return - def isTaskComplete(self) -> bool: + def isTaskComplete(self, task: RunningTask = RunningTask.NONE) -> bool: """Check if the task request of any type is complete yet.""" - if not self.result_future: + # Find the result future to spin + if task is None: + self.error('Task is None, cannot check for completion') + return False + + result_future = None + if task != RunningTask.COMPUTE_AND_TRACK_ROUTE: + result_future = self.result_future + else: + result_future = self.route_result_future + if not result_future: # task was cancelled or completed return True - rclpy.spin_until_future_complete(self, self.result_future, timeout_sec=0.10) - result_response = self.result_future.result() + + # Get the result of the future, if complete + rclpy.spin_until_future_complete(self, result_future, timeout_sec=0.10) + result_response = result_future.result() if result_response: self.status = result_response.status @@ -522,8 +585,7 @@ def isTaskComplete(self) -> bool: 'Task with failed with' f' status code:{self.status}' f' error code:{result.error_code}' - f' error msg:{result.error_msg}' - ) + f' error msg:{result.error_msg}') return True else: self.setTaskError(0, 'No result received') @@ -536,9 +598,13 @@ def isTaskComplete(self) -> bool: self.debug('Task succeeded!') return True - def getFeedback(self) -> str: + def getFeedback(self, task: RunningTask = RunningTask.NONE) -> Any: """Get the pending action feedback message.""" - return self.feedback + if task != RunningTask.COMPUTE_AND_TRACK_ROUTE: + return self.feedback + if len(self.route_feedback) > 0: + return self.route_feedback.pop(0) + return None def getResult(self) -> TaskResult: """Get the pending action result message.""" @@ -690,6 +756,117 @@ def getPathThroughPoses( f' error msg:{rtn.error_msg}') return None + def _getRouteImpl( + self, start: Union[int, PoseStamped], + goal: Union[int, PoseStamped], use_start: bool = False + ) -> ComputeRoute.Result: + """ + Send a `ComputeRoute` action request. + + Internal implementation to get the full result, not just the sparse route and dense path. + """ + self.debug("Waiting for 'ComputeRoute' action server") + while not self.compute_route_client.wait_for_server(timeout_sec=1.0): + self.info("'ComputeRoute' action server not available, waiting...") + + goal_msg = ComputeRoute.Goal() + goal_msg.use_start = use_start + + # Support both ID based requests and PoseStamped based requests + if isinstance(start, int) and isinstance(goal, int): + goal_msg.start_id = start + goal_msg.goal_id = goal + goal_msg.use_poses = False + elif isinstance(start, PoseStamped) and isinstance(goal, PoseStamped): + goal_msg.start = start + goal_msg.goal = goal + goal_msg.use_poses = True + else: + self.error('Invalid start and goal types. Must be PoseStamped for pose or int for ID') + result = ComputeRoute.Result() + result.error_code = ComputeRoute.UNKNOWN + result.error_msg = 'Request type fields were invalid!' + return result + + self.info('Getting route...') + send_goal_future = self.compute_route_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle or not self.goal_handle.accepted: + self.error('Get route was rejected!') + result = ComputeRoute.Result() + result.error_code = ComputeRoute.UNKNOWN + result.error_msg = 'Get route was rejected!' + return result + + self.result_future = self.goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, self.result_future) + self.status = self.result_future.result().status # type: ignore[union-attr] + + return self.result_future.result().result # type: ignore[union-attr] + + def getRoute( + self, start: Union[int, PoseStamped], + goal: Union[int, PoseStamped], + use_start: bool = False) -> Optional[list[Union[Path, Route]]]: + """Send a `ComputeRoute` action request.""" + self.clearTaskError() + rtn = self._getRouteImpl(start, goal, use_start=False) + + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.setTaskError(rtn.error_code, rtn.error_msg) + self.warn( + 'Getting route failed with' + f' status code:{self.status}' + f' error code:{rtn.error_code}' + f' error msg:{rtn.error_msg}') + return None + + return [rtn.path, rtn.route] + + def getAndTrackRoute( + self, start: Union[int, PoseStamped], + goal: Union[int, PoseStamped], use_start: bool = False + ) -> Optional[RunningTask]: + """Send a `ComputeAndTrackRoute` action request.""" + self.clearTaskError() + self.debug("Waiting for 'ComputeAndTrackRoute' action server") + while not self.compute_and_track_route_client.wait_for_server(timeout_sec=1.0): + self.info("'ComputeAndTrackRoute' action server not available, waiting...") + + goal_msg = ComputeAndTrackRoute.Goal() + goal_msg.use_start = use_start + + # Support both ID based requests and PoseStamped based requests + if isinstance(start, int) and isinstance(goal, int): + goal_msg.start_id = start + goal_msg.goal_id = goal + goal_msg.use_poses = False + elif isinstance(start, PoseStamped) and isinstance(goal, PoseStamped): + goal_msg.start = start + goal_msg.goal = goal + goal_msg.use_poses = True + else: + self.setTaskError(ComputeAndTrackRoute.UNKNOWN, 'Request type fields were invalid!') + self.error('Invalid start and goal types. Must be PoseStamped for pose or int for ID') + return None + + self.info('Computing and tracking route...') + send_goal_future = self.compute_and_track_route_client.send_goal_async(goal_msg, + self._routeFeedbackCallback) # noqa: E128 + rclpy.spin_until_future_complete(self, send_goal_future) + self.route_goal_handle = send_goal_future.result() + + if not self.route_goal_handle or not self.route_goal_handle.accepted: + msg = 'Compute and track route was rejected!' + self.setTaskError(ComputeAndTrackRoute.UNKNOWN, msg) + self.error(msg) + return None + + self.route_result_future = self.route_goal_handle.get_result_async() + return RunningTask.COMPUTE_AND_TRACK_ROUTE + def _smoothPathImpl( self, path: Path, smoother_id: str = '', max_duration: float = 2.0, check_for_collision: bool = False @@ -932,6 +1109,11 @@ def _feedbackCallback(self, msg: NavigateToPose.Feedback) -> None: self.feedback = msg.feedback return + def _routeFeedbackCallback(self, msg: ComputeAndTrackRoute.Feedback) -> None: + self.debug('Received route action feedback message') + self.route_feedback.append(msg.feedback) + return + def _setInitialPose(self) -> None: msg = PoseWithCovarianceStamped() msg.pose.pose = self.initial_pose.pose diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py index 74a15c63541..ca1a6e188bc 100644 --- a/nav2_simple_commander/setup.py +++ b/nav2_simple_commander/setup.py @@ -27,6 +27,7 @@ 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', 'example_follow_path = nav2_simple_commander.example_follow_path:main', + 'example_route = nav2_simple_commander.example_route:main', 'demo_picking = nav2_simple_commander.demo_picking:main', 'demo_inspection = nav2_simple_commander.demo_inspection:main', 'demo_security = nav2_simple_commander.demo_security:main', diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp index 170d31d7cb7..2f793011094 100644 --- a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -98,7 +98,7 @@ class SavitzkyGolaySmoother : public nav2_core::Smoother nav_msgs::msg::Path & path, bool & reversing_segment); - bool do_refinement_; + bool do_refinement_, enforce_path_inversion_; int refinement_num_; rclcpp::Logger logger_{rclcpp::get_logger("SGSmoother")}; }; diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 1169c9d5ff9..5861f9dae0a 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -121,7 +121,7 @@ class SimpleSmoother : public nav2_core::Smoother double tolerance_, data_w_, smooth_w_; int max_its_, refinement_ctr_, refinement_num_; - bool do_refinement_; + bool do_refinement_, enforce_path_inversion_; std::shared_ptr costmap_sub_; rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")}; }; diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 1e85b02a42e..7523ffc0aa1 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -32,5 +32,6 @@ ament_cmake + diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp index 20b296a9f42..5ea1f4284cd 100644 --- a/nav2_smoother/src/savitzky_golay_smoother.cpp +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -38,8 +38,11 @@ void SavitzkyGolaySmoother::configure( node, name + ".do_refinement", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, name + ".refinement_num", rclcpp::ParameterValue(2)); + declare_parameter_if_not_declared( + node, name + ".enforce_path_inversion", rclcpp::ParameterValue(true)); node->get_parameter(name + ".do_refinement", do_refinement_); node->get_parameter(name + ".refinement_num", refinement_num_); + node->get_parameter(name + ".enforce_path_inversion", enforce_path_inversion_); } bool SavitzkyGolaySmoother::smooth( @@ -53,7 +56,11 @@ bool SavitzkyGolaySmoother::smooth( nav_msgs::msg::Path curr_path_segment; curr_path_segment.header = path.header; - std::vector path_segments = findDirectionalPathSegments(path); + std::vector path_segments{ + PathSegment{0u, static_cast(path.poses.size() - 1)}}; + if (enforce_path_inversion_) { + path_segments = findDirectionalPathSegments(path); + } for (unsigned int i = 0; i != path_segments.size(); i++) { if (path_segments[i].end - path_segments[i].start > 9) { diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index d1957e38e35..76be2eccfed 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -47,6 +47,8 @@ void SimpleSmoother::configure( node, name + ".do_refinement", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, name + ".refinement_num", rclcpp::ParameterValue(2)); + declare_parameter_if_not_declared( + node, name + ".enforce_path_inversion", rclcpp::ParameterValue(true)); node->get_parameter(name + ".tolerance", tolerance_); node->get_parameter(name + ".max_its", max_its_); @@ -54,6 +56,7 @@ void SimpleSmoother::configure( node->get_parameter(name + ".w_smooth", smooth_w_); node->get_parameter(name + ".do_refinement", do_refinement_); node->get_parameter(name + ".refinement_num", refinement_num_); + node->get_parameter(name + ".enforce_path_inversion", enforce_path_inversion_); } bool SimpleSmoother::smooth( @@ -69,7 +72,11 @@ bool SimpleSmoother::smooth( nav_msgs::msg::Path curr_path_segment; curr_path_segment.header = path.header; - std::vector path_segments = findDirectionalPathSegments(path); + std::vector path_segments{PathSegment{ + 0u, static_cast(path.poses.size() - 1)}}; + if (enforce_path_inversion_) { + path_segments = findDirectionalPathSegments(path); + } std::lock_guard lock(*(costmap->getMutex())); diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index e3152c0bde0..14accf51e39 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -113,6 +113,7 @@ if(BUILD_TESTING) add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) + add_subdirectory(src/route) add_subdirectory(src/gps_navigation) add_subdirectory(src/behaviors/wait) add_subdirectory(src/behaviors/spin) diff --git a/nav2_system_tests/src/behavior_tree/server_handler.cpp b/nav2_system_tests/src/behavior_tree/server_handler.cpp index b457fa6736f..9eed165cab3 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.cpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.cpp @@ -43,6 +43,10 @@ ServerHandler::ServerHandler() node_, "wait"); backup_server = std::make_unique>( node_, "backup"); + compute_route_server = std::make_unique>( + node_, "compute_route"); + smoother_server = std::make_unique>( + node_, "smooth_path"); drive_on_heading_server = std::make_unique>( node_, "drive_on_heading"); ntp_server = std::make_unique>( diff --git a/nav2_system_tests/src/behavior_tree/server_handler.hpp b/nav2_system_tests/src/behavior_tree/server_handler.hpp index 8ac79ad1afb..2b852cd085f 100644 --- a/nav2_system_tests/src/behavior_tree/server_handler.hpp +++ b/nav2_system_tests/src/behavior_tree/server_handler.hpp @@ -31,6 +31,8 @@ #include "nav2_msgs/action/wait.hpp" #include "nav2_msgs/action/drive_on_heading.hpp" #include "nav2_msgs/action/compute_path_through_poses.hpp" +#include "nav2_msgs/action/compute_route.hpp" +#include "nav2_msgs/action/smooth_path.hpp" #include "geometry_msgs/msg/point_stamped.hpp" @@ -119,6 +121,8 @@ class ServerHandler std::unique_ptr> spin_server; std::unique_ptr> wait_server; std::unique_ptr> backup_server; + std::unique_ptr> compute_route_server; + std::unique_ptr> smoother_server; std::unique_ptr> drive_on_heading_server; std::unique_ptr> ntp_server; diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py index c33f1711bd2..3a934d0b6eb 100755 --- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -50,7 +50,7 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites='', convert_types=True + source_file=params_file, root_key='', param_rewrites={}, convert_types=True ) return LaunchDescription( diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 49cee4ea48e..2efe32f7662 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -53,7 +53,7 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites='', convert_types=True + source_file=params_file, root_key='', param_rewrites={}, convert_types=True ) return LaunchDescription( diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index f46819ef7aa..4b2d6b2919b 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -67,8 +67,8 @@ def main(argv: list[str] = sys.argv[1:]): # type: ignore[no-untyped-def] for controller, error_code in follow_path.items(): success = navigator.followPath(path, controller) - if success: - while not navigator.isTaskComplete(): + if success is not None: + while not navigator.isTaskComplete(task=success): time.sleep(0.5) assert ( diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index 140be5ab708..d071eba1a6e 100755 --- a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -46,7 +46,7 @@ def generate_launch_description() -> LaunchDescription: configured_params = RewrittenYaml( source_file=params_file, root_key='', - param_rewrites='', + param_rewrites={}, convert_types=True, ) diff --git a/nav2_system_tests/src/route/CMakeLists.txt b/nav2_system_tests/src/route/CMakeLists.txt new file mode 100644 index 00000000000..a69bbac70a4 --- /dev/null +++ b/nav2_system_tests/src/route/CMakeLists.txt @@ -0,0 +1,13 @@ +ament_add_test(test_route + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_route_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=tester_node.py + ASTAR=True + CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + PLANNER=nav2_navfn_planner::NavfnPlanner +) diff --git a/nav2_system_tests/src/route/README.md b/nav2_system_tests/src/route/README.md new file mode 100644 index 00000000000..88cab0ab93a --- /dev/null +++ b/nav2_system_tests/src/route/README.md @@ -0,0 +1 @@ +This is a series of tests using the python3 simple commander API to test the route server in a practical routing and tracking task. diff --git a/nav2_system_tests/src/route/test_route_launch.py b/nav2_system_tests/src/route/test_route_launch.py new file mode 100755 index 00000000000..81848d5c6ce --- /dev/null +++ b/nav2_system_tests/src/route/test_route_launch.py @@ -0,0 +1,167 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2025 Open Navigation LLC +# +# 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. + +import os +from pathlib import Path +import sys + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription, LaunchService +from launch.actions import (AppendEnvironmentVariable, ExecuteProcess, IncludeLaunchDescription, + SetEnvironmentVariable) +from launch.launch_context import LaunchContext +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description() -> LaunchDescription: + sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + + world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro') + robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf.xacro') + + urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf') + with open(urdf, 'r') as infp: + robot_description = infp.read() + + map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml') + + bt_navigator_xml = os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML', ''), + ) + + params_file = os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml') + graph_filepath = os.path.join(nav2_bringup_dir, 'graphs', 'turtlebot3_graph.geojson') + + # Replace the default parameter values for testing special features + # without having multiple params_files inside the nav2 stack + context = LaunchContext() + param_substitutions = {} + + if os.getenv('ASTAR') == 'True': + param_substitutions.update({'use_astar': 'True'}) + + param_substitutions.update( + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER', '')} + ) + param_substitutions.update( + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER', '')} + ) + param_substitutions.update( + {'route_server.ros__parameters.max_planning_time': '0.0001'} + ) + + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True, + ) + + new_yaml = configured_params.perform(context) + + return LaunchDescription( + [ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models') + ), + AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + str(Path(os.path.join(sim_dir)).parent.resolve()) + ), + ExecuteProcess( + cmd=['gz', 'sim', '-r', '-s', world_sdf_xacro], + output='screen', + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'True', + 'robot_sdf': robot_sdf, + 'x_pose': '-2.0', + 'y_pose': '-0.5', + 'z_pose': '0.01', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), + ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + {'use_sim_time': True, 'robot_description': robot_description} + ], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + ), + launch_arguments={ + 'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'graph': graph_filepath, + 'use_sim_time': 'True', + 'params_file': new_yaml, + 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', + 'autostart': 'True', + }.items(), + ), + ] + ) + + +def main(argv: list[str] = sys.argv[1:]): # type: ignore[no-untyped-def] + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[ + os.path.join(os.getenv('TEST_DIR', ''), os.getenv('TESTER', '')), + '-r', + '-2.0', + '-0.5', + '2.0', + '0.0', + '-e', + 'True', + ], + name='tester_node', + output='screen', + ) + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return_code = lts.run(ls) + return return_code + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/route/tester_node.py b/nav2_system_tests/src/route/tester_node.py new file mode 100755 index 00000000000..f5b6b2924ad --- /dev/null +++ b/nav2_system_tests/src/route/tester_node.py @@ -0,0 +1,492 @@ +#! /usr/bin/env python3 +# Copyright 2025 Open Navigation LLC +# +# 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. + +import argparse +import math +import sys +import time + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import ComputeAndTrackRoute, ComputeRoute +from nav2_msgs.srv import ManageLifecycleNodes +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy +from rclpy.action import ActionClient # type: ignore[attr-defined] +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy +from std_srvs.srv import Trigger + + +class RouteTester(Node): + + def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''): + super().__init__(node_name='nav2_tester', namespace=namespace) + self.initial_pose_pub = self.create_publisher( + PoseWithCovarianceStamped, 'initialpose', 10 + ) + + pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + self.model_pose_sub = self.create_subscription( + PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos + ) + self.initial_pose_received = False + self.initial_pose = initial_pose + self.goal_pose = goal_pose + self.compute_action_client = ActionClient(self, ComputeRoute, 'compute_route') + self.compute_track_action_client = ActionClient( + self, ComputeAndTrackRoute, 'compute_and_track_route') + self.feedback_msgs: list[ComputeAndTrackRoute.Feedback] = [] + + self.navigator = BasicNavigator() + + def runComputeRouteTest(self, use_poses: bool = True) -> bool: + # Test 1: See if we can compute a route that is valid and correctly sized + self.info_msg("Waiting for 'ComputeRoute' action server") + while not self.compute_action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'ComputeRoute' action server not available, waiting...") + + route_msg = ComputeRoute.Goal() + if use_poses: + route_msg.start = self.getStampedPoseMsg(self.initial_pose) + route_msg.goal = self.getStampedPoseMsg(self.goal_pose) + route_msg.use_start = True + route_msg.use_poses = True + else: + # Same request, just now the node IDs to test + route_msg.start_id = 7 + route_msg.goal_id = 13 + route_msg.use_start = False + route_msg.use_poses = False + + self.info_msg('Sending ComputeRoute goal request...') + send_goal_future = self.compute_action_client.send_goal_async(route_msg) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle or not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + self.info_msg("Waiting for 'ComputeRoute' action to complete") + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status # type: ignore[union-attr] + result = get_result_future.result().result # type: ignore[union-attr] + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + + self.info_msg('Action completed! Checking validity of results...') + + # Check result for validity + assert (len(result.path.poses) == 80) + assert (result.route.route_cost > 6) + assert (result.route.route_cost < 7) + assert (len(result.route.nodes) == 5) + assert (len(result.route.edges) == 4) + assert (result.error_code == 0) + assert (result.error_msg == '') + + self.info_msg('Goal succeeded!') + return True + + def runComputeRouteSamePoseTest(self) -> bool: + # Test 2: try with the same start and goal point edge case + self.info_msg("Waiting for 'ComputeRoute' action server") + while not self.compute_action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'ComputeRoute' action server not available, waiting...") + + route_msg = ComputeRoute.Goal() + route_msg.start_id = 7 + route_msg.goal_id = 7 + route_msg.use_start = False + route_msg.use_poses = False + + self.info_msg('Sending ComputeRoute goal request...') + send_goal_future = self.compute_action_client.send_goal_async(route_msg) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle or not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + self.info_msg("Waiting for 'ComputeRoute' action to complete") + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status # type: ignore[union-attr] + result = get_result_future.result().result # type: ignore[union-attr] + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f'Goal failed with status code: {status}') + return False + + self.info_msg('Action completed! Checking validity of results...') + + # Check result for validity, should be a 1-node path as its the same + assert (len(result.path.poses) == 1) + assert (len(result.route.nodes) == 1) + assert (len(result.route.edges) == 0) + assert (result.error_code == 0) + assert (result.error_msg == '') + + self.info_msg('Goal succeeded!') + return True + + def runTrackRouteTest(self) -> bool: + # Test 3: See if we can compute and track a route with proper state + self.info_msg("Waiting for 'ComputeAndTrackRoute' action server") + while not self.compute_track_action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'ComputeAndTrackRoute' action server not available, waiting...") + + route_msg = ComputeAndTrackRoute.Goal() + route_msg.goal = self.getStampedPoseMsg(self.goal_pose) + route_msg.use_start = False # Use TF pose instead + route_msg.use_poses = True + + self.info_msg('Sending ComputeAndTrackRoute goal request...') + send_goal_future = self.compute_track_action_client.send_goal_async( + route_msg, feedback_callback=self.feedback_callback) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle or not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + # Trigger a reroute + time.sleep(1) + self.info_msg('Triggering a reroute') + srv_client = self.create_client(Trigger, 'route_server/ReroutingService/reroute') + while not srv_client.wait_for_service(timeout_sec=1.0): + self.info_msg('Reroute service not available, waiting...') + req = Trigger.Request() + future = srv_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + self.info_msg('Reroute triggered') + else: + self.error_msg('Reroute failed') + return False + # Wait a bit for it to compute the route and start tracking (but no movement) + + # Cancel it after a bit + time.sleep(2) + cancel_future = goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + status = cancel_future.result() + if status is not None and len(status.goals_canceling) > 0: + self.info_msg('Action cancel completed!') + else: + self.info_msg('Goal cancel failed') + return False + + # Send it again + self.info_msg('Sending ComputeAndTrackRoute goal request...') + send_goal_future = self.compute_track_action_client.send_goal_async( + route_msg, feedback_callback=self.feedback_callback) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle or not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + # Wait a bit for it to compute the route and start tracking (but no movement) + time.sleep(2) + + # Preempt with a new request type on the graph + route_msg.use_poses = False + route_msg.start_id = 7 + route_msg.goal_id = 13 + send_goal_future = self.compute_track_action_client.send_goal_async( + route_msg, feedback_callback=self.feedback_callback) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle or not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + self.feedback_msgs = [] + + self.info_msg("Waiting for 'ComputeAndTrackRoute' action to complete") + progressing = True + last_feedback_msg = None + follow_path_task = None + while progressing: + rclpy.spin_until_future_complete(self, get_result_future, timeout_sec=0.10) + if get_result_future.result() is not None: + status = get_result_future.result().status # type: ignore[union-attr] + if status == GoalStatus.STATUS_SUCCEEDED: + progressing = False + elif status == GoalStatus.STATUS_CANCELED or status == GoalStatus.STATUS_ABORTED: + self.info_msg(f'Goal failed with status code: {status}') + return False + + # Else, processing. Check feedback + while len(self.feedback_msgs) > 0: + feedback_msg = self.feedback_msgs.pop(0) + + # Start following the path + if (last_feedback_msg and feedback_msg.path != last_feedback_msg.path): + follow_path_task = self.navigator.followPath(feedback_msg.path) + + # Check if the feedback is valid, if changed (not for route operations) + if last_feedback_msg and \ + last_feedback_msg.current_edge_id != feedback_msg.current_edge_id and \ + int(feedback_msg.current_edge_id) != 0: + if last_feedback_msg.next_node_id != feedback_msg.last_node_id: + self.error_msg('Feedback state is not tracking in order!') + return False + + last_feedback_msg = feedback_msg + + # Validate the state of the final feedback message + if last_feedback_msg is None: + self.error_msg('No feedback message received!') + return False + + if int(last_feedback_msg.next_node_id) != 0: + self.error_msg('Terminal feedback state of nodes is not correct!') + return False + if int(last_feedback_msg.current_edge_id) != 0: + self.error_msg('Terminal feedback state of edges is not correct!') + return False + if int(last_feedback_msg.route.nodes[-1].nodeid) != 13: + self.error_msg('Final route node is not correct!') + return False + + while not self.navigator.isTaskComplete(task=follow_path_task): + time.sleep(0.1) + + self.info_msg('Action completed! Checking validity of terminal condition...') + + # Check result for validity + if not self.distanceFromGoal() < 1.0: + self.error_msg('Did not make it to the goal pose!') + return False + + self.info_msg('Goal succeeded!') + return True + + def feedback_callback(self, feedback_msg: ComputeAndTrackRoute.Feedback) -> None: + self.feedback_msgs.append(feedback_msg.feedback) + + def distanceFromGoal(self) -> float: + d_x = self.current_pose.position.x - self.goal_pose.position.x + d_y = self.current_pose.position.y - self.goal_pose.position.y + distance = math.sqrt(d_x * d_x + d_y * d_y) + self.info_msg(f'Distance from goal is: {distance}') + return distance + + def info_msg(self, msg: str) -> None: + self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + + def error_msg(self, msg: str) -> None: + self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + + def setInitialPose(self) -> None: + msg = PoseWithCovarianceStamped() + msg.pose.pose = self.initial_pose + msg.header.frame_id = 'map' + self.info_msg('Publishing Initial Pose') + self.initial_pose_pub.publish(msg) + self.currentPose = self.initial_pose + + def getStampedPoseMsg(self, pose: Pose) -> PoseStamped: + msg = PoseStamped() + msg.header.frame_id = 'map' + msg.pose = pose + return msg + + def poseCallback(self, msg: PoseWithCovarianceStamped) -> None: + self.info_msg('Received amcl_pose') + self.current_pose = msg.pose.pose + self.initial_pose_received = True + + def wait_for_node_active(self, node_name: str) -> None: + # Waits for the node within the tester namespace to become active + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{node_name}/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{node_service} service not available, waiting...') + req = GetState.Request() # empty request + state = 'UNKNOWN' + while state != 'active': + self.info_msg(f'Getting {node_name} state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label # type: ignore[union-attr] + self.info_msg(f'Result of get_state: {state}') + else: + self.error_msg( + f'Exception while calling service: {future.exception()!r}' + ) + time.sleep(5) + + def shutdown(self) -> None: + self.info_msg('Shutting down') + self.compute_action_client.destroy() + self.compute_track_action_client.destroy() + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down navigation lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down navigation lifecycle manager complete.') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + transition_service = 'lifecycle_manager_localization/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(f'{transition_service} service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down localization lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down localization lifecycle manager complete') + except Exception as e: # noqa: B902 + self.error_msg(f'Service call failed {e!r}') + + def wait_for_initial_pose(self) -> None: + self.initial_pose_received = False + while not self.initial_pose_received: + self.info_msg('Setting initial pose') + self.setInitialPose() + self.info_msg('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1) + + +def run_all_tests(robot_tester: RouteTester) -> bool: + # set transforms to use_sim_time + robot_tester.wait_for_node_active('amcl') + robot_tester.wait_for_initial_pose() + robot_tester.wait_for_node_active('bt_navigator') + result_poses = robot_tester.runComputeRouteTest(use_poses=True) + result_node_ids = robot_tester.runComputeRouteTest(use_poses=False) + result_same = robot_tester.runComputeRouteSamePoseTest() + result = result_poses and result_node_ids and result_same and robot_tester.runTrackRouteTest() + + if result: + robot_tester.info_msg('Test PASSED') + else: + robot_tester.error_msg('Test FAILED') + return result + + +def fwd_pose(x: float = 0.0, y: float = 0.0, z: float = 0.01) -> Pose: + initial_pose = Pose() + initial_pose.position.x = x + initial_pose.position.y = y + initial_pose.position.z = z + initial_pose.orientation.x = 0.0 + initial_pose.orientation.y = 0.0 + initial_pose.orientation.z = 0.0 + initial_pose.orientation.w = 1.0 + return initial_pose + + +def main(argv: list[str] = sys.argv[1:]): # type: ignore[no-untyped-def] + # The robot(s) positions from the input arguments + parser = argparse.ArgumentParser(description='Route server tester node') + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument( + '-r', + '--robot', + action='append', + nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', + ) + args, unknown = parser.parse_known_args() + + rclpy.init() + + # Create test object + init_x, init_y, final_x, final_y = args.robot[0] + tester = RouteTester( + initial_pose=fwd_pose(float(init_x), float(init_y)), + goal_pose=fwd_pose(float(final_x), float(final_y)), + ) + tester.info_msg( + 'Starting tester, robot going from ' + + init_x + + ', ' + + init_y + + ' to ' + + final_x + + ', ' + + final_y + + ' via route server.' + ) + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + # run tests + passed = run_all_tests(tester) + + tester.shutdown() + tester.info_msg('Done Shutting Down.') + + if not passed: + tester.info_msg('Exiting failed') + exit(1) + else: + tester.info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 6a82f015113..275873fe906 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(pluginlib REQUIRED) nav2_package() @@ -56,6 +57,7 @@ ament_export_dependencies( tf2 tf2_geometry_msgs tf2_ros + pluginlib ) ament_export_targets(${library_name}) diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 5cae40170d8..74f0df2dc60 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -20,6 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" #include "rcl_interfaces/srv/list_parameters.hpp" +#include "pluginlib/exceptions.hpp" namespace nav2_util { @@ -143,11 +144,11 @@ std::string get_plugin_type_param( if (!node->get_parameter(plugin_name + ".plugin", plugin_type)) { RCLCPP_FATAL( node->get_logger(), "Can not get 'plugin' param value for %s", plugin_name.c_str()); - throw std::runtime_error("No 'plugin' param for param ns!"); + throw pluginlib::PluginlibException("No 'plugin' param for param ns!"); } } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { RCLCPP_FATAL(node->get_logger(), "'plugin' param not defined for %s", plugin_name.c_str()); - throw std::runtime_error("No 'plugin' param for param ns!"); + throw pluginlib::PluginlibException("No 'plugin' param for param ns!"); } return plugin_type; diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 9f32e1ffff8..883fb5451f8 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -588,17 +588,17 @@ class SimpleActionServer struct has_error_code>: std::true_type {}; template - std::string get_error_details_if_available(const T & result) + void log_error_details_if_available(const T & result) { if constexpr (has_error_code::value && has_error_msg::value) { - return " error_code:" + std::to_string(result->error_code) + - ", error_msg:'" + result->error_msg + "'."; + warn_msg("Aborting handle. error_code:" + std::to_string(result->error_code) + + ", error_msg:'" + result->error_msg + "'."); } else if constexpr (has_error_code::value) { - return " error_code:" + std::to_string(result->error_code) + "."; + warn_msg("Aborting handle. error_code:" + std::to_string(result->error_code) + "."); } else { - return "."; + warn_msg("Aborting handle."); } } @@ -619,7 +619,7 @@ class SimpleActionServer info_msg("Client requested to cancel the goal. Cancelling."); handle->canceled(result); } else { - warn_msg("Aborting handle" + get_error_details_if_available(result)); + log_error_details_if_available(result); handle->abort(result); } handle.reset(); diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 84e011cd8c6..bdc150c4120 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -29,6 +29,7 @@ tf2_geometry_msgs tf2_msgs tf2_ros + pluginlib ament_lint_common ament_lint_auto diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 96f69b54582..41261f512a5 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -27,6 +27,7 @@ target_link_libraries(${library_name} PUBLIC tf2_ros::tf2_ros tf2::tf2 ${tf2_geometry_msgs_TARGETS} + pluginlib::pluginlib ) target_link_libraries(${library_name} PRIVATE ${bond_TARGETS} diff --git a/navigation2/package.xml b/navigation2/package.xml index 45ad44912b3..325943f50d3 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -33,6 +33,7 @@ nav2_behaviors nav2_smoother nav2_regulated_pure_pursuit_controller + nav2_route nav2_rotation_shim_controller nav2_rviz_plugins nav2_simple_commander diff --git a/tools/pyproject.toml b/tools/pyproject.toml index b4a54ff7b49..257f616a767 100644 --- a/tools/pyproject.toml +++ b/tools/pyproject.toml @@ -43,8 +43,11 @@ module = [ "rcl_interfaces.*", "std_msgs.*", "zmq.*", + "std_srvs.*", "graphviz.*", "transforms3d.*", + "geopandas.*", + "pandas.*", ] ignore_missing_imports = true diff --git a/tools/update_readme_table.py b/tools/update_readme_table.py index 94e0daede86..d36b65eac72 100755 --- a/tools/update_readme_table.py +++ b/tools/update_readme_table.py @@ -18,8 +18,8 @@ import requests # Global information about current distributions, shouldn't need to update -OSs = {'humble': 'jammy', 'iron': 'jammy', 'jazzy': 'noble'} -Prefixs = {'humble': 'H', 'iron': 'I', 'jazzy': 'J'} +OSs = {'humble': 'jammy', 'jazzy': 'noble'} +Prefixs = {'humble': 'H', 'jazzy': 'J'} # Set your packages here Packages = [ @@ -39,6 +39,7 @@ 'nav2_dwb_controller', # Controller plugin for DWB packages 'nav2_graceful_controller', 'nav2_lifecycle_manager', + 'nav2_loopback_sim', 'nav2_map_server', 'nav2_mppi_controller', 'nav2_msgs', @@ -59,7 +60,7 @@ ] # Set which distributions you care about -Distros = ['humble', 'iron', 'jazzy'] +Distros = ['humble', 'jazzy'] def getSrcPath(package: str, prefix: str, OS: str) -> str: @@ -103,6 +104,7 @@ def main() -> None: else: entry += f'[![Build Status]({srcURL}badge/icon)]({srcURL}) | ' entry += f'[![Build Status]({binURL}badge/icon)]({binURL}) | ' + entry = entry[:-1] entry += '\n' body += entry From f5543c39aba5d3302208bc6e9696e7cbd41de8c9 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Wed, 30 Apr 2025 01:55:57 +0200 Subject: [PATCH 56/70] Feat/smac planner include orientation flexibility (#4127) * include functionality to allow multiple goal heading for smac planner Signed-off-by: stevedanomodolor * include missing parameter inclusion Signed-off-by: stevedanomodolor * increase test coverage Signed-off-by: stevedanomodolor --------- Signed-off-by: stevedanomodolor --- .../include/nav2_smac_planner/a_star.hpp | 47 +- .../nav2_smac_planner/analytic_expansion.hpp | 33 +- .../include/nav2_smac_planner/constants.hpp | 35 + .../nav2_smac_planner/goal_manager.hpp | 190 + .../include/nav2_smac_planner/node_2d.hpp | 22 +- .../include/nav2_smac_planner/node_hybrid.hpp | 6 +- .../nav2_smac_planner/node_lattice.hpp | 6 +- .../nav2_smac_planner/smac_planner_hybrid.hpp | 2 + .../smac_planner_lattice.hpp | 2 + .../include/nav2_smac_planner/types.hpp | 12 + .../sample_primitives/test/output.json | 3931 +++++++++++++++++ nav2_smac_planner/src/a_star.cpp | 142 +- nav2_smac_planner/src/analytic_expansion.cpp | 213 +- nav2_smac_planner/src/node_2d.cpp | 6 +- nav2_smac_planner/src/node_hybrid.cpp | 14 +- nav2_smac_planner/src/node_lattice.cpp | 15 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 76 +- .../src/smac_planner_lattice.cpp | 73 +- nav2_smac_planner/test/CMakeLists.txt | 10 + nav2_smac_planner/test/test_a_star.cpp | 145 +- nav2_smac_planner/test/test_goal_manager.cpp | 177 + nav2_smac_planner/test/test_node2d.cpp | 4 +- nav2_smac_planner/test/test_smac_hybrid.cpp | 108 +- nav2_smac_planner/test/test_smac_lattice.cpp | 84 +- 24 files changed, 5193 insertions(+), 160 deletions(-) create mode 100644 nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp create mode 100644 nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json create mode 100644 nav2_smac_planner/test/test_goal_manager.cpp diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 8b127960cc3..b0e3bab0188 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -32,6 +32,7 @@ #include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_lattice.hpp" #include "nav2_smac_planner/node_basic.hpp" +#include "nav2_smac_planner/goal_manager.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" @@ -54,6 +55,9 @@ class AStarAlgorithm typedef typename NodeT::CoordinateVector CoordinateVector; typedef typename NodeVector::iterator NeighborIterator; typedef std::function NodeGetter; + typedef GoalManager GoalManagerT; + typedef std::vector> GoalStateVector; + /** * @struct nav2_smac_planner::NodeComparator @@ -90,6 +94,8 @@ class AStarAlgorithm * or planning time exceeded * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns * false after this timeout + * @param lookup_table_size Size of the lookup table to store heuristic values + * @param dim_3_size Number of quantization bins */ void initialize( const bool & allow_unknown, @@ -125,11 +131,15 @@ class AStarAlgorithm * @param mx The node X index of the goal * @param my The node Y index of the goal * @param dim_3 The node dim_3 index of the goal + * @param goal_heading_mode The goal heading mode to use + * @param coarse_search_resolution The resolution to search for goal heading */ void setGoal( const float & mx, const float & my, - const unsigned int & dim_3); + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT, + const int & coarse_search_resolution = 1); /** * @brief Set the starting pose for planning, as a node index @@ -154,12 +164,6 @@ class AStarAlgorithm */ NodePtr & getStart(); - /** - * @brief Get pointer reference to goal node - * @return Node pointer reference to goal node - */ - NodePtr & getGoal(); - /** * @brief Get maximum number of on-approach iterations after within threshold * @return Reference to Maximum on-approach iterations parameter @@ -190,6 +194,18 @@ class AStarAlgorithm */ unsigned int & getSizeDim3(); + /** + * @brief Get the resolution of the coarse search + * @return Size of the goals to expand + */ + unsigned int getCoarseSearchResolution(); + + /** + * @brief Get the goals manager class + * @return Goal manager class + */ + GoalManagerT getGoalManager(); + protected: /** * @brief Get pointer to next goal in open set @@ -210,13 +226,6 @@ class AStarAlgorithm */ inline NodePtr addToGraph(const uint64_t & index); - /** - * @brief Check if this node is the goal node - * @param node Node pointer to check if its the goal node - * @return if node is goal - */ - inline bool isGoal(NodePtr & node); - /** * @brief Get cost of heuristic of node * @param node Node pointer to get heuristic for @@ -240,6 +249,11 @@ class AStarAlgorithm */ inline void clearGraph(); + /** + * @brief Check if node has been visited + * @param current_node Node to check if visited + * @return if node has been visited + */ inline bool onVisitationCheckNode(const NodePtr & node); /** @@ -260,12 +274,11 @@ class AStarAlgorithm unsigned int _x_size; unsigned int _y_size; unsigned int _dim3_size; + unsigned int _coarse_search_resolution; SearchInfo _search_info; - Coordinates _goal_coordinates; NodePtr _start; - NodePtr _goal; - + GoalManagerT _goal_manager; Graph _graph; NodeQueue _queue; diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 6be84d1c109..0eecb78185e 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -35,8 +35,10 @@ class AnalyticExpansion { public: typedef NodeT * NodePtr; + typedef std::vector NodeVector; typedef typename NodeT::Coordinates Coordinates; typedef std::function NodeGetter; + typedef typename NodeT::CoordinateVector CoordinateVector; /** * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes @@ -79,17 +81,22 @@ class AnalyticExpansion /** * @brief Attempt an analytic path completion * @param node The node to start the analytic path from - * @param goal The goal node to plan to + * @param coarse_check_goals Coarse list of goals nodes to plan to + * @param fine_check_goals Fine list of goals nodes to plan to + * @param goals_coords vector of goal coordinates to plan to * @param getter Gets a node at a set of coordinates * @param iterations Iterations to run over - * @param best_cost Best heuristic cost to propertionally expand more closer to the goal - * @return Node pointer reference to goal node if successful, else - * return nullptr + * @param closest_distance Closest distance to goal + * @return Node pointer reference to goal node with the best score out of the goals node if + * successful, else return nullptr */ NodePtr tryAnalyticExpansion( const NodePtr & current_node, - const NodePtr & goal_node, - const NodeGetter & getter, int & iterations, int & best_cost); + const NodeVector & coarse_check_goals, + const NodeVector & fine_check_goals, + const CoordinateVector & goals_coords, + const NodeGetter & getter, int & iterations, + int & closest_distance); /** * @brief Perform an analytic path expansion to the goal @@ -103,6 +110,20 @@ class AnalyticExpansion const NodePtr & node, const NodePtr & goal, const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space); + /** + * @brief Refined analytic path from the current node to the goal + * @param current_node The node to start the analytic path from + * @param goal_node The goal node to plan to + * @param getter The function object that gets valid nodes from the graph + * @param analytic_nodes The set of analytic nodes to refine + * @return The score of the refined path + */ + float refineAnalyticPath( + const NodePtr & current_node, + const NodePtr & goal_node, + const NodeGetter & getter, + AnalyticExpansionNodes & analytic_nodes); + /** * @brief Takes final analytic expansion and appends to current expanded node * @param node The node to start the analytic path from diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index af44ce53659..6344c86fb89 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -28,6 +28,14 @@ enum class MotionModel STATE_LATTICE = 4, }; +enum class GoalHeadingMode +{ + UNKNOWN = 0, + DEFAULT = 1, + BIDIRECTIONAL = 2, + ALL_DIRECTION = 3, +}; + inline std::string toString(const MotionModel & n) { switch (n) { @@ -59,6 +67,33 @@ inline MotionModel fromString(const std::string & n) } } +inline std::string toString(const GoalHeadingMode & n) +{ + switch (n) { + case GoalHeadingMode::DEFAULT: + return "DEFAULT"; + case GoalHeadingMode::BIDIRECTIONAL: + return "BIDIRECTIONAL"; + case GoalHeadingMode::ALL_DIRECTION: + return "ALL_DIRECTION"; + default: + return "Unknown"; + } +} + +inline GoalHeadingMode fromStringToGH(const std::string & n) +{ + if (n == "DEFAULT") { + return GoalHeadingMode::DEFAULT; + } else if (n == "BIDIRECTIONAL") { + return GoalHeadingMode::BIDIRECTIONAL; + } else if (n == "ALL_DIRECTION") { + return GoalHeadingMode::ALL_DIRECTION; + } else { + return GoalHeadingMode::UNKNOWN; + } +} + const float UNKNOWN_COST = 255.0; const float OCCUPIED_COST = 254.0; const float INSCRIBED_COST = 253.0; diff --git a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp new file mode 100644 index 00000000000..826fe9d47e5 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp @@ -0,0 +1,190 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// Copyright (c) 2024, Stevedan Ogochukwu Omodolor Omodia +// +// 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. Reserved. + +#ifndef NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ +#define NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ + +#include +#include +#include + +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/node_2d.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/node_lattice.hpp" +#include "nav2_smac_planner/node_basic.hpp" +#include "nav2_smac_planner/collision_checker.hpp" + + +namespace nav2_smac_planner +{ + +/** +* @class nav2_smac_planner::GoalManager +* @brief Responsible for managing multiple variables storing information on the goal +*/ +template +class GoalManager +{ +public: + typedef NodeT * NodePtr; + typedef std::vector NodeVector; + typedef std::unordered_set NodeSet; + typedef std::vector> GoalStateVector; + typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; + + /** + * @brief Constructor: Initializes empty goal state. sets and coordinate lists. + */ + GoalManager() + : _goals_set(NodeSet()), + _goals_state(GoalStateVector()), + _goals_coordinate(CoordinateVector()) + { + } + + /** + * @brief Destructor for the GoalManager + */ + ~GoalManager() = default; + + /** + * @brief Checks if the goals set is empty + * @return true if the goals set is empty + */ + bool goalsIsEmpty() + { + return _goals_state.empty(); + } + + /** + * @brief Sets the internal goals' state list to the provided goals. + * @param goals Vector of goals state to set, + */ + + void setGoalStates( + GoalStateVector & goals_state) + { + clear(); + _goals_state = goals_state; + } + + /** + * @brief Clears all internal goal data, including goals, states, and coordinates. + */ + void clear() + { + _goals_set.clear(); + _goals_state.clear(); + _goals_coordinate.clear(); + } + + /** + * @brief Populates coarse and fine goal lists for analytic expansion. + * @param coarse_check_goals Output list of goals for coarse search expansion. + * @param fine_check_goals Output list of goals for fine search refinement. + * @param coarse_search_resolution Number of fine goals per coarse goal. + */ + void prepareGoalsForAnalyticExpansion( + NodeVector & coarse_check_goals, NodeVector & fine_check_goals, + int coarse_search_resolution) + { + for (unsigned int i = 0; i < _goals_state.size(); i++) { + if (_goals_state[i].is_valid) { + if (i % coarse_search_resolution == 0) { + coarse_check_goals.push_back(_goals_state[i].goal); + } else { + fine_check_goals.push_back(_goals_state[i].goal); + } + } + } + } + + /** + * @brief Filters and marks invalid goals based on collision checking and tolerance thresholds. + * + * Stores only valid (or tolerably infeasible) goals into internal goal sets and coordinates. + * + * @param tolerance Heuristic tolerance allowed for infeasible goals. + * @param collision_checker Collision checker to validate goal positions. + * @param traverse_unknown Flag whether traversal through unknown space is allowed. + */ + void removeInvalidGoals( + const float & tolerance, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown) + { + for (unsigned int i = 0; i < _goals_state.size(); i++) { + if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) || + tolerance > 0.001) + { + _goals_state[i].is_valid = true; + _goals_set.insert(_goals_state[i].goal); + _goals_coordinate.push_back(_goals_state[i].goal->pose); + } else { + _goals_state[i].is_valid = false; + } + } + } + + /** + * @brief Check if a given node is part of the goal set. + * @param node Node pointer to check. + * @return if node matches any goal in the goal set. + */ + inline bool isGoal(NodePtr & node) + { + return _goals_set.find(node) != _goals_set.end(); + } + + /** + * @brief Get pointer reference to goals set vector + * @return unordered_set of node pointers reference to the goals nodes + */ + inline NodeSet & getGoalsSet() + { + return _goals_set; + } + + /** + * @brief Get pointer reference to goals state + * @return vector of node pointers reference to the goals state + */ + inline GoalStateVector & getGoalsState() + { + return _goals_state; + } + + /** + * @brief Get pointer reference to goals coordinates + * @return vector of goals coordinates reference to the goals coordinates + */ + inline CoordinateVector & getGoalsCoordinates() + { + return _goals_coordinate; + } + +protected: + NodeSet _goals_set; + GoalStateVector _goals_state; + CoordinateVector _goals_coordinate; + NodePtr primary_goal; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 1bd0993a8eb..6d54e1696ab 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -50,6 +50,16 @@ class Node2D : x(x_in), y(y_in) {} + inline bool operator==(const Coordinates & rhs) const + { + return this->x == rhs.x && this->y == rhs.y; + } + + inline bool operator!=(const Coordinates & rhs) const + { + return !(*this == rhs); + } + float x, y; }; typedef std::vector CoordinateVector; @@ -75,6 +85,15 @@ class Node2D return this->_index == rhs._index; } + /** + * @brief setting continuous coordinate search poses (in partial-cells) + * @param Pose pose + */ + inline void setPose(const Coordinates & pose_in) + { + pose = pose_in; + } + /** * @brief Reset method for new search */ @@ -224,7 +243,7 @@ class Node2D */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @brief Initialize the neighborhood to be used in A* @@ -264,6 +283,7 @@ class Node2D bool backtracePath(CoordinateVector & path); Node2D * parent; + Coordinates pose; static float cost_travel_multiplier; static std::vector _neighbors_grid_offsets; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index ee3a4bf231c..cc3650563a8 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -167,12 +167,12 @@ class NodeHybrid : x(x_in), y(y_in), theta(theta_in) {} - inline bool operator==(const Coordinates & rhs) + inline bool operator==(const Coordinates & rhs) const { return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; } - inline bool operator!=(const Coordinates & rhs) + inline bool operator!=(const Coordinates & rhs) const { return !(*this == rhs); } @@ -374,7 +374,7 @@ class NodeHybrid */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @brief Initialize motion models diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 824b435447d..5b07e5453bf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -314,7 +314,7 @@ class NodeLattice */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @brief Initialize motion models @@ -408,8 +408,8 @@ class NodeLattice bool backtracePath(CoordinateVector & path); /** - * \brief add node to the path - * \param current_node + * @brief add node to the path + * @param current_node */ void addNodeToPath(NodePtr current_node, CoordinateVector & path); diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 4469579f178..96f40f47664 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -123,6 +123,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner bool _debug_visualizations; std::string _motion_model_for_search; MotionModel _motion_model; + GoalHeadingMode _goal_heading_mode; + int _coarse_search_resolution; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _planned_footprints_publisher; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index e204c8e01fb..6db78ddccaf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -120,6 +120,8 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner _planned_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; + GoalHeadingMode _goal_heading_mode; + int _coarse_search_resolution; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index cc6c9975c5f..b0ccaa8bb29 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -188,6 +188,18 @@ struct MotionPrimitive MotionPoses poses; }; + /** + * @struct nav2_smac_planner::GoalState + * @brief A struct to store the goal state + */ + +template +struct GoalState +{ + NodeT * goal = nullptr; + bool is_valid = true; +}; + typedef std::vector MotionPrimitives; typedef std::vector MotionPrimitivePtrs; diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json new file mode 100644 index 00000000000..feae5b38326 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json @@ -0,0 +1,3931 @@ +{ + "version": 1.0, + "date_generated": "2022-03-17", + "lattice_metadata": { + "motion_model": "ackermann", + "turning_radius": 0.5, + "grid_resolution": 0.05, + "stopping_threshold": 5, + "num_of_headings": 15, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138 + ], + "number_of_trajectories": 80 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.04981, + -0.00236, + 6.18831988221979 + ], + [ + 0.09917, + -0.00944, + 6.093454457259993 + ], + [ + 0.14765, + -0.02115, + 5.998589032300196 + ], + [ + 0.19479, + -0.03741, + 5.903723607340399 + ], + [ + 0.24018, + -0.05805, + 5.808858182380602 + ], + [ + 0.28341, + -0.08291, + 5.713992757420805 + ], + [ + 0.3241, + -0.11175, + 5.619127332461009 + ], + [ + 0.36187, + -0.14431, + 5.524261907501212 + ], + [ + 0.39638, + -0.1803, + 5.429396482541415 + ], + [ + 0.42733, + -0.2194, + 5.334531057581619 + ], + [ + 0.45444, + -0.26126, + 5.239665632621822 + ], + [ + 0.47769, + -0.30538, + 5.176036589385496 + ], + [ + 0.5, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.05254, + -0.00218, + 6.200401159939406 + ], + [ + 0.10472, + -0.00869, + 6.117617012699226 + ], + [ + 0.15619, + -0.0195, + 6.034832865459045 + ], + [ + 0.20658, + -0.03452, + 5.952048718218864 + ], + [ + 0.25556, + -0.05366, + 5.869264570978684 + ], + [ + 0.30295, + -0.07648, + 5.81953769817878 + ], + [ + 0.35, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.05, + 0.0, + 0.0 + ], + [ + 0.1, + 0.0, + 0.0 + ], + [ + 0.15, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.05254, + 0.00218, + 0.0827841472401804 + ], + [ + 0.10472, + 0.00869, + 0.1655682944803608 + ], + [ + 0.15619, + 0.0195, + 0.2483524417205412 + ], + [ + 0.20658, + 0.03452, + 0.3311365889607216 + ], + [ + 0.25556, + 0.05366, + 0.413920736200902 + ], + [ + 0.30295, + 0.07648, + 0.4636476090008061 + ], + [ + 0.35, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.04981, + 0.00236, + 0.0948654249597968 + ], + [ + 0.09917, + 0.00944, + 0.1897308499195936 + ], + [ + 0.14765, + 0.02115, + 0.2845962748793904 + ], + [ + 0.19479, + 0.03741, + 0.3794616998391872 + ], + [ + 0.24018, + 0.05805, + 0.4743271247989839 + ], + [ + 0.28341, + 0.08291, + 0.5691925497587808 + ], + [ + 0.3241, + 0.11175, + 0.6640579747185776 + ], + [ + 0.36187, + 0.14431, + 0.7589233996783744 + ], + [ + 0.39638, + 0.1803, + 0.8537888246381711 + ], + [ + 0.42733, + 0.2194, + 0.9486542495979677 + ], + [ + 0.45444, + 0.26126, + 1.0435196745577644 + ], + [ + 0.47769, + 0.30538, + 1.1071487177940904 + ], + [ + 0.5, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.04747, + 0.02076, + 0.360614807000627 + ], + [ + 0.09683, + 0.03652, + 0.2575820050004478 + ], + [ + 0.14755, + 0.04712, + 0.15454920300026875 + ], + [ + 0.19909, + 0.05245, + 0.051516401000089584 + ], + [ + 0.25091, + 0.05245, + 6.231668906179497 + ], + [ + 0.30245, + 0.04712, + 6.128636104179318 + ], + [ + 0.35317, + 0.03652, + 6.025603302179138 + ], + [ + 0.40253, + 0.02076, + 5.922570500178959 + ], + [ + 0.45, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.04729, + 0.0212, + 0.37934804372793224 + ], + [ + 0.09619, + 0.03835, + 0.29504847845505844 + ], + [ + 0.14636, + 0.05131, + 0.21074891318218458 + ], + [ + 0.19745, + 0.06001, + 0.12644934790931073 + ], + [ + 0.24909, + 0.06437, + 0.04214978263643687 + ], + [ + 0.30091, + 0.06437, + 6.2410355245431495 + ], + [ + 0.35255, + 0.06001, + 6.156735959270275 + ], + [ + 0.40364, + 0.05131, + 6.0724363939974015 + ], + [ + 0.45381, + 0.03835, + 5.988136828724528 + ], + [ + 0.50271, + 0.0212, + 5.903837263451654 + ], + [ + 0.55, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.04705, + 0.02352, + 0.4636476090008061 + ], + [ + 0.09444, + 0.04634, + 0.413920736200902 + ], + [ + 0.14342, + 0.06548, + 0.3311365889607216 + ], + [ + 0.19381, + 0.0805, + 0.24835244172054122 + ], + [ + 0.24528, + 0.09131, + 0.16556829448036087 + ], + [ + 0.29746, + 0.09782, + 0.08278414724018052 + ], + [ + 0.35, + 0.1, + 0.0 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + 0.025, + 0.4636476090008061 + ], + [ + 0.1, + 0.05, + 0.4636476090008061 + ], + [ + 0.15, + 0.075, + 0.4636476090008061 + ], + [ + 0.2, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.04409, + 0.0241, + 0.536596705651906 + ], + [ + 0.08631, + 0.05134, + 0.6095458023030058 + ], + [ + 0.12643, + 0.08159, + 0.6824948989541056 + ], + [ + 0.16424, + 0.11469, + 0.7554439956052055 + ], + [ + 0.2, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 2, + "end_angle_index": 0, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03617, + 0.03288, + 0.6899154709776123 + ], + [ + 0.07532, + 0.06215, + 0.5944327785577764 + ], + [ + 0.11707, + 0.08756, + 0.4989500861379405 + ], + [ + 0.16106, + 0.10888, + 0.4034673937181046 + ], + [ + 0.20688, + 0.1259, + 0.30798470129826866 + ], + [ + 0.25412, + 0.13848, + 0.21250200887843262 + ], + [ + 0.30234, + 0.1465, + 0.11701931645859664 + ], + [ + 0.3511, + 0.14988, + 0.021536624038760666 + ], + [ + 0.4, + 0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03576, + 0.03531, + 0.7554439956052055 + ], + [ + 0.07357, + 0.06841, + 0.6824948989541056 + ], + [ + 0.11369, + 0.09866, + 0.6095458023030058 + ], + [ + 0.15591, + 0.1259, + 0.5365967056519059 + ], + [ + 0.2, + 0.15, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + 0.0375, + 0.7853981633974483 + ], + [ + 0.075, + 0.075, + 0.7853981633974483 + ], + [ + 0.1125, + 0.1125, + 0.7853981633974483 + ], + [ + 0.15, + 0.15, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03531, + 0.03576, + 0.8153523311896911 + ], + [ + 0.06841, + 0.07357, + 0.8883014278407909 + ], + [ + 0.09866, + 0.11369, + 0.9612505244918907 + ], + [ + 0.1259, + 0.15591, + 1.0341996211429907 + ], + [ + 0.15, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 2, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03288, + 0.03617, + 0.8808808558172843 + ], + [ + 0.06215, + 0.07532, + 0.9763635482371201 + ], + [ + 0.08756, + 0.11707, + 1.071846240656956 + ], + [ + 0.10888, + 0.16106, + 1.167328933076792 + ], + [ + 0.1259, + 0.20688, + 1.262811625496628 + ], + [ + 0.13848, + 0.25412, + 1.358294317916464 + ], + [ + 0.1465, + 0.30234, + 1.4537770103363 + ], + [ + 0.14988, + 0.3511, + 1.549259702756136 + ], + [ + 0.15, + 0.4, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.0241, + 0.04409, + 1.0341996211429905 + ], + [ + 0.05134, + 0.08631, + 0.9612505244918907 + ], + [ + 0.08159, + 0.12643, + 0.8883014278407909 + ], + [ + 0.11469, + 0.16424, + 0.8153523311896911 + ], + [ + 0.15, + 0.2, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + 0.05, + 1.1071487177940904 + ], + [ + 0.05, + 0.1, + 1.1071487177940904 + ], + [ + 0.075, + 0.15, + 1.1071487177940904 + ], + [ + 0.1, + 0.2, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.02352, + 0.04705, + 1.1071487177940904 + ], + [ + 0.04634, + 0.09444, + 1.1568755905939945 + ], + [ + 0.06548, + 0.14342, + 1.239659737834175 + ], + [ + 0.0805, + 0.19381, + 1.3224438850743554 + ], + [ + 0.09131, + 0.24528, + 1.4052280323145356 + ], + [ + 0.09782, + 0.29746, + 1.488012179554716 + ], + [ + 0.1, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.02076, + 0.04747, + 1.2101815197942696 + ], + [ + 0.03652, + 0.09683, + 1.3132143217944487 + ], + [ + 0.04712, + 0.14755, + 1.4162471237946277 + ], + [ + 0.05245, + 0.19909, + 1.519279925794807 + ], + [ + 0.05245, + 0.25091, + 1.622312727794986 + ], + [ + 0.04712, + 0.30245, + 1.7253455297951654 + ], + [ + 0.03652, + 0.35317, + 1.8283783317953444 + ], + [ + 0.02076, + 0.40253, + 1.9314111337955238 + ], + [ + 0.0, + 0.45, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.0212, + 0.04729, + 1.1914482830669642 + ], + [ + 0.03835, + 0.09619, + 1.2757478483398381 + ], + [ + 0.05131, + 0.14636, + 1.3600474136127119 + ], + [ + 0.06001, + 0.19745, + 1.4443469788855858 + ], + [ + 0.06437, + 0.24909, + 1.5286465441584596 + ], + [ + 0.06437, + 0.30091, + 1.6129461094313333 + ], + [ + 0.06001, + 0.35255, + 1.6972456747042073 + ], + [ + 0.05131, + 0.40364, + 1.7815452399770813 + ], + [ + 0.03835, + 0.45381, + 1.8658448052499552 + ], + [ + 0.0212, + 0.50271, + 1.9501443705228287 + ], + [ + 0.0, + 0.55, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 4, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.00236, + 0.04981, + 1.4759309018350997 + ], + [ + 0.00944, + 0.09917, + 1.381065476875303 + ], + [ + 0.02115, + 0.14765, + 1.2862000519155061 + ], + [ + 0.03741, + 0.19479, + 1.1913346269557095 + ], + [ + 0.05805, + 0.24018, + 1.0964692019959126 + ], + [ + 0.08291, + 0.28341, + 1.0016037770361157 + ], + [ + 0.11175, + 0.3241, + 0.9067383520763189 + ], + [ + 0.14431, + 0.36187, + 0.8118729271165221 + ], + [ + 0.1803, + 0.39638, + 0.7170075021567255 + ], + [ + 0.2194, + 0.42733, + 0.6221420771969288 + ], + [ + 0.26126, + 0.45444, + 0.5272766522371322 + ], + [ + 0.30538, + 0.47769, + 0.4636476090008061 + ], + [ + 0.35, + 0.5, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.00218, + 0.05254, + 1.488012179554716 + ], + [ + 0.00869, + 0.10472, + 1.4052280323145356 + ], + [ + 0.0195, + 0.15619, + 1.3224438850743554 + ], + [ + 0.03452, + 0.20658, + 1.239659737834175 + ], + [ + 0.05366, + 0.25556, + 1.1568755905939945 + ], + [ + 0.07648, + 0.30295, + 1.1071487177940904 + ], + [ + 0.1, + 0.35, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + 0.05, + 1.5707963267948966 + ], + [ + 0.0, + 0.1, + 1.5707963267948966 + ], + [ + 0.0, + 0.15, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.00218, + 0.05254, + 1.653580474035077 + ], + [ + -0.00869, + 0.10472, + 1.7363646212752575 + ], + [ + -0.0195, + 0.15619, + 1.8191487685154377 + ], + [ + -0.03452, + 0.20658, + 1.9019329157556182 + ], + [ + -0.05366, + 0.25556, + 1.9847170629957986 + ], + [ + -0.07648, + 0.30295, + 2.0344439357957027 + ], + [ + -0.1, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 4, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.00236, + 0.04981, + 1.6656617517546934 + ], + [ + -0.00944, + 0.09917, + 1.76052717671449 + ], + [ + -0.02115, + 0.14765, + 1.855392601674287 + ], + [ + -0.03741, + 0.19479, + 1.9502580266340837 + ], + [ + -0.05805, + 0.24018, + 2.0451234515938803 + ], + [ + -0.08291, + 0.28341, + 2.1399888765536774 + ], + [ + -0.11175, + 0.3241, + 2.234854301513474 + ], + [ + -0.14431, + 0.36187, + 2.3297197264732707 + ], + [ + -0.1803, + 0.39638, + 2.4245851514330674 + ], + [ + -0.2194, + 0.42733, + 2.519450576392864 + ], + [ + -0.26126, + 0.45444, + 2.6143160013526607 + ], + [ + -0.30538, + 0.47769, + 2.677945044588987 + ], + [ + -0.35, + 0.5, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.02076, + 0.04747, + 1.9314111337955235 + ], + [ + -0.03652, + 0.09683, + 1.8283783317953444 + ], + [ + -0.04712, + 0.14755, + 1.7253455297951654 + ], + [ + -0.05245, + 0.19909, + 1.622312727794986 + ], + [ + -0.05245, + 0.25091, + 1.519279925794807 + ], + [ + -0.04712, + 0.30245, + 1.4162471237946277 + ], + [ + -0.03652, + 0.35317, + 1.3132143217944487 + ], + [ + -0.02076, + 0.40253, + 1.2101815197942694 + ], + [ + 0.0, + 0.45, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.0212, + 0.04729, + 1.950144370522829 + ], + [ + -0.03835, + 0.09619, + 1.865844805249955 + ], + [ + -0.05131, + 0.14636, + 1.7815452399770813 + ], + [ + -0.06001, + 0.19745, + 1.6972456747042073 + ], + [ + -0.06437, + 0.24909, + 1.6129461094313335 + ], + [ + -0.06437, + 0.30091, + 1.5286465441584598 + ], + [ + -0.06001, + 0.35255, + 1.4443469788855858 + ], + [ + -0.05131, + 0.40364, + 1.3600474136127119 + ], + [ + -0.03835, + 0.45381, + 1.275747848339838 + ], + [ + -0.0212, + 0.50271, + 1.1914482830669644 + ], + [ + 0.0, + 0.55, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.02352, + 0.04705, + 2.0344439357957027 + ], + [ + -0.04634, + 0.09444, + 1.9847170629957986 + ], + [ + -0.06548, + 0.14342, + 1.9019329157556182 + ], + [ + -0.0805, + 0.19381, + 1.8191487685154377 + ], + [ + -0.09131, + 0.24528, + 1.7363646212752575 + ], + [ + -0.09782, + 0.29746, + 1.653580474035077 + ], + [ + -0.1, + 0.35, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + 0.05, + 2.0344439357957027 + ], + [ + -0.05, + 0.1, + 2.0344439357957027 + ], + [ + -0.075, + 0.15, + 2.0344439357957027 + ], + [ + -0.1, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.0241, + 0.04409, + 2.1073930324468026 + ], + [ + -0.05134, + 0.08631, + 2.1803421290979026 + ], + [ + -0.08159, + 0.12643, + 2.253291225749002 + ], + [ + -0.11469, + 0.16424, + 2.326240322400102 + ], + [ + -0.15, + 0.2, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 6, + "end_angle_index": 4, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03288, + 0.03617, + 2.260711797772509 + ], + [ + -0.06215, + 0.07532, + 2.165229105352673 + ], + [ + -0.08756, + 0.11707, + 2.069746412932837 + ], + [ + -0.10888, + 0.16106, + 1.9742637205130011 + ], + [ + -0.1259, + 0.20688, + 1.8787810280931652 + ], + [ + -0.13848, + 0.25412, + 1.7832983356733292 + ], + [ + -0.1465, + 0.30234, + 1.6878156432534932 + ], + [ + -0.14988, + 0.3511, + 1.5923329508336572 + ], + [ + -0.15, + 0.4, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03531, + 0.03576, + 2.326240322400102 + ], + [ + -0.06841, + 0.07357, + 2.253291225749002 + ], + [ + -0.09866, + 0.11369, + 2.1803421290979026 + ], + [ + -0.1259, + 0.15591, + 2.1073930324468026 + ], + [ + -0.15, + 0.2, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + 0.0375, + 2.356194490192345 + ], + [ + -0.075, + 0.075, + 2.356194490192345 + ], + [ + -0.1125, + 0.1125, + 2.356194490192345 + ], + [ + -0.15, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03576, + 0.03531, + 2.3861486579845876 + ], + [ + -0.07357, + 0.06841, + 2.4590977546356876 + ], + [ + -0.11369, + 0.09866, + 2.532046851286787 + ], + [ + -0.15591, + 0.1259, + 2.604995947937887 + ], + [ + -0.2, + 0.15, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 6, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03617, + 0.03288, + 2.4516771826121806 + ], + [ + -0.07532, + 0.06215, + 2.547159875032017 + ], + [ + -0.11707, + 0.08756, + 2.6426425674518526 + ], + [ + -0.16106, + 0.10888, + 2.7381252598716888 + ], + [ + -0.20688, + 0.1259, + 2.8336079522915245 + ], + [ + -0.25412, + 0.13848, + 2.9290906447113603 + ], + [ + -0.30234, + 0.1465, + 3.0245733371311965 + ], + [ + -0.3511, + 0.14988, + 3.1200560295510327 + ], + [ + -0.4, + 0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.04747, + 0.02076, + 2.780977846589166 + ], + [ + -0.09683, + 0.03652, + 2.8840106485893453 + ], + [ + -0.14755, + 0.04712, + 2.9870434505895243 + ], + [ + -0.19909, + 0.05245, + 3.0900762525897036 + ], + [ + -0.25091, + 0.05245, + 3.1931090545898826 + ], + [ + -0.30245, + 0.04712, + 3.296141856590062 + ], + [ + -0.35317, + 0.03652, + 3.399174658590241 + ], + [ + -0.40253, + 0.02076, + 3.5022074605904203 + ], + [ + -0.45, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.04729, + 0.0212, + 2.762244609861861 + ], + [ + -0.09619, + 0.03835, + 2.8465441751347345 + ], + [ + -0.14636, + 0.05131, + 2.9308437404076084 + ], + [ + -0.19745, + 0.06001, + 3.0151433056804824 + ], + [ + -0.24909, + 0.06437, + 3.0994428709533564 + ], + [ + -0.30091, + 0.06437, + 3.18374243622623 + ], + [ + -0.35255, + 0.06001, + 3.268042001499104 + ], + [ + -0.40364, + 0.05131, + 3.352341566771978 + ], + [ + -0.45381, + 0.03835, + 3.4366411320448518 + ], + [ + -0.50271, + 0.0212, + 3.5209406973177253 + ], + [ + -0.55, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.04409, + 0.0241, + 2.604995947937887 + ], + [ + -0.08631, + 0.05134, + 2.532046851286787 + ], + [ + -0.12643, + 0.08159, + 2.4590977546356876 + ], + [ + -0.16424, + 0.11469, + 2.3861486579845876 + ], + [ + -0.2, + 0.15, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + 0.025, + 2.677945044588987 + ], + [ + -0.1, + 0.05, + 2.677945044588987 + ], + [ + -0.15, + 0.075, + 2.677945044588987 + ], + [ + -0.2, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.04705, + 0.02352, + 2.677945044588987 + ], + [ + -0.09444, + 0.04634, + 2.727671917388891 + ], + [ + -0.14342, + 0.06548, + 2.8104560646290713 + ], + [ + -0.19381, + 0.0805, + 2.893240211869252 + ], + [ + -0.24528, + 0.09131, + 2.976024359109432 + ], + [ + -0.29746, + 0.09782, + 3.058808506349613 + ], + [ + -0.35, + 0.1, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.05254, + -0.00218, + 3.2243768008299734 + ], + [ + -0.10472, + -0.00869, + 3.307160948070154 + ], + [ + -0.15619, + -0.0195, + 3.3899450953103343 + ], + [ + -0.20658, + -0.03452, + 3.472729242550515 + ], + [ + -0.25556, + -0.05366, + 3.555513389790695 + ], + [ + -0.30295, + -0.07648, + 3.6052402625905993 + ], + [ + -0.35, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 8, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.04981, + -0.00236, + 3.23645807854959 + ], + [ + -0.09917, + -0.00944, + 3.331323503509387 + ], + [ + -0.14765, + -0.02115, + 3.4261889284691835 + ], + [ + -0.19479, + -0.03741, + 3.52105435342898 + ], + [ + -0.24018, + -0.05805, + 3.615919778388777 + ], + [ + -0.28341, + -0.08291, + 3.710785203348574 + ], + [ + -0.3241, + -0.11175, + 3.8056506283083706 + ], + [ + -0.36187, + -0.14431, + 3.9005160532681673 + ], + [ + -0.39638, + -0.1803, + 3.995381478227964 + ], + [ + -0.42733, + -0.2194, + 4.090246903187761 + ], + [ + -0.45444, + -0.26126, + 4.185112328147557 + ], + [ + -0.47769, + -0.30538, + 4.2487413713838835 + ], + [ + -0.5, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 8, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.04981, + 0.00236, + 3.0467272286299965 + ], + [ + -0.09917, + 0.00944, + 2.9518618036701993 + ], + [ + -0.14765, + 0.02115, + 2.8569963787104027 + ], + [ + -0.19479, + 0.03741, + 2.762130953750606 + ], + [ + -0.24018, + 0.05805, + 2.6672655287908094 + ], + [ + -0.28341, + 0.08291, + 2.5724001038310123 + ], + [ + -0.3241, + 0.11175, + 2.4775346788712156 + ], + [ + -0.36187, + 0.14431, + 2.382669253911419 + ], + [ + -0.39638, + 0.1803, + 2.2878038289516223 + ], + [ + -0.42733, + 0.2194, + 2.1929384039918256 + ], + [ + -0.45444, + 0.26126, + 2.098072979032029 + ], + [ + -0.47769, + 0.30538, + 2.0344439357957027 + ], + [ + -0.5, + 0.35, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.05254, + 0.00218, + 3.058808506349613 + ], + [ + -0.10472, + 0.00869, + 2.976024359109432 + ], + [ + -0.15619, + 0.0195, + 2.893240211869252 + ], + [ + -0.20658, + 0.03452, + 2.8104560646290713 + ], + [ + -0.25556, + 0.05366, + 2.727671917388891 + ], + [ + -0.30295, + 0.07648, + 2.677945044588987 + ], + [ + -0.35, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + -0.05, + 0.0, + 3.141592653589793 + ], + [ + -0.1, + 0.0, + 3.141592653589793 + ], + [ + -0.15, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.05, + -0.025, + 3.6052402625905993 + ], + [ + -0.1, + -0.05, + 3.6052402625905993 + ], + [ + -0.15, + -0.075, + 3.6052402625905993 + ], + [ + -0.2, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.04409, + -0.0241, + 3.678189359241699 + ], + [ + -0.08631, + -0.05134, + 3.751138455892799 + ], + [ + -0.12643, + -0.08159, + 3.8240875525438986 + ], + [ + -0.16424, + -0.11469, + 3.8970366491949986 + ], + [ + -0.2, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.04747, + -0.02076, + 3.5022074605904203 + ], + [ + -0.09683, + -0.03652, + 3.399174658590241 + ], + [ + -0.14755, + -0.04712, + 3.296141856590062 + ], + [ + -0.19909, + -0.05245, + 3.1931090545898826 + ], + [ + -0.25091, + -0.05245, + 3.0900762525897036 + ], + [ + -0.30245, + -0.04712, + 2.9870434505895243 + ], + [ + -0.35317, + -0.03652, + 2.8840106485893453 + ], + [ + -0.40253, + -0.02076, + 2.780977846589166 + ], + [ + -0.45, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.04729, + -0.0212, + 3.5209406973177253 + ], + [ + -0.09619, + -0.03835, + 3.4366411320448518 + ], + [ + -0.14636, + -0.05131, + 3.352341566771978 + ], + [ + -0.19745, + -0.06001, + 3.268042001499104 + ], + [ + -0.24909, + -0.06437, + 3.18374243622623 + ], + [ + -0.30091, + -0.06437, + 3.0994428709533564 + ], + [ + -0.35255, + -0.06001, + 3.0151433056804824 + ], + [ + -0.40364, + -0.05131, + 2.9308437404076084 + ], + [ + -0.45381, + -0.03835, + 2.8465441751347345 + ], + [ + -0.50271, + -0.0212, + 2.762244609861861 + ], + [ + -0.55, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.04705, + -0.02352, + 3.6052402625905993 + ], + [ + -0.09444, + -0.04634, + 3.555513389790695 + ], + [ + -0.14342, + -0.06548, + 3.472729242550515 + ], + [ + -0.19381, + -0.0805, + 3.3899450953103343 + ], + [ + -0.24528, + -0.09131, + 3.307160948070154 + ], + [ + -0.29746, + -0.09782, + 3.2243768008299734 + ], + [ + -0.35, + -0.1, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03576, + -0.03531, + 3.8970366491949986 + ], + [ + -0.07357, + -0.06841, + 3.8240875525438986 + ], + [ + -0.11369, + -0.09866, + 3.751138455892799 + ], + [ + -0.15591, + -0.1259, + 3.678189359241699 + ], + [ + -0.2, + -0.15, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + -0.0375, + -0.0375, + 3.9269908169872414 + ], + [ + -0.075, + -0.075, + 3.9269908169872414 + ], + [ + -0.1125, + -0.1125, + 3.9269908169872414 + ], + [ + -0.15, + -0.15, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.03531, + -0.03576, + 3.956944984779484 + ], + [ + -0.06841, + -0.07357, + 4.029894081430584 + ], + [ + -0.09866, + -0.11369, + 4.102843178081684 + ], + [ + -0.1259, + -0.15591, + 4.175792274732784 + ], + [ + -0.15, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 10, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03288, + -0.03617, + 4.022473509407077 + ], + [ + -0.06215, + -0.07532, + 4.117956201826914 + ], + [ + -0.08756, + -0.11707, + 4.2134388942467496 + ], + [ + -0.10888, + -0.16106, + 4.308921586666585 + ], + [ + -0.1259, + -0.20688, + 4.404404279086421 + ], + [ + -0.13848, + -0.25412, + 4.499886971506257 + ], + [ + -0.1465, + -0.30234, + 4.595369663926093 + ], + [ + -0.14988, + -0.3511, + 4.690852356345929 + ], + [ + -0.15, + -0.4, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 10, + "end_angle_index": 8, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + -0.03617, + -0.03288, + 3.8315081245674056 + ], + [ + -0.07532, + -0.06215, + 3.7360254321475694 + ], + [ + -0.11707, + -0.08756, + 3.6405427397277337 + ], + [ + -0.16106, + -0.10888, + 3.5450600473078975 + ], + [ + -0.20688, + -0.1259, + 3.4495773548880617 + ], + [ + -0.25412, + -0.13848, + 3.354094662468226 + ], + [ + -0.30234, + -0.1465, + 3.2586119700483898 + ], + [ + -0.3511, + -0.14988, + 3.1631292776285536 + ], + [ + -0.4, + -0.15, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + -0.0241, + -0.04409, + 4.175792274732784 + ], + [ + -0.05134, + -0.08631, + 4.102843178081684 + ], + [ + -0.08159, + -0.12643, + 4.029894081430584 + ], + [ + -0.11469, + -0.16424, + 3.956944984779484 + ], + [ + -0.15, + -0.2, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + -0.025, + -0.05, + 4.2487413713838835 + ], + [ + -0.05, + -0.1, + 4.2487413713838835 + ], + [ + -0.075, + -0.15, + 4.2487413713838835 + ], + [ + -0.1, + -0.2, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.02352, + -0.04705, + 4.2487413713838835 + ], + [ + -0.04634, + -0.09444, + 4.298468244183788 + ], + [ + -0.06548, + -0.14342, + 4.381252391423968 + ], + [ + -0.0805, + -0.19381, + 4.464036538664148 + ], + [ + -0.09131, + -0.24528, + 4.546820685904329 + ], + [ + -0.09782, + -0.29746, + 4.629604833144509 + ], + [ + -0.1, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + -0.02076, + -0.04747, + 4.3517741733840625 + ], + [ + -0.03652, + -0.09683, + 4.454806975384242 + ], + [ + -0.04712, + -0.14755, + 4.557839777384421 + ], + [ + -0.05245, + -0.19909, + 4.6608725793846 + ], + [ + -0.05245, + -0.25091, + 4.763905381384779 + ], + [ + -0.04712, + -0.30245, + 4.866938183384958 + ], + [ + -0.03652, + -0.35317, + 4.969970985385137 + ], + [ + -0.02076, + -0.40253, + 5.073003787385317 + ], + [ + 0.0, + -0.45, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + -0.0212, + -0.04729, + 4.333040936656757 + ], + [ + -0.03835, + -0.09619, + 4.4173405019296315 + ], + [ + -0.05131, + -0.14636, + 4.501640067202505 + ], + [ + -0.06001, + -0.19745, + 4.5859396324753785 + ], + [ + -0.06437, + -0.24909, + 4.670239197748253 + ], + [ + -0.06437, + -0.30091, + 4.754538763021126 + ], + [ + -0.06001, + -0.35255, + 4.838838328294001 + ], + [ + -0.05131, + -0.40364, + 4.923137893566874 + ], + [ + -0.03835, + -0.45381, + 5.007437458839748 + ], + [ + -0.0212, + -0.50271, + 5.091737024112621 + ], + [ + 0.0, + -0.55, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 12, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + -0.00236, + -0.04981, + 4.617523555424893 + ], + [ + -0.00944, + -0.09917, + 4.522658130465096 + ], + [ + -0.02115, + -0.14765, + 4.427792705505299 + ], + [ + -0.03741, + -0.19479, + 4.332927280545503 + ], + [ + -0.05805, + -0.24018, + 4.2380618555857055 + ], + [ + -0.08291, + -0.28341, + 4.143196430625909 + ], + [ + -0.11175, + -0.3241, + 4.048331005666112 + ], + [ + -0.14431, + -0.36187, + 3.9534655807063155 + ], + [ + -0.1803, + -0.39638, + 3.858600155746519 + ], + [ + -0.2194, + -0.42733, + 3.763734730786722 + ], + [ + -0.26126, + -0.45444, + 3.6688693058269255 + ], + [ + -0.30538, + -0.47769, + 3.6052402625905993 + ], + [ + -0.35, + -0.5, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + -0.00218, + -0.05254, + 4.629604833144509 + ], + [ + -0.00869, + -0.10472, + 4.546820685904329 + ], + [ + -0.0195, + -0.15619, + 4.464036538664148 + ], + [ + -0.03452, + -0.20658, + 4.381252391423968 + ], + [ + -0.05366, + -0.25556, + 4.298468244183788 + ], + [ + -0.07648, + -0.30295, + 4.2487413713838835 + ], + [ + -0.1, + -0.35, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.15, + "arc_length": 0.0, + "straight_length": 0.15, + "poses": [ + [ + 0.0, + -0.05, + 4.71238898038469 + ], + [ + 0.0, + -0.1, + 4.71238898038469 + ], + [ + 0.0, + -0.15, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.00218, + -0.05254, + 4.79517312762487 + ], + [ + 0.00869, + -0.10472, + 4.87795727486505 + ], + [ + 0.0195, + -0.15619, + 4.960741422105231 + ], + [ + 0.03452, + -0.20658, + 5.0435255693454115 + ], + [ + 0.05366, + -0.25556, + 5.126309716585592 + ], + [ + 0.07648, + -0.30295, + 5.176036589385496 + ], + [ + 0.1, + -0.35, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 64, + "start_angle_index": 12, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.52586, + "trajectory_length": 0.64852, + "arc_length": 0.58221, + "straight_length": 0.06631, + "poses": [ + [ + 0.00236, + -0.04981, + 4.807254405344486 + ], + [ + 0.00944, + -0.09917, + 4.902119830304283 + ], + [ + 0.02115, + -0.14765, + 4.9969852552640805 + ], + [ + 0.03741, + -0.19479, + 5.091850680223876 + ], + [ + 0.05805, + -0.24018, + 5.186716105183674 + ], + [ + 0.08291, + -0.28341, + 5.2815815301434705 + ], + [ + 0.11175, + -0.3241, + 5.376446955103267 + ], + [ + 0.14431, + -0.36187, + 5.471312380063064 + ], + [ + 0.1803, + -0.39638, + 5.5661778050228605 + ], + [ + 0.2194, + -0.42733, + 5.661043229982657 + ], + [ + 0.26126, + -0.45444, + 5.755908654942454 + ], + [ + 0.30538, + -0.47769, + 5.81953769817878 + ], + [ + 0.35, + -0.5, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 65, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.02076, + -0.04747, + 5.073003787385317 + ], + [ + 0.03652, + -0.09683, + 4.969970985385137 + ], + [ + 0.04712, + -0.14755, + 4.866938183384958 + ], + [ + 0.05245, + -0.19909, + 4.763905381384779 + ], + [ + 0.05245, + -0.25091, + 4.6608725793846 + ], + [ + 0.04712, + -0.30245, + 4.557839777384421 + ], + [ + 0.03652, + -0.35317, + 4.454806975384242 + ], + [ + 0.02076, + -0.40253, + 4.3517741733840625 + ], + [ + 0.0, + -0.45, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 66, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.0212, + -0.04729, + 5.091737024112622 + ], + [ + 0.03835, + -0.09619, + 5.007437458839748 + ], + [ + 0.05131, + -0.14636, + 4.923137893566874 + ], + [ + 0.06001, + -0.19745, + 4.838838328294001 + ], + [ + 0.06437, + -0.24909, + 4.754538763021126 + ], + [ + 0.06437, + -0.30091, + 4.670239197748253 + ], + [ + 0.06001, + -0.35255, + 4.5859396324753785 + ], + [ + 0.05131, + -0.40364, + 4.501640067202505 + ], + [ + 0.03835, + -0.45381, + 4.4173405019296315 + ], + [ + 0.0212, + -0.50271, + 4.333040936656758 + ], + [ + 0.0, + -0.55, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 67, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.02352, + -0.04705, + 5.176036589385496 + ], + [ + 0.04634, + -0.09444, + 5.126309716585592 + ], + [ + 0.06548, + -0.14342, + 5.0435255693454115 + ], + [ + 0.0805, + -0.19381, + 4.960741422105231 + ], + [ + 0.09131, + -0.24528, + 4.87795727486505 + ], + [ + 0.09782, + -0.29746, + 4.79517312762487 + ], + [ + 0.1, + -0.35, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 68, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.025, + -0.05, + 5.176036589385496 + ], + [ + 0.05, + -0.1, + 5.176036589385496 + ], + [ + 0.075, + -0.15, + 5.176036589385496 + ], + [ + 0.1, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 69, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.0241, + -0.04409, + 5.248985686036596 + ], + [ + 0.05134, + -0.08631, + 5.321934782687696 + ], + [ + 0.08159, + -0.12643, + 5.394883879338796 + ], + [ + 0.11469, + -0.16424, + 5.467832975989895 + ], + [ + 0.15, + -0.2, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 70, + "start_angle_index": 14, + "end_angle_index": 12, + "left_turn": false, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03288, + -0.03617, + 5.402304451362302 + ], + [ + 0.06215, + -0.07532, + 5.306821758942466 + ], + [ + 0.08756, + -0.11707, + 5.21133906652263 + ], + [ + 0.10888, + -0.16106, + 5.115856374102794 + ], + [ + 0.1259, + -0.20688, + 5.020373681682958 + ], + [ + 0.13848, + -0.25412, + 4.9248909892631225 + ], + [ + 0.1465, + -0.30234, + 4.829408296843287 + ], + [ + 0.14988, + -0.3511, + 4.73392560442345 + ], + [ + 0.15, + -0.4, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 71, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03531, + -0.03576, + 5.467832975989895 + ], + [ + 0.06841, + -0.07357, + 5.394883879338796 + ], + [ + 0.09866, + -0.11369, + 5.321934782687696 + ], + [ + 0.1259, + -0.15591, + 5.248985686036596 + ], + [ + 0.15, + -0.2, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 72, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.21213, + "arc_length": 0.0, + "straight_length": 0.21213, + "poses": [ + [ + 0.0375, + -0.0375, + 5.497787143782138 + ], + [ + 0.075, + -0.075, + 5.497787143782138 + ], + [ + 0.1125, + -0.1125, + 5.497787143782138 + ], + [ + 0.15, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 73, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.03576, + -0.03531, + 5.527741311574381 + ], + [ + 0.07357, + -0.06841, + 5.60069040822548 + ], + [ + 0.11369, + -0.09866, + 5.67363950487658 + ], + [ + 0.15591, + -0.1259, + 5.74658860152768 + ], + [ + 0.2, + -0.15, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 74, + "start_angle_index": 14, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.51213, + "trajectory_length": 0.4401, + "arc_length": 0.40223, + "straight_length": 0.03787, + "poses": [ + [ + 0.03617, + -0.03288, + 5.593269836201974 + ], + [ + 0.07532, + -0.06215, + 5.6887525286218095 + ], + [ + 0.11707, + -0.08756, + 5.784235221041646 + ], + [ + 0.16106, + -0.10888, + 5.879717913461482 + ], + [ + 0.20688, + -0.1259, + 5.975200605881318 + ], + [ + 0.25412, + -0.13848, + 6.070683298301153 + ], + [ + 0.30234, + -0.1465, + 6.166165990720989 + ], + [ + 0.3511, + -0.14988, + 6.261648683140826 + ], + [ + 0.4, + -0.15, + 0.0 + ] + ] + }, + { + "trajectory_id": 75, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 0.68895, + "trajectory_length": 0.25129, + "arc_length": 0.22167, + "straight_length": 0.02962, + "poses": [ + [ + 0.04409, + -0.0241, + 5.74658860152768 + ], + [ + 0.08631, + -0.05134, + 5.67363950487658 + ], + [ + 0.12643, + -0.08159, + 5.60069040822548 + ], + [ + 0.16424, + -0.11469, + 5.527741311574381 + ], + [ + 0.2, + -0.15, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 76, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.22361, + "arc_length": 0.0, + "straight_length": 0.22361, + "poses": [ + [ + 0.05, + -0.025, + 5.81953769817878 + ], + [ + 0.1, + -0.05, + 5.81953769817878 + ], + [ + 0.15, + -0.075, + 5.81953769817878 + ], + [ + 0.2, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 77, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.63541, + "trajectory_length": 0.36821, + "arc_length": 0.29461, + "straight_length": 0.07361, + "poses": [ + [ + 0.04705, + -0.02352, + 5.81953769817878 + ], + [ + 0.09444, + -0.04634, + 5.869264570978684 + ], + [ + 0.14342, + -0.06548, + 5.952048718218864 + ], + [ + 0.19381, + -0.0805, + 6.034832865459045 + ], + [ + 0.24528, + -0.09131, + 6.117617012699226 + ], + [ + 0.29746, + -0.09782, + 6.200401159939406 + ], + [ + 0.35, + -0.1, + 0.0 + ] + ] + }, + { + "trajectory_id": 78, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.50312, + "trajectory_length": 0.46654, + "arc_length": 0.46654, + "straight_length": 0.0, + "poses": [ + [ + 0.04747, + -0.02076, + 5.922570500178959 + ], + [ + 0.09683, + -0.03652, + 6.025603302179139 + ], + [ + 0.14755, + -0.04712, + 6.128636104179318 + ], + [ + 0.19909, + -0.05245, + 6.231668906179497 + ], + [ + 0.25091, + -0.05245, + 0.05151640100008953 + ], + [ + 0.30245, + -0.04712, + 0.1545492030002687 + ], + [ + 0.35317, + -0.03652, + 0.25758200500044787 + ], + [ + 0.40253, + -0.02076, + 0.36061480700062715 + ], + [ + 0.45, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 79, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.61492, + "trajectory_length": 0.57021, + "arc_length": 0.57021, + "straight_length": 0.0, + "poses": [ + [ + 0.04729, + -0.0212, + 5.903837263451654 + ], + [ + 0.09619, + -0.03835, + 5.988136828724528 + ], + [ + 0.14636, + -0.05131, + 6.0724363939974015 + ], + [ + 0.19745, + -0.06001, + 6.156735959270275 + ], + [ + 0.24909, + -0.06437, + 6.2410355245431495 + ], + [ + 0.30091, + -0.06437, + 0.04214978263643693 + ], + [ + 0.35255, + -0.06001, + 0.1264493479093109 + ], + [ + 0.40364, + -0.05131, + 0.21074891318218475 + ], + [ + 0.45381, + -0.03835, + 0.2950484784550586 + ], + [ + 0.50271, + -0.0212, + 0.37934804372793235 + ], + [ + 0.55, + 0.0, + 0.4636476090008061 + ] + ] + } + ] +} diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 52b94db14af..2762ae37c8a 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -43,9 +43,8 @@ AStarAlgorithm::AStarAlgorithm( _x_size(0), _y_size(0), _search_info(search_info), - _goal_coordinates(Coordinates()), _start(nullptr), - _goal(nullptr), + _goal_manager(GoalManagerT()), _motion_model(motion_model) { _graph.reserve(100000); @@ -192,35 +191,102 @@ template<> void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const GoalHeadingMode & /*goal_heading_mode*/, + const int & /*coarse_search_resolution*/) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } - _goal = addToGraph( + NodePtr goal = addToGraph( Node2D::getIndex( static_cast(mx), static_cast(my), getSizeX())); - _goal_coordinates = Node2D::Coordinates(mx, my); + + goal->setPose(Node2D::Coordinates(mx, my)); + GoalStateVector goals_state; + goals_state.push_back({goal, true}); + _goal_manager.setGoalStates(goals_state); + _coarse_search_resolution = 1; } template void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode, + const int & coarse_search_resolution) { - _goal = addToGraph( - NodeT::getIndex( - static_cast(mx), - static_cast(my), - dim_3)); + // Default to minimal resolution unless overridden for ALL_DIRECTION + _coarse_search_resolution = 1; + + unsigned int num_bins = NodeT::motion_table.num_angle_quantization; + GoalStateVector goals_state; + + // set goal based on heading mode + switch (goal_heading_mode) { + case GoalHeadingMode::DEFAULT: { + // add a single goal node with single heading + auto goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); + goal->setPose(typename NodeT::Coordinates( + static_cast(mx), static_cast(my), static_cast(dim_3))); + goals_state.push_back({goal, true}); + break; + } + + case GoalHeadingMode::BIDIRECTIONAL: { + // Add two goals, one for each direction + // add goal in original direction + auto goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); + goal->setPose(typename NodeT::Coordinates( + static_cast(mx), static_cast(my), static_cast(dim_3))); + goals_state.push_back({goal, true}); + + // Add goal node in opposite (180°) direction + unsigned int opposite_heading = (dim_3 + (num_bins / 2)) % num_bins; + auto opposite_goal = addToGraph(NodeT::getIndex(mx, my, opposite_heading)); + opposite_goal->setPose(typename NodeT::Coordinates( + static_cast(mx), static_cast(my), static_cast(opposite_heading))); + goals_state.push_back({opposite_goal, true}); + break; + } - typename NodeT::Coordinates goal_coords(mx, my, dim_3); + case GoalHeadingMode::ALL_DIRECTION: { + // Set the coarse search resolution only for all direction + _coarse_search_resolution = coarse_search_resolution; + + // Add goal nodes for all headings + for (unsigned int i = 0; i < num_bins; ++i) { + auto goal = addToGraph(NodeT::getIndex(mx, my, i)); + goal->setPose(typename NodeT::Coordinates( + static_cast(mx), static_cast(my), static_cast(i))); + goals_state.push_back({goal, true}); + } + break; + } + case GoalHeadingMode::UNKNOWN: + throw std::runtime_error("Goal heading is UNKNOWN."); + } - if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { + // check if we need to reset the obstacle heuristic, we only need to + // check that the x and y component has changed + const auto & previous_goals = _goal_manager.getGoalsState(); + + // Lambda to check if goal has changed + auto goalHasChanged = [&]() -> bool { + if (previous_goals.empty()) { + return true; + } + + const auto & prev = previous_goals.front().goal; + const auto & curr = goals_state.front().goal; + return (prev->pose.x != curr->pose.x) || (prev->pose.y != curr->pose.y); + }; + + if (!_search_info.cache_obstacle_heuristic || goalHasChanged()) { if (!_start) { throw std::runtime_error("Start must be set before goal."); } @@ -229,8 +295,8 @@ void AStarAlgorithm::setGoal( _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); } - _goal_coordinates = goal_coords; - _goal->setPose(_goal_coordinates); + // assign the goals state + _goal_manager.setGoalStates(goals_state); } template @@ -242,14 +308,15 @@ bool AStarAlgorithm::areInputsValid() } // Check if points were filled in - if (!_start || !_goal) { + if (!_start || _goal_manager.goalsIsEmpty()) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } + // remove invalid goals + _goal_manager.removeInvalidGoals(getToleranceHeuristic(), _collision_checker, _traverse_unknown); + // Check if ending point is valid - if (getToleranceHeuristic() < 0.001 && - !_goal->isNodeValid(_traverse_unknown, _collision_checker)) - { + if (_goal_manager.getGoalsSet().empty()) { throw nav2_core::GoalOccupied("Goal was in lethal cost"); } @@ -273,6 +340,10 @@ bool AStarAlgorithm::createPath( return false; } + NodeVector coarse_check_goals, fine_check_goals; + _goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, + _coarse_search_resolution); + // 0) Add starting point to the open set addNode(0.0, getStart()); getStart()->setAccumulatedCost(0.0); @@ -339,13 +410,14 @@ bool AStarAlgorithm::createPath( // 2.1) Use an analytic expansion (if available) to generate a path expansion_result = nullptr; expansion_result = _expander->tryAnalyticExpansion( - current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance); + current_node, coarse_check_goals, fine_check_goals, + _goal_manager.getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { current_node = expansion_result; } // 3) Check if we're at the goal, backtrace if required - if (isGoal(current_node)) { + if (_goal_manager.isGoal(current_node)) { return current_node->backtracePath(path); } else if (_best_heuristic_node.first < getToleranceHeuristic()) { // Optimization: Let us find when in tolerance and refine within reason @@ -386,24 +458,12 @@ bool AStarAlgorithm::createPath( return false; } -template -bool AStarAlgorithm::isGoal(NodePtr & node) -{ - return node == getGoal(); -} - template typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() { return _start; } -template -typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() -{ - return _goal; -} - template typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() { @@ -426,9 +486,7 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = NodeT::getHeuristicCost( - node_coords, _goal_coordinates); - + float heuristic = NodeT::getHeuristicCost(node_coords, _goal_manager.getGoalsCoordinates()); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; } @@ -493,6 +551,18 @@ unsigned int & AStarAlgorithm::getSizeDim3() return _dim3_size; } +template +unsigned int AStarAlgorithm::getCoarseSearchResolution() +{ + return _coarse_search_resolution; +} + +template +typename AStarAlgorithm::GoalManagerT AStarAlgorithm::getGoalManager() +{ + return _goal_manager; +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index ec0ccc31f02..da46f781975 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -48,7 +48,10 @@ void AnalyticExpansion::setCollisionChecker( template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodePtr & goal_node, + const NodePtr & current_node, + const NodeVector & coarse_check_goals, + const NodeVector & fine_check_goals, + const CoordinateVector & goals_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { @@ -60,10 +63,14 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic const Coordinates node_coords = NodeT::getCoords( current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); + + AnalyticExpansionNodes current_best_analytic_nodes = {}; + NodePtr current_best_goal = nullptr; + float current_best_score = std::numeric_limits::max(); + closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose))); - + static_cast(NodeT::getHeuristicCost(node_coords, goals_coords))); // We want to expand at a rate of d/expansion_ratio, // but check to see if we are so close that we would be expanding every iteration // If so, limit it to the expansion ratio (rounded up) @@ -80,81 +87,53 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic if (analytic_iterations <= 0) { // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space); - if (!analytic_nodes.empty()) { - // If we have a valid path, attempt to refine it - NodePtr node = current_node; - NodePtr test_node = current_node; - AnalyticExpansionNodes refined_analytic_nodes; - for (int i = 0; i < 8; i++) { - // Attempt to create better paths in 5 node increments, need to make sure - // they exist for each in order to do so (maximum of 40 points back). - if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent && - test_node->parent->parent->parent->parent && - test_node->parent->parent->parent->parent->parent) - { - test_node = test_node->parent->parent->parent->parent->parent; - refined_analytic_nodes = - getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space); - if (refined_analytic_nodes.empty()) { - break; - } - analytic_nodes = refined_analytic_nodes; - node = test_node; - } else { - break; + bool found_valid_expansion = false; + + // First check the coarse search resolution goals + for (auto & current_goal_node : coarse_check_goals) { + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath( + current_node, current_goal_node, getter, + current_node->motion_table.state_space); + if (!analytic_nodes.empty()) { + found_valid_expansion = true; + bool score = refineAnalyticPath( + current_node, current_goal_node, getter, analytic_nodes); + // Update the best score if we found a better path + if (score < current_best_score) { + current_best_analytic_nodes = analytic_nodes; + current_best_goal = current_goal_node; + current_best_score = score; } } + } - // The analytic expansion can short-cut near obstacles when closer to a goal - // So, we can attempt to refine it more by increasing the possible radius - // higher than the minimum turning radius and use the best solution based on - // a scoring function similar to that used in traversal cost estimation. - auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.size() < 2) { - return std::numeric_limits::max(); - } - - float score = 0.0; - float normalized_cost = 0.0; - // Analytic expansions are consistently spaced - const float distance = hypotf( - expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, - expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); - const float & weight = expansion[0].node->motion_table.cost_penalty; - for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { - normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function - score += distance * (1.0 + weight * normalized_cost); + // perform a final search if we found a goal + if (found_valid_expansion) { + for (auto & current_goal_node : fine_check_goals) { + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath( + current_node, current_goal_node, getter, + current_node->motion_table.state_space); + if (!analytic_nodes.empty()) { + bool score = refineAnalyticPath( + current_node, current_goal_node, getter, analytic_nodes); + // Update the best score if we found a better path + if (score < current_best_score) { + current_best_analytic_nodes = analytic_nodes; + current_best_goal = current_goal_node; + current_best_score = score; } - return score; - }; - - float best_score = scoringFn(analytic_nodes); - float score = std::numeric_limits::max(); - float min_turn_rad = node->motion_table.min_turning_radius; - const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius - while (min_turn_rad < max_min_turn_rad) { - min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps - ompl::base::StateSpacePtr state_space; - if (node->motion_table.motion_model == MotionModel::DUBIN) { - state_space = std::make_shared(min_turn_rad); - } else { - state_space = std::make_shared(min_turn_rad); - } - refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); - score = scoringFn(refined_analytic_nodes); - if (score <= best_score) { - analytic_nodes = refined_analytic_nodes; - best_score = score; } } - - return setAnalyticPath(node, goal_node, analytic_nodes); } } + if (!current_best_analytic_nodes.empty()) { + return setAnalyticPath( + current_node, current_best_goal, + current_best_analytic_nodes); + } analytic_iterations--; } @@ -292,6 +271,87 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion +float AnalyticExpansion::refineAnalyticPath( + const NodePtr & current_node, + const NodePtr & goal_node, + const NodeGetter & getter, + AnalyticExpansionNodes & analytic_nodes) +{ + NodePtr node = current_node; + NodePtr test_node = node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if (test_node->parent && test_node->parent->parent && + test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) + { + test_node = test_node->parent->parent->parent->parent->parent; + // print the goals pose + refined_analytic_nodes = + getAnalyticPath( + test_node, goal_node, getter, + test_node->motion_table.state_space); + if (refined_analytic_nodes.empty()) { + break; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { + break; + } + } + + // The analytic expansion can short-cut near obstacles when closer to a goal + // So, we can attempt to refine it more by increasing the possible radius + // higher than the minimum turning radius and use the best solution based on + // a scoring function similar to that used in traversal cost estimation. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + if (expansion.size() < 2) { + return std::numeric_limits::max(); + } + + float score = 0.0; + float normalized_cost = 0.0; + // Analytic expansions are consistently spaced + const float distance = hypotf( + expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, + expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); + const float & weight = expansion[0].node->motion_table.cost_penalty; + for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); + } + return score; + }; + + float best_score = scoringFn(analytic_nodes); + float score = std::numeric_limits::max(); + float min_turn_rad = node->motion_table.min_turning_radius; + const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius + while (min_turn_rad < max_min_turn_rad) { + min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps + ompl::base::StateSpacePtr state_space; + if (node->motion_table.motion_model == MotionModel::DUBIN) { + state_space = std::make_shared(min_turn_rad); + } else { + state_space = std::make_shared(min_turn_rad); + } + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); + score = scoringFn(refined_analytic_nodes); + if (score <= best_score) { + analytic_nodes = refined_analytic_nodes; + best_score = score; + } + } + + return best_score; +} + template typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( const NodePtr & node, @@ -345,6 +405,16 @@ getAnalyticPath( return AnalyticExpansionNodes(); } +template<> +float AnalyticExpansion::refineAnalyticPath( + const NodePtr &, + const NodePtr &, + const NodeGetter &, + AnalyticExpansionNodes &) +{ + return std::numeric_limits::max(); +} + template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( const NodePtr &, @@ -356,7 +426,10 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr &, const NodePtr &, + const NodePtr &, + const NodeVector &, + const NodeVector &, + const CoordinateVector &, const NodeGetter &, int &, int &) { diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index bf64ae29e3e..11fdf8b3b41 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -85,12 +85,12 @@ float Node2D::getTraversalCost(const NodePtr & child) float Node2D::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates) + const CoordinateVector & goals_coords) { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. - auto dx = goal_coordinates.x - node_coords.x; - auto dy = goal_coordinates.y - node_coords.y; + auto dx = goals_coords[0].x - node_coords.x; + auto dy = goals_coords[0].y - node_coords.y; return std::sqrt(dx * dx + dy * dy); } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 9d46ab0f0ff..bbbae87ae7c 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -442,12 +442,18 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) float NodeHybrid::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coords) + const CoordinateVector & goals_coords) { + // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = - getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty); - const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); - return std::max(obstacle_heuristic, dist_heuristic); + getObstacleHeuristic(node_coords, goals_coords[0], motion_table.cost_penalty); + float distance_heuristic = std::numeric_limits::max(); + for (unsigned int i = 0; i < goals_coords.size(); i++) { + distance_heuristic = std::min( + distance_heuristic, + getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic)); + } + return std::max(obstacle_heuristic, distance_heuristic); } void NodeHybrid::initMotionModel( diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index dcbc33f197d..13d052f4c7a 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -345,13 +345,18 @@ float NodeLattice::getTraversalCost(const NodePtr & child) float NodeLattice::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coords) + const CoordinateVector & goals_coords) { // get obstacle heuristic value + // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = getObstacleHeuristic( - node_coords, goal_coords, motion_table.cost_penalty); - const float distance_heuristic = - getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); + node_coords, goals_coords[0], motion_table.cost_penalty); + float distance_heuristic = std::numeric_limits::max(); + for (unsigned int i = 0; i < goals_coords.size(); i++) { + distance_heuristic = std::min( + distance_heuristic, + getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic)); + } return std::max(obstacle_heuristic, distance_heuristic); } @@ -467,7 +472,7 @@ void NodeLattice::precomputeDistanceHeuristic( int dim_3_size_int = static_cast(dim_3_size); // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal - // to help drive the search towards admissible approaches. Deu to symmetries in the + // to help drive the search towards admissible approaches. Due to symmetries in the // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror // around the X axis any relative node lookup. This reduces memory overhead and increases // the size of a window a platform can store in memory. diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 8522817ed9e..a51e87fb025 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -171,7 +171,24 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, "service_introspection_mode", rclcpp::ParameterValue("disabled")); + std::string goal_heading_type; + nav2_util::declare_parameter_if_not_declared( + node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); + node->get_parameter(name + ".goal_heading_mode", goal_heading_type); + _goal_heading_mode = fromStringToGH(goal_heading_type); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".coarse_search_resolution", rclcpp::ParameterValue(1)); + node->get_parameter(name + ".coarse_search_resolution", _coarse_search_resolution); + + if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { + std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; + throw nav2_core::PlannerException(error_msg); + } + _motion_model = fromString(_motion_model_for_search); + if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( _logger, @@ -194,6 +211,21 @@ void SmacPlannerHybrid::configure( _max_iterations = std::numeric_limits::max(); } + if (_coarse_search_resolution <= 0) { + RCLCPP_WARN( + _logger, "coarse iteration resolution selected as <= 0, " + "disabling coarse iteration resolution search for goal heading" + ); + + _coarse_search_resolution = 1; + } + + if (_angle_quantizations % _coarse_search_resolution != 0) { + std::string error_msg = "coarse iteration should be an increment" + " of the number of angular bins configured"; + throw nav2_core::PlannerException(error_msg); + } + if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) { RCLCPP_WARN( _logger, "Min turning radius cannot be less than the search grid cell resolution!"); @@ -405,7 +437,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin)); + _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin), + _goal_heading_mode, _coarse_search_resolution); // Setup message nav_msgs::msg::Path plan; @@ -667,6 +700,31 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para int angle_quantizations = parameter.as_int(); _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); + + if (_angle_quantizations % _coarse_search_resolution != 0) { + RCLCPP_WARN( + _logger, "coarse iteration should be an increment of the " + "number of angular bins configured. Disabling course research!" + ); + _coarse_search_resolution = 1; + } + } else if (name == _name + ".coarse_search_resolution") { + _coarse_search_resolution = parameter.as_int(); + if (_coarse_search_resolution <= 0) { + RCLCPP_WARN( + _logger, "coarse iteration resolution selected as <= 0. " + "Disabling course research!" + ); + _coarse_search_resolution = 1; + } + if (_angle_quantizations % _coarse_search_resolution != 0) { + RCLCPP_WARN( + _logger, + "coarse iteration should be an increment of the " + "number of angular bins configured. Disabling course research!" + ); + _coarse_search_resolution = 1; + } } } else if (type == ParameterType::PARAMETER_STRING) { if (name == _name + ".motion_model_for_search") { @@ -679,6 +737,22 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", _motion_model_for_search.c_str()); } + } else if (name == _name + ".goal_heading_mode") { + std::string goal_heading_type = parameter.as_string(); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + RCLCPP_INFO( + _logger, + "GoalHeadingMode type set to '%s'.", + goal_heading_type.c_str()); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } else { + _goal_heading_mode = goal_heading_mode; + } } } } diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 82bdcc9810e..9616237f6b0 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -141,6 +141,22 @@ void SmacPlannerLattice::configure( node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); node->get_parameter(name + ".debug_visualizations", _debug_visualizations); + std::string goal_heading_type; + nav2_util::declare_parameter_if_not_declared( + node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); + node->get_parameter(name + ".goal_heading_mode", goal_heading_type); + _goal_heading_mode = fromStringToGH(goal_heading_type); + + nav2_util::declare_parameter_if_not_declared( + node, name + ".coarse_search_resolution", rclcpp::ParameterValue(1)); + node->get_parameter(name + ".coarse_search_resolution", _coarse_search_resolution); + + if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { + std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; + throw nav2_core::PlannerException(error_msg); + } + _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); @@ -160,6 +176,20 @@ void SmacPlannerLattice::configure( _max_iterations = std::numeric_limits::max(); } + if (_coarse_search_resolution <= 0) { + RCLCPP_WARN( + _logger, "coarse iteration resolution selected as <= 0, " + "disabling coarse iteration resolution search for goal heading" + ); + _coarse_search_resolution = 1; + } + + if (_metadata.number_of_headings % _coarse_search_resolution != 0) { + std::string error_msg = "coarse iteration should be an increment of" + " the number of angular bins configured"; + throw nav2_core::PlannerException(error_msg); + } + float lookup_table_dim = static_cast(_lookup_table_size) / static_cast(_costmap->getResolution()); @@ -314,7 +344,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } _a_star->setGoal( mx_goal, my_goal, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)), + _goal_heading_mode, _coarse_search_resolution); // Setup message nav_msgs::msg::Path plan; @@ -556,6 +587,23 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par } else if (name == _name + ".terminal_checking_interval") { reinit_a_star = true; _terminal_checking_interval = parameter.as_int(); + } else if (name == _name + ".coarse_search_resolution") { + _coarse_search_resolution = parameter.as_int(); + if (_coarse_search_resolution <= 0) { + RCLCPP_WARN( + _logger, "coarse iteration resolution selected as <= 0. " + "Disabling course research!" + ); + _coarse_search_resolution = 1; + } + if (_metadata.number_of_headings % _coarse_search_resolution != 0) { + RCLCPP_WARN( + _logger, + "coarse iteration should be an increment of the number<" + " of angular bins configured. Disabling course research!" + ); + _coarse_search_resolution = 1; + } } } else if (type == ParameterType::PARAMETER_STRING) { if (name == _name + ".lattice_filepath") { @@ -567,6 +615,29 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); + if (_metadata.number_of_headings % _coarse_search_resolution != 0) { + RCLCPP_WARN( + _logger, "coarse iteration should be an increment of the number " + "of angular bins configured. Disabling course research!" + ); + _coarse_search_resolution = 1; + } + } else if (name == _name + ".goal_heading_mode") { + std::string goal_heading_type = parameter.as_string(); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + RCLCPP_INFO( + _logger, + "GoalHeadingMode type set to '%s'.", + goal_heading_type.c_str()); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } else { + _goal_heading_mode = goal_heading_mode; + } } } } diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index fc4abf20c75..24c86d9c24f 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -135,3 +135,13 @@ target_link_libraries(test_lattice_node rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ) + + +# Test Goal Manager +ament_add_gtest(test_goal_manager test_goal_manager.cpp) +target_link_libraries(test_goal_manager + ${library_name} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 53f9302b6f4..7338f4e43c0 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -123,13 +123,30 @@ TEST(AStarTest, test_a_star_2d) } EXPECT_TRUE(a_star_2.getStart() != nullptr); - EXPECT_TRUE(a_star_2.getGoal() != nullptr); + EXPECT_NE(a_star_2.getGoalManager().getGoalsState().size(), 0); EXPECT_EQ(a_star_2.getSizeX(), 100u); EXPECT_EQ(a_star_2.getSizeY(), 100u); EXPECT_EQ(a_star_2.getSizeDim3(), 1u); EXPECT_EQ(a_star_2.getToleranceHeuristic(), 20.0); EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10); + // test unused functions + nav2_smac_planner::AnalyticExpansion expander( + nav2_smac_planner::MotionModel::TWOD, info, false, 1); + + auto analytic_expansion_nodes = + nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes(); + EXPECT_EQ(expander.setAnalyticPath(nullptr, nullptr, analytic_expansion_nodes), nullptr); + int dummy_int1 = 0; + int dummy_int2 = 0; + EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {}, + nullptr, dummy_int1, dummy_int2), nullptr); + EXPECT_EQ(expander.refineAnalyticPath(nullptr, nullptr, nullptr, + analytic_expansion_nodes), std::numeric_limits::max()); + nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes + expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr); + EXPECT_EQ(expected_nodes.size(), 0); + delete costmapA; } @@ -391,10 +408,17 @@ TEST(AStarTest, test_se2_single_pose_path) }; // functional case testing a_star.setCollisionChecker(checker.get()); + nav2_smac_planner::NodeHybrid::CoordinateVector path; + + // test with no goals set nor start + EXPECT_THROW( + a_star.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); + a_star.setStart(10u, 10u, 0u); // Goal is one costmap cell away a_star.setGoal(12u, 10u, 0u); - nav2_smac_planner::NodeHybrid::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Check that the path is length one @@ -406,6 +430,102 @@ TEST(AStarTest, test_se2_single_pose_path) nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } +TEST(AStarTest, test_goal_heading_mode) +{ + auto lnode = std::make_shared("test"); + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.minimum_turning_radius = 8; // in grid coordinates + info.retrospective_penalty = 0.015; + info.analytic_expansion_max_length = 20.0; // in grid coordinates + info.analytic_expansion_ratio = 3.5; + unsigned int size_theta = 72; + info.cost_penalty = 1.7; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::DUBIN, info); + int max_iterations = 10000; + float tolerance = 10.0; + int it_on_approach = 10; + int terminal_checking_interval = 5000; + double max_planning_time = 120.0; + int num_it = 0; + + // BIDIRECTIONAL goal heading mode + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + + std::unique_ptr checker = + std::make_unique(costmap_ros, size_theta, lnode); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + a_star.setCollisionChecker(checker.get()); + + EXPECT_THROW( + a_star.setGoal( + 80u, 80u, 40u, + nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL), + std::runtime_error); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + nav2_smac_planner::NodeHybrid::CoordinateVector path; + std::unique_ptr>> expansions = nullptr; + expansions = std::make_unique>>(); + + auto dummy_cancel_checker = []() { + return false; + }; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); + EXPECT_EQ(a_star.getGoalManager().getGoalsState().size(), 2); + EXPECT_EQ(a_star.getGoalManager().getGoalsState().size(), + a_star.getGoalManager().getGoalsCoordinates().size()); + + + // ALL_DIRECTION goal heading mode + unsigned int coarse_search_resolution = 16; + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION, + coarse_search_resolution); + EXPECT_TRUE(a_star.getCoarseSearchResolution() == coarse_search_resolution); + + unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; + + // get number of valid goal states + unsigned int num_valid_goals = 0; + auto goals_state = a_star.getGoalManager().getGoalsState(); + for (unsigned int i = 0; i < goals_state.size(); i++) { + if(goals_state[i].is_valid) { + num_valid_goals++; + } + } + EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == num_bins); + EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == num_valid_goals); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); + EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == + a_star.getGoalManager().getGoalsCoordinates().size()); + + // UNKNOWN goal heading mode + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + + EXPECT_THROW( + a_star.setGoal( + 80u, 80u, 10u, + nav2_smac_planner::GoalHeadingMode::UNKNOWN), std::runtime_error); +} + TEST(AStarTest, test_constants) { nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown @@ -417,6 +537,15 @@ TEST(AStarTest, test_constants) mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp")); + nav2_smac_planner::GoalHeadingMode gh = nav2_smac_planner::GoalHeadingMode::UNKNOWN; + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("Unknown")); + gh = nav2_smac_planner::GoalHeadingMode::DEFAULT; // default + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("DEFAULT")); + gh = nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL; // bidirectional + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("BIDIRECTIONAL")); + gh = nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION; // all_direction + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("ALL_DIRECTION")); + EXPECT_EQ( nav2_smac_planner::fromString( "2D"), nav2_smac_planner::MotionModel::TWOD); @@ -425,6 +554,18 @@ TEST(AStarTest, test_constants) nav2_smac_planner::fromString( "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP); EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN); + + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "DEFAULT"), nav2_smac_planner::GoalHeadingMode::DEFAULT); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "BIDIRECTIONAL"), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "ALL_DIRECTION"), nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH("NONE"), nav2_smac_planner::GoalHeadingMode::UNKNOWN); } int main(int argc, char **argv) diff --git a/nav2_smac_planner/test/test_goal_manager.cpp b/nav2_smac_planner/test/test_goal_manager.cpp new file mode 100644 index 00000000000..cbd17b87895 --- /dev/null +++ b/nav2_smac_planner/test/test_goal_manager.cpp @@ -0,0 +1,177 @@ +// Copyright (c) 2023 Open Navigation LLC +// Copyright (c) 2024 Stevedan Ogochukwu Omodolor Omodia +// +// 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 "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_smac_planner/goal_manager.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_smac_planner/collision_checker.hpp" + +using namespace nav2_smac_planner; // NOLINT + +using GoalManagerHybrid = GoalManager; +using NodePtr = NodeHybrid *; +using NodeVector = GoalManagerHybrid::NodeVector; +using CoordinateVector = GoalManagerHybrid::CoordinateVector; +using GoalStateVector = GoalManagerHybrid::GoalStateVector; + +TEST(GoalManagerTest, test_goal_manager) +{ + auto node = std::make_shared("test_node"); + + auto costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + + // Create an island of lethal cost in the middle + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmapA->setCost(i, j, 254); + } + } + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + + auto checker = std::make_unique( + costmap_ros, 72, node); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + GoalManagerHybrid goal_manager; + float tolerance = 20.0f; + bool allow_unknow = false; + + EXPECT_TRUE(goal_manager.goalsIsEmpty()); + + // Create two valid goals + NodePtr pose_a = new NodeHybrid(48); + NodePtr pose_b = new NodeHybrid(49); + pose_a->setPose(NodeHybrid::Coordinates(0, 0, 0)); + pose_b->setPose(NodeHybrid::Coordinates(0, 0, 10)); + + GoalStateVector goals_state = { + {pose_a, true}, + {pose_b, true} + }; + + goal_manager.setGoalStates(goals_state); + EXPECT_FALSE(goal_manager.goalsIsEmpty()); + EXPECT_EQ(goal_manager.getGoalsState().size(), 2u); + EXPECT_TRUE(goal_manager.getGoalsSet().empty()); + EXPECT_TRUE(goal_manager.getGoalsCoordinates().empty()); + + goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); + + EXPECT_EQ(goal_manager.getGoalsSet().size(), 2); + EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 2); + for (const auto & goal_state : goal_manager.getGoalsState()) { + EXPECT_TRUE(goal_state.is_valid); + } + + // Test is goal + EXPECT_TRUE(goal_manager.isGoal(goals_state[0].goal)); + EXPECT_TRUE(goal_manager.isGoal(goals_state[1].goal)); + + // Re-populate and validate reset state + goal_manager.setGoalStates(goals_state); + EXPECT_EQ(goal_manager.getGoalsSet().size(), 0); + EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 0); + + // Add invalid goal + NodeHybrid pose_c(50); + pose_c.setPose(NodeHybrid::Coordinates(50, 50, 0)); // inside lethal zone + goals_state.push_back({&pose_c, true}); + + goal_manager.setGoalStates(goals_state); + EXPECT_EQ(goal_manager.getGoalsState().size(), 3); + + // Tolerance is high, one goal is invalid + // all goals are valid + goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); + EXPECT_EQ(goal_manager.getGoalsSet().size(), 3); + EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 3); + + for (const auto & goal_state : goal_manager.getGoalsState()) { + EXPECT_TRUE(goal_state.is_valid); + } + + // Test with low tolerance, expect invalid goal to be filtered out + goal_manager.setGoalStates(goals_state); + int low_tolerance = 0.0f; + goal_manager.removeInvalidGoals(low_tolerance, checker.get(), allow_unknow); + + EXPECT_EQ(goal_manager.getGoalsSet().size(), 2); + EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 2); + + // expect last goal to be invalid + for (const auto & goal_state : goal_manager.getGoalsState()) { + if (goal_state.goal == goals_state[2].goal) { + EXPECT_FALSE(goal_state.is_valid); + } else { + EXPECT_TRUE(goal_state.is_valid); + } + } + + // Prepare goals for expansion + GoalStateVector expansion_goals; + unsigned int test_goal_size = 16; + + for (unsigned int i = 0; i < test_goal_size; ++i) { + auto goal = new NodeHybrid(i); + goal->setPose(NodeHybrid::Coordinates(i, i, 0)); + expansion_goals.push_back({goal, true}); + } + + goal_manager.setGoalStates(expansion_goals); + goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); + + NodeVector coarse_check_goals; + NodeVector fine_check_goals; + + // Resolution 1: everything coarse + goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, 1); + EXPECT_EQ(coarse_check_goals.size(), test_goal_size); + EXPECT_TRUE(fine_check_goals.empty()); + + // Resolution 4: every 4th coarse + coarse_check_goals.clear(); + fine_check_goals.clear(); + + goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, 4); + EXPECT_EQ(coarse_check_goals.size(), 4u); // indices 0, 4, 8, 12 + EXPECT_EQ(fine_check_goals.size(), 12u); + + delete costmapA; + nav2_smac_planner::NodeHybrid::destroyStaticAssets(); +} + + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + rclcpp::init(0, nullptr); + + int result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + + return result; +} diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 4e9b39be67b..526a263a526 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -67,8 +67,10 @@ TEST(Node2DTest, test_node_2d) // check heuristic cost computation nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0); + nav2_smac_planner::Node2D::CoordinateVector B_vec; nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0); - EXPECT_NEAR(testB.getHeuristicCost(A, B), 11.18, 0.02); + B_vec.push_back(B); + EXPECT_NEAR(testB.getHeuristicCost(A, B_vec), 11.18, 0.02); // check operator== works on index unsigned char costC = '2'; diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index f269e9599c5..28c7719355a 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -29,6 +29,36 @@ #include "nav2_smac_planner/smac_planner_hybrid.hpp" #include "nav2_smac_planner/smac_planner_2d.hpp" +// Simple wrapper to be able to call a private member +class HybridWrap : public nav2_smac_planner::SmacPlannerHybrid +{ +public: + void callDynamicParams(std::vector parameters) + { + dynamicParametersCallback(parameters); + } + + int getCoarseSearchResolution() + { + return _coarse_search_resolution; + } + + int getMaxIterations() + { + return _max_iterations; + } + + int getMaxOnApproachIterations() + { + return _max_on_approach_iterations; + } + + nav2_smac_planner::GoalHeadingMode getGoalHeadingMode() + { + return _goal_heading_mode; + } +}; + // SMAC smoke tests for plugin-level issues rather than algorithms // (covered by more extensively testing in other files) // System tests in nav2_system_tests will actually plan with this work @@ -59,7 +89,46 @@ TEST(SmacTest, test_smac_se2) goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; goal.pose.orientation.w = 1.0; - auto planner = std::make_unique(); + auto planner = std::make_unique(); + + // invalid goal heading mode + nodeSE2->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN")); + nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); + EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); + nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT"))); + + // Invalid motion model should not change the default value + nodeSE2->set_parameter(rclcpp::Parameter("test.motion_model_for_search", std::string("invalid"))); + EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); + nodeSE2->set_parameter(rclcpp::Parameter("test.motion_model_for_search", std::string("DUBIN"))); + + // invalid coarse search resolution + nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", -1)); + nodeSE2->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", -1)); + nodeSE2->set_parameter(rclcpp::Parameter("test.max_iterations", -1)); + EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); + + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + EXPECT_EQ(planner->getMaxIterations(), std::numeric_limits::max()); + EXPECT_EQ(planner->getMaxOnApproachIterations(), std::numeric_limits::max()); + + + // valid Configuration + nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 1)); + nodeSE2->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", 1000)); + nodeSE2->set_parameter(rclcpp::Parameter("test.max_iterations", 1000000)); + EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); + + + // angle_quantizations not multiple of coarse search resolution would trigger a throw + nodeSE2->set_parameter(rclcpp::Parameter("test.angle_quantization_bins", 72)); + nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 5)); + EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); + + // valid configuration + nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 4)); + EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); + planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -93,7 +162,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); - auto planner = std::make_unique(); + auto planner = std::make_unique(); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -107,7 +176,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test.downsample_costmap", true), rclcpp::Parameter("test.downsampling_factor", 2), - rclcpp::Parameter("test.angle_quantization_bins", 100), + rclcpp::Parameter("test.angle_quantization_bins", 72), rclcpp::Parameter("test.allow_unknown", false), rclcpp::Parameter("test.max_iterations", -1), rclcpp::Parameter("test.minimum_turning_radius", 1.0), @@ -125,7 +194,9 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.terminal_checking_interval", 42), - rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); + rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP")), + rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL")), + rclcpp::Parameter("test.coarse_search_resolution", -1)}); rclcpp::spin_until_future_complete( nodeSE2->get_node_base_interface(), @@ -133,7 +204,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.downsample_costmap").as_bool(), true); EXPECT_EQ(nodeSE2->get_parameter("test.downsampling_factor").as_int(), 2); - EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 100); + EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 72); EXPECT_EQ(nodeSE2->get_parameter("test.allow_unknown").as_bool(), false); EXPECT_EQ(nodeSE2->get_parameter("test.max_iterations").as_int(), -1); EXPECT_EQ(nodeSE2->get_parameter("test.minimum_turning_radius").as_double(), 1.0); @@ -154,6 +225,9 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ( nodeSE2->get_parameter("test.motion_model_for_search").as_string(), std::string("REEDS_SHEPP")); + EXPECT_EQ( + nodeSE2->get_parameter("test.goal_heading_mode").as_string(), + std::string("BIDIRECTIONAL")); auto results2 = rec_param->set_parameters_atomically( {rclcpp::Parameter("resolution", 0.2)}); @@ -161,6 +235,30 @@ TEST(SmacTest, test_smac_se2_reconfigure) nodeSE2->get_node_base_interface(), results2); EXPECT_EQ(nodeSE2->get_parameter("resolution").as_double(), 0.2); + EXPECT_EQ(nodeSE2->get_parameter("test.coarse_search_resolution").as_int(), -1); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + + // test coarse resolution edge cases. Consider when coarse resolution + // is not multiple of angle bin quantization(72) + std::vector parameters; + parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 7)); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + + // same test as before but the error comes from the angular bin + parameters.clear(); + parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 4)); + parameters.push_back(rclcpp::Parameter("test.angle_quantization_bins", 87)); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + + // test invalid goal heading mode does not modify current + // goal heading mode + parameters.clear(); + parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))); + parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("invalid"))); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getGoalHeadingMode(), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); } int main(int argc, char **argv) diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index 949b48a5305..a1eb918502f 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -36,6 +36,26 @@ class LatticeWrap : public nav2_smac_planner::SmacPlannerLattice { dynamicParametersCallback(parameters); } + + int getCoarseSearchResolution() + { + return _coarse_search_resolution; + } + + int getMaxIterations() + { + return _max_iterations; + } + + int getMaxOnApproachIterations() + { + return _max_on_approach_iterations; + } + + nav2_smac_planner::GoalHeadingMode getGoalHeadingMode() + { + return _goal_heading_mode; + } }; // SMAC smoke tests for plugin-level issues rather than algorithms @@ -63,10 +83,38 @@ TEST(SmacTest, test_smac_lattice) goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; goal.pose.orientation.w = 1.0; - auto planner = std::make_unique(); + auto planner = std::make_unique(); try { + // invalid goal heading mode + nodeLattice->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN")); + nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); + EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error); + nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT"))); + + // invalid Configuration resolution + nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", -1)); + nodeLattice->set_parameter(rclcpp::Parameter("test.max_iterations", -1)); + nodeLattice->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", -1)); + + EXPECT_NO_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + EXPECT_EQ(planner->getMaxIterations(), std::numeric_limits::max()); + EXPECT_EQ(planner->getMaxOnApproachIterations(), std::numeric_limits::max()); + + + // Valid configuration + nodeLattice->set_parameter(rclcpp::Parameter("test.max_iterations", 1000000)); + nodeLattice->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", 1000)); + + // Coarse search resolution will throw, not multiple of number of heading(16 default) + nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 3)); + EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error); + + // Valid configuration + nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 4)); // Expect to throw due to invalid prims file in param planner->configure(nodeLattice, "test", nullptr, costmap_ros); + EXPECT_EQ(planner->getCoarseSearchResolution(), 4); } catch (...) { } planner->activate(); @@ -143,9 +191,41 @@ TEST(SmacTest, test_smac_lattice_reconfigure) results); } catch (...) { } + // test edge cases Goal heading mode, make sure we don't reset the goal when invalid + std::vector parameters; + parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))); + parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("invalid"))); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getGoalHeadingMode(), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + + // test coarse resolution edge cases. + // Negative coarse search resolution + parameters.clear(); + parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", -1)); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + + // test value when coarse resolution + // is not multiple number_of_headings + parameters.clear(); + parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 5)); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + + // Similar modulous test but when the issue is from the number + // of heading, test output includes number of heading 15 + parameters.clear(); + + parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 4)); + parameters.push_back(rclcpp::Parameter("test.lattice_filepath", + ament_index_cpp::get_package_share_directory("nav2_smac_planner") + + "/sample_primitives/test/output.json")); + EXPECT_NO_THROW(planner->callDynamicParams(parameters)); + EXPECT_EQ(planner->getCoarseSearchResolution(), 1); + // So instead, let's call manually on a change - std::vector parameters; + parameters.clear(); parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI"))); EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error); } From 07097d8b247ed329084fb0efafb9c436b056d9f0 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 2 May 2025 00:24:34 +0800 Subject: [PATCH 57/70] Support zero value for std_wz in mppi controller (#5110) * Support zero value for std_wz in mppi controller Signed-off-by: mini-1235 * Update for better readability Signed-off-by: mini-1235 --------- Signed-off-by: mini-1235 --- nav2_mppi_controller/src/optimizer.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index d4f70373e23..ecabc483884 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -438,13 +438,16 @@ void Optimizer::updateControlSequence() auto & s = settings_; auto vx_T = control_sequence_.vx.transpose(); - auto wz_T = control_sequence_.wz.transpose(); auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; - auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx); - const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz); costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval(); - costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); + + if (s.sampling_std.wz > 0.0f) { + auto wz_T = control_sequence_.wz.transpose(); + auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; + const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz); + costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); + } if (is_holo) { auto vy_T = control_sequence_.vy.transpose(); From 9754779147586f6be54bf08321cb6ca0c79348cf Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 2 May 2025 18:43:30 -0700 Subject: [PATCH 58/70] Update update_ci_image.yaml to include jazzy build (#5120) Signed-off-by: Steve Macenski --- .github/workflows/update_ci_image.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index 16d172aff84..503baea5806 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -8,6 +8,7 @@ on: push: branches: - main + - jazzy paths: - '**/package.xml' - '**/*.repos' From 867d1c31634f8a68a3afd6c2a0e0b3cc66bc9760 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 2 May 2025 19:06:25 -0700 Subject: [PATCH 59/70] Update update_ci_image.yaml to add humble Signed-off-by: Steve Macenski --- .github/workflows/update_ci_image.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index 503baea5806..fac69ee5737 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -9,6 +9,7 @@ on: branches: - main - jazzy + - humble paths: - '**/package.xml' - '**/*.repos' From c6dae60b24b870c30de46a87dafd58739e81bb20 Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Mon, 5 May 2025 20:30:58 +0200 Subject: [PATCH 60/70] message_filters hpp headers have been backported (#5127) * message_filters hpp headers have been backported Signed-off-by: Tim Clephas * fixup! message_filters hpp headers have been backported Signed-off-by: Tim Clephas --------- Signed-off-by: Tim Clephas --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 6 ------ nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp | 6 ------ nav2_costmap_2d/plugins/obstacle_layer.cpp | 1 + 3 files changed, 1 insertion(+), 12 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 5910181b3a6..c7341a5985a 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -28,13 +28,7 @@ #include #include -// For compatibility with Jazzy -#include "rclcpp/version.h" -#if RCLCPP_VERSION_GTE(29, 0, 0) #include "message_filters/subscriber.hpp" -#else -#include "message_filters/subscriber.h" -#endif #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 05252015fb3..1cdec808da7 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -49,13 +49,7 @@ #include "tf2_ros/message_filter.h" #pragma GCC diagnostic pop -// For compatibility with Jazzy -#include "rclcpp/version.h" -#if RCLCPP_VERSION_GTE(29, 0, 0) #include "message_filters/subscriber.hpp" -#else -#include "message_filters/subscriber.h" -#endif #include "nav_msgs/msg/occupancy_grid.hpp" #include "sensor_msgs/msg/laser_scan.hpp" diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index a05548a8cde..3bffea43fbe 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -46,6 +46,7 @@ #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" #include "nav2_costmap_2d/costmap_math.hpp" +#include "rclcpp/version.h" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer) From bce5122f658e6e39d752886cb3dd3aae7fb8c0c3 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 5 May 2025 11:32:10 -0700 Subject: [PATCH 61/70] Update PULL_REQUEST_TEMPLATE.md Signed-off-by: Steve Macenski --- .github/PULL_REQUEST_TEMPLATE.md | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 5b818d8e6e5..c9ca0fae67e 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -13,13 +13,6 @@ --- -## Description of testing performed - - ## Description of contribution in a few bullet points --- From a291975e15ff28752cd166aad0234ef910ba2fc1 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Mon, 5 May 2025 19:33:39 +0100 Subject: [PATCH 62/70] Bibtex formatting (#5126) * Align citation text to be within the bullet points. Signed-off-by: Leander Stephen D'Souza * Align formatting to match bibtex guide. Signed-off-by: Leander Stephen D'Souza * Add justified spacing for readability. Signed-off-by: Leander Stephen D'Souza --------- Signed-off-by: Leander Stephen D'Souza --- README.md | 96 +++++++++++++++++++++++++++---------------------------- 1 file changed, 48 insertions(+), 48 deletions(-) diff --git a/README.md b/README.md index 2eea3cbeaee..64125080c7f 100644 --- a/README.md +++ b/README.md @@ -45,71 +45,71 @@ Please thank our amazing sponsors for their generous support of Nav2 on behalf o If you use the navigation framework, an algorithm from this repository, or ideas from it please cite this work in your papers! - - S. Macenski, F. Martín, R. White, J. Clavero. [**The Marathon 2: A Navigation System**](https://arxiv.org/abs/2003.00368). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020. - - ```bibtex - @InProceedings{macenski2020marathon2, - title = {The Marathon 2: A Navigation System}, - author = {Macenski, Steve and Martín, Francisco and White, Ruffin and Ginés Clavero, Jonatan}, - year = {2020}, - booktitle = {2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, - url = {https://github.com/ros-planning/navigation2}, - pdf = {https://arxiv.org/abs/2003.00368} - } -``` +- S. Macenski, F. Martín, R. White, J. Clavero. [**The Marathon 2: A Navigation System**](https://arxiv.org/abs/2003.00368). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020. + + ```bibtex + @inproceedings{macenski2020marathon2, + title = {The Marathon 2: A Navigation System}, + author = {Macenski, Steve and Martín, Francisco and White, Ruffin and Ginés Clavero, Jonatan}, + year = {2020}, + booktitle = {2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + url = {https://github.com/ros-planning/navigation2}, + pdf = {https://arxiv.org/abs/2003.00368} + } + ``` If you use **any** of the algorithms in Nav2 or the analysis of the algorithms in your work, please cite this work in your papers! - S. Macenski, T. Moore, DV Lu, A. Merzlyakov, M. Ferguson, [**From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2**](https://arxiv.org/pdf/2307.15236.pdf), Robotics and Autonomous Systems, 2023. -```bibtex + ```bibtex @article{macenski2023survey, - title={From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2}, - author={S. Macenski, T. Moore, DV Lu, A. Merzlyakov, M. Ferguson}, - year={2023}, - journal = {Robotics and Autonomous Systems} + title = {From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2}, + author = {S. Macenski, T. Moore, DV Lu, A. Merzlyakov, M. Ferguson}, + year = {2023}, + journal = {Robotics and Autonomous Systems} } -``` + ``` If you use the Smac Planner (Hybrid A*, State Lattice, 2D), please cite this work in your papers! - S. Macenski, M. Booker, J. Wallace, [**Open-Source, Cost-Aware Kinematically Feasible Planning for Mobile and Surface Robotics**](https://arxiv.org/abs/2401.13078). 2024. -```bibtex -@article{macenski2024smac, - title={Open-Source, Cost-Aware Kinematically Feasible Planning for Mobile and Surface Robotics}, - author={Steve Macenski and Matthew Booker and Josh Wallace}, - year={2024}, - journal = {Arxiv} -} -``` + ```bibtex + @article{macenski2024smac, + title = {Open-Source, Cost-Aware Kinematically Feasible Planning for Mobile and Surface Robotics}, + author = {Steve Macenski and Matthew Booker and Josh Wallace}, + year = {2024}, + journal = {Arxiv} + } + ``` If you use the Regulated Pure Pursuit Controller algorithm or software from this repository, please cite this work in your papers! - S. Macenski, S. Singh, F. Martin, J. Gines, [**Regulated Pure Pursuit for Robot Path Tracking**](https://arxiv.org/abs/2305.20026). Autonomous Robots, 2023. -```bibtex -@article{macenski2023regulated, - title={Regulated Pure Pursuit for Robot Path Tracking}, - author={Steve Macenski and Shrijit Singh and Francisco Martin and Jonatan Gines}, - year={2023}, - journal = {Autonomous Robots} -} -``` - - If you use our work on VSLAM and formal comparisons for service robot needs, please cite the paper: - - - A. Merzlyakov, S. Macenski. [**A Comparison of Modern General-Purpose Visual SLAM Approaches**](https://arxiv.org/abs/2107.07589). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2021. - - ```bibtex - @InProceedings{vslamComparison2021, - title = {A Comparison of Modern General-Purpose Visual SLAM Approaches}, - author = {Merzlyakov, Alexey and Macenski, Steven}, - year = {2021}, - booktitle = {2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, - pdf = {https://arxiv.org/abs/2107.07589} - } -``` + ```bibtex + @article{macenski2023regulated, + title = {Regulated Pure Pursuit for Robot Path Tracking}, + author = {Steve Macenski and Shrijit Singh and Francisco Martin and Jonatan Gines}, + year = {2023}, + journal = {Autonomous Robots} + } + ``` + +If you use our work on VSLAM and formal comparisons for service robot needs, please cite the paper: + +- A. Merzlyakov, S. Macenski. [**A Comparison of Modern General-Purpose Visual SLAM Approaches**](https://arxiv.org/abs/2107.07589). IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2021. + + ```bibtex + @inproceedings{vslamComparison2021, + title = {A Comparison of Modern General-Purpose Visual SLAM Approaches}, + author = {Merzlyakov, Alexey and Macenski, Steven}, + year = {2021}, + booktitle = {2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + pdf = {https://arxiv.org/abs/2107.07589} + } + ``` ## Build Status From 6e58fbe2888cf88d3040edb4b083b8dc1dfd0cd6 Mon Sep 17 00:00:00 2001 From: moooeeeep Date: Thu, 8 May 2025 02:41:52 +0200 Subject: [PATCH 63/70] Make sure fixed curvature lookahead distance doesn't overshoot distance to cusp (#5134) Related issue: https://github.com/ros-navigation/navigation2/issues/5098 --- .../src/regulated_pure_pursuit_controller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 896a1ab45af..49617ea558e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -187,6 +187,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); + double curv_lookahead_dist = params_->curvature_lookahead_dist; // Check for reverse driving if (params_->allow_reversing) { @@ -197,6 +198,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity if (dist_to_cusp < lookahead_dist) { lookahead_dist = dist_to_cusp; } + if (dist_to_cusp < curv_lookahead_dist) { + curv_lookahead_dist = dist_to_cusp; + } } // Get the particular point on the path at the lookahead distance @@ -211,7 +215,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity double regulation_curvature = lookahead_curvature; if (params_->use_fixed_curvature_lookahead) { auto curvature_lookahead_pose = getLookAheadPoint( - params_->curvature_lookahead_dist, + curv_lookahead_dist, transformed_plan, params_->interpolate_curvature_after_goal); rotate_to_path_carrot_pose = curvature_lookahead_pose; regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position); From 6a51f1ac33ccbe980e2c4efb2ab09c49d2515d34 Mon Sep 17 00:00:00 2001 From: Pradheep Krishna Date: Fri, 9 May 2025 19:01:15 +0200 Subject: [PATCH 64/70] removing the start navigation message in the paused state from rviz buttons (#5137) Signed-off-by: Pradheep --- nav2_rviz_plugins/src/nav2_panel.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index a956108ce5f..c8d8e0e3408 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -320,9 +320,8 @@ Nav2Panel::Nav2Panel(QWidget * parent) paused_->assignProperty(pause_resume_button_, "toolTip", resume_msg); paused_->assignProperty(pause_resume_button_, "enabled", true); - paused_->assignProperty(navigation_mode_button_, "text", "Start navigation"); - paused_->assignProperty(navigation_mode_button_, "toolTip", resume_msg); - paused_->assignProperty(navigation_mode_button_, "enabled", true); + paused_->assignProperty(navigation_mode_button_, "text", ""); + paused_->assignProperty(navigation_mode_button_, "enabled", false); paused_->assignProperty(save_waypoints_button_, "text", "Save WPs"); paused_->assignProperty(save_waypoints_button_, "enabled", false); From 5f34bc362b65a6d4f3bd08f21c0531f6f1468b06 Mon Sep 17 00:00:00 2001 From: Leander Stephen D'Souza Date: Fri, 9 May 2025 22:22:28 +0100 Subject: [PATCH 65/70] Added keepout filter for depot and warehouse maps (#5125) * Added keepout region to warehouse map. Signed-off-by: Leander Stephen D'Souza * Support keepout_map as a launch argument. Signed-off-by: Leander Stephen D'Souza * Added a dictionary to switch between tb4 maps. Signed-off-by: Leander Stephen D'Souza * Added dedicated launch file for map modifiers. Signed-off-by: Leander Stephen D'Souza * Added support for depot keepout filter. Signed-off-by: Leander Stephen D'Souza * Updated keepout masks for depot and warehouse. Signed-off-by: Leander Stephen D'Souza * Renamed keepout filter launch arguments to keepout zones. Signed-off-by: Leander Stephen D'Souza * Renamed keepout map launch argument to keepout mask Signed-off-by: Leander Stephen D'Souza * Renamed map_modifier.launch.py to keepout_zone_launch.py. Signed-off-by: Leander Stephen D'Souza * Preserve duplication of nodes for keepout test. Signed-off-by: Leander Stephen D'Souza * Removed padding from keepout zones from depot map. Signed-off-by: Leander Stephen D'Souza * Added keepout zone at the centre of the warehouse map. Signed-off-by: Leander Stephen D'Souza * Added utility script to handle namespaces for keepout_filter. Signed-off-by: Leander Stephen D'Souza * Used utility function to simplify namespace calls. Signed-off-by: Leander Stephen D'Souza * Moved joinWithParentNamespace to the Layer object. Signed-off-by: Leander Stephen D'Souza --------- Signed-off-by: Leander Stephen D'Souza --- nav2_bringup/launch/bringup_launch.py | 31 +++ nav2_bringup/launch/keepout_zone_launch.py | 219 ++++++++++++++++++ nav2_bringup/launch/tb4_simulation_launch.py | 46 +++- nav2_bringup/maps/depot_keepout.pgm | Bin 0 -> 185487 bytes nav2_bringup/maps/depot_keepout.yaml | 7 + nav2_bringup/maps/warehouse_keepout.pgm | Bin 0 -> 1684105 bytes nav2_bringup/maps/warehouse_keepout.yaml | 7 + nav2_bringup/params/nav2_params.yaml | 23 ++ .../include/nav2_costmap_2d/costmap_layer.hpp | 18 -- .../include/nav2_costmap_2d/layer.hpp | 18 ++ .../costmap_filters/keepout_filter.cpp | 4 +- nav2_costmap_2d/src/costmap_layer.cpp | 16 -- nav2_costmap_2d/src/layer.cpp | 18 ++ .../costmap_filters/test_keepout_launch.py | 1 + 14 files changed, 364 insertions(+), 44 deletions(-) create mode 100644 nav2_bringup/launch/keepout_zone_launch.py create mode 100644 nav2_bringup/maps/depot_keepout.pgm create mode 100644 nav2_bringup/maps/depot_keepout.yaml create mode 100644 nav2_bringup/maps/warehouse_keepout.pgm create mode 100644 nav2_bringup/maps/warehouse_keepout.yaml diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 4f60682632c..05f54d67d8e 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -35,6 +35,7 @@ def generate_launch_description() -> LaunchDescription: namespace = LaunchConfiguration('namespace') slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') + keepout_mask_yaml_file = LaunchConfiguration('keepout_mask') graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') @@ -43,6 +44,7 @@ def generate_launch_description() -> LaunchDescription: use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') use_localization = LaunchConfiguration('use_localization') + use_keepout_zones = LaunchConfiguration('use_keepout_zones') # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -73,6 +75,11 @@ def generate_launch_description() -> LaunchDescription: 'map', default_value='', description='Full path to map yaml file to load' ) + declare_keepout_mask_yaml_cmd = DeclareLaunchArgument( + 'keepout_mask', default_value='', + description='Full path to keepout mask yaml file to load' + ) + declare_graph_file_cmd = DeclareLaunchArgument( 'graph', default_value='', description='Path to the graph file to load' @@ -83,6 +90,11 @@ def generate_launch_description() -> LaunchDescription: description='Whether to enable localization or not' ) + declare_use_keepout_zones_cmd = DeclareLaunchArgument( + 'use_keepout_zones', default_value='True', + description='Whether to enable keepout zones or not' + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', @@ -160,6 +172,23 @@ def generate_launch_description() -> LaunchDescription: 'container_name': 'nav2_container', }.items(), ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'keepout_zone_launch.py') + ), + condition=IfCondition(PythonExpression([use_keepout_zones])), + launch_arguments={ + 'namespace': namespace, + 'keepout_mask': keepout_mask_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container', + }.items(), + ), + IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(launch_dir, 'navigation_launch.py') @@ -188,6 +217,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_keepout_mask_yaml_cmd) ld.add_action(declare_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) @@ -196,6 +226,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_log_level_cmd) ld.add_action(declare_use_localization_cmd) + ld.add_action(declare_use_keepout_zones_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/launch/keepout_zone_launch.py b/nav2_bringup/launch/keepout_zone_launch.py new file mode 100644 index 00000000000..58d5a5987bf --- /dev/null +++ b/nav2_bringup/launch/keepout_zone_launch.py @@ -0,0 +1,219 @@ +# Copyright (c) 2025 Leander Stephen Desouza +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node, SetParameter +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description() -> LaunchDescription: + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + keepout_mask_yaml_file = LaunchConfiguration('keepout_mask') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + use_keepout_zones = LaunchConfiguration('use_keepout_zones') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True, + ), + allow_substs=True, + ) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_keepout_mask_yaml_cmd = DeclareLaunchArgument( + 'keepout_mask', + default_value='', + description='Full path to keepout mask yaml file to load', + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true', + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='False', + description='Use composed bringup if True', + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='nav2_container', + description='the name of container that nodes will load in if use composition', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_use_keepout_zones_cmd = DeclareLaunchArgument( + 'use_keepout_zones', default_value='True', + description='Whether to enable keepout zones or not' + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', description='log level' + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + Node( + condition=IfCondition(use_keepout_zones), + package='nav2_map_server', + executable='map_server', + name='filter_mask_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, {'yaml_filename': keepout_mask_yaml_file}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + condition=IfCondition(use_keepout_zones), + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_keepout_zone', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ], + ) + # LoadComposableNode for map server twice depending if we should use the + # value of map from a CLI or launch default or user defined value in the + # yaml configuration file. They are separated since the conditions + # currently only work on the LoadComposableNodes commands and not on the + # ComposableNode node function itself + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + LoadComposableNodes( + target_container=container_name_full, + condition=IfCondition(use_keepout_zones), + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='filter_mask_server', + parameters=[ + configured_params, + {'yaml_filename': keepout_mask_yaml_file} + ], + remappings=remappings, + ), + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::CostmapFilterInfoServer', + name='costmap_filter_info_server', + parameters=[configured_params], + remappings=remappings, + ), + ], + ), + + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_keepout_zone', + parameters=[ + {'autostart': autostart, 'node_names': lifecycle_nodes} + ], + ), + ], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_keepout_mask_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_use_keepout_zones_cmd) + ld.add_action(declare_log_level_cmd) + + # Add the actions to launch all of the map modifier nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index 0dc98654ae8..ba78b556ddc 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -27,6 +27,19 @@ from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node +# Define local map types +MAP_POSES_DICT = { + 'depot': { + 'x': -8.00, 'y': 0.00, 'z': 0.01, + 'R': 0.00, 'P': 0.00, 'Y': 0.00 + }, + 'warehouse': { + 'x': 2.12, 'y': -21.3, 'z': 0.01, + 'R': 0.00, 'P': 0.00, 'Y': 1.57 + } +} +MAP_TYPE = 'depot' # Change this to 'warehouse' for warehouse map + def generate_launch_description() -> LaunchDescription: # Get the launch directory @@ -41,12 +54,14 @@ def generate_launch_description() -> LaunchDescription: slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') map_yaml_file = LaunchConfiguration('map') + keepout_mask_yaml_file = LaunchConfiguration('keepout_mask') graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') + use_keepout_zones = LaunchConfiguration('use_keepout_zones') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -56,12 +71,12 @@ def generate_launch_description() -> LaunchDescription: headless = LaunchConfiguration('headless') world = LaunchConfiguration('world') pose = { - 'x': LaunchConfiguration('x_pose', default='-8.00'), # Warehouse: 2.12 - 'y': LaunchConfiguration('y_pose', default='0.00'), # Warehouse: -21.3 - 'z': LaunchConfiguration('z_pose', default='0.01'), - 'R': LaunchConfiguration('roll', default='0.00'), - 'P': LaunchConfiguration('pitch', default='0.00'), - 'Y': LaunchConfiguration('yaw', default='0.00'), # Warehouse: 1.57 + 'x': LaunchConfiguration('x_pose', default=MAP_POSES_DICT[MAP_TYPE]['x']), + 'y': LaunchConfiguration('y_pose', default=MAP_POSES_DICT[MAP_TYPE]['y']), + 'z': LaunchConfiguration('z_pose', default=MAP_POSES_DICT[MAP_TYPE]['z']), + 'R': LaunchConfiguration('roll', default=MAP_POSES_DICT[MAP_TYPE]['R']), + 'P': LaunchConfiguration('pitch', default=MAP_POSES_DICT[MAP_TYPE]['P']), + 'Y': LaunchConfiguration('yaw', default=MAP_POSES_DICT[MAP_TYPE]['Y']), } robot_name = LaunchConfiguration('robot_name') robot_sdf = LaunchConfiguration('robot_sdf') @@ -79,10 +94,16 @@ def generate_launch_description() -> LaunchDescription: declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - default_value=os.path.join(bringup_dir, 'maps', 'depot.yaml'), # Try warehouse.yaml! + default_value=os.path.join(bringup_dir, 'maps', f'{MAP_TYPE}.yaml'), description='Full path to map file to load', ) + declare_keepout_mask_yaml_cmd = DeclareLaunchArgument( + 'keepout_mask', + default_value=os.path.join(bringup_dir, 'maps', f'{MAP_TYPE}_keepout.yaml'), + description='Full path to keepout mask file to load', + ) + declare_graph_file_cmd = DeclareLaunchArgument( 'graph', default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot4_graph.geojson'), @@ -118,6 +139,11 @@ def generate_launch_description() -> LaunchDescription: description='Whether to respawn if a node crashes. Applied when composition is disabled.', ) + declare_use_keepout_zones_cmd = DeclareLaunchArgument( + 'use_keepout_zones', default_value='True', + description='Whether to enable keepout zones or not' + ) + declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), @@ -146,7 +172,7 @@ def generate_launch_description() -> LaunchDescription: declare_world_cmd = DeclareLaunchArgument( 'world', - default_value=os.path.join(sim_dir, 'worlds', 'depot.sdf'), # Try warehouse.sdf! + default_value=os.path.join(sim_dir, 'worlds', f'{MAP_TYPE}.sdf'), description='Full path to world model file to load', ) @@ -189,12 +215,14 @@ def generate_launch_description() -> LaunchDescription: 'namespace': namespace, 'slam': slam, 'map': map_yaml_file, + 'keepout_mask': keepout_mask_yaml_file, 'graph': graph_filepath, 'use_sim_time': use_sim_time, 'params_file': params_file, 'autostart': autostart, 'use_composition': use_composition, 'use_respawn': use_respawn, + 'use_keepout_zones': use_keepout_zones, }.items(), ) @@ -253,6 +281,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_namespace_cmd) ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_keepout_mask_yaml_cmd) ld.add_action(declare_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) @@ -268,6 +297,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_robot_name_cmd) ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_use_keepout_zones_cmd) ld.add_action(set_env_vars_resources) ld.add_action(world_sdf_xacro) diff --git a/nav2_bringup/maps/depot_keepout.pgm b/nav2_bringup/maps/depot_keepout.pgm new file mode 100644 index 0000000000000000000000000000000000000000..5d1aeea4dbb4d257fa8be252444371f42af05b12 GIT binary patch literal 185487 zcmeHQ+pa7*4L#5E6-VNk(P&`c19(6@faVuKjL=935I2dB+`lx!b}nV7Vpmetm#R~% zbxxP<*p9QZI!RaW-TV9RzWUkc-~983KmPlNKY#wy|33frcfbGs^M8N%=f8gZ``VM0pA1G~R|OZZrU$J`4mMDx?9Mrt zylgBwdlbD$DK=@8LUQC(i!octARdy2MG@Fd`b%EiAKU5i6FzhJ zl&=!CS%qj16`T6tA##N&*0?IAsizh;E=WJkldjih5=U!#yJm`ido@h~TwG&1f%@j6 z)N6oVZa)=OV^wT&r6+OXE2Ae*99b1==0y3iNX5eK$tPXKUfj}^(~6Jyac=s@CDlio zI7vmGsc*TmTngYQ$s^B_OCre3oyybFdv+2-dFt6po4QJIgT5;4Q6dl6KGOIYvgcQo zrlRv~ODbBL$Wm~*me%fdaZ1vncipXV#MzST`H11b8#_MDy@kr7>wR23*3QI+uYWgxza)RLU zo;CDcdC!O!4J4JdYeY{BlB%AVpHt@v>0UZr%Yij55aq1i@p_YqxPCIazX^zQblc@r zH99pCx5&k5e4|ZdySYC6-#_1g9s~}Km z)D@gjfLjhQFlz+5l&VJUsbtv`l@1XIbP%XC>I$6zHi26XFfeNbx|FI$?Wtt!i3$ve z9s-p{UBQXU>(&YHLO2D({0;=Vl&VIjB3`#%@!7u9J-3*2(Rfw>^i zrBpQ*ujZQ0TW7p8-xLbiYIH!NJrT@uuz|TEP-)Z^ zs+$#L-LfWVK%hmSOQ~vXy|XUuP8-ZXK+~jhsTCZUz%2(Dm^A_|o2;%qm5lS20t2Fl zfaOxFIZ@!20}RX>ftF2H*PcqoSzdtw(L=y;snwh)aLWM(W{p70CaY^tC4*ZE42T{A zmP@VXMB%*U00Xl|pkTd4*|=iR&%1jEe9BwH3BW0tgbzk3~nhfAbJQ` zF14ByCEUV~K{)8ZoDpc*WOWB7yiat1fmtJ9xzuXfQ_0|#0t2FlK+7hpJ5e}qIl#cI z5wKipHSMWna7%#!(L5th#mqho2>3cVcl|ofmtJ9xzuXfQ_0|#0^^|ALT>cQ+$MIV zOeXWc33oKnssdwhvVdC-Fgwf!?^c=2*M?jDamfZV+3s7-NrLm11I!L{3%SuJ^O@L{ z^0;J!nN}4TgOdc@a)1%@+|3z-cdJC*blTc*t9Jq-lMOan6sFot%t}#?TEw2H(P(4> zv9W~e3Uh4;BiD^ObSRHhZfgA`V%>~Q4nYQLa*|-(GQb$1W?y&V1ek8Nl`xk#nO@o;nHSR7gACNvI2Uym zzY_O_UKv9g8vVt%4;i70TW|c~1oQ#|TbRq6OfO%M%nNDkK?Z7SoQn!>6(s1&V9=)c zA%EeT*d>Un_6;$dfQ{I;In3ovW}BqwL9*hqaSbw1lM@VXDPRO9ViteOn$|6%mulY- z!wJ}kZJWbf6E#WRAX(e5qX!wN$qT}looZz0rN+!66Fo7(X1@6@oB-X+fE?!XCezCs zB=bTVdys*e8t0;7-6}}%m0%*Mh*61G>1&2P=yagS;RLL_V?&s0xT4%gkSwmEwgwrf zsk&JK+){WDNc4{x#Sa$P;z6eZlNe6G$~!iMxrQsseFVwkDr#$xftsqD6~L{EN5+T{ z$;9IKigKW@F0l|!KpeVi4Rd*uxyq6aK{79-u?HEbsc|kUxK)slC5x1WTSPLZmk(YI zO+q*UjqjEk=JF=fOBy8eLK=IJftniUqT=3ZK_cykq$P6k;zbY9J@A%Lk-`aRe7Dpv z*I+%hk04p~iG?5oH94{1)&k73Dbi6ZAG@fPab27ODV%^HuGbXi8Y^c>8zfttLMg~V zO-?BGLpE#WQc_EEG$l#ZQ^A@AX`0xo!Zk)2Z=9c&3RFe=+fbts=!(0Qkm_CAJ z#?M!R4AkVr3AemPC*}(@Gq;#rd=ZNAX2_zP<#}@)z$l!6NzlSv1GSj$f@H?eSAq=G zZ)T`kB!O->m0!~_f> zly=0lSe2g+;UDL!4i1FX98N&>8WX}?rKYh|vgLdE22*(9$?hZpw=yt<5=%lY+6f64 z;Tj3aA-8Am?j@XnZiH_PbAekvaHnh`5~b5sI%VV2k| zj`P$*Wp(Sr=s)DPmqytaPCE3Bp5 za(zXg)Q14bePu*S_vN}(hQxbMTft4cmnPY@bK1&J$SwYK;eVr$q0eB3sc5Y3yy7)f?xco|0SXvtr^`nd(yQ zv#$hM>VmE!6@92<3wWaqWmue==tf=qcyTB%L}j&N6RlP2=xRD-EjwXn-4l6@57&8T zJO|CxtIl9G-O`iSpesu+Ah4;(j;I1_>j~Gb*DnqV7)Pvl&CRWoZN|(!SAQi|TO_Em z*J2ddX~L` z)k~3WI#VDA8K}t#JSDfv4W@h((<%Kq<^q)mSkP-Y0YO}^Da&BCgi*x;o}_BeLc8YEl!_6

YU;=Etv70O zo1G|_dkBOR&eny?jA3FQl~xDcov^TW`4k-B$i(=n?+z1UN$vb3HJb)BQut z%&jwir1+4*y*Nq8hsj#PTvy>%TDE4ZokZ#$?jav0AqaEL&Q_3YvaBh0-V(X%sUSFa z5eTMm8sIS3>}&&ib$!?w|m_l&t=G-_sd~nMmC};WEHP)tpmJtBAEEV`| zBB*6C*{gwD7CD@^EFMQRaLb|xZdp8zXyBGbPq<}G&~_pKZdu^4Zdp8zXyBGb5APE# z9!E5A%c2KvSv-zt;Fd)Xd!ohThz4$1^uR5P#}N(OvgmKHDqDR~!NUROoJ_NumiyO}J7LOwuxMk4;w=5nSICKsexM-IOZ09jm%=QR|B^!a#*)49!E5A%c4iz5=g8S+8zYJ zEsL6P%bK9=L;&2fz{$N+Yl5~D0dUI#2X0wBj%eVPMGxGvcpTBdEsGwwW$`$ofm;?m zoaHSZM>KHDqK9?M;&DU+w=8<#mc`?U25wpOz%7f%5e?k3=z&`nk0TnmWzhq-EFMQR zaLb~H_lXvdBO176(ZjlB@i?M^TNXX6TNaNa8n|WA!@6bhIHG}D7Co$67LOwuxMk4; zw=5nv>XAl6lX3&D~3(lm%%BC|83eFy z&7cL}83e$s8MNR#g8;ZSgBE;e5WsnB1}*r`AOLR7patI<1h6O0patI<1i-BswBS2~ z0Jt@S7JO$AzYG2uqV!-1>YG2ux`zu1>YG2z^xgy z;5&l=xHW?od}j~fISAa^4IP0u5_=H_1!azmvgd<6v4={> zrm-hdiJG4~-pZr{uPLj+Oo>>x#^8!r5od@ZuhABd&u;~{d~9^fCt0e;nyj;tS~Uht_yRE7x``@j1>VU22)DbE)s}bgP2{GQnm03!lVFhjR96(eMBmz zl|1LwYlCBTiY_aRA++|AJ&{JeZh=K3kPzbKYGON-1ec{$EnMR;BPVXnMBL9efn08B z@m@$w8>U#DqRR?7-Wz?7I9}9=P0XhcS>XAIO5)gJe_2Y^!uQTHxV4QOlPLX@ zD;?;0Z!Bl|Er!)8x~#B`P~QW4VjmY}P3w~^Tx{hvYl}}RI&Gzw8V}w`Kxv z<}0M2F*CxvSjjPq_s8MKrD;`$KRjCPh;>V&v6lH|D+hOC(?Teb@(X&guA|ukZuN12 zm6;(Z$cGnnelHJNz={~7RJ^WxjmPJ=V%_qw;TFP&{_&cXKfvG*G3cRKwy|(sms6$k z7$LtE?-PA&gjx)67UaXRLN(qgCqF@N|rAXM{janms6$k299AB+}egJ zty_dNziZ^JFp08NlCqJY%+PhO8v}Hk!7Ue>+K{M3Dw9k86Ka`R)yWmE%c)YiJG6}k z;ns!^nh61Sd2$;vY-NV7d)+(#Shog|s}!ojXyA28l`9_{XzNDN_-XkTl&(HYshZ^S z)H*P7aI4ZF5;DRI(CNnngUvE$1Gger(MAX`shB48l<>&cqPig6np+GQ9$N?qMTA`h z7r#%;@+Nq<<`#YWx&z$k`eedJeNzYqlP;}ilt}{ic!CX|_hvsRy#lHv0 z|G=#i@bQb{*76JT;MQfiC6jo<#ev^q$OGI0xArZYZqx#(>+<@B@)~5mPjG%2%T=YF zP@nrA1pV0X&T&0HY++QXDP1>9yegxXx_5$kT5hHBY);(HVebfApl+p&WUc%4^o;>} z%_sdlv2dzfz4BjZ&u^*uYXj2@vb-$IGUnBCJubBsdn4PhpOjl0RllQIfBzPJmreQ_ zL4LmhZaLGzh@EJ0aH>3mIa4UTeXY+7-$r%qDsVl0V}M>W)-4a2*1k|kh()Ag4|-l2 z+K{AQk4tUE{c&wt%b{V@@%9c(p~NS(A2Q9rEr%Q6mIKVnoGFx!THIRpMR03{l^F7y zpME2(8i7P1CRAlrVi&ljVNx>IEd@p>E)=4JR#~(7y7gA5J-u8{4{>oDX`I{OxY4(2 zAa2QVt6>(-TOJFrCwj=N?82=?tY+O>xmv^Z)~U8)JDy<;d*UcmY3rk9iuXYd;wwG2 zw#52XO6`nYM<0c#@xz|j0OG1l1tOAAODM%(0xsloLsG+aIOJ8jF+jH&=Pef++aS!M zRjZJTX+>PadCS72HE>I#5y*y9Lah9>yn*aTd#k~9?EP_VTjk}r?FNr@me-^Lw>DUT zb!!8l(ky?9vmD?x#jtJ_h}tXGtrihS$GYVJBg{R)bVk!L15KEbvD(`DYrMA8m-^ z8t3bL(z8^$F+jH&+;X9@4V7C&7P=uJ$meJ4*lNohT~%sFG^&GJqev;gt!+@bl6X~0 zmAdU{CN#WHoB#{1QwV@tQ*gm~0s-ua6JWu03IT9y3NAQLAb>q_5Ug~vT~6kMVRv@J zx-~fa%q{+1jPyOfBo4;i*bUqooP1i~^DmI4d-CAcCUEj<-1LIO^0nmo;~sEplN2~_ zZ9nb=R4H{qBEZ~|FO=FDk1!zMA&|M{HNy@B zb|V08?S>A48wh|~8vvnsHv-_+Zs-uWfk3)A_uDJ8z#oRjOeP;Pc_-x!N%;M|Wh%Yt0V~2_Qi!)ZVkaEK=JY*51BEJQ**>3+vPzMYFd?Fa^UA=tPx9tm3pzpmbsPs z%z4>Cv5E?_kJ80bFk4boX^XDf(-tG$$`eCX>X2Mgkn~bRq<_(pC$%Px%VotrRtDQV zqDfUFjxY2PQst)?c01ISj~5*d=^;iYVPtdx=R!C4Xt>Cwl!XSvi;Hwwo6C3Q|>zJ vi~o{mvGt|wDHnb!k$rD#^?loxR=0UBseOS%y5u~dXdnOtfB+D94FdlMB{xr& literal 0 HcmV?d00001 diff --git a/nav2_bringup/maps/depot_keepout.yaml b/nav2_bringup/maps/depot_keepout.yaml new file mode 100644 index 00000000000..31c69b3d93d --- /dev/null +++ b/nav2_bringup/maps/depot_keepout.yaml @@ -0,0 +1,7 @@ +image: depot_keepout.pgm +mode: trinary +resolution: 0.05 +origin: [0.0, 0.0, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 diff --git a/nav2_bringup/maps/warehouse_keepout.pgm b/nav2_bringup/maps/warehouse_keepout.pgm new file mode 100644 index 0000000000000000000000000000000000000000..c654e209c42e5c82c4c4007f17e51ed08601fbdf GIT binary patch literal 1684105 zcmeIbTdwT9vSv4*Pb)UyXBVoBWIV^GAW6dOx+bh zDMB2%|2-8+No4Sg7$l`s=K7a^{-^)yAOHP-`PcvX|Nhti^&kJs|M!po;eY&(|MHLj z_h0|Z|Mj2#kN^CS|K{KP+kf-#|M4&X(|`QO|M|cFKmXf*{h$8xKmEIZ`)~ilKmOf+ z`0xJPfBN_T{LlaNn_fSE)?PnP{6wa;Yzr? zs^mTa$XgB#19G^sh-&=#O??{5hBw{=jsGa{7uplpVH;?Vu*dm*FoxR-(|&RV1>7qD z?KSO@_3%ox2k`T9$9<&f017}mv_nBJ3P3xw_mb=M6WXC23VKlh+H2aU4=A{9?nF6=w2wFGxJ%a=1l@EO+M&IhWbeMC(0-8i>M%?c@TmZ_S8+?v6)s)3SBGJufKLUW zy^33*eSD~{e!8YQ3=;)>Dgf#jaIr;I1U3r$SIt&v9d@7I1)#l(AGB9uV7^ZQXs==#ZpsDV$A@LdOB?RVSMp$bR{+{`1cMagjk@XC_{cuA zXL&IED*){|h5_wqLXSnkjGtKWt(dG2hI<8S+Ivs!NsIAcZ?C|2A7na!0)qlI?b8)& zx5`X6>7c#a40wCTp}j;H+KU!gZ&U!D zwzCN1E(#>=K zfAx`-{IXJ%`cVMdmtz&~%MG8hN7_dV+Lx80)QRXz0?@t~sR8u>d5w5&FP=mp3fNWv+85&! zXs>R~MfU%EkiU2mg(zTK0cc;2PojO;%dc51i$tk21)zO7U=!`(dOf_Rd4BmwN>RXz z0?@u3DdzTNT`2Xe0JJYhZ=!wN$x41%Bubqr0PV{Gn{LyMJ0tDGox0^?DMbMv3PAgE zoaQTeK7dM;V0cc-V@q_m9jrz2^G`BB%Ln#VqC;;utDu$b8@v3^-8LS4k z%hT#THZs`pPTjIQl%jxw0?@uJVg&CeU#hpFJ+M6iKGOaN*q6gWsfq&7zAR(C%1%Eq zy?o^L(vLRAH_(1vPR`s_;2pHOgL99Dz|BMdrP(V`wm$Z-g zjWV>Ny{>l{U7B4jj3ZUJB^Y0tv865)_*B3#?TM1eqB3ksi6eR8Y1T2cKk?EbEhhA$J=RzOuHS^7v()qg1su~ps^5zC zKwi2>9-0`QXRo+aiTsD_@lAs(5*0_O|iy9dg22Yk{#OoXGZHI|A>kJU~UHkx-oUFTUk#qd$pq5{yqD9dzBJ?@*R)Sugj zM+K(#hiRR;eK{JG>L{>6yXmwWe&;1jg@Xd06QqtPp_ZZANrh&#k86g}=SKZd6NHLh zpnzlANA(9{+CIZR-3Zj5+ehDrete8zFUTvpU{L{=w2zO?=D9t7G_I}fIMt4|ww>E2 z&zak`Lr8ry1y*R+pDbrBUo9BX4scY2>*HR-*O2zojp2E{x=&a2;XD*rQ@|zdaWbE1 zkLK{8+gb-Nmfu2qbUXn*EKl^;C>OiWX|GhkAMNQmxA7VFX7g_RNPmKFXk!>vG*e)# zFpgGbky6{5ZBZ8rLHzlz#kufx23)6N4im9E=zFe9SW?_u8xnUTJd5xeE>Jnu33Go>qgoq*O}Y3V@Q27 z1y*P`{m&1Mb8Y>baq0`;X|!uQ?@numDki1}*~(8$p9-wdu768PQJxP$jUp56>LiZ!jv3Ud4e$Ah|C$P{&~AE? z8Oiu~x<*rySl5l$?QO3Xw1uHQ&kC&2ZhIv^tauA04W3C^qFp`A(|kVEJ1ix*C$FwC z71j!@(C%PvpBC1`FSV+>8wQZbUl~!7U27+`>op3rOV#B3eFau%w>`HHXiR;ZFxl_v zsUv2z=Wnl^s%HD(VMNIT)$l_$(-5|O9(z&XngT1d=N!CNYumHNIdN*E741X4P3QKH ztA*h^I1MgI+D3xfeJQX)yMwvCCQ;qp4-)I^YsKh(w9)DbU`2na%Dn=9X|F$lgLe0c zNC$3Ez$NWrvQ3v7ao!CJVD0POk9oK4+&%?)ymHX)#|;@o6Mhs}pT*P-x6$SzeQ#mDGIoxz5TCQ z6oJ}VA1it0_I5&3b5g(|?bG+drydNLxV+r`p4;Pn0+$_0xy07G0)A0Zzlkx`t9Mb+VC;GojH;!7Y?3R-%3A!bH2j1j1uS3b>>_ zz#ZrqRStH1Ig&iN8r=@v7*;1!nah5p{;)N#fIr%YWqh0pBjLY(zfBTh53l$QzMLhs zoeMEgKuLiW+AUwVPoKAsm%F}c9|x^Sd*nR)zan2Lz*K_*S%DSWO=n$P|AtrWgX6h9 ztx?9Aee9BH*>lcdcvS$}pNrf$YK0LiyrBKLIP}x9OWG$)A|wMQP8Yz?-VbLsdg_w) zaUvWA9BiK};EHzFS0pwv887w~fcAZjS{*Td;6BwJM=hsolvessueJivuC3_t`k32~ zYsHZo3izdc`jncLyha*OM;8kCrG0vq4%)lOQ#z@kfJ55H=S`<4XNP~E&F4+J^s5|k z*!mq;^-epYk%-iBg92V@k5hV-$1^44UAmuv_GyVy9-_S=_B7F(0^VsKDDnAQt>F&* zFlIq}Z_qu~U+=U#p4&&)!zbeIIFe+>f&<$MKIT;F9)m zGb=ra61KzZF#)eP{4uvf`*k5X^QZz2Y5)9Ts_~1WN4sn3xSDpp6=Nwful2w%Jv=3{PfcTJp<3j{|-rh5d?fVYx z?KNI&VOq)$_vE>5UsH*4Jq4g$Ps@`vjUTrM%&RpDPuH9jk;Bg@;F9)n**kP|yx4UK z$wWJ!z@5ogj+H3jlJ<0$E_OWrLbK1yU1*miF6UlQz%T9Y@6ruV4Wu9U9j@V>4qF$} zj#C~{0NNjsJvREWA{p=4eH1?SRRl5bP{1SY@l&_3gdaY3i+`N{R%@a?J`>1u@^>VA z_igy2J^frw{1FJx*X?PI5_^$4!cF<^fov*(TiU082X4Gg=ke*9xI&2|*Fbx^CGW8# zP4kn=H5Gt%O*K!|H4;8NH_Jl)lyV$=QGsRJO|K{?O1!V+rz?_S6%Xwf)0fjuDX>ht z>7R9k_EQn&U=&CStkItK8{Edr-AH_NZ6M$DN@QKYY1E2$=$PBrWnzz>0v>6f9-u3? zvYkFlH}(VC^|G0owiST(ZG9T<4NH>9JG3{{n5sV%fc6hL?tHVflIIh+J0s9tTg$YY zeiC(}Bz|-?UGKVH$^UTO-W_#Mb=V{A<&o3$DVEDAv5Z>WrF$yO-4>?qS?Yv=6J4$GoB8 z4itd)$AUb+ZZ~}a^_WgHtfm08t4VpbB4|IW7{?10Sf<_dZLets96o-Vo}4xPRR4N> zmS}f*yK%jX?6IYQN7`q1=}Zq!tMOiLc#;F0!xPkwyjG(LlNsISHjf_7s8 zdA~c$v^%*c5AE+yPNMQOI zO4G7mPBBh2qku=+N8E>pQbyA!C27=xb~7}*-4S=R$8Uj$SN!$H@Cuv{|9QMk2kmc% z%Uu6F(jFoIn9c+8=7imMD1L+(UaaKzrQ!%31(s>|Mb8)eI>NNBWf-sG^!}4p^1~q3 zx>@S)S%GERKW5IJ-DM1c-O=+*6`U?{U^Y74Sk)n z;%9=a`SFG4i#2M3CjDoT8p%sUdca(`+#$cKdDC{=Th~95yzKp zRMX+NY?VfZ=&Mpydesb0y_NCOFL$)3f9Ua+1gDXX6tHx`Qvg-97HJP-rfMUqmF4-b zqeP^uHz;5@=d}P@q}}pLp1J*o#P6~RXxAE46zgGb?~=vlowmH5)xu|aZa4f19CLf~ zyzjGterdPdbLRFwX>8qV%Q@e6ZnxZXXm6eOeb(Q>W&5Ayl{~cfnejdM8rrp15SsNc zxA)9syS_uaCZ$?+L3_K5@404Z*CJH29%%2G$##8*c1=pP>Vo!m8Q*iw(5^+OW@M(5}d+T2;*L?K8i}7NA`XQN@a&y+;Z9e~ zu7UO*S!~~DXjhw%6)S@F_L<*f3(&3zsajRg-Xn|c`wZ=Bh$>bD?d>za#}=Sn5mL3P zpuI;H+xHpT)eu#z2-@3cevd6cyCS4&RY7}?EVl16w5uViSP`_h&-@-+fObVl)vAK_ z9$9SP=YDDbHQaS*Z=d-+wqQ8r)n-bbrRy=X>GNEHerdP-Z@e_IajKB3A&~ zHR#l?1KQ=8O$~Depk13(jryQ{o~e|`6@YdPI+g1PKZh%iEH$9Ow*vjrZupnCUuJALASKq?)lG8 zb@d@L)z(M@=hha3Y4B0&YLgpnXs(;Po>sRpOsBYnIe6y)CpW=ZI2uLxivA z+t-;&PUWi$wfa;lN;IfcRx0_@@Wk7Ubgb;ed5sSZ;bU$u@qO4zSsT|{gB|`p?9#0H zFgeZ2o-c@Asrz-Ey)-Hg!d$OL`^=P8&&R8^2eoqLevW#ENV-wE6(?wod8Q)i)lARXF2pt=9eb?`oXynICSw`c17|#tPbB9dlvl z>XbQWe>8mV)Rsqoxb^BcwTiB2uN;4Mfb4ayQvRg*(U+6wHaz^ptyjOPMa;C@`UI}H zRj=eL$6p;Fd!4J4KWTpS<>a{y5C3rM)o*GMGgb~(^7Uu#uMUvC&ebV%&i?4jxl>yn z{o&TD-_$BX`>R8()ww!l&eyn z{o&TD-_$B*+D)zG(`W4YqCFU)taAL-0kYS*O8JxKM_*2!+wkxYw_g3G7SSf{;la5Y z^udPq2a~c+nRE7s)8|fYdGv={uYOajn6c7-+ESuncB?=S?GizS=hi86&i-il+^H>( z{&4HnZ)z1YR;E_+%wbmKhEk@|A zKGm7`to>Gn=l!a6hW5EWl(bTy&b(*sRz0*=Z>=-5+l$Y8J*qSBS^K>jfA_4`8QQdbrAZr4X^HP<>ryS3=N*`GS|p0(es@@L;_ouU0x8{V~4pw7H!?Uuc?Rd=m3 zwA+f!yS=G1?^*lZI@i6cb%yp@8SJ(S)S36J-L{{WYOi&+Nju+)v6L@*zSNobtgY84 z>owIn+oYW*XRYVZHUH|&d)B_@zsIv`oipuxE5>6eo%vj6-m|td;RNp1I%nGXR*XPV zPDKH$3fQNe>vpTTgJ0ehfOc;Y!(SA*s(^jkxl4C7ww#9op9(TEvN9;ssi?DXC-encks)b0?_U) zV)%;!R~3NvtIBa63V2fh+Py^#e^KD70?>X{InF}?Zwf%Ww}{~{3S3nH+OI0dc_`pb z0ciIYG5ke=s|rB-RpmGj1-vN$?cO4WzbJ530S~nQ&R*g9y2|mZedjzB=u3eLn!h%G zmo9r%F%Rv1WhuSYQ2^R?bfgv(u&)5L+m8_*qJWM9UDIAVuOlY4pnz!wO2Yr`@4CHo z-gJy`4h3`+CMJ8=MdsM3LI1b+DF>M(|KR7g936G3b;|=3EBfzC-k{xCI3Zw ze0#!04t1x%cfj3s;F817(0<0%n`p0|!Vf02p9zt_W4~eVWNd$i_A}x_yFZxBd;FbL zXA;tU=hhwUn)V_~80$YKcjoq@%^R&*xZl{xd7qx3J$P|~rmw&ZP=?bh$LGB|T@osc zuaWrCK3xYV_)%aCaTRVqu1h?vpsAjdm(+OV+$i>ur2OcZOuX<6PH`JC|g7!+8 z_n9eka=72u#kC%#uE%Za{&5o5YTYeA0aW(tgLdZjU5z#}CLV$4^wYcIZ%BYU#vKv{oPG_7%wCw(qKzhZQt;&h01MPeFn96QXhG zz5*$+nA`WYy6uP*6llLq@;(|$i3RP3;+=v5?S_K#23JyILA#-7rl2&XJ$@t@USk`B zX-ENFX`(OHoE(9UE(RU z59*C${l{O4s+?D#P-l9+m@#!fI(wDwmxpZ_nk&bp8dp^@w@bz3e8URVnVz|VcEb_F zH59n20JL-6el>SE&zk~XX|HdsdD9$!QQ#Q`p#2%$X%z+TSHLUnns@2$SDhA7;0Xnw z{R!1+69t}70NS5Woi#QUIC!{@0;d#!_EU0kFbX(U0NNb~57$xPlmgIx zN-hpY0mlj~(QfPaR=FqdIK6P)lmbh%-|(+Kp3nf=Pl&~#D6pXbv~P&RE);l`0?_`d zaMBqR*ic}JcCOnu1Y;KpxK#k!-G&awQQ)Kk(0)=d4o3mE3M|piJ$bj`#BmhZP+*C6 zuG=>RV;2gzRRG%Eh7QM3;G_c3eo`zvDdg|>?0ch_oyzNjlt00cdZfEcK#*H3gvET14;% z1zITp?X8rhUKFsV0JK|+2>zfzD+Qpvm9o@}0@f6Oc54yA9~5Y%0JOJKmU>aZngY;n zEh6}X0<9E)_EySLFA7*w0NSlZ1b*S-N?Gbf0c#3CyS0em4+^wW0NPtAOT8#y zO#x`P77_eGfmRAYdn;wB7X_>-0PWTyfY0JJZNLJcIj&Z+}pHntqo8(EhG?(i;?5R{+}A zwP6nmyh{NGwEvl|+QXVYU21UM{;n*fH}@!Dp7!wXw@Rpaca_U|#_Tv=4;GKcP1x?Sr}BH;zC7 z6sQ%jPkXvGH!k3ZJ9f}shY%YD4l3Y@_WVR0v>%KvhoL~N0JPU?V57jB6@d0PN0+{# zK&`;DwAZb#*x|-pUco0LtmMnlb#Jgy00q_+DAfFY;1jsI^%Z-W_UhyI^}ewO1yG zRkR25zQxs@ou{qG6@d0TLolN~--@V=aL}DT(N;eSK>MAs4j@-pub9w&XGpqg3)=4t z_cHCFG)(7VLJxn&*PTAmRzC_r`<<~4EU!%HzrWd!h(0eAXrKVJHxPBT$}QUSFaYhV z`msZ%0JO{WJ9jp;pVNz@brpbiT|KvJJ4!oC`E4E9drbjozoy%@vleL|SMKo+U8pqP zuH(A>n!22|rvS9?DWti7f%c#9V9jtdexQ9cnc6i)r>?AkdD<5MiN7ab`@Lf4KH4#Y zH7V)iq5G1pZhgfLQ(nd)8G(nDqw8XWVH!|Ry;$3V zyTQo4!=)OJ#1*u+itSO_TeVNUeJcR%cgFu9?RUCFTOBE2p7td`LRsziifu58!SS$g zN_XW3BfcF=27iZJ^7k#S3~Gc+?G&goWdxASLK8y(MB2LZyDqx=W z-tN)`__-(FFOO||4()CA{j7ayp;AruqZtlL)BNYzyh8z*0?;ng@7&qYeoilr)>Xhf z?Mr}!vfA$zTURKR+Lk)AG`FiXMkOflr9e#zcj>;wQGXj~ukOE3_vfafxCyU#Q(nDY zDnx;(fO*>AewQxZmHSM^I3(~wfd&dJ(0*gG)vd4CON-x7wsLe`+(0d9mBI1(bHLwA z`Jg|zOO@vQB?W3crc8wPC81Qxj=&zj^P|!jm7u_v0?_^?NYl4*ZXdq_7pM4O#_zjM zKO!FsCbOvzRNk)w(0*s^2igz(xzi`w>PG=+zctpIq{pY|hS#kw(O5qUK>PEt9xl_l zR)0PaulE4jUms#Rhys%W&`v6#KraeFdoPiupC|zB$!P(WV+XxG=28d1Qe0?=+VE_j6k`U*h1zNXZO0yY(ZcAIg* zD-_UIz$xt?|5|0;hsxF$iyBeDwE`6#e~tb-a7({bRlB`ryN(Oaqkz5wH7bdA{Z`g$ ztXp5PC#OD;KlHP7f7cqN90fX6pk@r(J52(*-H8Iw-btY8CJKDq?P6{xAyDAW3NW|7 zIlA->1!@JLy;cJo1>URxw7)sJ^bG}S1)#lF0~-b2tN^r|jcy$pfo-;bICi@NbsD96 z@;|qEQMbNg_xp6siXaaZ-2E9hztD2A!kIClsjD2-=^Bz-pUyXbjt{{(B5?g>NJA(>pC_#vkr}6yFt&XvXKIH z8bNy_k*`%-hsLmdt#h39gaUOMLHiRCSZ%WojbVG$e|DHtpiU!bHx-^bG=^(*uGm9+Pkhmokq}pUB?Dz)}b+MH|RN4Hd3HYBWQ0V^0jK~&=|I_ zb?)OV6TSQJ=%sIJp`qPWkn7MGwiPrp(L05Hn6IK18rn^Ts}7A}TR}4uy;JCi`6_Cm zp^eKb&@sr|$TFa@|HOo$w${w^rKe9vD`_$k}Xg`t_4TRsT^5^GI_$REUq%pQJ z=U0Slbm~Ynn;#QnJ9@Mi<(v19d8E>4Eh2NfRML_5^l#QpGkr{tb$~qe2aH z1;RwH@v14AQdOtoL(bK$uh>ic*!}n0m%N?FjuI3&r+^ZZHDl0zXo%W62-?qOF-IR$ z0NU$;YRTTBveiClKbE~5bxr|jUn0CNYen={z0iIxi#ht30?=NQy<~4u*=irOAIn~j zI;Q}%FA-jswIX_}UT8m;#THHmby+K-x9Wv<_-vs-GupXs-@eWg#ysNM0j1+1m~Sfp}j?}Q|Bva-(e^0Ui>u}K7k|u`%(bf>v??1 z-XBWzk6>=^i^98l+l=-w#*?-XBWzk63UYF^!yk8@<_eJ4dy@mFg>?M1jP5mRF{jU7)s~u=x zBD^lsXL-LyXzz={yLt=lHQ7t{KAZYSK>JwQ~=uR zfojQC#rR-l&@N1CPG3|2+Ls8g%MJpndKk1XCM$(P1)#kqd&ySC_+VwwE=+4qUsM3v zmk6)R4g#ur7_=`YD}_P@puHwL+p6rIuMFCSY0c@23P5`%yf)4Qs(J>rFD5I6LIt3` zhCJJ5B^VL;KIH{~NzccOFpHGoXDIED979=!f>XSkOM#gpxNa0PT78 zUK^@2_4&$}+uzLGbA5yM8uDzP2UPV8Xn&4~H@Dmk?SG~yhVRErSMb5ebblMF?4GYI zx%lSHqi^>s&DS5BNum(EeWHp6ppCv=@9LZ_fCh`_?h-VJSacEHk(F3_w?X@0j-Afcp~ZvFziX+5K%F4 zT3oykhW6#0p!BT@K>Olieq-RcW7p!~^wt!lU$-d$?a+Q(veD4P3iLsHShFwa%u0Si z6pCJ~0JK}2+y5*LPA|?vx^|BO&~B0T#ihtS2|+6_Rsh7I&hh(YcYHW~ppE70V6*q+m)(`eni_gn z0NU%3D%l4{kcNkZVThaZy@T1L|InTYueS|RK2SBZH%WCW4PW~_kbb*|)T9jaQn)7G zJ?Xv!mZwU?EBVNK6mZ-Qe+~T^-4Zx-tb%yd#*x*o@yPnO!;^EREF4pvG47uV#?wn{ zA15uGmMZ_)4t3Q7%Ew*{Z-R3}Qi-{n(XLai*srMqXSA0joboWF-%u9*#Q(-koj}9h zx}cqXV^i%q(YoeHya1XuN5r%+9GPX&LMo$ySPS6uV{b&KkfV2)+c@Jxcp|!D3y9^J;#YurP|9~ z`#RCv+#cMI!7+r?7~n*E@UM>e)AqRBIO)?@im1LF$x^M-Qe}0#pW8TvV37rjm@4x?0hAUhR?vyXuVvcajgu zt@QrTHl4)wv;wuhJ>Qqz+NsWAlwYFRDVl`IzA z8GWdny4@UC@_F*;m3CYGP5@4rL;FQmE1E9W(BQPnsaws|m6Tixqx-G?UvDh+JZ^{H z_elFBpJMcDSuNOAf>>~8^r3RG}81GyCzus8tdE7$#w6eP{XF9W%)qo5eIH>w2ndS zy&=v=R}y~pHtebWuI;M=rpTvegyFAUvS448<+bP4$u-!o?P=YyIu-k-eT;wpTZ?qw zu4T1gpYOrT&rlxSov2l(BD81LrAxb7RtvV1Y`o7+Xz!VJ|(B4*GYDWP(3P8J^Sl|f?v{eAw+v-d0 zC}2kcXtxs!JVAlB3P5{XeW@J<>?i>3c4C1iD9}~`Xm6`8wWEL?1)$wdEbs&c+A09; zZS|#g6tJTJwA+aVo}fTm1)#mHzSNEab`*eiJF&nM6lkjew71ol+EKuc0?=+J7I=aJ zZ54p_w)#>#3fNHq+U>*wPf(z(0?^)8Uus7II|@L%omk)r3ba)K+S}?&?I>VJ0cf`q z3p_!AwhBOdTYae=1?(sQ?RH{;Cn(TX0cdZlFSVn99R;A>PAu>Q1==bA?QQj?b`-Fq z0JPhQ1)iWlTLqxKt-jQb0(KODb~~}a6BKBx0JOK&m)cRljsnnbCl+{u0&Nw5_O|*` zI||rQ0NU-u0#8t&tpd>AR$ppI0XqsnyPa6z2@14T0NUH?OYJCNM*)|#|M~f;{jP00 z%HjzMSW}=T<>%-3_s81nci|O$to^QSYx?333fNJgCI#B<#05`KKwkl9*VmL9QNX4G z&~7s>c!dJ`3P8KQrqqZ6HWh$&n{mM_6wp@y+VwT1Mij8A0JPhT3tpjsz5>v$uPHU6 zfK3IU-DX_y3I+5PfOdUNsSyQiDgf;^uXAlC}2|oXtx;`yg~te1)yDD zQ))y3n+ia?&A8wd3g{~U?fRNhBMR750NQQF1+P#*Ujb;>*OVGjz@`GwZZj@;g#!8t zK)b%C)QAE$6`0c=e$aK{haVp)`dRUXU!k;_On7Bn0cbZKAlyTNCJGFb`r3T@#N@u_ z3v+vu_)}?53XH!ny|Q|S_MTFKzP?@o=JwZzm=2=Aq(G$o&)Vvlxt%;gffp&jO8!N` zq$?<}slZ5k_^05Wn8%meTw7;wmu@qT>_mZm1qRyVKm52v-VK!HmNFt=Zl zjgwHop#se94ugivC~!#uXul*IC!v5t1*Qx3Y4gN|`d6FbZrtKM1n$x~%r0C$sld|5T32r##Q zCm!#cDDVx$J?|q%V~P)d(|j9?2Of(T@xaJK7$V{*@#jxGW;}kZU?>!nAoaHMuJAn>IthNt1^cZ z$3Io+)aJZ=C>h(VcaEnHe)S>%RaowG0V&ow7~)$_O*HGwL^0X^Hk_UqWE&U*V|j zHkm-J?^mEowHaXx$X0~TX#cHBs%oc|bam#IS&cpmC)U;0?hY+~oH}nKUI&=FZVx`M zxE+dX!fX5eSxFC@C@{~pYCiqQ!wTh*q?){`WR94uJU>5lDA|N<<($pp+JTh=$3v!;caD$YoDQ+VdGsUs zH1;s{A?&CLukH7zB|U7SK+LxBu6NwTe2e$L)3zE4>MQ2Yv>e|(^>^faqJ3w$i3(OQ zH4*emrNhS!F%n;|n(*4b(gx?CK>Sqe>opa{P2M}aiE71TE z1;crR`8hbmqVk$<)P+Ol^!^DS(S5ygrJ;OaIRX9IMyxI#G3}ik{;KP-w^7*PHGJ4O z*)`#{{g{_Di~^n%2pkQAK;%i+?;6L(wyxvUh62_VNE7!k{ZE$}^Q(hDuT3P6duEt;;c!HEgC;d0Cu)&guDG=nX6R zJ3~ra))a`Bu<@1r8t=5FcX^xl=aOI~%*L{ONH(VOj9Xbz*R{lI$|=RF3iIJpx(d-hJHsiU-W^fQ={StL%?jByT?b zk<|2kZjA0!JGA$g>hyS+pQmrX4REBDZhVt&br1G{$u@}}dq3U`RUiCw`NZA&26mO%=qNj=WA@8chBksCj%0Z#q7VSX{TK z9AMjv0*UtYn0%yRv?C7#L_;coqdbK%&43ahl=K}??z%&p}m{jrlVg91mFSy z!{^%Rqqm_T9+nGMsf_0h>)~|7a8@Mho3nJl(A@AQITGGX%}*uqkfDn4eLgsyIvq9~ z6c4Q5$2a5oW9d-$Sby5#j`pg5m?$7ppk`xUQH|1Uui(^KU55*{8$D3XM0*@W4jg^r zkb0@DX`gU`Ls0+)Y6Y6nUN?e`0w`cW0cbZ61YAIYTNIdRk2e->X^%!w00o{>V4^*K z%J-BDw2J~LaEk)aev8aBf&vB<;JV#F5O4tnZczZ*Z;_csP{4o!6YbokGY|`0K!LUj zaNXWkUus7II|{Ipw-XCIL4mdkFt@kWm)cRljsnc>c4C1iD9~1c@KjCw;yrii+R9Ar zC}2PVR`LddfD0&aivrMoi_A2F0tOU-b^}4c1r)eNf$6%PZ^hiAHjSXblL}08dw`zr zz&#lU+D3u)3P5{%jcEY|Y$(8WyNx*D1q!rRfa~`58q)#_*ie9#yp1^E1q!rR0NUGY zObaMrLjhLuHsXL6D9~O3?$Wi_m=;jLh61eQZNvdDP@ug6(B58ST0j9C3UJ+SBMx|h z0__#xy1l)|w15IO6ksK9BMx|h0__!GZf~zKEuer61*Yrvu;Dv!HX?!-D9}m)=Jr;~ zQZEWvQvlknMFf9Ppp^p9-bz{OMFDFH@Oitnh~N(jv{GQ2+qp~EN?__m0W%6fyO~hn z2nsY-fR%h>fvFw^%qYNJIy0fb5fo^w04w>%0#iK-m{EYa-ApKO1O*x^FkQEEPrk9v zRF4ABE5O|Td<^ga1@2LR>-KwerWF);UIFIz=VO2eC~%JgeBOSK&a{F8&nv*({(KDZ z00r(*fa~^qbfy&)cwPZ$e?A6yfCBd@z~}Av=u9gp@Vo-trF%XGcz^=;D8OC1dvvB1 z6nI_%=Jw}ffCngWj{?wskIuA$0?#V|?a#*m4^ZG91^5K+9-V0g1)f)cx&8SV-~kHU zqX4wuqcg3b!1D_53EcBBzylPxM*(QRM`v0=f#(%qCI5U3@BjtwQ2^TS(V13I;CTgD z$v+F`!X3h0@{rP!l@g~?VbV$?Vh4#&sS)- z7n605pnY8%_UtPF?fV+pIRe`4#KNN|A%D1TcM~{hcM~d;jxx8K49kjB(7qxJ#fKDt z_CqomISAT~1j4Bs(C#U4(C#T(27Tqa-C$7GT!QvBWhg(Q0CW2hm8={C?N%b;(+_C( z6gO!16fJwcLc6_~ta}9Q>)NnqUxAQ6eBQnbW60PV+gGI12Nn+StjCs@gQ3LUh2ik3ZJq1|3g z);)sub#2(QuRzEjuG{xzGI9j88wrG0H%8d`_uE~>587RX$*hyC-`r6*jsI|>`LI|`OTSGjIC7?f3)pnX*ub{tTE zmHYvb%p3ykWHz?Y#apbHsavc3x0*tQRtxEQLqfU8uDlO zrK&$aKL)+SrB4N*{ZkL#om7C8{7J!{JRI7eRR0BSL%X9OLc6128FY1moqs*TU~J|t zLHk@2N{%ZK@`vm8<8nQGB(y&q0O!qjJhy+N;~fo7G9-*Q#EIx)Nb1nxWZ z9zK(p`TTT&D@bNyx$<7a*)q3l)aI#<)brs&Cj5c+84dddo&p{|WKOXjw;m#{D{u(t z(Dks@t=N2RU$J-5|9$*!KEiyIYX?0>y!Kzg?tCRbjq(u^1uZazXQAkFLX8gXS|x}4j+#{OoO_nT8r#&*?t zjkvBNd3JNLlGWZi0cl>3*NE#1(>1M-Pt9*A6 zab+pGo!iG$_VujAK7KbJp`46utMeFfWkouseIz~o^)S@m%<_J7%E{Qax-_qQiny{O z&^`qp+V|m}9ig0zZL5R!tbmUTSH*Pjn7K2bpDs|0NBgQ~VzqD5S_>V|?O)Ni7MTTq zzC3>Z?ShYXi)M1!b+wge-O}Dl(3bVi^7ZM0mYt<;eFdOhU(;qaJ`MM*{boI-;wB0} zdlOmPR63*W>4G-hq}EmnOtgoem~Pbq^{OfGG~BcH)p|xnws%6` z&C_tt+P5ti-hC^;O8#3B!|!LbJzZc}D_mPpfVq7^6stu~!#!)?s!;f~sK5k!c({h^ z_C>Thk+_U!WNP{Ow6oB?4Dj7LuM%&W`Ms&fAg9to?htPis;InA@ca-gy3uwx^n^< zC;;sWBHmTS({Rt)zpLZ4qoV+{>*#n(Ei>AlF1V%FG@_;e*X?Rjwy)@ExM%IR?=mfD zD!|;Xsb=H4X0$zB(73-;udD!byRx7yt9u&mS^F(JOWo=Ug!ui&SJJ-vE3`AC?dbxAK2gPu3UJ+iquA1> zo`!qYzO+M}f3E^ux8JL`sI?hwPZx-K!^yWRz}$YjPWaBD1{7c=Zy*RqU3ePqS^GyF=QtY*Ft^)? z!yzwbv^`yL$ZHNVqX4v<3B|r6Ps2THf8S+}u%rOATZ+bxFEiSnF4*yx-NqE)y4_en zw%mCd?pga=&a%&*0?h69VzTPdjJBr>Ry}2hNd=hOO@?LJsi)zdwZH5prB)SSZnqkl z1;1vrJzcQiBSm%<2d>{v!~&nwO?Gr@}K~;dx+BHOt3d&1 zca#QC!#!)?k#@KW?Y?qgM%&W`zU0GO=5|Lp@HE`B_8n=5t3d&1ca#P*+MX_Oq#dq8 zyRRI08tz&9zU0GO=5|LpFr)410!P~6YES^$9i_q3aL?Lzq#dq8yRRIW(e`wKFZuA6 zx!q9?JPr4(eMj2iYEXc0#W+fX8EsD&IMNPRBkepV?Ztp1AbhUE@nAeBL$$nqhQn3&J}?6&J%#{cccKccNA>8+PMPI-gyGh{f-oX_Kt#0 zS36e#+B;7Gy5Erk(B4t7>1yW+Kzrv2K=(UR0NOhWHeKyp0ch_$0qFh<6!`iVySCo` z#@El!3pz~~P@qQzw&?!e2kkv(2z`Ey0^dMyzK8bL#GQ`xsK6%O-#ekb#}uK@uTkI| z=*{=g{+hVckscM;r2Bg(wD*`I^!YUkd;`7t9@<|McRJFe0-JPy?}YXqQ-nUhMuBgj zH{V11YvN8vdQ@PO?(dz@-eZc;=hrCk4fN)FXn#%I=}3~O+^&iO@+gOGv7dOzK3>G z5ry_T9659A!c+EC!>Mz89h@~fV)&pv1R!YFh=_DYse25 zgI{nmC-YS!$g-bfPan}uM-K?CG4LH|#_=JCNskBmp*@UZS0PX5d0;&xUN;@xhfFR7 zb9*%&K>L2m-F<{S#hnam`@+hlfOcr#&$9=Qkf%7br{!F@UHM=nZ)mz=dEaDizab(g zn}GH#n8v-DT-hs)eZ;FHpxs^$KzojfTngrPXwT_n(U7M&D|uG(73A4uZnwufwC9+} zrC@H)tWRS4mJH)y=TBuf1FfXovRwJbUm6d5Sw(#O!M< zmjc?MeLv40Jc1O@;v+?tV#TOf-Z!~Rx1!>M#qtzqZs(r-68q5pAl55Q%W41fmIuh> zQm~S*u!r^)EMDwg+6XkStB!D4xevyx{ezr;SYKZx~8)6kxUkxRi! zzQP{bb7q+_+skB)pXg(5uLc2V z-%q)_kC0Dj&|XHyw>)UHl7$t^`zExnsQ6$pv}f_rYG9DCtud5+m@4IZ7-9#Za<>XKIHibXx|s~?jz)LJG7V4kn8pgJO5V9-6$Vu z58AUx{K9mWV#O#ZAF!C)S5$nk7}~S=aw)h=SB(PDo-@mgAy09>-_AlITTb7=pj-G?Ib$UA|cn~ts^UKPuw z@R8e9mrt4%%ljtR?I+c};c$71I~msYwUkQ%?bRT_b^CtG-F<{S#ot4___qk59op{( z_*8px+Fz66NU@hW70df3EBQS+@82JyHZHlY|8?;p%K>HOO1x&^TTyEt@#+Xx@+&Gn zSWJp%@sT1+v0_v#@0-xRqT++aA!^rW_2JC-T(?V=_+@Ud1_5Y)5bKqu#jwBU_gdsq zKs&UraAu=ep5n~yWpVVDTNo?3S+Trtx|-Xi%Q`0zKzq(?xfJhN=VVbnjD5tbBV5s5 z2JCBAH?+TI`3mhHXfM2}Sl&0GeL=y8i{vTpWLVqRSuO>0do>6QwEIX7dE|ZclW)b8 zQ9vGZX#b|&$HGq@c{j9oa)Y8!y=KW{4(-ssmR%;wLwK&;m&j*18P@jI7Ap}xZ-4hq z`HJOzv!|7OP-8#83>*RNIS3lRROKV+xyvVweZ;FH#_7O?Ao6kE4efc7zGyh<=ze0$ zW6s0QuVk)~VTE+6d$=rTf=Vm8rqXfGd_`0*h~@;1luwBe(lD&LC9Q~Te` z*u))5BRhft7qx;`PtTA5YkvR?vQ2wDw2lu#}WNX+e7t z%*lf8#W1bCKLB_6@f=ty~C_y*-`#=>#Xkyhca4C+{ri4Kbgp&S?eh?ZrOc zLJmtx*wb6?(iO>^4DrX)fkv|lChj`N^Bt1Op-PvDZ6S<61=cD@yp0`F#3$bAN}Mza5+I0q+o7OYG!_z+mj>~wmB@-Ky}V!C13P%GQ_t}yqs3h-d^nEE#$D2 zls##&k}rZm`;>V(t)Trli|vohVJRtl*5bN7D+cY*-k#a(Ey!tSC7)sEx;-UgPAlg2 zB#DJ>4ogYdvlerER*boQ%ITa|&~Ab9 za50tT9=9{HmM#Rz-sWNZtl?ysmv@wZE2gA|lc2YwJk^}j3fhw-7PdJoC1p=q+@&jm zLHm@`Ijx}G0_Dkv9F~%@CoO0%f!B{8#>%u)GC`fR znA?kB%4Q? znV`;EuX(zLe;G{*MsJu5av_B-5$;(b%*1qDV?Ty#A-o8x%esF5}teT|OtFFL2}$h4<|aqA-GRmXvx zRxY}pz5NW^iFcp0f1YSa0*#JBdkXI|?R6@sGG({2oK}WXeO;~WZHnXRZ5`sT+Ui~F zF5I){%eB+p3@5OX&#GnG<7#d!9oBXC#L`eJsrSE~(Eg|D=eUwL8T}lVoUx}3XwQ;G z+NYIH61GYMLo~K_CH1}++Vcc%JNh{+Ib+Wn+>|K30*X>!2D(FdAWAWApd$GpU`|hsWCpA+4jYU7Xl@s>7@fzB7R`Ryvy~w71 zVNNS!>Au|4?CtsFpU$X;`Jstaqp|KMw=VaSPv9!)!iO&@pOUb-AnoX4n|jaOZYtb4 zELF~)G+#5ft9}PA2^&9sH5Kt&oJqa!o_1T&&tW-_yHKf`w7UA=z$uFa4H->#kL;dCgU zz!lA)eOh3YrH!Q($C@u15R|&VYlHTpjUDSp?VkL&maG2S>EunGg!$4-j;Fu zr1d$sSCNnAdc$|`k$045=}Kngx!r8MZ_2NnR)$i2z3bWA63LTRjr3RS>s{+E+_OjC z(f81RW*vZ`PWSE!F?R?&zVdv?ZG>+u7>V$S5zrUyOjaO)g_5^v}7TP`Fp1m!RJZpLA zrpEW%p}k~=lLcLdvYCf%Ps z0mM|Kc#9YZ^x8Zm&+VH>nb@fi_usIRx6gZ++EZAr3~1vyu@Vzu6FH$GMR;`f1yYD~ z98b<^vaSI?Jhd1(NzyU-Ob=II7_0P&xQ6u>LjKEIYRp8=m`w_Jq-hrpnv&P z2jkj4?J6OUOFCXbfuyDbIRfp;+nmuC4EsF|6PiLt-!ZpqC=rsxe$ek2jk1^yCISuG zV;y(sDr!Tcqw$i(eouF03Z1wxpnsq84lQ=Y+C3nP=^`_-x8LDB{N6>}qmzeww02;u z#6?gyb5ui#c(Z1+k$!fjv~nSc9D>kI50RI5)Wz+{eaw_--*2doBl@G=LtHgz_t4J+ zKBfd+Tf-bF_b|X8kV1S&PAeCJWN(jzc<~q)OBkJ}YtlDf#X0h91@f44k1j_r4V^A9 zkVoD{H=#YJr-~HZkxzO;ds4#ERz9qiU#hayuM55?NCEAW?paG#@)t6(*(o_Jil$;#R^hD`xL6ICA8m@yvJI}VL1f$;z7_}WO0$)$1+296f1d|er?W%_N=Rd z6wK{O%}jeLu)LK=p1X9G zlAW|%bW`PHC$8H|YIvB*5|K@}%<2<>M{yyJLi&nhcO z!9Dq;W~MzASl-Gb&r05Me3yKa$DHf-q-)mF1qRvMvPe5=x#*_Kjc8W#B{f*dPrT>3 z-Gd^~UNU&kg7%`2hulu{Z__QbHI$?!D*sC3(4K`+kbeJRj-Wv?Mno9%fGunsJNjeu8$RTjiO_dLsS;?0{ zp)=a!%pM*x`06#Zs|}~h70HJ+=Jo=6Z!7sYTm1QrDH#b*sW}V)%!2D1ZIGJ$ow?DyTAb2^XR;5yyh-lnUQw;ymmmob`^l*^3+6ooZ+Jgozd>( z5>7s>z2`0+vnRY>yx1Cz~zBY{Sl)G5Mu#&GbN*?o$uiJ;4^9hNXb`Pr^ zdE}wJ0?CCbk9p6u#~XBs_TTVkvRz=1z0DmsYq;npb9*%o$z$Fx?ZfT4f(W!%d1c5{ zNb%R)rAzZs$=cJFhjpuB_N9C9aa(IuUAKEs9uchOB&^3^yZk9j||4(H>C5KMxCZT)m2G&Ud{|>{ujcq21{wy^Sk=&OEFgnB(4M2BAVo)W zdzNk4fc9lwo-I{KaXxP^lRI1C!fG&DGV9R?Kv1Kw0oG!a*rDtlbGS@ zt%i+gRV|Oai*7QvR|zAJIdglKa}tx4d{)GQ!Q7ss0PR@{xdH8R4fSh)_N*ziXDQ^y zB<3Fv3;FsVy196ZmX*9bW$vv3+7 ztXy-K+ZoL5?X(8qK7q?}PGUlPR>XqA z+@7NV?O6)BF^S33HF7BxYk>AFCcjIUrH~uU?Q#wEYk>BwDYR!PdlnDcvlMb8(=M;7eGT%+I~nDnJtvbq<~&{V zEha1ZtcV2z+H(}3Jxd`s^4#v>(ncP6=5~3?+*^Y@=FIImV$hzYFk~>d=U_m4mO^gi zr)%U@wXZ=QdFJ-^QPOQe9&_%==ZG-3XDJLB%6)X<)#Fl4Zj&%uE9EQQ>F zcDaW7H9&jTRE73jTjvI}TTB3G&rwmK-NR*S?r}o{+8c^{xoT+7(y7p%YwO&Ac8duB z?Kvtcv@^GldO~~7D?|UmDk`)yw~u;4d(JCE2DIm3Kzo)#Za}+SL;V_{J!=Z>Sqix^iODx!K^}Ro+uKJ;w*`63xo*!9$+UBqE^G3 z0PR@{xshp?SJl1-dE}Yf+eb;a1$oSw+jB&qJxgK8U~bRBfc7kf+<o?O6)B0qt@P^=p9ktSPi-Dda|`U0zlD8sw2@CEq?ux-H0K z&UJf^NT$7)>vj+Q48I%p^DBJ){GDDo)IJTl640I_pJ~q}h7Eb-ncEGAchNO@%%MFC z3+-77Lk4qu4hFPmDda|`-NOo39(m?=dCJ^dgFNQU?Kxu5o~1BkFt_JmKzo)#Ze-f! zRkg1{9(m^W_EFMpK^}AF_8gH+J1hCD$@2ztdyWCLXDQ@Hrd?iD`x@kt?`UpsAC6mE zkjI?4JqIe&&fK0gdEU^N+n?`+iU-iHBI8&|rd_elW>qOf-ov2wN9F07W>L7eVuhIh zI$6=R7Z%#vYka;1=Ju=^v}Y;gh9lYo0^;XMJNh*Xd2X1fK_TW&f_{VC-O=u1DX%cD zx#;JC1XrOw=y{;2_aBmJw=ahR0`c~&i;l{3{3l%e`%m8xF8O*c2K@zEC`2>Uu25Iw zD!K`LOitl9;D%xC1sSfPM3fTR2RM79-9y}!Qv3}Wt}yHk}rwCWZZ|uWF#U z!x8P_p%YR1k7Ilqb;=M+3(78*OqUVctapav4ppfD(p*?g%gZ7l}oo_p% z{g03NP9epejBC62DM;br&g`xZDhG5%yAuW!Qrt;TcavB_3TRI%Lwi!f(pDkGp}kxI zyhRIfibA_s!Uh#+(B8nUC#qDCg1J2f4%(9vmbMBh{+9VnK?-P3xt(eEaQjLgc_$3W zJNlXoSCHa0S|n&MZ!_(eYq;@5dE`5y{l>VoY)a!JI44nWDdSjork%OHz1W9akPmCG z`A)Zj6wK`@@iOhH!17iec|LEq9N#71np!jJ*&IT#93cp2k! zpPLzlb^|wY*FJemb0#r^3eWVbg#86k2$nw z<)J-GVaQNOajx6BZl7gzXS6$E0NQh+DoDX~dr}kHlM_PTJ*f%pNeN3^g%oFQXKtURG_=noF;D>QIS2|; zFt;Z)p*<;KX{(Up1|F7DQB*++Xit%V_N0WRtvtoOeE(-xb-5HgU6V!bjP~%42NCE9hU1(2ASlTM2IJCPPb0#r^3Z-ZL7qNd9&^6mo?}>O&qdD}Ym}#Qapc2qJQx_%_5~Jq^gS7_ zAO&-K%DBF0_Y!x76!$T#r8HdMDoEia>gytC?SXbJB{l0&NO5TAF5N68p?xNafdXjH zK~Ru_xjm>kjQ-G`^U9E+kmAhkHs|*3J4c&Q+uXVLi;E6v=Q;V3!aLS>Y8V>Xc3e>y z_)N|1@ka61E66e4NZvYVd0zm2;}vygZr{naM|LZuIJB4H(2^GGJciFve!~aho3GfX z@yjCfzhO`-uc7nh?W91*wAWfh`w^~}8-@0)i-Ht+ZnqKf*;l)&f^%1OdZ=k*C6}7Mu@o~sm z_#1*A0zdp0He&`1xiAz?oe5XS>c~YF}x(fSvd6Oi86h zQr23;+e{mZih1O!9{Q7kM)Vr^qF3IhpOhF2p zbNf8!<|Vrkis}pmZyj0g`A=fn$c;qx+V)5MpwU-9Qz~81c}rgLzU0>_zCREC>Z#bJ zrz%j8!WixK{rKU2d&<5@WsteLVLmKvLU9s5ahu*Bc@`%h8;!978@aJYc`9#5dx*e? zDi6O=NO4;ox~`{!6pOS!7x9E_9-Zqs?BhZCe1w)UHEkm|64h&4dE~2pjzjFI<6vLU zNcA)29j!B5-Mu?2RFEQRVI<-++UtTNv-M^6({mgZ1$Tz>tF1N4Q@K3yRX@KgvoyJX z`*tC|RZqWE_VrdlibDIRjt@n=hI??RE@(-6fPHMrMs6gk*S7M=SN;61?4j5%viI%V zXO5Ov4|ot8IKUR4y-S$DIYs9xL3A+XWUU0LNFO{>@4rmP>JAcZL{(mEr4 z(3W2DQ@8kTVZoUW8Moz>&+;ShVxXbjV!A7&xRY^hSA7L3Owk^}UVm*zO>k-eJ+?Ys zMQm2I?w#(QwK+YN%Ok(7rfzQpwaGhbE5o0a;x$?bWSmaTv>V_(QN6a6LjdjajJdZ4 zdCcSG>%BhPZ7tJoz{NGnQ@K3yPU0@_=xZ`uL5kOCAx9M2 z(7t^i+(<`naRb_Kk@;LB3MuYnT-((}K?>&ftgkY?zVS2pxmyE~W4>WNzQr=R&Dek1F`)MnVIp2ZHB2Rf`BRA6Z`Px(V$&XOs%S*y(kIoMGLV7?P3Y_E0ELvhq=96 zxW7gVX{JqiWkZj(yK|{r4uOq+?gmYxqXvFwaJQdF4q$FSqSC=*nA;Bub?0Gn+TU|$ z3fdJj584&Q)T#>Fi;~b@v=Cd+E|yTg0%$LqLVM9dYE5}%<9aq-pRaA@5ZLJFZqVc% zWhGy3(!EEE-Ne~DfVq87p$GOudl8+vy=Wn}pj|AXeg)8;HC2#;xxJ`4taxm!aMxDw zsazg;8~x1UuNm@=z9++(+t;$>eEF1DHYi9`uWjWJ*y!hOnB*OWcCOnO(u|e-f+$ZH zL3@b+b9>Q3Y(cwNLj4M$y=V&UMGLV7?P3Y_D}eT*DYO?Y#1^!RCDg9~+KZ;pUbK)} zQ(oD)d$o3VE|tq6u+h)mpvgPRT{`a4Eu@*Nx&2QS!b>K!FQkC&BIb6*__(579C>J0 zOr3kG633i>zr6$s+KU!uELQR*6wqF@5L?hLmQcR}XfK*Vd(lE{LAzK&{R*JHXbSB` z3$X?5VhQytfcBy(v==SJ7PN~c)UN>Ai>AbP7a?$cb`%idP1NJ4;$tW-HD06$AAJ9IH z4&BjiKk`3+e#Bq)`wey2@9FLg6*?gv;RE(zp4e@wivxP2-9g;rB1W9!p`S0va1AA( zy$ocoXs=gi*X)o!rSC>9l@yPa813H)w^XDUa6i%Liw|`}yHemC%y^-v*M_o>vtB~` zN1ragaS-)e#u!huUk-LT(E|fIw7=p$U64X!rsB$O9j<$9&kMlVyCm zYI*4CZYb2GupjkZEi?}BLwg#TuBLxHFJ_O1YEA1(G8v1~qO*9`sk-!O&6J7471OPM}>UYi_Is z+7-oyc11C@s$y<0NA zi>A+G7Sj?1^CxCgi5vn; z{VXw7YACl?b zgXFYxPriKI{vIvFSqSZ73H2+0_M$0sd(lE{LAzK&{R*JHXbSB`3$X?5VhQytfcBy( zv==SJ7PN~c)UN>Ai>AD%^?L}*7 zFItE#SG0>G?_|b<_LA4)nDgZ9x0seFNZ)vyZ{-kJ>Su|;%#wHXJ)bc{`z$@K7cjS9 z*RI)_&|aj4_M(N@f_AZl`V~NX(G=Q?7Gew9#S-dQ0PRInXfIlbEoc`@s9yoJ7fqqP zXd$*-(az`X+aI!5d!5{m+|OTV-`D8y5w2(#N8Sko%r0NRVD&|b68ekWw@oKc7VPIQwC`(l_y||DizDxZ0p|9Szv7tl zEZw)5mMBP1&ds-S2rTup#9(I0JIb?k)zZgF)Msg+U%=e1uc^33XfF~nw-+tM7PN~c z)UN>Ai>A?+KU!q3);mJ>Q?~mMN?=mT8J%Z z7fYyL0kjuQp}lA!wxC@sp?(F>UNnXFqJ`LkcCm!|6+nB@6xxdxVhh^E66#j~?L|{) zFItE#XctSUUjeiiO`*MLA-14hETMh{&|Wl!_M(N@f_AZl`V~NX(G=Q?7Gld4?R>wz zdis0Ye9g1r&@N`YRN#tsapavaz})_wpxZ;+&R!=`FMV!iL3=Z4uU8E1MLY2b&o^Gb z#k4f~rSG@Sw?C}pcV@A&+fqbJwq3BG{eoy~r!cp#Df3`Cb9)gV+KU!q3);mJ>Q?~m zMN?=mT8J%Z7fYyL0kjuQp}lA!wxC@sp?(F>UNnXFqJ`LkcCm!|707Q#^1F29hUI&- z5N8^+izU>rfVsVB%G_SG5L?hLmQcR}XfK*Vd(lE{LAzK&{R*JHXbSB`3$X?5VhQyt zfcBy(v==SJmMhwMmae+@v~BV%UD4+?TJ%M`c${|F( zA58s}j$5{%y+jAvixy(b)ky`?jy~qshMGLXj8SOrj<2#D(^vh1haMdwCf0 zqUB?1{{!tMuc5tYA+}u6E{;63ixcMV3dAwz>6#KUSG3E&6;nEH*X^DdL9hmu64go9q5_u;Uv?~Uxv$3V zw$tC?-a>7UtJef&x%F_?nyEGlnkp{xK5fmv4UE0b`U)<5(-?;vJ#cP(g zXA|ET?c=rcgxyGc^>@0mu1d~#clnGV89Jlg2LKARI~mvZv9DZ>`_fBe9;JAT7W-;dM=T)#?Ug`QOaQIx9!cmrip=kqs0=yLs{u-w8q0e%j$P7^w3zzuj*xL2XlJ~$J^%i&pYz*D=hgHZpe>!=>{C* z<;Qpl67JQHcF-akOOh;=4TKjZUv@7&#ndBp-E-(mA9_h=*F=4zY zX=K=f_7WTgE=)yzoU_IV4}B8^dzcqa2sAZiN{`KNsr~AkKU*FUEI3p z=FroN9W1AL>FR2<(~j5Co;o(fIf7UheDFKsTK$a#k4J1DSDlj0*h+76do69HvN2Pz zxJ$f$B$2W1opR^Rw9katjN0bTI1X&RhP}Pfz9opAeb8QF@iN*=#~!eJ(Vix*tJ|IN z?*aXnn}qhF3$zz4q*iaVZ!dSGYHFPi?L|pwFIq?~U$jdj&)hD}m;29`#yqS8y@b8F zxV~t=x`I#Vc?rA1+|KvgQ_!J3k0QO%-pxur|IkqmwNrz+T`zFHXqQI5qq*HnS9eoc z8go|iMdIFQ?`Cd?c2@EQ^U3l>dkNq*%L@WiRIYV$Jj+X0XBqijz})_=$o2QoeqFnE zXF_|?R)uySOS;nOMwTzyrI8O04L8zJw_0c~dP-v+mO4(NUi#e1@{6I_&xiJ1i5@tBxxI)k9pN1<=}M;?S$)y&Wx|8@ zlJ3x6w2)f9XfKPiil4b!_8?QhURJ{EYQz5Gk|m*@Bhl3cX5HK@Qo{$OESNAIXz zhjup~y*8HA744THuTJcQ_BdqN@9KVXhfY+%yKKHeihZ$mkMKtOki=L2#ALA__td*# zdOr2rqV>V!*}L(#eL(nu)ysf?oM=Dlw**<)dGumlesu)gi`zLmMd|mH?xPt3Vn>*7F?dGIw<K(L0doPryes1yb?tRb>?TLb3w(ll-t9Q`;=T9$LPCrKlws?5=K4^#b-bR6U6TQ_t zXoq%a-y-Bq`=A}#q5VxnZ}kP*p&i<{2zk>!Xoq%ae-qJLeHm&0?fTzu?=9PV@9!25 z@7~AU&fMM`{qH7vt9Q^2?a;nO$eZ>-JG4Xln~2`(3$#N!v~LmerhU*3?a=-vqPO}2 z?a&VGTZFu6AGAX|w7-eyt-g%3^Lcx38Q*h%w|IE>KIV4j_TI>UH_=jnu(^~P4rgppdH#xAb6t_TRgmbAGAaJji8xm z>fJjnu(^~P4rgppdH#xAb6t_TRgmbAGAaJji8xm>fJ z!5f|U6SDr{SMVVJ7hS15ie}swyo{)ZKjH0U#rJXVSQ?K?=6K}r zHWbB^(mBJM@w`y@JG`bQ!e+2Sd93Qd>(D_5>Ur>x8xxJD3@Gq|l)X&LNPGA@j3Qx- z$OW|#28B3C)J1c9filter_mask_topic; + mask_topic_ = joinWithParentNamespace(msg->filter_mask_topic); // Setting new filter mask subscriber RCLCPP_INFO( diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index 72a0120cd58..9b597962d10 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -273,20 +273,4 @@ CombinationMethod CostmapLayer::combination_method_from_int(const int value) return CombinationMethod::Max; } } - -std::string CostmapLayer::joinWithParentNamespace(const std::string & topic) -{ - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - - if (topic[0] != '/') { - std::string node_namespace = node->get_namespace(); - std::string parent_namespace = node_namespace.substr(0, node_namespace.rfind("/")); - return parent_namespace + "/" + topic; - } - - return topic; -} } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index 852b4b29b5a..dba276d0364 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -116,4 +116,22 @@ Layer::getFullName(const std::string & param_name) return std::string(name_ + "." + param_name); } + +std::string +Layer::joinWithParentNamespace(const std::string & topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (topic[0] != '/') { + std::string node_namespace = node->get_namespace(); + std::string parent_namespace = node_namespace.substr(0, node_namespace.rfind("/")); + return parent_namespace + "/" + topic; + } + + return topic; +} + } // end namespace nav2_costmap_2d diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index 71d4189994a..b9ccee55d04 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -146,6 +146,7 @@ def generate_launch_description() -> LaunchDescription: launch_arguments={ 'namespace': '', 'map': map_yaml_file, + 'use_keepout_zones': 'False', 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, From df92ffd4fd9d860cae0e10a3378f3c6189b39c80 Mon Sep 17 00:00:00 2001 From: Semyon Date: Sat, 10 May 2025 03:08:46 +0300 Subject: [PATCH 66/70] Fix lattice backward 180 deg issue (#5141) Signed-off-by: selazarev --- nav2_smac_planner/src/node_lattice.cpp | 37 +++++++++++++------------- 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index 13d052f4c7a..d9a7c8ebfa5 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -499,7 +499,6 @@ void NodeLattice::getNeighbors( NodeVector & neighbors) { uint64_t index = 0; - float angle; bool backwards = false; NodePtr neighbor = nullptr; Coordinates initial_node_coords, motion_projection; @@ -515,6 +514,24 @@ void NodeLattice::getNeighbors( motion_projection.y = this->pose.y + (end_pose._y / grid_resolution); motion_projection.theta = motion_primitives[i]->end_angle /*this is the ending angular bin*/; + // if i >= idx, then we're in a reversing primitive. In that situation, + // the orientation of the robot is mirrored from what it would otherwise + // appear to be from the motion primitives file. We want to take this into + // account in case the robot base footprint is asymmetric. + backwards = false; + if (i >= direction_change_index) { + backwards = true; + float opposite_heading_theta = + motion_projection.theta - (motion_table.num_angle_quantization / 2); + if (opposite_heading_theta < 0) { + opposite_heading_theta += motion_table.num_angle_quantization; + } + if (opposite_heading_theta > motion_table.num_angle_quantization) { + opposite_heading_theta -= motion_table.num_angle_quantization; + } + motion_projection.theta = opposite_heading_theta; + } + index = NodeLattice::getIndex( static_cast(motion_projection.x), static_cast(motion_projection.y), @@ -524,28 +541,12 @@ void NodeLattice::getNeighbors( // Cache the initial pose in case it was visited but valid // don't want to disrupt continuous coordinate expansion initial_node_coords = neighbor->pose; - // if i >= idx, then we're in a reversing primitive. In that situation, - // the orientation of the robot is mirrored from what it would otherwise - // appear to be from the motion primitives file. We want to take this into - // account in case the robot base footprint is asymmetric. - backwards = false; - angle = motion_projection.theta; - if (i >= direction_change_index) { - backwards = true; - angle = motion_projection.theta - (motion_table.num_angle_quantization / 2); - if (angle < 0) { - angle += motion_table.num_angle_quantization; - } - if (angle > motion_table.num_angle_quantization) { - angle -= motion_table.num_angle_quantization; - } - } neighbor->setPose( Coordinates( motion_projection.x, motion_projection.y, - angle)); + motion_projection.theta)); // Using a special isNodeValid API here, giving the motion primitive to use to // validity check the transition of the current node to the new node over From a438883774b357c034f8970a2223150f888e46c1 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 12 May 2025 19:33:19 +0200 Subject: [PATCH 67/70] [lifecycle_manager] expose service_timeout (#4838) * [lifecycle_manager] expose service_timeout Signed-off-by: Guillaume Doisy * restore original change_state, and detect non_default_timeout Signed-off-by: Guillaume Doisy * lint Signed-off-by: Guillaume Doisy * spell Signed-off-by: Guillaume Doisy * collapse change_state and remove non_default_timeout logic Signed-off-by: Guillaume Doisy * Update nav2_util/src/lifecycle_service_client.cpp Co-authored-by: Steve Macenski Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Steve Macenski --- .../lifecycle_manager.hpp | 1 + .../src/lifecycle_manager.cpp | 13 +++++++-- .../nav2_util/lifecycle_service_client.hpp | 9 +++--- nav2_util/src/lifecycle_service_client.cpp | 29 +++++++------------ 4 files changed, 26 insertions(+), 26 deletions(-) diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index 2f3aa38bf2a..8c053bb81eb 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -239,6 +239,7 @@ class LifecycleManager : public rclcpp::Node rclcpp::TimerBase::SharedPtr bond_timer_; rclcpp::TimerBase::SharedPtr bond_respawn_timer_; std::chrono::milliseconds bond_timeout_; + std::chrono::milliseconds service_timeout_; // A map of all nodes to check bond connection std::map> bond_map_; diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index de1d918ae8e..34f2a793c60 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -42,6 +42,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) declare_parameter("node_names", rclcpp::PARAMETER_STRING_ARRAY); declare_parameter("autostart", rclcpp::ParameterValue(false)); declare_parameter("bond_timeout", 4.0); + declare_parameter("service_timeout", 5.0); declare_parameter("bond_respawn_max_duration", 10.0); declare_parameter("attempt_respawn_reconnection", true); @@ -54,6 +55,11 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) bond_timeout_ = std::chrono::duration_cast( std::chrono::duration(bond_timeout_s)); + double service_timeout_s; + get_parameter("service_timeout", service_timeout_s); + service_timeout_ = std::chrono::duration_cast( + std::chrono::duration(service_timeout_s)); + double respawn_timeout_s; get_parameter("bond_respawn_max_duration", respawn_timeout_s); bond_respawn_max_duration_ = rclcpp::Duration::from_seconds(respawn_timeout_s); @@ -254,8 +260,9 @@ LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t { message(transition_label_map_[transition] + node_name); - if (!node_map_[node_name]->change_state(transition) || - !(node_map_[node_name]->get_state() == transition_state_map_[transition])) + if (!node_map_[node_name]->change_state(transition, std::chrono::milliseconds(-1), + service_timeout_) || + !(node_map_[node_name]->get_state(service_timeout_) == transition_state_map_[transition])) { RCLCPP_ERROR(get_logger(), "Failed to change state for node: %s", node_name.c_str()); return false; @@ -547,7 +554,7 @@ LifecycleManager::checkBondRespawnConnection() } try { - node_map_[node_name]->get_state(); // Only won't throw if the server exists + node_map_[node_name]->get_state(service_timeout_); // Only won't throw if the server exists live_servers++; } catch (...) { break; diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp index 46d322f7709..341a7706b3f 100644 --- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp +++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp @@ -24,6 +24,7 @@ #include "nav2_util/service_client.hpp" #include "nav2_util/node_utils.hpp" + namespace nav2_util { @@ -43,16 +44,14 @@ class LifecycleServiceClient */ bool change_state( const uint8_t transition, // takes a lifecycle_msgs::msg::Transition id - const std::chrono::seconds timeout); - - /// Trigger a state change, returning result - bool change_state(std::uint8_t transition); + const std::chrono::milliseconds transition_timeout = std::chrono::milliseconds(-1), + const std::chrono::milliseconds wait_for_service_timeout = std::chrono::milliseconds(5000)); /// Get the current state as a lifecycle_msgs::msg::State id value /** * Throws std::runtime_error on failure */ - uint8_t get_state(const std::chrono::seconds timeout = std::chrono::seconds(2)); + uint8_t get_state(const std::chrono::milliseconds timeout = std::chrono::milliseconds(2000)); protected: rclcpp::Node::SharedPtr node_; diff --git a/nav2_util/src/lifecycle_service_client.cpp b/nav2_util/src/lifecycle_service_client.cpp index 9674e76e090..4ff638cf2d4 100644 --- a/nav2_util/src/lifecycle_service_client.cpp +++ b/nav2_util/src/lifecycle_service_client.cpp @@ -22,7 +22,7 @@ #include "lifecycle_msgs/srv/get_state.hpp" using nav2_util::generate_internal_node; -using std::chrono::seconds; +using std::chrono::milliseconds; using std::make_shared; using std::string; using namespace std::chrono_literals; @@ -67,33 +67,26 @@ LifecycleServiceClient::LifecycleServiceClient( bool LifecycleServiceClient::change_state( const uint8_t transition, - const seconds timeout) + const milliseconds transition_timeout, + const milliseconds wait_for_service_timeout) { - if (!change_state_.wait_for_service(timeout)) { + if (!change_state_.wait_for_service(wait_for_service_timeout)) { throw std::runtime_error("change_state service is not available!"); } auto request = std::make_shared(); request->transition.id = transition; - auto response = change_state_.invoke(request, timeout); - return response.get(); -} - -bool LifecycleServiceClient::change_state( - std::uint8_t transition) -{ - if (!change_state_.wait_for_service(5s)) { - throw std::runtime_error("change_state service is not available!"); + if (transition_timeout > 0ms) { + auto response = change_state_.invoke(request, transition_timeout); + return response.get(); + } else { + auto response = std::make_shared(); + return change_state_.invoke(request, response); } - - auto request = std::make_shared(); - auto response = std::make_shared(); - request->transition.id = transition; - return change_state_.invoke(request, response); } uint8_t LifecycleServiceClient::get_state( - const seconds timeout) + const milliseconds timeout) { if (!get_state_.wait_for_service(timeout)) { throw std::runtime_error("get_state service is not available!"); From a61751a7ebe7f64c12385adda3fdc249b28116e2 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 12 May 2025 10:36:02 -0700 Subject: [PATCH 68/70] Update simple_action_server.hpp for description (#5150) Signed-off-by: Steve Macenski --- nav2_util/include/nav2_util/simple_action_server.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 883fb5451f8..c595b838dc5 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -139,7 +139,8 @@ class SimpleActionServer } /** - * @brief handle the goal requested: accept or reject. This implementation always accepts. + * @brief handle the goal requested: accept or reject. + * This implementation always accepts when the server is active. * @param uuid Goal ID * @param Goal A shared pointer to the specific goal * @return GoalResponse response of the goal processed From d362e74611ffb93ab2dae2c610a5b3ccccc8c5b9 Mon Sep 17 00:00:00 2001 From: Adi Vardi <57910756+adivardi@users.noreply.github.com> Date: Mon, 12 May 2025 20:36:10 +0300 Subject: [PATCH 69/70] reset motion model (#5149) Signed-off-by: Adi Vardi --- nav2_mppi_controller/src/optimizer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index ecabc483884..6acbd6223c0 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -147,6 +147,8 @@ void Optimizer::reset() generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); noise_generator_.reset(settings_, isHolonomic()); + motion_model_->initialize(settings_.constraints, settings_.model_dt); + RCLCPP_INFO(logger_, "Optimizer reset"); } From 711817f995d5a33dba29e7330874bd93b50fe860 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 13 May 2025 22:18:21 +0200 Subject: [PATCH 70/70] Show error if inflation radius is smaller than circumscribed radius (#5148) * Warn if inflation radius is smaller than circumscribed radius Signed-off-by: Tony Najjar * Update nav2_mppi_controller/src/critics/cost_critic.cpp Co-authored-by: Steve Macenski Signed-off-by: Tony Najjar * Update nav2_smac_planner/include/nav2_smac_planner/utils.hpp Co-authored-by: Steve Macenski Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar Signed-off-by: Tony Najjar Co-authored-by: Steve Macenski --- nav2_mppi_controller/src/critics/cost_critic.cpp | 13 +++++++++++++ .../include/nav2_smac_planner/utils.hpp | 13 +++++++++++++ 2 files changed, 26 insertions(+) diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index c39d669bde3..3850552dcb6 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -96,6 +96,19 @@ float CostCritic::findCircumscribedCost( inflation_layer_name_); if (inflation_layer != nullptr) { const double resolution = costmap->getCostmap()->getResolution(); + double inflation_radius = inflation_layer->getInflationRadius(); + if (inflation_radius < circum_radius) { + RCLCPP_ERROR( + rclcpp::get_logger("computeCircumscribedCost"), + "The inflation radius (%f) is smaller than the circumscribed radius (%f) " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!", + inflation_radius, circum_radius); + result = 0.0; + return result; + } result = inflation_layer->computeCost(circum_radius / resolution); } else { RCLCPP_WARN( diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp index e14f4e77ed9..9e82257c9a9 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -82,6 +82,19 @@ inline double findCircumscribedCost(std::shared_ptrgetLayeredCostmap()->getCircumscribedRadius(); double resolution = costmap->getCostmap()->getResolution(); + double inflation_radius = inflation_layer->getInflationRadius(); + if (inflation_radius < circum_radius) { + RCLCPP_ERROR( + rclcpp::get_logger("computeCircumscribedCost"), + "The inflation radius (%f) is smaller than the circumscribed radius (%f) " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!", + inflation_radius, circum_radius); + result = 0.0; + return result; + } result = static_cast(inflation_layer->computeCost(circum_radius / resolution)); } else { RCLCPP_WARN(