From 99578bd90ebcc17151391c2e56e2df966c0afa54 Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Sun, 16 Mar 2025 12:07:05 +0000 Subject: [PATCH 01/14] Add pre-commit Signed-off-by: Nils-ChristianIseke --- .../srv/assets/foxglove/nav2_layout.json | 2 +- .../caddy/srv/nav2/github-markdown.css | 2 +- .github/mergify.yml | 4 +- .gitignore | 1 - .pre-commit-config.yaml | 96 +++++++++++++++++++ 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 | 14 ++- .../cloned_multi_tb3_simulation_launch.py | 21 ++-- nav2_bringup/launch/localization_launch.py | 13 ++- nav2_bringup/launch/navigation_launch.py | 14 ++- nav2_bringup/launch/rviz_launch.py | 5 +- nav2_bringup/launch/slam_launch.py | 15 ++- .../launch/tb3_loopback_simulation.launch.py | 13 +-- nav2_bringup/launch/tb3_simulation_launch.py | 17 ++-- .../launch/tb4_loopback_simulation.launch.py | 16 ++-- nav2_bringup/launch/tb4_simulation_launch.py | 20 ++-- .../unique_multi_tb3_simulation_launch.py | 22 ++--- nav2_collision_monitor/README.md | 2 +- .../launch/collision_detector_node.launch.py | 10 +- .../launch/collision_monitor_node.launch.py | 13 ++- .../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/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 | 14 ++- .../dwb_core/src/dwb_local_planner.cpp | 2 +- nav2_graceful_controller/README.md | 40 ++++---- nav2_lifecycle_manager/README.md | 2 +- .../launch/loopback_simulation.launch.py | 1 - .../nav2_loopback_sim/loopback_simulator.py | 28 +++--- nav2_loopback_sim/nav2_loopback_sim/utils.py | 4 +- nav2_loopback_sim/setup.py | 1 - nav2_map_server/README.md | 3 +- .../cmake_modules/FindGRAPHICSMAGICKCPP.cmake | 2 +- 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 | 19 ++-- .../launch/follow_path_example_launch.py | 19 ++-- .../launch/inspection_demo_launch.py | 19 ++-- .../nav_through_poses_example_launch.py | 19 ++-- .../launch/nav_to_pose_example_launch.py | 19 ++-- .../launch/picking_demo_launch.py | 19 ++-- .../launch/recoveries_example_launch.py | 19 ++-- .../launch/security_demo_launch.py | 19 ++-- .../waypoint_follower_example_launch.py | 19 ++-- .../nav2_simple_commander/demo_inspection.py | 3 +- .../nav2_simple_commander/demo_picking.py | 17 ++-- .../nav2_simple_commander/demo_recoveries.py | 6 +- .../nav2_simple_commander/demo_security.py | 5 +- .../example_follow_path.py | 4 +- .../example_nav_through_poses.py | 3 +- .../example_nav_to_pose.py | 3 +- .../example_waypoint_follower.py | 3 +- .../footprint_collision_checker.py | 6 +- .../nav2_simple_commander/robot_navigator.py | 38 +++++--- nav2_simple_commander/pytest.ini | 2 +- nav2_simple_commander/setup.py | 1 - .../test/test_footprint_collision_checker.py | 3 +- 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 | 11 +-- .../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 | 4 +- .../trajectory_generator.py | 5 +- 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 | 12 +-- .../src/behaviors/backup/backup_tester.py | 9 +- .../backup/test_backup_behavior.launch.py | 12 +-- .../drive_on_heading/drive_tester.py | 9 +- .../test_drive_on_heading_behavior.launch.py | 12 +-- .../src/behaviors/spin/spin_tester.py | 9 +- .../spin/test_spin_behavior.launch.py | 12 +-- .../wait/test_wait_behavior_launch.py | 12 +-- .../costmap_filters/test_keepout_launch.py | 12 +-- .../src/costmap_filters/test_speed_launch.py | 12 +-- .../src/costmap_filters/tester_node.py | 5 +- .../src/error_codes/test_error_codes.py | 13 ++- .../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 | 12 +-- .../src/gps_navigation/tester.py | 5 +- nav2_system_tests/src/localization/README.md | 2 +- .../localization/test_localization_launch.py | 3 +- nav2_system_tests/src/planning/README.md | 2 +- .../planning/test_planner_costmaps_launch.py | 1 - .../planning/test_planner_random_launch.py | 1 - .../src/system/nav2_system_params.yaml | 1 - ...nav_through_poses_tester_error_msg_node.py | 8 +- .../system/nav_through_poses_tester_node.py | 7 +- .../src/system/nav_to_pose_tester_node.py | 7 +- .../src/system/test_multi_robot_launch.py | 18 ++-- .../src/system/test_system_launch.py | 12 +-- .../test_system_with_obstacle_launch.py | 12 +-- .../src/system/test_wrong_init_pose_launch.py | 11 +-- .../test_system_failure_launch.py | 12 +-- .../src/system_failure/tester_node.py | 6 +- nav2_system_tests/src/updown/README.md | 2 +- .../src/updown/test_updown_launch.py | 1 - .../src/updown/test_updown_reliability | 2 +- .../src/waypoint_follower/test_case_py.launch | 16 ++-- .../src/waypoint_follower/tester.py | 11 ++- 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/planner_benchmarking/metrics.py | 4 +- tools/pyproject.toml | 12 +++ tools/skip_keys.txt | 2 +- tools/smoother_benchmarking/README.md | 12 +-- tools/smoother_benchmarking/metrics.py | 5 +- .../smoother_benchmark_bringup.py | 3 +- tools/underlay.repos | 5 +- tools/update_readme_table.py | 16 +++- 175 files changed, 759 insertions(+), 688 deletions(-) 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/.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..918c86fc381 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,96 @@ + +# 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: 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..c40ab088de5 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -15,17 +15,15 @@ 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 +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import SetEnvironmentVariable from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import Node from launch_ros.actions import PushROSNamespace from launch_ros.descriptions import ParameterFile diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index ecffa7771c8..991ef06986b 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -20,20 +20,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import LogInfo +from launch.actions import OpaqueFunction +from launch.actions import RegisterEventHandler from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch.substitutions import LaunchConfiguration +from launch.substitutions import TextSubstitution from nav2_common.launch import ParseMultiRobotPose diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 909b81eb3e1..b3d925e5b89 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -15,17 +15,20 @@ 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 DeclareLaunchArgument +from launch.actions import GroupAction from launch.actions import SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import EqualsSubstitution -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration from launch.substitutions import NotEqualsSubstitution -from launch_ros.actions import LoadComposableNodes, SetParameter +from launch.substitutions import PythonExpression +from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode, ParameterFile +from launch_ros.actions import SetParameter +from launch_ros.descriptions import ComposableNode +from launch_ros.descriptions import 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..ef8f3a2ae3c 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -15,14 +15,18 @@ 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.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import SetEnvironmentVariable from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes, SetParameter +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression +from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode, ParameterFile +from launch_ros.actions import SetParameter +from launch_ros.descriptions import ComposableNode +from launch_ros.descriptions import 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..7f2157c21de 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -15,9 +15,10 @@ 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.actions import DeclareLaunchArgument +from launch.actions import EmitEvent +from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.events import Shutdown from launch.substitutions import LaunchConfiguration diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index f9294b711d2..8bec9888c7d 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -15,15 +15,20 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node, SetParameter, SetRemap +from launch_ros.actions import Node +from launch_ros.actions import SetParameter +from launch_ros.actions import SetRemap from launch_ros.descriptions import ParameterFile -from nav2_common.launch import HasNodeParams, RewrittenYaml +from nav2_common.launch import HasNodeParams +from nav2_common.launch import RewrittenYaml def generate_launch_description(): diff --git a/nav2_bringup/launch/tb3_loopback_simulation.launch.py b/nav2_bringup/launch/tb3_loopback_simulation.launch.py index 3553852597c..0d3f0f58688 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation.launch.py @@ -17,19 +17,16 @@ 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 +from launch.actions import GroupAction +from launch.actions import 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.actions import Node +from launch_ros.actions import 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..b174e686bc5 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -18,20 +18,17 @@ 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 +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import LaunchConfiguration +from launch.substitutions import 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..39b066d4d1a 100644 --- a/nav2_bringup/launch/tb4_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb4_loopback_simulation.launch.py @@ -17,19 +17,17 @@ 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 +from launch.actions import GroupAction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.actions import 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..34a435b0f70 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -18,21 +18,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch.substitutions import 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..aa68cdc77f9 100644 --- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py @@ -25,22 +25,20 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import LogInfo +from launch.actions import OpaqueFunction +from launch.actions import RegisterEventHandler from launch.conditions import IfCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch.substitutions import LaunchConfiguration +from launch.substitutions import TextSubstitution def generate_launch_description(): 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..1e4d2878db7 100644 --- a/nav2_collision_monitor/launch/collision_detector_node.launch.py +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -17,15 +17,17 @@ 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 DeclareLaunchArgument +from launch.actions import GroupAction from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration from launch.substitutions import NotEqualsSubstitution -from launch_ros.actions import LoadComposableNodes, SetParameter +from launch.substitutions import PythonExpression +from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node from launch_ros.actions import PushROSNamespace +from launch_ros.actions import 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..09de463f8cf 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -17,16 +17,19 @@ 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 DeclareLaunchArgument +from launch.actions import GroupAction from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration from launch.substitutions import NotEqualsSubstitution -from launch_ros.actions import LoadComposableNodes, SetParameter +from launch.substitutions import PythonExpression +from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node from launch_ros.actions import PushROSNamespace -from launch_ros.descriptions import ComposableNode, ParameterFile +from launch_ros.actions import SetParameter +from launch_ros.descriptions import ComposableNode +from launch_ros.descriptions import 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/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..2fa57342242 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -12,12 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -from math import acos, cos, sin +from math import acos +from math import cos +from math import sin import time import unittest from action_msgs.msg import GoalStatus -from geometry_msgs.msg import TransformStamped, Twist, TwistStamped +from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import Twist +from geometry_msgs.msg import TwistStamped from launch import LaunchDescription # from launch.actions import SetEnvironmentVariable from launch_ros.actions import Node @@ -26,7 +30,9 @@ import launch_testing.asserts import launch_testing.markers import launch_testing.util -from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot +from nav2_msgs.action import DockRobot +from nav2_msgs.action import NavigateToPose +from nav2_msgs.action import UndockRobot import pytest import rclpy from rclpy.action.client import ActionClient @@ -34,7 +40,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 +47,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_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..bff1dbf0a00 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -15,27 +15,33 @@ import math -from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, TwistStamped -from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 +from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import Twist +from geometry_msgs.msg import TwistStamped +from geometry_msgs.msg import Vector3 from nav2_simple_commander.line_iterator import LineIterator from nav_msgs.msg import Odometry from nav_msgs.srv import GetMap import rclpy from rclpy.duration import Duration from rclpy.node import Node -from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from rclpy.qos import DurabilityPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import ReliabilityPolicy from rosgraph_msgs.msg import Clock from sensor_msgs.msg import LaserScan -from tf2_ros import Buffer, TransformBroadcaster, TransformListener +from tf2_ros import Buffer +from tf2_ros import TransformBroadcaster +from tf2_ros import TransformListener import tf_transformations -from .utils import ( - addYawToQuat, - getMapOccupancy, - matrixToTransform, - transformStampedToMatrix, - worldToMap, -) +from .utils import addYawToQuat +from .utils import getMapOccupancy +from .utils import matrixToTransform +from .utils import transformStampedToMatrix +from .utils import 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..66f6c3d42f9 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -15,11 +15,11 @@ import math -from geometry_msgs.msg import Quaternion, Transform +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import Transform 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_mppi_controller/README.md b/nav2_mppi_controller/README.md index bc3214c1824..635ca6b8545 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..32fea955cce 100644 --- a/nav2_simple_commander/launch/assisted_teleop_example_launch.py +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -18,20 +18,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py index b07fef9ba68..a5e1b51039e 100644 --- a/nav2_simple_commander/launch/follow_path_example_launch.py +++ b/nav2_simple_commander/launch/follow_path_example_launch.py @@ -17,20 +17,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py index be98c14150d..ed1f37168d2 100644 --- a/nav2_simple_commander/launch/inspection_demo_launch.py +++ b/nav2_simple_commander/launch/inspection_demo_launch.py @@ -17,20 +17,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import Node 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..61fd7a55148 100644 --- a/nav2_simple_commander/launch/nav_through_poses_example_launch.py +++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py @@ -17,20 +17,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import Node 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..47cf326ef50 100644 --- a/nav2_simple_commander/launch/nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py @@ -17,20 +17,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py index f4bd8346013..acbb53cc831 100644 --- a/nav2_simple_commander/launch/picking_demo_launch.py +++ b/nav2_simple_commander/launch/picking_demo_launch.py @@ -17,20 +17,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py index c7afd73f31c..17462255f0e 100644 --- a/nav2_simple_commander/launch/recoveries_example_launch.py +++ b/nav2_simple_commander/launch/recoveries_example_launch.py @@ -17,20 +17,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py index 240f430b236..a9c568cee6c 100644 --- a/nav2_simple_commander/launch/security_demo_launch.py +++ b/nav2_simple_commander/launch/security_demo_launch.py @@ -17,20 +17,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py index 10f36ce7922..a9505f33034 100644 --- a/nav2_simple_commander/launch/waypoint_follower_example_launch.py +++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py @@ -17,20 +17,19 @@ 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 +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import 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.substitutions import Command +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index 6fee029cd8b..45f4fa66405 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -16,7 +16,8 @@ from copy import deepcopy from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import TaskResult import rclpy """ diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index 6f2951d012a..4974533d606 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -14,11 +14,17 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult - +from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import 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 +41,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..4df2580348d 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py @@ -14,11 +14,11 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import TaskResult 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 +64,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..49cfb1a907c 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -16,12 +16,11 @@ from copy import deepcopy from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult - +from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import 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..534370d39a6 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -14,10 +14,10 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import TaskResult import rclpy - """ Basic navigation demo to follow a given path after smoothing """ 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 cbcb4e3926d..047f523c32b 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 @@ -14,7 +14,8 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import TaskResult import rclpy from rclpy.duration import Duration 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 5846823440c..dc94126043f 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 @@ -14,7 +14,8 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import TaskResult import rclpy from rclpy.duration import Duration 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 0cf065a7817..f3299127ad3 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -14,7 +14,8 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import TaskResult import rclpy from rclpy.duration import Duration diff --git a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py index 5cf2f81794a..1c7e04a3944 100644 --- a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py +++ b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py @@ -21,9 +21,11 @@ and calculate the cost of a Footprint """ -from math import cos, sin +from math import cos +from math import sin -from geometry_msgs.msg import Point32, Polygon +from geometry_msgs.msg import Point32 +from geometry_msgs.msg import Polygon from nav2_simple_commander.costmap_2d import PyCostmap2D from nav2_simple_commander.line_iterator import LineIterator diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index d8a044c0b35..c29e7d443fc 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -23,26 +23,34 @@ from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import 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 AssistedTeleop +from nav2_msgs.action import BackUp +from nav2_msgs.action import ComputePathThroughPoses +from nav2_msgs.action import ComputePathToPose +from nav2_msgs.action import DockRobot +from nav2_msgs.action import DriveOnHeading +from nav2_msgs.action import FollowGPSWaypoints +from nav2_msgs.action import FollowPath +from nav2_msgs.action import FollowWaypoints +from nav2_msgs.action import NavigateThroughPoses +from nav2_msgs.action import NavigateToPose 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 Spin +from nav2_msgs.action import UndockRobot +from nav2_msgs.srv import ClearCostmapAroundRobot +from nav2_msgs.srv import ClearCostmapExceptRegion +from nav2_msgs.srv import ClearEntireCostmap +from nav2_msgs.srv import GetCostmap +from nav2_msgs.srv import LoadMap +from nav2_msgs.srv import 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 +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import 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_simple_commander/test/test_footprint_collision_checker.py b/nav2_simple_commander/test/test_footprint_collision_checker.py index 8ffc4b653b9..b770ba7971b 100644 --- a/nav2_simple_commander/test/test_footprint_collision_checker.py +++ b/nav2_simple_commander/test/test_footprint_collision_checker.py @@ -14,7 +14,8 @@ import unittest -from geometry_msgs.msg import Point32, Polygon +from geometry_msgs.msg import Point32 +from geometry_msgs.msg import Polygon from nav2_simple_commander.costmap_2d import PyCostmap2D from nav2_simple_commander.footprint_collision_checker import FootprintCollisionChecker from nav_msgs.msg import OccupancyGrid 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..9da58922858 100644 --- a/nav2_smac_planner/lattice_primitives/lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -15,14 +15,13 @@ from collections import defaultdict from enum import Enum -from helper import angle_difference, interpolate_yaws - +from helper import angle_difference +from helper import interpolate_yaws import numpy as np - from rtree import index - -from trajectory import Path, Trajectory, TrajectoryParameters - +from trajectory import Path +from trajectory import Trajectory +from trajectory import 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..4163ab4b413 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory.py +++ b/nav2_smac_planner/lattice_primitives/trajectory.py @@ -14,8 +14,8 @@ from dataclasses import dataclass -from helper import angle_difference, normalize_angle - +from helper import angle_difference +from helper import 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..8de82bae20b 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py @@ -16,8 +16,9 @@ from typing import Tuple, Union import numpy as np - -from trajectory import Path, Trajectory, TrajectoryParameters +from trajectory import Path +from trajectory import Trajectory +from trajectory import 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..5a2cb90d36a 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,15 @@ 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.actions import AppendEnvironmentVariable +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..b3405553eb3 100755 --- a/nav2_system_tests/src/behaviors/backup/backup_tester.py +++ b/nav2_system_tests/src/behaviors/backup/backup_tester.py @@ -17,18 +17,19 @@ import time from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Point32, PolygonStamped +from geometry_msgs.msg import Point32 +from geometry_msgs.msg import PolygonStamped 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 QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile +from rclpy.qos import 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..aefcd8a80b3 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,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, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable) +from launch.actions import AppendEnvironmentVariable +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..ef20886cad7 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 @@ -17,18 +17,19 @@ import time from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Point32, PolygonStamped +from geometry_msgs.msg import Point32 +from geometry_msgs.msg import PolygonStamped 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 QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile +from rclpy.qos import 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..f50e3cbf883 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,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, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable) +from launch.actions import AppendEnvironmentVariable +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..494b262b7d9 100755 --- a/nav2_system_tests/src/behaviors/spin/spin_tester.py +++ b/nav2_system_tests/src/behaviors/spin/spin_tester.py @@ -17,18 +17,19 @@ import time from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Point32, PolygonStamped +from geometry_msgs.msg import Point32 +from geometry_msgs.msg import PolygonStamped 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 QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile +from rclpy.qos import 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..0c7fca53e79 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,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, - DeclareLaunchArgument, - ExecuteProcess, - IncludeLaunchDescription, - SetEnvironmentVariable) +from launch.actions import AppendEnvironmentVariable +from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..3a853ab4506 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,15 @@ 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.actions import AppendEnvironmentVariable +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..1663e93b926 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,16 @@ 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.actions import AppendEnvironmentVariable +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..66a44217dd9 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,16 @@ 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.actions import AppendEnvironmentVariable +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..f3e7043fd6c 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -32,12 +32,13 @@ from nav2_msgs.srv import ManageLifecycleNodes from nav_msgs.msg import OccupancyGrid from nav_msgs.msg import Path - import rclpy from rclpy.action import ActionClient from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile +from rclpy.qos import 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..3a7f5339380 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -18,13 +18,12 @@ import time from geometry_msgs.msg import PoseStamped -from nav2_msgs.action import ( - ComputePathThroughPoses, - ComputePathToPose, - FollowPath, - SmoothPath, -) -from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from nav2_msgs.action import ComputePathThroughPoses +from nav2_msgs.action import ComputePathToPose +from nav2_msgs.action import FollowPath +from nav2_msgs.action import SmoothPath +from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import TaskResult from nav_msgs.msg import Path import rclpy 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..10b2e59dd00 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,15 @@ 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.actions import AppendEnvironmentVariable +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..34c6b99b180 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -18,12 +18,11 @@ from action_msgs.msg import GoalStatus from geographic_msgs.msg import GeoPose -from nav2_msgs.action import ComputePathToPose, FollowGPSWaypoints +from nav2_msgs.action import ComputePathToPose +from nav2_msgs.action import 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..79a454026a4 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -21,7 +21,8 @@ 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 AppendEnvironmentVariable +from launch.actions import ExecuteProcess from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_ros.actions 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..8fc008dc625 100755 --- a/nav2_system_tests/src/planning/test_planner_costmaps_launch.py +++ b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py @@ -20,7 +20,6 @@ from launch import LaunchDescription from launch import 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..df1c5d469e6 100755 --- a/nav2_system_tests/src/planning/test_planner_random_launch.py +++ b/nav2_system_tests/src/planning/test_planner_random_launch.py @@ -20,7 +20,6 @@ from launch import LaunchDescription from launch import 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 9f246d60139..6e37506ca81 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,7 +17,6 @@ import argparse import sys import time - from typing import Optional from action_msgs.msg import GoalStatus @@ -28,15 +27,14 @@ 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 QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile - +from rclpy.qos import 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 faea3a6dba1..7d0a4758078 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,7 +17,6 @@ import argparse import sys import time - from typing import Optional from action_msgs.msg import GoalStatus @@ -27,13 +26,13 @@ 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 QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile +from rclpy.qos import 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..6601338a358 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,7 +18,6 @@ import math import sys import time - from typing import Optional from action_msgs.msg import GoalStatus @@ -28,13 +27,13 @@ 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 QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile +from rclpy.qos import 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..b47df2ba76e 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,16 @@ 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 import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import TextSubstitution -from launch_ros.actions import Node, PushROSNamespace - +from launch_ros.actions import Node +from launch_ros.actions import 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..3976875c976 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -20,20 +20,16 @@ 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.actions import AppendEnvironmentVariable +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..7bbbf961e58 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,16 @@ 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.actions import AppendEnvironmentVariable +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..7924e32b907 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 @@ -22,17 +22,14 @@ 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.actions import AppendEnvironmentVariable +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..e02b5179f7e 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,16 @@ 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.actions import AppendEnvironmentVariable +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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..5d70552c606 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -27,13 +27,13 @@ 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 QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile +from rclpy.qos import 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..d66896d5b82 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -16,7 +16,6 @@ from ament_index_python.packages import get_package_prefix from ament_index_python.packages import 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_py.launch b/nav2_system_tests/src/waypoint_follower/test_case_py.launch index 7a19668050d..95a65858fb9 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_py.launch +++ b/nav2_system_tests/src/waypoint_follower/test_case_py.launch @@ -18,20 +18,16 @@ from pathlib import Path 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.actions import AppendEnvironmentVariable +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.actions import 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 @@ -48,8 +44,8 @@ def generate_launch_description(): 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'), + bt_navigator_xml = os.path.join(get_package_share_directory( + 'nav2_bt_navigator'), 'behavior_trees', os.getenv('BT_NAVIGATOR_XML')) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 85dba0be401..876d6893e7b 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -17,17 +17,20 @@ import time from action_msgs.msg import GoalStatus -from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped -from nav2_msgs.action import ComputePathToPose, FollowWaypoints +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from nav2_msgs.action import ComputePathToPose +from nav2_msgs.action import 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 QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSProfile +from rclpy.qos import 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/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 0ae0f883efe..b2d4db1db7c 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -17,7 +17,9 @@ import math import os import pickle -from random import randint, seed, uniform +from random import randint +from random import seed +from random import uniform import time from geometry_msgs.msg import PoseStamped diff --git a/tools/pyproject.toml b/tools/pyproject.toml new file mode 100644 index 00000000000..12829ae0e71 --- /dev/null +++ b/tools/pyproject.toml @@ -0,0 +1,12 @@ +[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 = true +[tool.isort] +profile = "google" 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..5ae6c1b46f8 100644 --- a/tools/smoother_benchmarking/metrics.py +++ b/tools/smoother_benchmarking/metrics.py @@ -17,7 +17,9 @@ import math import os import pickle -from random import randint, seed, uniform +from random import randint +from random import seed +from random import uniform from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator @@ -25,7 +27,6 @@ import rclpy from transforms3d.euler import euler2quat - # Note: Map origin is assumed to be (0,0) diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py index c19d1f2f703..86381e3e0fd 100644 --- a/tools/smoother_benchmarking/smoother_benchmark_bringup.py +++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py @@ -16,7 +16,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node 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..92ecd96d786 100755 --- a/tools/update_readme_table.py +++ b/tools/update_readme_table.py @@ -61,11 +61,16 @@ # 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/' + def getBinPath(package, prefix, OS): + return f'https://build.ros2.org/job/{prefix}bin_u{OS[0]}64__{package}__ubuntu_{OS}_' \ + f'amd64__binary/' + def createPreamble(Distros): table = '| Package | ' @@ -77,6 +82,7 @@ def createPreamble(Distros): table += ' :---: | :---: |' return table + def main(): header = createPreamble(Distros) @@ -98,13 +104,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 7aa81c1ac142b97ef70adb6ae9334b70f4ae8c10 Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Sun, 16 Mar 2025 12:12:42 +0000 Subject: [PATCH 02/14] Add codespell workflow Signed-off-by: Nils-ChristianIseke --- .github/workflows/codespell.yml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 .github/workflows/codespell.yml diff --git a/.github/workflows/codespell.yml b/.github/workflows/codespell.yml new file mode 100644 index 00000000000..ca65c82d8fc --- /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 \ No newline at end of file From 035db98f0ac2b795ab6b170e4abba1b5b6bb9486 Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Sun, 16 Mar 2025 13:05:02 +0000 Subject: [PATCH 03/14] Codespell write_changes=false. As otherwise CI does not fail. Signed-off-by: Nils-ChristianIseke --- .github/workflows/codespell.yml | 2 +- tools/pyproject.toml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/codespell.yml b/.github/workflows/codespell.yml index ca65c82d8fc..61bb416e3b0 100644 --- a/.github/workflows/codespell.yml +++ b/.github/workflows/codespell.yml @@ -11,4 +11,4 @@ jobs: - name: Install codespell run: pip install codespell - name: Run codespell - run: codespell --toml ./tools/pyproject.toml \ No newline at end of file + run: codespell --toml ./tools/pyproject.toml diff --git a/tools/pyproject.toml b/tools/pyproject.toml index 12829ae0e71..b1111696f3c 100644 --- a/tools/pyproject.toml +++ b/tools/pyproject.toml @@ -7,6 +7,6 @@ interactive = 0 quiet = 34 skip="*.pgm,./build/*,./install/*,./log*,./.venv/*,./.git*,*.toml" uri-ignore-words-list = "segue" -write-changes = true +write-changes = false [tool.isort] profile = "google" From 91e8bc558c39f9ef3177c2f4f4db0b77bde9cbe9 Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Mon, 17 Mar 2025 21:34:07 +0000 Subject: [PATCH 04/14] Configure isort Signed-off-by: Nils-ChristianIseke --- nav2_bringup/launch/bringup_launch.py | 12 +++---- .../cloned_multi_tb3_simulation_launch.py | 14 +++----- nav2_bringup/launch/localization_launch.py | 17 +++------ nav2_bringup/launch/navigation_launch.py | 14 +++----- nav2_bringup/launch/rviz_launch.py | 4 +-- nav2_bringup/launch/slam_launch.py | 14 +++----- .../launch/tb3_loopback_simulation.launch.py | 7 ++-- nav2_bringup/launch/tb3_simulation_launch.py | 10 ++---- .../launch/tb4_loopback_simulation.launch.py | 10 ++---- nav2_bringup/launch/tb4_simulation_launch.py | 12 ++----- .../unique_multi_tb3_simulation_launch.py | 14 +++----- .../launch/collision_detector_node.launch.py | 12 ++----- .../launch/collision_monitor_node.launch.py | 15 +++----- .../test/integration/costmap_tests_launch.py | 6 ++-- .../test/test_docking_server.py | 12 ++----- .../test/launch_bond_test.py | 3 +- .../test/launch_lifecycle_test.py | 3 +- .../nav2_loopback_sim/loopback_simulator.py | 23 ++++-------- nav2_loopback_sim/nav2_loopback_sim/utils.py | 3 +- .../test/component/test_map_saver_launch.py | 6 ++-- .../test/component/test_map_server_launch.py | 6 ++-- .../launch/assisted_teleop_example_launch.py | 12 ++----- .../launch/follow_path_example_launch.py | 12 ++----- .../launch/inspection_demo_launch.py | 12 ++----- .../nav_through_poses_example_launch.py | 12 ++----- .../launch/nav_to_pose_example_launch.py | 12 ++----- .../launch/picking_demo_launch.py | 12 ++----- .../launch/recoveries_example_launch.py | 12 ++----- .../launch/security_demo_launch.py | 12 ++----- .../waypoint_follower_example_launch.py | 12 ++----- .../nav2_simple_commander/demo_inspection.py | 3 +- .../nav2_simple_commander/demo_picking.py | 3 +- .../nav2_simple_commander/demo_recoveries.py | 3 +- .../nav2_simple_commander/demo_security.py | 3 +- .../example_follow_path.py | 3 +- .../example_nav_through_poses.py | 3 +- .../example_nav_to_pose.py | 3 +- .../example_waypoint_follower.py | 3 +- .../footprint_collision_checker.py | 6 ++-- .../nav2_simple_commander/robot_navigator.py | 35 +++++-------------- .../test/test_footprint_collision_checker.py | 3 +- .../lattice_primitives/lattice_generator.py | 7 ++-- .../lattice_primitives/trajectory.py | 3 +- .../trajectory_generator.py | 4 +-- .../test_assisted_teleop_behavior_launch.py | 9 ++--- .../src/behaviors/backup/backup_tester.py | 8 ++--- .../backup/test_backup_behavior.launch.py | 10 ++---- .../drive_on_heading/drive_tester.py | 8 ++--- .../test_drive_on_heading_behavior.launch.py | 10 ++---- .../src/behaviors/spin/spin_tester.py | 8 ++--- .../spin/test_spin_behavior.launch.py | 10 ++---- .../wait/test_wait_behavior_launch.py | 9 ++--- .../costmap_filters/test_keepout_launch.py | 9 ++--- .../src/costmap_filters/test_speed_launch.py | 9 ++--- .../src/costmap_filters/tester_node.py | 12 ++----- .../src/error_codes/test_error_codes.py | 8 ++--- .../error_codes/test_error_codes_launch.py | 6 ++-- .../src/gps_navigation/test_case_py.launch.py | 9 ++--- .../src/gps_navigation/tester.py | 3 +- .../localization/test_localization_launch.py | 7 ++-- .../planning/test_planner_costmaps_launch.py | 3 +- .../planning/test_planner_random_launch.py | 3 +- ...nav_through_poses_tester_error_msg_node.py | 9 ++--- .../system/nav_through_poses_tester_node.py | 9 ++--- .../src/system/nav_to_pose_tester_node.py | 9 ++--- .../src/system/test_multi_robot_launch.py | 12 +++---- .../src/system/test_system_launch.py | 9 ++--- .../test_system_with_obstacle_launch.py | 9 ++--- .../src/system/test_wrong_init_pose_launch.py | 9 ++--- .../test_system_failure_launch.py | 9 ++--- .../src/system_failure/tester_node.py | 9 ++--- .../src/updown/test_updown_launch.py | 3 +- .../src/waypoint_follower/test_case_launch.py | 9 ++--- .../src/waypoint_follower/tester.py | 11 ++---- tools/planner_benchmarking/metrics.py | 4 +-- tools/pyproject.toml | 2 ++ tools/smoother_benchmarking/metrics.py | 4 +-- .../smoother_benchmark_bringup.py | 3 +- 78 files changed, 190 insertions(+), 478 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index c40ab088de5..ffbfd1bf8cc 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -16,16 +16,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 -from launch.substitutions import PythonExpression -from launch_ros.actions import Node -from launch_ros.actions import PushROSNamespace +from launch.substitutions import LaunchConfiguration, PythonExpression +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 991ef06986b..149b3174054 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -20,19 +20,13 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import GroupAction -from launch.actions import IncludeLaunchDescription -from launch.actions import LogInfo -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import TextSubstitution +from launch.substitutions import LaunchConfiguration, TextSubstitution from nav2_common.launch import ParseMultiRobotPose diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index b3d925e5b89..87134e19d0c 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -16,19 +16,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import 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 -from launch.substitutions import NotEqualsSubstitution -from launch.substitutions import PythonExpression -from launch_ros.actions import LoadComposableNodes -from launch_ros.actions import Node -from launch_ros.actions import SetParameter -from launch_ros.descriptions import ComposableNode -from launch_ros.descriptions import ParameterFile +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 ef8f3a2ae3c..f8d08a4ac26 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -16,17 +16,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression -from launch_ros.actions import LoadComposableNodes -from launch_ros.actions import Node -from launch_ros.actions import SetParameter -from launch_ros.descriptions import ComposableNode -from launch_ros.descriptions import ParameterFile +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 diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 7f2157c21de..1cda9d6ea7a 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -16,9 +16,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import EmitEvent -from launch.actions import RegisterEventHandler +from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.events import Shutdown from launch.substitutions import LaunchConfiguration diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index 8bec9888c7d..7d851577252 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -16,19 +16,13 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch_ros.actions import SetParameter -from launch_ros.actions import SetRemap +from launch_ros.actions import Node, SetParameter, SetRemap from launch_ros.descriptions import ParameterFile -from nav2_common.launch import HasNodeParams -from nav2_common.launch import RewrittenYaml +from nav2_common.launch import HasNodeParams, RewrittenYaml def generate_launch_description(): diff --git a/nav2_bringup/launch/tb3_loopback_simulation.launch.py b/nav2_bringup/launch/tb3_loopback_simulation.launch.py index 0d3f0f58688..f32114eb29a 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation.launch.py @@ -18,14 +18,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import 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 -from launch_ros.actions import SetParameter +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 b174e686bc5..ed12d8bcea2 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -19,16 +19,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import PythonExpression +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 39b066d4d1a..aeb966c8a6e 100644 --- a/nav2_bringup/launch/tb4_loopback_simulation.launch.py +++ b/nav2_bringup/launch/tb4_loopback_simulation.launch.py @@ -18,15 +18,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch_ros.actions import SetParameter +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 34a435b0f70..f71b3e950e0 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -19,18 +19,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression +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 aa68cdc77f9..838c69f3a9e 100644 --- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py @@ -26,19 +26,13 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import GroupAction -from launch.actions import IncludeLaunchDescription -from launch.actions import LogInfo -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import TextSubstitution +from launch.substitutions import LaunchConfiguration, TextSubstitution def generate_launch_description(): diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py index 1e4d2878db7..491c2e8f6ec 100644 --- a/nav2_collision_monitor/launch/collision_detector_node.launch.py +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -18,16 +18,10 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction +from launch.actions import DeclareLaunchArgument, GroupAction from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch.substitutions import NotEqualsSubstitution -from launch.substitutions import PythonExpression -from launch_ros.actions import LoadComposableNodes -from launch_ros.actions import Node -from launch_ros.actions import PushROSNamespace -from launch_ros.actions import SetParameter +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 09de463f8cf..c54d890e28b 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -18,18 +18,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction +from launch.actions import DeclareLaunchArgument, GroupAction from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch.substitutions import NotEqualsSubstitution -from launch.substitutions import PythonExpression -from launch_ros.actions import LoadComposableNodes -from launch_ros.actions import Node -from launch_ros.actions import PushROSNamespace -from launch_ros.actions import SetParameter -from launch_ros.descriptions import ComposableNode -from launch_ros.descriptions import ParameterFile +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_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_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 2fa57342242..560ad02e6f1 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -12,16 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -from math import acos -from math import cos -from math import sin +from math import acos, cos, sin import time import unittest from action_msgs.msg import GoalStatus -from geometry_msgs.msg import TransformStamped -from geometry_msgs.msg import Twist -from geometry_msgs.msg import TwistStamped +from geometry_msgs.msg import TransformStamped, Twist, TwistStamped from launch import LaunchDescription # from launch.actions import SetEnvironmentVariable from launch_ros.actions import Node @@ -30,9 +26,7 @@ import launch_testing.asserts import launch_testing.markers import launch_testing.util -from nav2_msgs.action import DockRobot -from nav2_msgs.action import NavigateToPose -from nav2_msgs.action import UndockRobot +from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot import pytest import rclpy from rclpy.action.client import ActionClient 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/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index bff1dbf0a00..18cbcaf5199 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -15,33 +15,22 @@ import math -from geometry_msgs.msg import PoseWithCovarianceStamped -from geometry_msgs.msg import Quaternion -from geometry_msgs.msg import TransformStamped -from geometry_msgs.msg import Twist -from geometry_msgs.msg import TwistStamped -from geometry_msgs.msg import 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 import rclpy from rclpy.duration import Duration from rclpy.node import Node -from rclpy.qos import DurabilityPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import ReliabilityPolicy +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from rosgraph_msgs.msg import Clock from sensor_msgs.msg import LaserScan -from tf2_ros import Buffer -from tf2_ros import TransformBroadcaster -from tf2_ros import TransformListener +from tf2_ros import Buffer, TransformBroadcaster, TransformListener import tf_transformations -from .utils import addYawToQuat -from .utils import getMapOccupancy -from .utils import matrixToTransform -from .utils import transformStampedToMatrix -from .utils import 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 66f6c3d42f9..ff5c6012e37 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -15,8 +15,7 @@ import math -from geometry_msgs.msg import Quaternion -from geometry_msgs.msg import Transform +from geometry_msgs.msg import Quaternion, Transform import numpy as np import tf_transformations 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_simple_commander/launch/assisted_teleop_example_launch.py b/nav2_simple_commander/launch/assisted_teleop_example_launch.py index 32fea955cce..39172c22185 100644 --- a/nav2_simple_commander/launch/assisted_teleop_example_launch.py +++ b/nav2_simple_commander/launch/assisted_teleop_example_launch.py @@ -19,18 +19,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/follow_path_example_launch.py b/nav2_simple_commander/launch/follow_path_example_launch.py index a5e1b51039e..7e6b9f2ad4d 100644 --- a/nav2_simple_commander/launch/follow_path_example_launch.py +++ b/nav2_simple_commander/launch/follow_path_example_launch.py @@ -18,18 +18,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py index ed1f37168d2..8320de3a828 100644 --- a/nav2_simple_commander/launch/inspection_demo_launch.py +++ b/nav2_simple_commander/launch/inspection_demo_launch.py @@ -18,18 +18,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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 61fd7a55148..696bba3a6cc 100644 --- a/nav2_simple_commander/launch/nav_through_poses_example_launch.py +++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py @@ -18,18 +18,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node 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 47cf326ef50..6c49217d570 100644 --- a/nav2_simple_commander/launch/nav_to_pose_example_launch.py +++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py @@ -18,18 +18,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py index acbb53cc831..33db3c070c1 100644 --- a/nav2_simple_commander/launch/picking_demo_launch.py +++ b/nav2_simple_commander/launch/picking_demo_launch.py @@ -18,18 +18,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/recoveries_example_launch.py b/nav2_simple_commander/launch/recoveries_example_launch.py index 17462255f0e..554a0386400 100644 --- a/nav2_simple_commander/launch/recoveries_example_launch.py +++ b/nav2_simple_commander/launch/recoveries_example_launch.py @@ -18,18 +18,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py index a9c568cee6c..23c41cdae28 100644 --- a/nav2_simple_commander/launch/security_demo_launch.py +++ b/nav2_simple_commander/launch/security_demo_launch.py @@ -18,18 +18,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py index a9505f33034..9bdcfbcb07d 100644 --- a/nav2_simple_commander/launch/waypoint_follower_example_launch.py +++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py @@ -18,18 +18,12 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import AppendEnvironmentVariable -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import 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 -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression +from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py index 45f4fa66405..6fee029cd8b 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -16,8 +16,7 @@ from copy import deepcopy from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -from nav2_simple_commander.robot_navigator import TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy """ diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py index 4974533d606..8559609e84c 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_picking.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -14,8 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -from nav2_simple_commander.robot_navigator import TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration diff --git a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py index 4df2580348d..7675ce199e2 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_recoveries.py @@ -14,8 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -from nav2_simple_commander.robot_navigator import TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py index 49cfb1a907c..d0cf0af5bf7 100644 --- a/nav2_simple_commander/nav2_simple_commander/demo_security.py +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -16,8 +16,7 @@ from copy import deepcopy from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -from nav2_simple_commander.robot_navigator import TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration 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 534370d39a6..8f825c42693 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_follow_path.py +++ b/nav2_simple_commander/nav2_simple_commander/example_follow_path.py @@ -14,8 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -from nav2_simple_commander.robot_navigator import TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy """ 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 047f523c32b..cbcb4e3926d 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 @@ -14,8 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -from nav2_simple_commander.robot_navigator import TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration 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 dc94126043f..5846823440c 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 @@ -14,8 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -from nav2_simple_commander.robot_navigator import TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration 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 f3299127ad3..0cf065a7817 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -14,8 +14,7 @@ # limitations under the License. from geometry_msgs.msg import PoseStamped -from nav2_simple_commander.robot_navigator import BasicNavigator -from nav2_simple_commander.robot_navigator import TaskResult +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy from rclpy.duration import Duration diff --git a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py index 1c7e04a3944..5cf2f81794a 100644 --- a/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py +++ b/nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py @@ -21,11 +21,9 @@ and calculate the cost of a Footprint """ -from math import cos -from math import sin +from math import cos, sin -from geometry_msgs.msg import Point32 -from geometry_msgs.msg import Polygon +from geometry_msgs.msg import Point32, Polygon from nav2_simple_commander.costmap_2d import PyCostmap2D from nav2_simple_commander.line_iterator import LineIterator diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index c29e7d443fc..61182260692 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -19,38 +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 -from nav2_msgs.action import BackUp -from nav2_msgs.action import ComputePathThroughPoses -from nav2_msgs.action import ComputePathToPose -from nav2_msgs.action import DockRobot -from nav2_msgs.action import DriveOnHeading -from nav2_msgs.action import FollowGPSWaypoints -from nav2_msgs.action import FollowPath -from nav2_msgs.action import FollowWaypoints -from nav2_msgs.action import NavigateThroughPoses -from nav2_msgs.action import NavigateToPose -from nav2_msgs.action import SmoothPath -from nav2_msgs.action import Spin -from nav2_msgs.action import UndockRobot -from nav2_msgs.srv import ClearCostmapAroundRobot -from nav2_msgs.srv import ClearCostmapExceptRegion -from nav2_msgs.srv import ClearEntireCostmap -from nav2_msgs.srv import GetCostmap -from nav2_msgs.srv import LoadMap -from nav2_msgs.srv import 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 -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class TaskResult(Enum): diff --git a/nav2_simple_commander/test/test_footprint_collision_checker.py b/nav2_simple_commander/test/test_footprint_collision_checker.py index b770ba7971b..8ffc4b653b9 100644 --- a/nav2_simple_commander/test/test_footprint_collision_checker.py +++ b/nav2_simple_commander/test/test_footprint_collision_checker.py @@ -14,8 +14,7 @@ import unittest -from geometry_msgs.msg import Point32 -from geometry_msgs.msg import Polygon +from geometry_msgs.msg import Point32, Polygon from nav2_simple_commander.costmap_2d import PyCostmap2D from nav2_simple_commander.footprint_collision_checker import FootprintCollisionChecker from nav_msgs.msg import OccupancyGrid diff --git a/nav2_smac_planner/lattice_primitives/lattice_generator.py b/nav2_smac_planner/lattice_primitives/lattice_generator.py index 9da58922858..f8356ab9322 100644 --- a/nav2_smac_planner/lattice_primitives/lattice_generator.py +++ b/nav2_smac_planner/lattice_primitives/lattice_generator.py @@ -15,13 +15,10 @@ from collections import defaultdict from enum import Enum -from helper import angle_difference -from helper import interpolate_yaws +from helper import angle_difference, interpolate_yaws import numpy as np from rtree import index -from trajectory import Path -from trajectory import Trajectory -from trajectory import TrajectoryParameters +from trajectory import Path, Trajectory, TrajectoryParameters from trajectory_generator import TrajectoryGenerator diff --git a/nav2_smac_planner/lattice_primitives/trajectory.py b/nav2_smac_planner/lattice_primitives/trajectory.py index 4163ab4b413..4b2ed6537b2 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory.py +++ b/nav2_smac_planner/lattice_primitives/trajectory.py @@ -14,8 +14,7 @@ from dataclasses import dataclass -from helper import angle_difference -from helper import normalize_angle +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 8de82bae20b..8efe06149a3 100644 --- a/nav2_smac_planner/lattice_primitives/trajectory_generator.py +++ b/nav2_smac_planner/lattice_primitives/trajectory_generator.py @@ -16,9 +16,7 @@ from typing import Tuple, Union import numpy as np -from trajectory import Path -from trajectory import Trajectory -from trajectory import TrajectoryParameters +from trajectory import Path, Trajectory, TrajectoryParameters logger = logging.getLogger(__name__) 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 5a2cb90d36a..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,12 +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 -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 diff --git a/nav2_system_tests/src/behaviors/backup/backup_tester.py b/nav2_system_tests/src/behaviors/backup/backup_tester.py index b3405553eb3..711441bd4cb 100755 --- a/nav2_system_tests/src/behaviors/backup/backup_tester.py +++ b/nav2_system_tests/src/behaviors/backup/backup_tester.py @@ -17,8 +17,7 @@ import time from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Point32 -from geometry_msgs.msg import PolygonStamped +from geometry_msgs.msg import Point32, PolygonStamped from nav2_msgs.action import BackUp from nav2_msgs.msg import Costmap from nav2_msgs.srv import ManageLifecycleNodes @@ -26,10 +25,7 @@ from rclpy.action import ActionClient from rclpy.duration import Duration from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy +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 aefcd8a80b3..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,13 +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 -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 ef20886cad7..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 @@ -17,8 +17,7 @@ import time from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Point32 -from geometry_msgs.msg import PolygonStamped +from geometry_msgs.msg import Point32, PolygonStamped from nav2_msgs.action import DriveOnHeading from nav2_msgs.msg import Costmap from nav2_msgs.srv import ManageLifecycleNodes @@ -26,10 +25,7 @@ from rclpy.action import ActionClient from rclpy.duration import Duration from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy +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 f50e3cbf883..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,13 +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 -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 494b262b7d9..917df80aeb9 100755 --- a/nav2_system_tests/src/behaviors/spin/spin_tester.py +++ b/nav2_system_tests/src/behaviors/spin/spin_tester.py @@ -17,8 +17,7 @@ import time from action_msgs.msg import GoalStatus -from geometry_msgs.msg import Point32 -from geometry_msgs.msg import PolygonStamped +from geometry_msgs.msg import Point32, PolygonStamped from nav2_msgs.action import Spin from nav2_msgs.msg import Costmap from nav2_msgs.srv import ManageLifecycleNodes @@ -26,10 +25,7 @@ from rclpy.action import ActionClient from rclpy.duration import Duration from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy +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 0c7fca53e79..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,13 +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 -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 3a853ab4506..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,12 +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 -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 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 1663e93b926..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,12 +20,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 -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 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 66a44217dd9..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,12 +20,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 -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py index f3e7043fd6c..c978e570b13 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -23,22 +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 -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy +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 3a7f5339380..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,8 @@ import time from geometry_msgs.msg import PoseStamped -from nav2_msgs.action import ComputePathThroughPoses -from nav2_msgs.action import ComputePathToPose -from nav2_msgs.action import FollowPath -from nav2_msgs.action import SmoothPath -from nav2_simple_commander.robot_navigator import BasicNavigator -from nav2_simple_commander.robot_navigator import TaskResult +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/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py index 10b2e59dd00..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,12 +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 -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 diff --git a/nav2_system_tests/src/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py index 34c6b99b180..6829dc6a494 100755 --- a/nav2_system_tests/src/gps_navigation/tester.py +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -18,8 +18,7 @@ from action_msgs.msg import GoalStatus from geographic_msgs.msg import GeoPose -from nav2_msgs.action import ComputePathToPose -from nav2_msgs.action import FollowGPSWaypoints +from nav2_msgs.action import ComputePathToPose, FollowGPSWaypoints from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters import rclpy diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index 79a454026a4..1520ac09bbe 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -19,11 +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 -from launch.actions import 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/test_planner_costmaps_launch.py b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py index 8fc008dc625..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,8 +17,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_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 df1c5d469e6..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,8 +17,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_testing.legacy import LaunchTestService 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 6e37506ca81..8935cff9a22 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 @@ -20,9 +20,7 @@ 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 @@ -31,10 +29,7 @@ from rclpy.action.client import ActionClient from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy +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 7d0a4758078..11624528010 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 @@ -20,19 +20,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 NavigateThroughPoses from nav2_msgs.srv import ManageLifecycleNodes import rclpy from rclpy.action import ActionClient from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy +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 6601338a358..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 @@ -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 -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy +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 b47df2ba76e..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,16 +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 ExecuteProcess -from launch.actions import GroupAction -from launch.actions import IncludeLaunchDescription -from launch.actions import SetEnvironmentVariable +from launch import LaunchDescription, LaunchService +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 -from launch_ros.actions import PushROSNamespace +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 3976875c976..ce604fc454a 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -20,12 +20,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 -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 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 7bbbf961e58..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,12 +20,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 -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 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 7924e32b907..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,12 +20,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 -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 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 e02b5179f7e..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,12 +20,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 -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index 5d70552c606..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 -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class NavTester(Node): diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py index d66896d5b82..8a70be4e727 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -14,8 +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/waypoint_follower/test_case_launch.py b/nav2_system_tests/src/waypoint_follower/test_case_launch.py index 1f9f0a00548..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,12 +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 -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription -from launch.actions import 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 diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 876d6893e7b..87c7b332c94 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -17,20 +17,15 @@ import time from action_msgs.msg import GoalStatus -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import PoseWithCovarianceStamped -from nav2_msgs.action import ComputePathToPose -from nav2_msgs.action import FollowWaypoints +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped +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 -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy class WaypointFollowerTest(Node): diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index b2d4db1db7c..0ae0f883efe 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -17,9 +17,7 @@ import math import os import pickle -from random import randint -from random import seed -from random import uniform +from random import randint, seed, uniform import time from geometry_msgs.msg import PoseStamped diff --git a/tools/pyproject.toml b/tools/pyproject.toml index b1111696f3c..e0001ef7bff 100644 --- a/tools/pyproject.toml +++ b/tools/pyproject.toml @@ -10,3 +10,5 @@ uri-ignore-words-list = "segue" write-changes = false [tool.isort] profile = "google" +force_single_line = false +line_length = 99 diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py index 5ae6c1b46f8..9c8b716c214 100644 --- a/tools/smoother_benchmarking/metrics.py +++ b/tools/smoother_benchmarking/metrics.py @@ -17,9 +17,7 @@ import math import os import pickle -from random import randint -from random import seed -from random import uniform +from random import randint, seed, uniform from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator diff --git a/tools/smoother_benchmarking/smoother_benchmark_bringup.py b/tools/smoother_benchmarking/smoother_benchmark_bringup.py index 86381e3e0fd..c19d1f2f703 100644 --- a/tools/smoother_benchmarking/smoother_benchmark_bringup.py +++ b/tools/smoother_benchmarking/smoother_benchmark_bringup.py @@ -16,8 +16,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ExecuteProcess -from launch.actions import IncludeLaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node From d94737fbf276aff45152d82e61cfed9436c7be40 Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Mon, 17 Mar 2025 21:48:43 +0000 Subject: [PATCH 05/14] add precommit Signed-off-by: Nils-ChristianIseke --- .github/workflows/precommit.yml | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 .github/workflows/precommit.yml diff --git a/.github/workflows/precommit.yml b/.github/workflows/precommit.yml new file mode 100644 index 00000000000..c5e34d5e247 --- /dev/null +++ b/.github/workflows/precommit.yml @@ -0,0 +1,17 @@ +name: Pre-Commit - Rolling + +on: + workflow_dispatch: + pull_request: + branches: + - main + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: rolling From 5377b656361ef220dce6d6af36013060798f06de Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Mon, 17 Mar 2025 21:58:21 +0000 Subject: [PATCH 06/14] Introducing some issues. Signed-off-by: Nils-ChristianIseke --- .pre-commit-config.yaml | 12 ++++++------ .../nav2_behavior_tree/behavior_tree_engine.hpp | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 918c86fc381..3588a5e6546 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,12 +63,6 @@ repos: 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. @@ -76,6 +70,12 @@ repos: types: [c++] args: ["--reformat"] entry: ament_uncrustify + - id: ament_cpplint + name: ament_cpplint + description: Code style checking using cpplint. + language: system + types: [c++] + entry: ament_cpplint - id: ament_xmllint name: ament_xmllint description: Check XML markup using xmllint. diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index a0d86649c00..8cff42073d5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -15,7 +15,7 @@ #ifndef NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_ #define NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_ - +//TOLONGLINE AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA #include #include #include @@ -33,7 +33,7 @@ namespace nav2_behavior_tree * @enum nav2_behavior_tree::BtStatus * @brief An enum class representing BT execution status */ -enum class BtStatus { SUCCEEDED, FAILED, CANCELED }; +enum class BtStatus { SUCCEEDED, FAILED, CANCELED}; /** * @class nav2_behavior_tree::BehaviorTreeEngine From 194fc4c6b7913f11bba85c67bde792be6d157f1c Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Fri, 21 Mar 2025 08:46:53 +0000 Subject: [PATCH 07/14] Revert "Introducing some issues." This reverts commit 5377b656361ef220dce6d6af36013060798f06de. Signed-off-by: Nils-ChristianIseke --- .pre-commit-config.yaml | 12 ++++++------ .../nav2_behavior_tree/behavior_tree_engine.hpp | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 3588a5e6546..918c86fc381 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -63,6 +63,12 @@ repos: 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. @@ -70,12 +76,6 @@ repos: types: [c++] args: ["--reformat"] entry: ament_uncrustify - - id: ament_cpplint - name: ament_cpplint - description: Code style checking using cpplint. - language: system - types: [c++] - entry: ament_cpplint - id: ament_xmllint name: ament_xmllint description: Check XML markup using xmllint. diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 8cff42073d5..a0d86649c00 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -15,7 +15,7 @@ #ifndef NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_ #define NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_ -//TOLONGLINE AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + #include #include #include @@ -33,7 +33,7 @@ namespace nav2_behavior_tree * @enum nav2_behavior_tree::BtStatus * @brief An enum class representing BT execution status */ -enum class BtStatus { SUCCEEDED, FAILED, CANCELED}; +enum class BtStatus { SUCCEEDED, FAILED, CANCELED }; /** * @class nav2_behavior_tree::BehaviorTreeEngine From 0e886d2177ab140f36a293bfce2f001e88703742 Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Fri, 21 Mar 2025 08:53:17 +0000 Subject: [PATCH 08/14] Removing pre-commit workflow. Signed-off-by: Nils-ChristianIseke --- .github/workflows/precommit.yml | 17 ----------------- 1 file changed, 17 deletions(-) delete mode 100644 .github/workflows/precommit.yml diff --git a/.github/workflows/precommit.yml b/.github/workflows/precommit.yml deleted file mode 100644 index c5e34d5e247..00000000000 --- a/.github/workflows/precommit.yml +++ /dev/null @@ -1,17 +0,0 @@ -name: Pre-Commit - Rolling - -on: - workflow_dispatch: - pull_request: - branches: - - main - -concurrency: - group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: true - -jobs: - pre-commit: - uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master - with: - ros_distro: rolling From 39adf5960822121d180aaa9e8cfc2fb667df293b Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Fri, 21 Mar 2025 09:20:35 +0000 Subject: [PATCH 09/14] Fix formatting error Signed-off-by: Nils-ChristianIseke --- tools/update_readme_table.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tools/update_readme_table.py b/tools/update_readme_table.py index 92ecd96d786..b85f942df9c 100755 --- a/tools/update_readme_table.py +++ b/tools/update_readme_table.py @@ -67,9 +67,10 @@ def getSrcPath(package, prefix, OS): def getBinPath(package, prefix, OS): - def getBinPath(package, prefix, OS): - return f'https://build.ros2.org/job/{prefix}bin_u{OS[0]}64__{package}__ubuntu_{OS}_' \ - f'amd64__binary/' + return ( + f'https://build.ros2.org/job/{prefix}bin_u{OS[0]}64__{package}__ubuntu_{OS}_' + 'amd64__binary/' + ) def createPreamble(Distros): From 8a7ca3983a0b93ef5a94e9517e9598de96be2fe6 Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Fri, 21 Mar 2025 09:42:55 +0000 Subject: [PATCH 10/14] Merge remote-tracking branch 'origin/main' into precommit Signed-off-by: Nils-ChristianIseke --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 9 +- nav2_amcl/src/amcl_node.cpp | 1 - .../include/nav2_behavior_tree/bt_utils.hpp | 20 +-- .../compute_path_through_poses_action.hpp | 2 +- .../action/navigate_through_poses_action.hpp | 4 +- .../remove_in_collision_goals_action.hpp | 8 +- .../action/remove_passed_goals_action.hpp | 6 +- .../are_error_codes_present_condition.hpp | 9 +- .../globally_updated_goal_condition.hpp | 6 +- .../condition/goal_updated_condition.hpp | 6 +- .../decorator/goal_updated_controller.hpp | 4 +- .../plugins/decorator/goal_updater_node.hpp | 14 +- .../plugins/decorator/speed_controller.hpp | 4 +- .../remove_in_collision_goals_action.cpp | 10 +- .../action/remove_passed_goals_action.cpp | 12 +- .../globally_updated_goal_condition.cpp | 2 +- .../condition/goal_updated_condition.cpp | 2 +- .../decorator/goal_updated_controller.cpp | 2 +- .../plugins/decorator/goal_updater_node.cpp | 8 +- .../plugins/decorator/speed_controller.cpp | 2 +- ...test_compute_path_through_poses_action.cpp | 26 ++-- .../action/test_get_pose_from_path_action.cpp | 12 +- .../test_navigate_through_poses_action.cpp | 10 +- .../test_remove_in_collision_goals_action.cpp | 62 ++++----- .../test_remove_passed_goals_action.cpp | 28 ++-- .../test_are_error_codes_present.cpp | 2 +- .../test_goal_updated_controller.cpp | 8 +- .../decorator/test_goal_updater_node.cpp | 34 ++--- .../decorator/test_speed_controller.cpp | 2 +- nav2_behavior_tree/test/test_bt_utils.cpp | 56 ++++---- .../navigators/navigate_through_poses.hpp | 1 - .../src/navigators/navigate_through_poses.cpp | 20 +-- .../nav2_collision_monitor/polygon.hpp | 2 + nav2_collision_monitor/src/polygon.cpp | 11 +- .../plugins/simple_goal_checker.cpp | 2 +- nav2_controller/plugins/test/goal_checker.cpp | 103 ++++++++++++++ .../plugins/test/progress_checker.cpp | 131 +++++++++++++----- .../nav2_costmap_2d/obstacle_layer.hpp | 8 ++ .../plugin_container_layer.hpp | 2 +- .../nav2_costmap_2d/range_sensor_layer.hpp | 1 - .../include/nav2_costmap_2d/static_layer.hpp | 3 +- .../include/nav2_costmap_2d/voxel_layer.hpp | 1 - nav2_costmap_2d/plugins/obstacle_layer.cpp | 26 +++- .../action/ComputePathThroughPoses.action | 2 +- nav2_msgs/action/NavigateThroughPoses.action | 2 +- nav2_planner/src/planner_server.cpp | 6 +- .../include/nav2_rviz_plugins/nav2_panel.hpp | 6 +- nav2_rviz_plugins/src/nav2_panel.cpp | 76 +++++----- .../nav2_simple_commander/robot_navigator.py | 2 +- ...nav_through_poses_tester_error_msg_node.py | 2 +- .../system/nav_through_poses_tester_node.py | 4 +- nav2_waypoint_follower/CMakeLists.txt | 12 +- 52 files changed, 503 insertions(+), 291 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 2c971bc741c..7928f5e9545 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -28,8 +28,15 @@ #include #include -#include "geometry_msgs/msg/pose_stamped.hpp" +// 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" #include "nav2_amcl/motion_model/motion_model.hpp" #include "nav2_amcl/sensors/laser/laser.hpp" diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 016837e2eee..7ae2bb61048 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -28,7 +28,6 @@ #include #include -#include "message_filters/subscriber.hpp" #include "nav2_amcl/angleutils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_amcl/pf/pf.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index 0de7da7925c..63595fef985 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -24,9 +24,9 @@ #include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "nav_msgs/msg/path.hpp" +#include "nav_msgs/msg/goals.hpp" namespace BT { @@ -136,20 +136,20 @@ inline std::vector convertFromString(const Stri } /** - * @brief Parse XML string to geometry_msgs::msg::PoseStampedArray + * @brief Parse XML string to nav_msgs::msg::Goals * @param key XML string - * @return geometry_msgs::msg::PoseStampedArray + * @return nav_msgs::msg::Goals */ template<> -inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView key) +inline nav_msgs::msg::Goals convertFromString(const StringView key) { auto parts = BT::splitString(key, ';'); if ((parts.size() - 2) % 9 != 0) { - throw std::runtime_error("invalid number of fields for PoseStampedArray attribute)"); + throw std::runtime_error("invalid number of fields for Goals attribute)"); } else { - geometry_msgs::msg::PoseStampedArray pose_stamped_array; - pose_stamped_array.header.stamp = rclcpp::Time(BT::convertFromString(parts[0])); - pose_stamped_array.header.frame_id = BT::convertFromString(parts[1]); + nav_msgs::msg::Goals goals_array; + goals_array.header.stamp = rclcpp::Time(BT::convertFromString(parts[0])); + goals_array.header.frame_id = BT::convertFromString(parts[1]); for (size_t i = 2; i < parts.size(); i += 9) { geometry_msgs::msg::PoseStamped pose_stamped; pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString(parts[i])); @@ -161,9 +161,9 @@ inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView k pose_stamped.pose.orientation.y = BT::convertFromString(parts[i + 6]); pose_stamped.pose.orientation.z = BT::convertFromString(parts[i + 7]); pose_stamped.pose.orientation.w = BT::convertFromString(parts[i + 8]); - pose_stamped_array.poses.push_back(pose_stamped); + goals_array.goals.push_back(pose_stamped); } - return pose_stamped_array; + return goals_array; } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index b4e0dd43f94..17b2e1af987 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -80,7 +80,7 @@ class ComputePathThroughPosesAction { return providedBasicPorts( { - BT::InputPort( + BT::InputPort( "goals", "Destinations to plan through"), BT::InputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 9a579d2dfd5..8d636d84e45 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -18,7 +18,7 @@ #include #include -#include "geometry_msgs/msg/pose_stamped_array.hpp" +#include "nav_msgs/msg/goals.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -73,7 +73,7 @@ class NavigateThroughPosesAction : public BtActionNode( + BT::InputPort( "goals", "Destinations to plan through"), BT::InputPort("behavior_tree", "Behavior tree to run"), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp index 5ed0e213ea7..7d3a4615bdb 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/pose_stamped_array.hpp" +#include "nav_msgs/msg/goals.hpp" #include "nav2_behavior_tree/bt_service_node.hpp" #include "nav2_msgs/srv/get_costs.hpp" @@ -52,7 +52,7 @@ class RemoveInCollisionGoals : public BtServiceNode { return providedBasicPorts( { - BT::InputPort("input_goals", + BT::InputPort("input_goals", "Original goals to remove from"), BT::InputPort( "cost_threshold", 254.0, @@ -61,7 +61,7 @@ class RemoveInCollisionGoals : public BtServiceNode BT::InputPort( "consider_unknown_as_obstacle", false, "Whether to consider unknown cost as obstacle"), - BT::OutputPort("output_goals", + BT::OutputPort("output_goals", "Goals with in-collision goals removed"), }); } @@ -70,7 +70,7 @@ class RemoveInCollisionGoals : public BtServiceNode bool use_footprint_; bool consider_unknown_as_obstacle_; double cost_threshold_; - geometry_msgs::msg::PoseStampedArray input_goals_; + nav_msgs::msg::Goals input_goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index ccb400cdab4..4a17896f89c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -19,7 +19,7 @@ #include #include -#include "geometry_msgs/msg/pose_stamped_array.hpp" +#include "nav_msgs/msg/goals.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "behaviortree_cpp/action_node.h" @@ -43,9 +43,9 @@ class RemovePassedGoals : public BT::ActionNodeBase static BT::PortsList providedPorts() { return { - BT::InputPort("input_goals", + BT::InputPort("input_goals", "Original goals to remove viapoints from"), - BT::OutputPort("output_goals", + BT::OutputPort("output_goals", "Goals with passed viapoints removed"), BT::InputPort("radius", 0.5, "radius to goal for it to be considered for removal"), BT::InputPort("robot_base_frame", "Robot base frame"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp index 209958c38d4..67833a383b3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp @@ -34,7 +34,12 @@ class AreErrorCodesPresent : public BT::ConditionNode const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf) { - getInput>("error_codes_to_check", error_codes_to_check_); //NOLINT + std::vector error_codes_to_check_vector; + getInput("error_codes_to_check", error_codes_to_check_vector); //NOLINT + + error_codes_to_check_ = std::set( + error_codes_to_check_vector.begin(), + error_codes_to_check_vector.end()); } AreErrorCodesPresent() = delete; @@ -55,7 +60,7 @@ class AreErrorCodesPresent : public BT::ConditionNode return { BT::InputPort("error_code", "The active error codes"), //NOLINT - BT::InputPort>("error_codes_to_check", "Error codes to check")//NOLINT + BT::InputPort>("error_codes_to_check", "Error codes to check")//NOLINT }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 5e730017d92..3c5e9cc5f9c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" -#include "geometry_msgs/msg/pose_stamped_array.hpp" +#include "nav_msgs/msg/goals.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -59,7 +59,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -70,7 +70,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode bool first_time; rclcpp::Node::SharedPtr node_; geometry_msgs::msg::PoseStamped goal_; - geometry_msgs::msg::PoseStampedArray goals_; + nav_msgs::msg::Goals goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index 4ea81d896b2..7f37723a2b3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -19,7 +19,7 @@ #include #include "behaviortree_cpp/condition_node.h" -#include "geometry_msgs/msg/pose_stamped_array.hpp" +#include "nav_msgs/msg/goals.hpp" #include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree @@ -56,7 +56,7 @@ class GoalUpdatedCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -65,7 +65,7 @@ class GoalUpdatedCondition : public BT::ConditionNode private: geometry_msgs::msg::PoseStamped goal_; - geometry_msgs::msg::PoseStampedArray goals_; + nav_msgs::msg::Goals goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp index 49215ae9a14..f24a7ff3d76 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -50,7 +50,7 @@ class GoalUpdatedController : public BT::DecoratorNode static BT::PortsList providedPorts() { return { - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -66,7 +66,7 @@ class GoalUpdatedController : public BT::DecoratorNode bool goal_was_updated_; geometry_msgs::msg::PoseStamped goal_; - geometry_msgs::msg::PoseStampedArray goals_; + nav_msgs::msg::Goals goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 2bf6b43e9c6..71bcf8f5598 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -21,7 +21,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_stamped_array.hpp" +#include "nav_msgs/msg/goals.hpp" #include "behaviortree_cpp/decorator_node.h" @@ -54,10 +54,10 @@ class GoalUpdater : public BT::DecoratorNode { return { BT::InputPort("input_goal", "Original Goal"), - BT::InputPort("input_goals", "Original Goals"), + BT::InputPort("input_goals", "Original Goals"), BT::OutputPort("output_goal", "Received Goal by subscription"), - BT::OutputPort("output_goals", + BT::OutputPort("output_goals", "Received Goals by subscription") }; } @@ -85,16 +85,16 @@ class GoalUpdater : public BT::DecoratorNode /** * @brief Callback function for goals update topic - * @param msg Shared pointer to geometry_msgs::msg::PoseStampedArray message + * @param msg Shared pointer to nav_msgs::msg::Goals message */ - void callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg); + void callback_updated_goals(const nav_msgs::msg::Goals::SharedPtr msg); rclcpp::Subscription::SharedPtr goal_sub_; - rclcpp::Subscription::SharedPtr goals_sub_; + rclcpp::Subscription::SharedPtr goals_sub_; geometry_msgs::msg::PoseStamped last_goal_received_; bool last_goal_received_set_{false}; - geometry_msgs::msg::PoseStampedArray last_goals_received_; + nav_msgs::msg::Goals last_goals_received_; bool last_goals_received_set_{false}; rclcpp::Node::SharedPtr node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 3d1dcff2962..632a941acbe 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -58,7 +58,7 @@ class SpeedController : public BT::DecoratorNode BT::InputPort("max_rate", 1.0, "Maximum rate"), BT::InputPort("min_speed", 0.0, "Minimum speed"), BT::InputPort("max_speed", 0.5, "Maximum speed"), - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -120,7 +120,7 @@ class SpeedController : public BT::DecoratorNode // current goal geometry_msgs::msg::PoseStamped goal_; - geometry_msgs::msg::PoseStampedArray goals_; + nav_msgs::msg::Goals goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index 76b50ac3f51..68a83771cbe 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -39,7 +39,7 @@ void RemoveInCollisionGoals::on_tick() getInput("input_goals", input_goals_); getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_); - if (input_goals_.poses.empty()) { + if (input_goals_.goals.empty()) { setOutput("output_goals", input_goals_); should_send_request_ = false; return; @@ -47,7 +47,7 @@ void RemoveInCollisionGoals::on_tick() request_ = std::make_shared(); request_->use_footprint = use_footprint_; - for (const auto & goal : input_goals_.poses) { + for (const auto & goal : input_goals_.goals) { request_->poses.push_back(goal); } } @@ -63,16 +63,16 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion( return BT::NodeStatus::FAILURE; } - geometry_msgs::msg::PoseStampedArray valid_goal_poses; + nav_msgs::msg::Goals valid_goal_poses; for (size_t i = 0; i < response->costs.size(); ++i) { if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || response->costs[i] < cost_threshold_) { - valid_goal_poses.poses.push_back(input_goals_.poses[i]); + valid_goal_poses.goals.push_back(input_goals_.goals[i]); } } // Inform if all goals have been removed - if (valid_goal_poses.poses.empty()) { + if (valid_goal_poses.goals.empty()) { RCLCPP_INFO( node_->get_logger(), "All goals are in collision and have been removed from the list"); diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 9b44a0635ef..b3cc3a39ead 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -49,10 +49,10 @@ inline BT::NodeStatus RemovePassedGoals::tick() initialize(); } - geometry_msgs::msg::PoseStampedArray goal_poses; + nav_msgs::msg::Goals goal_poses; getInput("input_goals", goal_poses); - if (goal_poses.poses.empty()) { + if (goal_poses.goals.empty()) { setOutput("output_goals", goal_poses); return BT::NodeStatus::SUCCESS; } @@ -61,21 +61,21 @@ inline BT::NodeStatus RemovePassedGoals::tick() geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( - current_pose, *tf_, goal_poses.poses[0].header.frame_id, robot_base_frame_, + current_pose, *tf_, goal_poses.goals[0].header.frame_id, robot_base_frame_, transform_tolerance_)) { return BT::NodeStatus::FAILURE; } double dist_to_goal; - while (goal_poses.poses.size() > 1) { - dist_to_goal = euclidean_distance(goal_poses.poses[0].pose, current_pose.pose); + while (goal_poses.goals.size() > 1) { + dist_to_goal = euclidean_distance(goal_poses.goals[0].pose, current_pose.pose); if (dist_to_goal > viapoint_achieved_radius_) { break; } - goal_poses.poses.erase(goal_poses.poses.begin()); + goal_poses.goals.erase(goal_poses.goals.begin()); } setOutput("output_goals", goal_poses); diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index fcd3a665134..929f87399db 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -38,7 +38,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() return BT::NodeStatus::SUCCESS; } - geometry_msgs::msg::PoseStampedArray current_goals; + nav_msgs::msg::Goals current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index 06ddf76ae95..488a7472d37 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -33,7 +33,7 @@ BT::NodeStatus GoalUpdatedCondition::tick() return BT::NodeStatus::FAILURE; } - geometry_msgs::msg::PoseStampedArray current_goals; + nav_msgs::msg::Goals current_goals; geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goals", current_goals); BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index d78c5d1e1fd..ad139506fa6 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -44,7 +44,7 @@ BT::NodeStatus GoalUpdatedController::tick() setStatus(BT::NodeStatus::RUNNING); - geometry_msgs::msg::PoseStampedArray current_goals; + nav_msgs::msg::Goals current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 4b970fbd051..de969937704 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -72,7 +72,7 @@ void GoalUpdater::createROSInterfaces() goals_updater_topic_ = goals_updater_topic_new; rclcpp::SubscriptionOptions sub_option; sub_option.callback_group = callback_group_; - goals_sub_ = node_->create_subscription( + goals_sub_ = node_->create_subscription( goals_updater_topic_, rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goals, this, _1), @@ -87,7 +87,7 @@ inline BT::NodeStatus GoalUpdater::tick() } geometry_msgs::msg::PoseStamped goal; - geometry_msgs::msg::PoseStampedArray goals; + nav_msgs::msg::Goals goals; getInput("input_goal", goal); getInput("input_goals", goals); @@ -120,7 +120,7 @@ inline BT::NodeStatus GoalUpdater::tick() } if (last_goals_received_set_) { - if (last_goals_received_.poses.empty()) { + if (last_goals_received_.goals.empty()) { setOutput("output_goals", goals); } else if (last_goals_received_.header.stamp == rclcpp::Time(0)) { RCLCPP_WARN( @@ -154,7 +154,7 @@ GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::Shared } void -GoalUpdater::callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg) +GoalUpdater::callback_updated_goals(const nav_msgs::msg::Goals::SharedPtr msg) { last_goals_received_ = *msg; last_goals_received_set_ = true; diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 347f1ccd25c..b6891108861 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -66,7 +66,7 @@ inline BT::NodeStatus SpeedController::tick() first_tick_ = true; } - geometry_msgs::msg::PoseStampedArray current_goals; + nav_msgs::msg::Goals current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index bb813205456..550047e56e2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -44,7 +44,7 @@ class ComputePathThroughPosesActionServer const auto goal = goal_handle->get_goal(); auto result = std::make_shared(); result->path.poses.resize(2); - result->path.poses[1].pose.position.x = goal->goals.poses[0].pose.position.x; + result->path.poses[1].pose.position.x = goal->goals.goals[0].pose.position.x; if (goal->use_start) { result->path.poses[0].pose.position.x = goal->start.pose.position.x; } else { @@ -137,9 +137,9 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - geometry_msgs::msg::PoseStampedArray goals; - goals.poses.resize(1); - goals.poses[0].pose.position.x = 1.0; + nav_msgs::msg::Goals goals; + goals.goals.resize(1); + goals.goals[0].pose.position.x = 1.0; config_->blackboard->set("goals", goals); // tick until node succeeds @@ -150,7 +150,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); - EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.goals[0].pose.position.x, 1.0); EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); @@ -166,7 +166,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal - goals.poses[0].pose.position.x = -2.5; + goals.goals[0].pose.position.x = -2.5; config_->blackboard->set("goals", goals); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { @@ -174,7 +174,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) } EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, -2.5); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.goals[0].pose.position.x, -2.5); EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); @@ -202,9 +202,9 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) config_->blackboard->set("start", start); // create new goal and set it on blackboard - geometry_msgs::msg::PoseStampedArray goals; - goals.poses.resize(1); - goals.poses[0].pose.position.x = 1.0; + nav_msgs::msg::Goals goals; + goals.goals.resize(1); + goals.goals[0].pose.position.x = 1.0; config_->blackboard->set("goals", goals); // tick until node succeeds @@ -215,7 +215,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); - EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.goals[0].pose.position.x, 1.0); EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0); EXPECT_TRUE(action_server_->getCurrentGoal()->use_start); EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); @@ -232,7 +232,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal and new start - goals.poses[0].pose.position.x = -2.5; + goals.goals[0].pose.position.x = -2.5; start.pose.position.x = -1.5; config_->blackboard->set("goals", goals); config_->blackboard->set("start", start); @@ -242,7 +242,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) } EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, -2.5); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.goals[0].pose.position.x, -2.5); EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); diff --git a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp index 3596af739c3..2e9fcbceadd 100644 --- a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp @@ -21,7 +21,7 @@ #include #include "nav_msgs/msg/path.hpp" -#include "geometry_msgs/msg/pose_stamped_array.hpp" +#include "nav_msgs/msg/goals.hpp" #include "behaviortree_cpp/bt_factory.h" @@ -97,11 +97,11 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) // create new path and set it on blackboard nav_msgs::msg::Path path; - geometry_msgs::msg::PoseStampedArray goals; - goals.poses.resize(2); - goals.poses[0].pose.position.x = 1.0; - goals.poses[1].pose.position.x = 2.0; - path.poses = goals.poses; + nav_msgs::msg::Goals goals; + goals.goals.resize(2); + goals.goals[0].pose.position.x = 1.0; + goals.goals[1].pose.position.x = 2.0; + path.poses = goals.goals; path.header.frame_id = "test_frame_1"; config_->blackboard->set("path", path); diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index cf0c1079068..f73dca6ac28 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -74,7 +74,7 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test "wait_for_service_timeout", std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); - geometry_msgs::msg::PoseStampedArray poses; + nav_msgs::msg::Goals poses; config_->blackboard->set( "goals", poses); @@ -132,10 +132,10 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - geometry_msgs::msg::PoseStampedArray poses; - poses.poses.resize(1); - poses.poses[0].pose.position.x = -2.5; - poses.poses[0].pose.orientation.x = 1.0; + nav_msgs::msg::Goals poses; + poses.goals.resize(1); + poses.goals[0].pose.position.x = -2.5; + poses.goals[0].pose.orientation.x = 1.0; config_->blackboard->set("goals", poses); // tick until node succeeds diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp index ea750fe4193..4a475b0fd8c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -148,19 +148,19 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - geometry_msgs::msg::PoseStampedArray poses; - poses.poses.resize(4); - poses.poses[0].pose.position.x = 0.0; - poses.poses[0].pose.position.y = 0.0; + nav_msgs::msg::Goals poses; + poses.goals.resize(4); + poses.goals[0].pose.position.x = 0.0; + poses.goals[0].pose.position.y = 0.0; - poses.poses[1].pose.position.x = 0.5; - poses.poses[1].pose.position.y = 0.0; + poses.goals[1].pose.position.x = 0.5; + poses.goals[1].pose.position.y = 0.0; - poses.poses[2].pose.position.x = 1.0; - poses.poses[2].pose.position.y = 0.0; + poses.goals[2].pose.position.x = 1.0; + poses.goals[2].pose.position.y = 0.0; - poses.poses[3].pose.position.x = 2.0; - poses.poses[3].pose.position.y = 0.0; + poses.goals[3].pose.position.x = 2.0; + poses.goals[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -172,13 +172,13 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); // check that it removed the point in range - geometry_msgs::msg::PoseStampedArray output_poses; + nav_msgs::msg::Goals output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.poses.size(), 3u); - EXPECT_EQ(output_poses.poses[0], poses.poses[0]); - EXPECT_EQ(output_poses.poses[1], poses.poses[1]); - EXPECT_EQ(output_poses.poses[2], poses.poses[2]); + EXPECT_EQ(output_poses.goals.size(), 3u); + EXPECT_EQ(output_poses.goals[0], poses.goals[0]); + EXPECT_EQ(output_poses.goals[1], poses.goals[1]); + EXPECT_EQ(output_poses.goals[2], poses.goals[2]); } TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fail) @@ -195,19 +195,19 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fa tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - geometry_msgs::msg::PoseStampedArray poses; - poses.poses.resize(4); - poses.poses[0].pose.position.x = 0.0; - poses.poses[0].pose.position.y = 0.0; + nav_msgs::msg::Goals poses; + poses.goals.resize(4); + poses.goals[0].pose.position.x = 0.0; + poses.goals[0].pose.position.y = 0.0; - poses.poses[1].pose.position.x = 0.5; - poses.poses[1].pose.position.y = 0.0; + poses.goals[1].pose.position.x = 0.5; + poses.goals[1].pose.position.y = 0.0; - poses.poses[2].pose.position.x = 1.0; - poses.poses[2].pose.position.y = 0.0; + poses.goals[2].pose.position.x = 1.0; + poses.goals[2].pose.position.y = 0.0; - poses.poses[3].pose.position.x = 2.0; - poses.poses[3].pose.position.y = 0.0; + poses.goals[3].pose.position.x = 2.0; + poses.goals[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -219,14 +219,14 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fa // check that it failed and returned the original goals EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); - geometry_msgs::msg::PoseStampedArray output_poses; + nav_msgs::msg::Goals output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.poses.size(), 4u); - EXPECT_EQ(output_poses.poses[0], poses.poses[0]); - EXPECT_EQ(output_poses.poses[1], poses.poses[1]); - EXPECT_EQ(output_poses.poses[2], poses.poses[2]); - EXPECT_EQ(output_poses.poses[3], poses.poses[3]); + EXPECT_EQ(output_poses.goals.size(), 4u); + EXPECT_EQ(output_poses.goals[0], poses.goals[0]); + EXPECT_EQ(output_poses.goals[1], poses.goals[1]); + EXPECT_EQ(output_poses.goals[2], poses.goals[2]); + EXPECT_EQ(output_poses.goals[3], poses.goals[3]); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 9ed40d8bad7..74fd7c6fdad 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -114,19 +114,19 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - geometry_msgs::msg::PoseStampedArray poses; - poses.poses.resize(4); - poses.poses[0].pose.position.x = 0.0; - poses.poses[0].pose.position.y = 0.0; + nav_msgs::msg::Goals poses; + poses.goals.resize(4); + poses.goals[0].pose.position.x = 0.0; + poses.goals[0].pose.position.y = 0.0; - poses.poses[1].pose.position.x = 0.5; - poses.poses[1].pose.position.y = 0.0; + poses.goals[1].pose.position.x = 0.5; + poses.goals[1].pose.position.y = 0.0; - poses.poses[2].pose.position.x = 1.0; - poses.poses[2].pose.position.y = 0.0; + poses.goals[2].pose.position.x = 1.0; + poses.goals[2].pose.position.y = 0.0; - poses.poses[3].pose.position.x = 2.0; - poses.poses[3].pose.position.y = 0.0; + poses.goals[3].pose.position.x = 2.0; + poses.goals[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -136,12 +136,12 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) } // check that it removed the point in range - geometry_msgs::msg::PoseStampedArray output_poses; + nav_msgs::msg::Goals output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.poses.size(), 2u); - EXPECT_EQ(output_poses.poses[0], poses.poses[2]); - EXPECT_EQ(output_poses.poses[1], poses.poses[3]); + EXPECT_EQ(output_poses.goals.size(), 2u); + EXPECT_EQ(output_poses.goals[0], poses.goals[2]); + EXPECT_EQ(output_poses.goals[1], poses.goals[3]); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp index 111cda7d005..aa0372f49b3 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp @@ -28,7 +28,7 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF void SetUp() { uint16_t error_code = ActionResult::NONE; - std::set error_codes_to_check = {ActionResult::UNKNOWN}; //NOLINT + std::vector error_codes_to_check = {ActionResult::UNKNOWN}; //NOLINT config_->blackboard->set("error_code", error_code); config_->blackboard->set("error_codes_to_check", error_codes_to_check); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp index d1c3c5e4207..63b4cb336b0 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -32,8 +32,8 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree // Setting fake goals on blackboard geometry_msgs::msg::PoseStamped goal1; goal1.header.stamp = node_->now(); - geometry_msgs::msg::PoseStampedArray poses1; - poses1.poses.push_back(goal1); + nav_msgs::msg::Goals poses1; + poses1.goals.push_back(goal1); config_->blackboard->set("goal", goal1); config_->blackboard->set("goals", poses1); bt_node_ = std::make_shared( @@ -63,8 +63,8 @@ TEST_F(GoalUpdatedControllerTestFixture, test_behavior) // Creating updated fake-goals geometry_msgs::msg::PoseStamped goal2; goal2.header.stamp = node_->now(); - geometry_msgs::msg::PoseStampedArray poses2; - poses2.poses.push_back(goal2); + nav_msgs::msg::Goals poses2; + poses2.goals.push_back(goal2); // starting in idle EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 2f70061da7f..fa9cc46ab24 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -98,17 +98,17 @@ TEST_F(GoalUpdaterTestFixture, test_tick) // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; - geometry_msgs::msg::PoseStampedArray goals; + nav_msgs::msg::Goals goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; - goals.poses.push_back(goal); + goals.goals.push_back(goal); config_->blackboard->set("goal", goal); config_->blackboard->set("goals", goals); // tick tree without publishing updated goal and get updated_goal tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - geometry_msgs::msg::PoseStampedArray updated_goals; + nav_msgs::msg::Goals updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); } @@ -130,31 +130,31 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) auto goal_updater_pub = node_->create_publisher("goal_update", 10); auto goals_updater_pub = - node_->create_publisher("goals_update", 10); + node_->create_publisher("goals_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; - geometry_msgs::msg::PoseStampedArray goals; + nav_msgs::msg::Goals goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; goals.header.stamp = goal.header.stamp; - goals.poses.push_back(goal); + goals.goals.push_back(goal); config_->blackboard->set("goal", goal); config_->blackboard->set("goals", goals); // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update; - geometry_msgs::msg::PoseStampedArray goals_to_update; + nav_msgs::msg::Goals goals_to_update; goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0); goal_to_update.pose.position.x = 2.0; goals_to_update.header.stamp = goal_to_update.header.stamp; - goals_to_update.poses.push_back(goal_to_update); + goals_to_update.goals.push_back(goal_to_update); goal_updater_pub->publish(goal_to_update); goals_updater_pub->publish(goals_to_update); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - geometry_msgs::msg::PoseStampedArray updated_goals; + nav_msgs::msg::Goals updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); @@ -181,31 +181,31 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) auto goal_updater_pub = node_->create_publisher("goal_update", 10); auto goals_updater_pub = - node_->create_publisher("goals_update", 10); + node_->create_publisher("goals_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; - geometry_msgs::msg::PoseStampedArray goals; + nav_msgs::msg::Goals goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; - goals.poses.push_back(goal); + goals.goals.push_back(goal); config_->blackboard->set("goal", goal); config_->blackboard->set("goals", goals); // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update_1; - geometry_msgs::msg::PoseStampedArray goals_to_update_1; + nav_msgs::msg::Goals goals_to_update_1; goal_to_update_1.header.stamp = node_->now(); goal_to_update_1.pose.position.x = 2.0; goals_to_update_1.header.stamp = goal_to_update_1.header.stamp; - goals_to_update_1.poses.push_back(goal_to_update_1); + goals_to_update_1.goals.push_back(goal_to_update_1); geometry_msgs::msg::PoseStamped goal_to_update_2; - geometry_msgs::msg::PoseStampedArray goals_to_update_2; + nav_msgs::msg::Goals goals_to_update_2; goal_to_update_2.header.stamp = node_->now(); goal_to_update_2.pose.position.x = 3.0; goals_to_update_2.header.stamp = goal_to_update_2.header.stamp; - goals_to_update_2.poses.push_back(goal_to_update_2); + goals_to_update_2.goals.push_back(goal_to_update_2); goal_updater_pub->publish(goal_to_update_1); goals_updater_pub->publish(goals_to_update_1); @@ -213,7 +213,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) goals_updater_pub->publish(goals_to_update_2); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - geometry_msgs::msg::PoseStampedArray updated_goals; + nav_msgs::msg::Goals updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index ae07ee3c21b..45b35f00daf 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -42,7 +42,7 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi goal.header.stamp = node_->now(); config_->blackboard->set("goal", goal); - geometry_msgs::msg::PoseStampedArray fake_poses; + nav_msgs::msg::Goals fake_poses; config_->blackboard->set("goals", fake_poses); // NOLINT config_->input_ports["min_rate"] = 0.1; diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 87527e05e8e..8d68a5a1c79 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -258,68 +258,68 @@ TEST(PoseStampedVectorPortTest, test_correct_syntax) EXPECT_EQ(values[1].pose.orientation.w, 14.0); } -TEST(PoseStampedArrayPortTest, test_wrong_syntax) +TEST(GoalsArrayPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + )"; BT::BehaviorTreeFactory factory; - factory.registerNodeType>( - "PoseStampedArrayPortTest"); + factory.registerNodeType>( + "GoalsArrayPortTest"); EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( - + )"; EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } -TEST(PoseStampedArrayPortTest, test_correct_syntax) +TEST(GoalsArrayPortTest, test_correct_syntax) { std::string xml_txt = R"( - + )"; BT::BehaviorTreeFactory factory; - factory.registerNodeType>( - "PoseStampedArrayPortTest"); + factory.registerNodeType>( + "GoalsArrayPortTest"); auto tree = factory.createTreeFromText(xml_txt); tree = factory.createTreeFromText(xml_txt); - geometry_msgs::msg::PoseStampedArray values; + nav_msgs::msg::Goals values; tree.rootNode()->getInput("test", values); - EXPECT_EQ(rclcpp::Time(values.poses[0].header.stamp).nanoseconds(), 0); - EXPECT_EQ(values.poses[0].header.frame_id, "map"); - EXPECT_EQ(values.poses[0].pose.position.x, 1.0); - EXPECT_EQ(values.poses[0].pose.position.y, 2.0); - EXPECT_EQ(values.poses[0].pose.position.z, 3.0); - EXPECT_EQ(values.poses[0].pose.orientation.x, 4.0); - EXPECT_EQ(values.poses[0].pose.orientation.y, 5.0); - EXPECT_EQ(values.poses[0].pose.orientation.z, 6.0); - EXPECT_EQ(values.poses[0].pose.orientation.w, 7.0); - EXPECT_EQ(rclcpp::Time(values.poses[1].header.stamp).nanoseconds(), 0); - EXPECT_EQ(values.poses[1].header.frame_id, "odom"); - EXPECT_EQ(values.poses[1].pose.position.x, 8.0); - EXPECT_EQ(values.poses[1].pose.position.y, 9.0); - EXPECT_EQ(values.poses[1].pose.position.z, 10.0); - EXPECT_EQ(values.poses[1].pose.orientation.x, 11.0); - EXPECT_EQ(values.poses[1].pose.orientation.y, 12.0); - EXPECT_EQ(values.poses[1].pose.orientation.z, 13.0); - EXPECT_EQ(values.poses[1].pose.orientation.w, 14.0); + EXPECT_EQ(rclcpp::Time(values.goals[0].header.stamp).nanoseconds(), 0); + EXPECT_EQ(values.goals[0].header.frame_id, "map"); + EXPECT_EQ(values.goals[0].pose.position.x, 1.0); + EXPECT_EQ(values.goals[0].pose.position.y, 2.0); + EXPECT_EQ(values.goals[0].pose.position.z, 3.0); + EXPECT_EQ(values.goals[0].pose.orientation.x, 4.0); + EXPECT_EQ(values.goals[0].pose.orientation.y, 5.0); + EXPECT_EQ(values.goals[0].pose.orientation.z, 6.0); + EXPECT_EQ(values.goals[0].pose.orientation.w, 7.0); + EXPECT_EQ(rclcpp::Time(values.goals[1].header.stamp).nanoseconds(), 0); + EXPECT_EQ(values.goals[1].header.frame_id, "odom"); + EXPECT_EQ(values.goals[1].pose.position.x, 8.0); + EXPECT_EQ(values.goals[1].pose.position.y, 9.0); + EXPECT_EQ(values.goals[1].pose.position.z, 10.0); + EXPECT_EQ(values.goals[1].pose.orientation.x, 11.0); + EXPECT_EQ(values.goals[1].pose.orientation.y, 12.0); + EXPECT_EQ(values.goals[1].pose.orientation.z, 13.0); + EXPECT_EQ(values.goals[1].pose.orientation.w, 14.0); } TEST(PathPortTest, test_wrong_syntax) diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index 38f7228c7b5..de68270e29b 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -21,7 +21,6 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav_msgs/msg/path.hpp" diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index d4b319f125e..bcadbafa10c 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -113,10 +113,10 @@ NavigateThroughPosesNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); - geometry_msgs::msg::PoseStampedArray goal_poses; + nav_msgs::msg::Goals goal_poses; [[maybe_unused]] auto res = blackboard->get(goals_blackboard_id_, goal_poses); - if (goal_poses.poses.size() == 0) { + if (goal_poses.goals.size() == 0) { bt_action_server_->publishFeedback(feedback_msg); return; } @@ -178,7 +178,7 @@ NavigateThroughPosesNavigator::onLoop() feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; - feedback_msg->number_of_poses_remaining = goal_poses.poses.size(); + feedback_msg->number_of_poses_remaining = goal_poses.goals.size(); bt_action_server_->publishFeedback(feedback_msg); } @@ -228,9 +228,9 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr return false; } - geometry_msgs::msg::PoseStampedArray pose_stamped_array = goal->poses; + nav_msgs::msg::Goals goals_array = goal->poses; int i = 0; - for (auto & goal_pose : pose_stamped_array.poses) { + for (auto & goal_pose : goals_array.goals) { if (!nav2_util::transformPoseInTargetFrame( goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, feedback_utils_.transform_tolerance)) @@ -246,11 +246,11 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr i++; } - if (pose_stamped_array.poses.size() > 0) { + if (goals_array.goals.size() > 0) { RCLCPP_INFO( logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)", - pose_stamped_array.poses.size(), pose_stamped_array.poses.back().pose.position.x, - pose_stamped_array.poses.back().pose.position.y); + goals_array.goals.size(), goals_array.goals.back().pose.position.x, + goals_array.goals.back().pose.position.y); } // Reset state for new action feedback @@ -259,8 +259,8 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard->set(goals_blackboard_id_, - std::move(pose_stamped_array)); + blackboard->set(goals_blackboard_id_, + std::move(goals_array)); return true; } diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index b1438dc01f8..5dff3d7371c 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -298,6 +298,8 @@ class Polygon std::string base_frame_id_; /// @brief Transform tolerance tf2::Duration transform_tolerance_; + /// @brief Collision monitor node's clock + rclcpp::Clock::SharedPtr node_clock_; // Visualization /// @brief Whether to publish the polygon diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index a528c74d640..1e087070191 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -39,7 +39,8 @@ Polygon::Polygon( : node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING), slowdown_ratio_(0.0), linear_limit_(0.0), angular_limit_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer), - base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance) + base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance), + node_clock_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str()); } @@ -51,6 +52,7 @@ Polygon::~Polygon() polygon_pub_.reset(); poly_.clear(); dyn_params_handler_.reset(); + node_clock_.reset(); } bool Polygon::configure() @@ -60,6 +62,7 @@ bool Polygon::configure() throw std::runtime_error{"Failed to lock node"}; } + node_clock_ = node->get_clock(); std::string polygon_sub_topic, polygon_pub_topic, footprint_topic; if (!getParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) { @@ -588,9 +591,11 @@ Polygon::dynamicParametersCallback( void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) { - RCLCPP_INFO( + RCLCPP_INFO_THROTTLE( logger_, - "[%s]: Polygon shape update has been arrived", + *node_clock_, + 2000, + "[%s]: Polygon shape update has arrived", polygon_name_.c_str()); updatePolygon(msg); } diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 30b90f4f4ab..d49b94c2873 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -114,7 +114,7 @@ bool SimpleGoalChecker::isGoalReached( double dyaw = angles::shortest_angular_distance( tf2::getYaw(query_pose.orientation), tf2::getYaw(goal_pose.orientation)); - return fabs(dyaw) < yaw_goal_tolerance_; + return fabs(dyaw) <= yaw_goal_tolerance_; } bool SimpleGoalChecker::getTolerances( diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 303921bb17e..5faba1bdc79 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -40,6 +40,7 @@ #include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "eigen3/Eigen/Geometry" using nav2_controller::SimpleGoalChecker; using nav2_controller::StoppedGoalChecker; @@ -237,6 +238,108 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) EXPECT_EQ(pose_tol.position.y, 200.0); } +TEST(StoppedGoalChecker, is_reached) +{ + auto x = std::make_shared("goal_checker"); + + SimpleGoalChecker gc; + StoppedGoalChecker sgc; + auto costmap = std::make_shared("test_costmap"); + + sgc.initialize(x, "test", costmap); + gc.initialize(x, "test2", costmap); + geometry_msgs::msg::Pose goal_pose; + geometry_msgs::msg::Twist velocity; + geometry_msgs::msg::Pose current_pose; + + // Current linear x position is tolerance away from goal + current_pose.position.x = 0.25; + velocity.linear.x = 0.25; + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current linear x speed exceeds tolerance + velocity.linear.x = 0.25 + std::numeric_limits::epsilon(); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current linear x position is further than tolerance away from goal + current_pose.position.x = 0.25 + std::numeric_limits::epsilon(); + velocity.linear.x = 0.25; + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + current_pose.position.x = 0.0; + velocity.linear.x = 0.0; + + // Current linear position is tolerance away from goal + current_pose.position.x = 0.25 / std::sqrt(2); + current_pose.position.y = 0.25 / std::sqrt(2); + velocity.linear.x = 0.25 / std::sqrt(2); + velocity.linear.y = 0.25 / std::sqrt(2); + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current linear speed exceeds tolerance + velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current linear position is further than tolerance away from goal + current_pose.position.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + current_pose.position.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); + velocity.linear.x = 0.25 / std::sqrt(2); + velocity.linear.y = 0.25 / std::sqrt(2); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + current_pose.position.x = 0.0; + velocity.linear.x = 0.0; + + + // Current angular position is tolerance away from goal + auto quat = + (Eigen::AngleAxisd::Identity() * Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ())).coeffs(); + // epsilon for orientation is a lot bigger than double limit, probably from TF getYaw + auto quat_epsilon = + (Eigen::AngleAxisd::Identity() * + Eigen::AngleAxisd(0.25 + 1.0E-15, Eigen::Vector3d::UnitZ())).coeffs(); + + current_pose.orientation.z = quat[2]; + current_pose.orientation.w = quat[3]; + velocity.angular.z = 0.25; + EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current angular speed exceeds tolerance + velocity.angular.z = 0.25 + std::numeric_limits::epsilon(); + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); + sgc.reset(); + gc.reset(); + + // Current angular position is further than tolerance away from goal + current_pose.orientation.z = quat_epsilon[2]; + current_pose.orientation.w = quat_epsilon[3]; + velocity.angular.z = 0.25; + EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); + EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp index 228f51fe9c9..4c5e0b28a90 100644 --- a/nav2_controller/plugins/test/progress_checker.cpp +++ b/nav2_controller/plugins/test/progress_checker.cpp @@ -84,12 +84,11 @@ class TestLifecycleNode : public nav2_util::LifecycleNode } }; -void checkMacro( +bool checkMacro( nav2_core::ProgressChecker & pc, double x0, double y0, double theta0, double x1, double y1, double theta1, - int delay, - bool expected_result) + int delay) { pc.reset(); geometry_msgs::msg::PoseStamped pose0, pose1; @@ -101,11 +100,7 @@ void checkMacro( pose1.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1); EXPECT_TRUE(pc.check(pose0)); rclcpp::sleep_for(std::chrono::milliseconds(delay)); - if (expected_result) { - EXPECT_TRUE(pc.check(pose1)); - } else { - EXPECT_FALSE(pc.check(pose1)); - } + return pc.check(pose1); } TEST(SimpleProgressChecker, progress_checker_reset) @@ -125,7 +120,7 @@ TEST(SimpleProgressChecker, unit_tests) SimpleProgressChecker pc; pc.initialize(x, "nav2_controller"); - double time_allowance = 0.5; + double time_allowance = 0.1; int half_time_allowance_ms = static_cast(time_allowance * 0.5 * 1000); int twice_time_allowance_ms = static_cast(time_allowance * 2.0 * 1000); @@ -147,23 +142,83 @@ TEST(SimpleProgressChecker, unit_tests) // BELOW time allowance (set to time_allowance) // no movement - checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms)); // translation below required_movement_radius (default 0.5) - checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true); - checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms)); // translation above required_movement_radius (default 0.5) - checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true); - checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms)); // ABOVE time allowance (set to time_allowance) // no movement - checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false); + EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); // translation below required_movement_radius (default 0.5) - checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false); - checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false); + EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms)); + EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms)); // translation above required_movement_radius (default 0.5) - checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true); - checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms)); +} + +TEST(SimpleProgressChecker, required_movement_radius) { + auto lifecycle_node = std::make_shared("progress_checker"); + + SimpleProgressChecker progress_checker; + progress_checker.initialize(lifecycle_node, "nav2_controller"); + + auto parameters_client = std::make_shared( + lifecycle_node->get_node_base_interface(), lifecycle_node->get_node_topics_interface(), + lifecycle_node->get_node_graph_interface(), + lifecycle_node->get_node_services_interface()); + + constexpr double radius = 0.25; + constexpr double time_allowance = 0.05; + constexpr double twice_time_allowance_ms = time_allowance * 2.0 * 1000; + auto results = parameters_client->set_parameters_atomically( + {rclcpp::Parameter("nav2_controller.required_movement_radius", radius), + rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)}); + rclcpp::spin_until_future_complete( + lifecycle_node->get_node_base_interface(), + results); + + EXPECT_EQ( + lifecycle_node->get_parameter("nav2_controller.required_movement_radius").as_double(), + radius); + + // ABOVE time allowance (set to time_allowance) + // no movement + EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); + // translation at required_movement_radius one axis + EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, radius, 0, 0, twice_time_allowance_ms)); + EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, 0, radius, 0, twice_time_allowance_ms)); + constexpr auto axis = radius / std::sqrt(2); + EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, axis, axis, 0, twice_time_allowance_ms)); + // translation at required_movement_radius both axes + + // translation above required_movement_radius one axis + constexpr auto above = radius + std::numeric_limits::epsilon(); + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, above, 0, 0, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, above, 0, twice_time_allowance_ms)); + // translation at required_movement_radius both axes + constexpr auto above_axis = axis + std::numeric_limits::epsilon(); + EXPECT_TRUE( + checkMacro(progress_checker, 0, 0, 0, above_axis, above_axis, 0, twice_time_allowance_ms)); + + // Edge case, negative radius always true + results = parameters_client->set_parameters_atomically( + {rclcpp::Parameter("nav2_controller.required_movement_radius", -radius)}); + rclcpp::spin_until_future_complete( + lifecycle_node->get_node_base_interface(), + results); + EXPECT_EQ( + lifecycle_node->get_parameter("nav2_controller.required_movement_radius").as_double(), + -radius); + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); + // translation at required_movement_radius one axis + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, radius, 0, 0, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, radius, 0, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, axis, axis, 0, twice_time_allowance_ms)); } TEST(PoseProgressChecker, pose_progress_checker_reset) @@ -183,7 +238,7 @@ TEST(PoseProgressChecker, unit_tests) PoseProgressChecker rpc; rpc.initialize(x, "nav2_controller"); - double time_allowance = 0.5; + double time_allowance = 0.1; int half_time_allowance_ms = static_cast(time_allowance * 0.5 * 1000); int twice_time_allowance_ms = static_cast(time_allowance * 2.0 * 1000); @@ -205,35 +260,35 @@ TEST(PoseProgressChecker, unit_tests) // BELOW time allowance (set to time_allowance) // no movement - checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms)); // translation below required_movement_radius (default 0.5) - checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms)); // rotation below required_movement_angle (default 0.5) - checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms)); // translation above required_movement_radius (default 0.5) - checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms)); // rotation above required_movement_angle (default 0.5) - checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms)); // ABOVE time allowance (set to time_allowance) // no movement - checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false); + EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); // translation below required_movement_radius (default 0.5) - checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false); - checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false); + EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms)); + EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms)); // rotation below required_movement_angle (default 0.5) - checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms, false); - checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms, false); + EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms)); + EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms)); // translation above required_movement_radius (default 0.5) - checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms)); // rotation above required_movement_angle (default 0.5) - checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms, true); - checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms, true); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms)); + EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms)); } int main(int argc, char ** argv) 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 858d89b0aca..05252015fb3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -48,7 +48,15 @@ #pragma GCC diagnostic ignored "-Wreorder" #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" #include "sensor_msgs/msg/point_cloud.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp index 2b34d409a50..2e00a412637 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp @@ -20,6 +20,7 @@ #include #include #include + #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" @@ -27,7 +28,6 @@ #include "nav2_costmap_2d/observation_buffer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "tf2_ros/message_filter.h" -#include "message_filters/subscriber.hpp" #include "pluginlib/class_loader.hpp" using nav2_costmap_2d::LETHAL_OBSTACLE; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp index 8d39116c607..3f5fce1537f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -42,7 +42,6 @@ #include #include "map_msgs/msg/occupancy_grid_update.hpp" -#include "message_filters/subscriber.hpp" #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index c938f30f55e..307385bd4a8 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -43,11 +43,10 @@ #include #include "map_msgs/msg/occupancy_grid_update.hpp" -#include "message_filters/subscriber.hpp" +#include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" -#include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/footprint.hpp" namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index ce66cf0c333..477020a3b65 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -39,7 +39,6 @@ #define NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_ #include -#include "message_filters/subscriber.hpp" #include #include diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 51c018ae427..a05548a8cde 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -230,8 +230,19 @@ void ObstacleLayer::onInitialize() // create a callback for the topic if (data_type == "LaserScan") { - auto sub = std::make_shared> sub; + + // For Jazzy compatibility + #if RCLCPP_VERSION_GTE(29, 0, 0) + sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); + #else + sub = std::make_shared>( + node, topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt); + #endif + sub->unsubscribe(); auto filter = std::make_shared>( @@ -259,8 +270,19 @@ void ObstacleLayer::onInitialize() observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05)); } else { - auto sub = std::make_shared> sub; + + // For Jazzy compatibility + #if RCLCPP_VERSION_GTE(29, 0, 0) + sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); + #else + sub = std::make_shared>( + node, topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt); + #endif + sub->unsubscribe(); if (inf_is_valid) { diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index 0e07373c338..757f679e85a 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,5 +1,5 @@ #goal definition -geometry_msgs/PoseStampedArray goals +nav_msgs/Goals goals geometry_msgs/PoseStamped start string planner_id bool use_start # If false, use current robot pose as path start, if true, use start above instead diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 0efffe433ec..a4301f36e4a 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,6 +1,6 @@ #goal definition -geometry_msgs/PoseStampedArray poses +nav_msgs/Goals poses string behavior_tree --- #result definition diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 933bd7664d1..be65ffef9c8 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -394,7 +394,7 @@ void PlannerServer::computePlanThroughPoses() getPreemptedGoalIfRequested(action_server_poses_, goal); - if (goal->goals.poses.empty()) { + if (goal->goals.goals.empty()) { throw nav2_core::NoViapointsGiven("No viapoints given"); } @@ -409,7 +409,7 @@ void PlannerServer::computePlanThroughPoses() }; // Get consecutive paths through these points - for (unsigned int i = 0; i != goal->goals.poses.size(); i++) { + for (unsigned int i = 0; i != goal->goals.goals.size(); i++) { // Get starting point if (i == 0) { curr_start = start; @@ -419,7 +419,7 @@ void PlannerServer::computePlanThroughPoses() curr_start = concat_path.poses.back(); curr_start.header = concat_path.header; } - curr_goal = goal->goals.poses[i]; + curr_goal = goal->goals.goals[i]; // Transform them into the global frame if (!transformPosesToGlobalFrame(curr_start, curr_goal)) { diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index b4c25d5fb3c..2239c47fe5d 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -102,7 +102,7 @@ private Q_SLOTS: std::vector orientation); void startWaypointFollowing(std::vector poses); void startNavigation(geometry_msgs::msg::PoseStamped); - void startNavThroughPoses(geometry_msgs::msg::PoseStampedArray poses); + void startNavThroughPoses(nav_msgs::msg::Goals poses); using NavigationGoalHandle = rclcpp_action::ClientGoalHandle; using WaypointFollowerGoalHandle = @@ -196,8 +196,8 @@ private Q_SLOTS: QState * accumulated_wp_{nullptr}; QState * accumulated_nav_through_poses_{nullptr}; - geometry_msgs::msg::PoseStampedArray acummulated_poses_; - geometry_msgs::msg::PoseStampedArray store_poses_; + nav_msgs::msg::Goals acummulated_poses_; + nav_msgs::msg::Goals store_poses_; // Publish the visual markers with the waypoints void updateWpNavigationMarkers(); diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 4b3a5f49051..a956108ce5f 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -675,7 +675,7 @@ void Nav2Panel::loophandler() void Nav2Panel::handleGoalLoader() { - acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); + acummulated_poses_ = nav_msgs::msg::Goals(); std::cout << "Loading Waypoints!" << std::endl; @@ -699,7 +699,7 @@ void Nav2Panel::handleGoalLoader() auto waypoint = waypoint_iter[it->first.as()]; auto pose = waypoint["pose"].as>(); auto orientation = waypoint["orientation"].as>(); - acummulated_poses_.poses.push_back(convert_to_msg(pose, orientation)); + acummulated_poses_.goals.push_back(convert_to_msg(pose, orientation)); } // Publishing Waypoint Navigation marker after loading wp's @@ -731,7 +731,7 @@ void Nav2Panel::handleGoalSaver() { // Check if the waypoints are accumulated - if (acummulated_poses_.poses.empty()) { + if (acummulated_poses_.goals.empty()) { std::cout << "No accumulated Points to Save!" << std::endl; return; } else { @@ -744,19 +744,19 @@ void Nav2Panel::handleGoalSaver() out << YAML::BeginMap; // Save WPs to data structure - for (unsigned int i = 0; i < acummulated_poses_.poses.size(); ++i) { + for (unsigned int i = 0; i < acummulated_poses_.goals.size(); ++i) { out << YAML::Key << "waypoint" + std::to_string(i); out << YAML::BeginMap; out << YAML::Key << "pose"; std::vector pose = - {acummulated_poses_.poses[i].pose.position.x, acummulated_poses_.poses[i].pose.position.y, - acummulated_poses_.poses[i].pose.position.z}; + {acummulated_poses_.goals[i].pose.position.x, acummulated_poses_.goals[i].pose.position.y, + acummulated_poses_.goals[i].pose.position.z}; out << YAML::Value << pose; out << YAML::Key << "orientation"; std::vector orientation = - {acummulated_poses_.poses[i].pose.orientation.w, acummulated_poses_.poses[i].pose.orientation.x, - acummulated_poses_.poses[i].pose.orientation.y, - acummulated_poses_.poses[i].pose.orientation.z}; + {acummulated_poses_.goals[i].pose.orientation.w, acummulated_poses_.goals[i].pose.orientation.x, + acummulated_poses_.goals[i].pose.orientation.y, + acummulated_poses_.goals[i].pose.orientation.z}; out << YAML::Value << orientation; out << YAML::EndMap; } @@ -837,10 +837,10 @@ Nav2Panel::onInitialize() // Clearing all the stored values once reaching the final goal if ( loop_count_ == stoi(nr_of_loops_->displayText().toStdString()) && - goal_index_ == static_cast(store_poses_.poses.size()) - 1 && + goal_index_ == static_cast(store_poses_.goals.size()) - 1 && msg->status_list.back().status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { - store_poses_ = geometry_msgs::msg::PoseStampedArray(); + store_poses_ = nav_msgs::msg::Goals(); waypoint_status_indicator_->clear(); loop_no_ = "0"; loop_count_ = 0; @@ -936,8 +936,8 @@ Nav2Panel::onCancel() &Nav2Panel::onCancelButtonPressed, this)); waypoint_status_indicator_->clear(); - store_poses_ = geometry_msgs::msg::PoseStampedArray(); - acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); + store_poses_ = nav_msgs::msg::Goals(); + acummulated_poses_ = nav_msgs::msg::Goals(); } void Nav2Panel::onResumedWp() @@ -967,12 +967,12 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) pose.pose.position.z = 0.0; pose.pose.orientation = orientationAroundZAxis(theta); - if (store_poses_.poses.empty()) { + if (store_poses_.goals.empty()) { if (state_machine_.configuration().contains(accumulating_)) { waypoint_status_indicator_->clear(); - acummulated_poses_.poses.push_back(pose); + acummulated_poses_.goals.push_back(pose); } else { - acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); + acummulated_poses_ = nav_msgs::msg::Goals(); updateWpNavigationMarkers(); std::cout << "Start navigation" << std::endl; startNavigation(pose); @@ -1031,7 +1031,7 @@ Nav2Panel::onCancelButtonPressed() void Nav2Panel::onAccumulatedWp() { - if (acummulated_poses_.poses.empty()) { + if (acummulated_poses_.goals.empty()) { state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); waypoint_status_indicator_->setText( " Note: Uh oh! Someone forgot to select the waypoints"); @@ -1053,7 +1053,7 @@ Nav2Panel::onAccumulatedWp() /** Making sure that the pose array does not get updated * between the process**/ - if (store_poses_.poses.empty()) { + if (store_poses_.goals.empty()) { std::cout << "Start waypoint" << std::endl; // Setting the final loop value on the text box for sanity @@ -1066,12 +1066,12 @@ Nav2Panel::onAccumulatedWp() if (store_initial_pose_) { try { init_transform = tf2_buffer_->lookupTransform( - acummulated_poses_.poses[0].header.frame_id, base_frame_, + acummulated_poses_.goals[0].header.frame_id, base_frame_, tf2::TimePointZero); } catch (const tf2::TransformException & ex) { RCLCPP_INFO( client_node_->get_logger(), "Could not transform %s to %s: %s", - acummulated_poses_.poses[0].header.frame_id.c_str(), base_frame_.c_str(), ex.what()); + acummulated_poses_.goals[0].header.frame_id.c_str(), base_frame_.c_str(), ex.what()); return; } @@ -1087,24 +1087,24 @@ Nav2Panel::onAccumulatedWp() initial_pose.pose.orientation.w = init_transform.transform.rotation.w; // inserting the acummulated pose - acummulated_poses_.poses.insert(acummulated_poses_.poses.begin(), initial_pose); + acummulated_poses_.goals.insert(acummulated_poses_.goals.begin(), initial_pose); updateWpNavigationMarkers(); initial_pose_stored_ = true; if (loop_count_ == 0) { goal_index_ = 1; } } else if (!store_initial_pose_ && initial_pose_stored_) { - acummulated_poses_.poses.erase( - acummulated_poses_.poses.begin(), - acummulated_poses_.poses.begin()); + acummulated_poses_.goals.erase( + acummulated_poses_.goals.begin(), + acummulated_poses_.goals.begin()); } } else { std::cout << "Resuming waypoint" << std::endl; } - startWaypointFollowing(acummulated_poses_.poses); + startWaypointFollowing(acummulated_poses_.goals); store_poses_ = acummulated_poses_; - acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); + acummulated_poses_ = nav_msgs::msg::Goals(); } void @@ -1117,8 +1117,8 @@ Nav2Panel::onAccumulatedNTP() void Nav2Panel::onAccumulating() { - acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); - store_poses_ = geometry_msgs::msg::PoseStampedArray(); + acummulated_poses_ = nav_msgs::msg::Goals(); + store_poses_ = nav_msgs::msg::Goals(); loop_count_ = 0; loop_no_ = "0"; initial_pose_stored_ = false; @@ -1255,7 +1255,7 @@ Nav2Panel::startWaypointFollowing(std::vector p } void -Nav2Panel::startNavThroughPoses(geometry_msgs::msg::PoseStampedArray poses) +Nav2Panel::startNavThroughPoses(nav_msgs::msg::Goals poses) { auto is_action_server_ready = nav_through_poses_action_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -1273,8 +1273,8 @@ Nav2Panel::startNavThroughPoses(geometry_msgs::msg::PoseStampedArray poses) RCLCPP_DEBUG( client_node_->get_logger(), "Sending a path of %zu waypoints:", - nav_through_poses_goal_.poses.poses.size()); - for (auto waypoint : nav_through_poses_goal_.poses.poses) { + nav_through_poses_goal_.poses.goals.size()); + for (auto waypoint : nav_through_poses_goal_.poses.goals) { RCLCPP_DEBUG( client_node_->get_logger(), "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); @@ -1385,14 +1385,14 @@ Nav2Panel::updateWpNavigationMarkers() auto marker_array = std::make_unique(); - for (size_t i = 0; i < acummulated_poses_.poses.size(); i++) { + for (size_t i = 0; i < acummulated_poses_.goals.size(); i++) { // Draw a green arrow at the waypoint pose visualization_msgs::msg::Marker arrow_marker; - arrow_marker.header = acummulated_poses_.poses[i].header; + arrow_marker.header = acummulated_poses_.goals[i].header; arrow_marker.id = getUniqueId(); arrow_marker.type = visualization_msgs::msg::Marker::ARROW; arrow_marker.action = visualization_msgs::msg::Marker::ADD; - arrow_marker.pose = acummulated_poses_.poses[i].pose; + arrow_marker.pose = acummulated_poses_.goals[i].pose; arrow_marker.scale.x = 0.3; arrow_marker.scale.y = 0.05; arrow_marker.scale.z = 0.02; @@ -1406,11 +1406,11 @@ Nav2Panel::updateWpNavigationMarkers() // Draw a red circle at the waypoint pose visualization_msgs::msg::Marker circle_marker; - circle_marker.header = acummulated_poses_.poses[i].header; + circle_marker.header = acummulated_poses_.goals[i].header; circle_marker.id = getUniqueId(); circle_marker.type = visualization_msgs::msg::Marker::SPHERE; circle_marker.action = visualization_msgs::msg::Marker::ADD; - circle_marker.pose = acummulated_poses_.poses[i].pose; + circle_marker.pose = acummulated_poses_.goals[i].pose; circle_marker.scale.x = 0.05; circle_marker.scale.y = 0.05; circle_marker.scale.z = 0.05; @@ -1424,11 +1424,11 @@ Nav2Panel::updateWpNavigationMarkers() // Draw the waypoint number visualization_msgs::msg::Marker marker_text; - marker_text.header = acummulated_poses_.poses[i].header; + marker_text.header = acummulated_poses_.goals[i].header; marker_text.id = getUniqueId(); marker_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_text.action = visualization_msgs::msg::Marker::ADD; - marker_text.pose = acummulated_poses_.poses[i].pose; + marker_text.pose = acummulated_poses_.goals[i].pose; marker_text.pose.position.z += 0.2; // draw it on top of the waypoint marker_text.scale.x = 0.07; marker_text.scale.y = 0.07; diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 61182260692..ed277bb2e51 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -619,7 +619,7 @@ def _getPathThroughPosesImpl( goal_msg.start = start goal_msg.goals.header.frame_id = 'map' goal_msg.goals.header.stamp = self.get_clock().now().to_msg() - goal_msg.goals.poses = goals + goal_msg.goals.goals = goals goal_msg.planner_id = planner_id goal_msg.use_start = use_start 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 8935cff9a22..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 @@ -132,7 +132,7 @@ def runNavigateAction(self, goal_msg = NavigateThroughPoses.Goal() goal_msg.poses.header.frame_id = 'map' goal_msg.poses.header.stamp = self.get_clock().now().to_msg() - goal_msg.poses.poses = [ + goal_msg.poses.goals = [ self.getStampedPoseMsg(self.goal_pose), self.getStampedPoseMsg(self.goal_pose), ] 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 11624528010..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 @@ -90,7 +90,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): goal_msg = NavigateThroughPoses.Goal() goal_msg.poses.header.frame_id = 'map' goal_msg.poses.header.stamp = self.get_clock().now().to_msg() - goal_msg.poses.poses = [ + goal_msg.poses.goals = [ self.getStampedPoseMsg(self.goal_pose), self.getStampedPoseMsg(self.goal_pose), ] @@ -166,7 +166,7 @@ def runNavigatePreemptionAction(self, block): ) goal_msg = NavigateThroughPoses.Goal() - goal_msg.poses.poses = [self.getStampedPoseMsg(self.initial_pose)] + goal_msg.poses.goals = [self.getStampedPoseMsg(self.initial_pose)] self.info_msg('Sending goal request...') send_goal_future = self.action_client.send_goal_async(goal_msg) diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index e9624f36c84..7dd40fdddd8 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -45,13 +45,23 @@ target_link_libraries(${library_name} PUBLIC rclcpp::rclcpp rclcpp_action::rclcpp_action rclcpp_lifecycle::rclcpp_lifecycle - robot_localization::rl_lib tf2_ros::tf2_ros ) target_link_libraries(${library_name} PRIVATE rclcpp_components::component ) +# Extract the major version (first number before the dot) +string(REGEX MATCH "^[0-9]+" ROS2_MAJOR_VERSION "${rclcpp_VERSION}") +# Convert to integer +if(ROS2_MAJOR_VERSION GREATER_EQUAL 29) + message(STATUS "ROS2_MAJOR_VERSION (${ROS2_MAJOR_VERSION}) is greater than 29 (Kilted and newer).") + target_link_libraries(${library_name} PUBLIC robot_localization::rl_lib) +else() + message(STATUS "ROS2_MAJOR_VERSION (${ROS2_MAJOR_VERSION}) is NOT greater than 29 (Jazzy and older).") + ament_target_dependencies(${library_name} PUBLIC robot_localization) +endif() + add_executable(${executable_name} src/main.cpp ) From b37509153dffbdd415376fc937beca596cf787ee Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Fri, 21 Mar 2025 21:00:33 +0000 Subject: [PATCH 11/14] Change v31 to v32 Signed-off-by: Nils-ChristianIseke --- .circleci/config.yml | 6 +++--- ament_lint | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) create mode 160000 ament_lint diff --git a/.circleci/config.yml b/.circleci/config.yml index c52e5a26e57..1fa5c7b80ef 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v31\ + - "<< parameters.key >>-v32\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v31\ + - "<< parameters.key >>-v32\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v31\ + key: "<< parameters.key >>-v32\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/ament_lint b/ament_lint new file mode 160000 index 00000000000..f14163fe2a8 --- /dev/null +++ b/ament_lint @@ -0,0 +1 @@ +Subproject commit f14163fe2a8d91886fedfc59c5f4fcb6d4e9453a From 6c606a76dc4eba89002ec30254dd085e9a3372cf Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Fri, 21 Mar 2025 21:28:50 +0000 Subject: [PATCH 12/14] Revert "Merge remote-tracking branch 'origin/main' into precommit" This reverts commit 8a7ca3983a0b93ef5a94e9517e9598de96be2fe6. Signed-off-by: Nils-ChristianIseke --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 9 +- nav2_amcl/src/amcl_node.cpp | 1 + .../include/nav2_behavior_tree/bt_utils.hpp | 20 +-- .../compute_path_through_poses_action.hpp | 2 +- .../action/navigate_through_poses_action.hpp | 4 +- .../remove_in_collision_goals_action.hpp | 8 +- .../action/remove_passed_goals_action.hpp | 6 +- .../are_error_codes_present_condition.hpp | 9 +- .../globally_updated_goal_condition.hpp | 6 +- .../condition/goal_updated_condition.hpp | 6 +- .../decorator/goal_updated_controller.hpp | 4 +- .../plugins/decorator/goal_updater_node.hpp | 14 +- .../plugins/decorator/speed_controller.hpp | 4 +- .../remove_in_collision_goals_action.cpp | 10 +- .../action/remove_passed_goals_action.cpp | 12 +- .../globally_updated_goal_condition.cpp | 2 +- .../condition/goal_updated_condition.cpp | 2 +- .../decorator/goal_updated_controller.cpp | 2 +- .../plugins/decorator/goal_updater_node.cpp | 8 +- .../plugins/decorator/speed_controller.cpp | 2 +- ...test_compute_path_through_poses_action.cpp | 26 ++-- .../action/test_get_pose_from_path_action.cpp | 12 +- .../test_navigate_through_poses_action.cpp | 10 +- .../test_remove_in_collision_goals_action.cpp | 62 ++++----- .../test_remove_passed_goals_action.cpp | 28 ++-- .../test_are_error_codes_present.cpp | 2 +- .../test_goal_updated_controller.cpp | 8 +- .../decorator/test_goal_updater_node.cpp | 34 ++--- .../decorator/test_speed_controller.cpp | 2 +- nav2_behavior_tree/test/test_bt_utils.cpp | 56 ++++---- .../navigators/navigate_through_poses.hpp | 1 + .../src/navigators/navigate_through_poses.cpp | 20 +-- .../nav2_collision_monitor/polygon.hpp | 2 - nav2_collision_monitor/src/polygon.cpp | 11 +- .../plugins/simple_goal_checker.cpp | 2 +- nav2_controller/plugins/test/goal_checker.cpp | 103 -------------- .../plugins/test/progress_checker.cpp | 131 +++++------------- .../nav2_costmap_2d/obstacle_layer.hpp | 8 -- .../plugin_container_layer.hpp | 2 +- .../nav2_costmap_2d/range_sensor_layer.hpp | 1 + .../include/nav2_costmap_2d/static_layer.hpp | 3 +- .../include/nav2_costmap_2d/voxel_layer.hpp | 1 + nav2_costmap_2d/plugins/obstacle_layer.cpp | 26 +--- .../action/ComputePathThroughPoses.action | 2 +- nav2_msgs/action/NavigateThroughPoses.action | 2 +- nav2_planner/src/planner_server.cpp | 6 +- .../include/nav2_rviz_plugins/nav2_panel.hpp | 6 +- nav2_rviz_plugins/src/nav2_panel.cpp | 76 +++++----- .../nav2_simple_commander/robot_navigator.py | 2 +- ...nav_through_poses_tester_error_msg_node.py | 2 +- .../system/nav_through_poses_tester_node.py | 4 +- nav2_waypoint_follower/CMakeLists.txt | 12 +- 52 files changed, 291 insertions(+), 503 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 7928f5e9545..2c971bc741c 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -28,15 +28,8 @@ #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 "message_filters/subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_amcl/motion_model/motion_model.hpp" #include "nav2_amcl/sensors/laser/laser.hpp" diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 7ae2bb61048..016837e2eee 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -28,6 +28,7 @@ #include #include +#include "message_filters/subscriber.hpp" #include "nav2_amcl/angleutils.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_amcl/pf/pf.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index 63595fef985..0de7da7925c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -24,9 +24,9 @@ #include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "nav_msgs/msg/path.hpp" -#include "nav_msgs/msg/goals.hpp" namespace BT { @@ -136,20 +136,20 @@ inline std::vector convertFromString(const Stri } /** - * @brief Parse XML string to nav_msgs::msg::Goals + * @brief Parse XML string to geometry_msgs::msg::PoseStampedArray * @param key XML string - * @return nav_msgs::msg::Goals + * @return geometry_msgs::msg::PoseStampedArray */ template<> -inline nav_msgs::msg::Goals convertFromString(const StringView key) +inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView key) { auto parts = BT::splitString(key, ';'); if ((parts.size() - 2) % 9 != 0) { - throw std::runtime_error("invalid number of fields for Goals attribute)"); + throw std::runtime_error("invalid number of fields for PoseStampedArray attribute)"); } else { - nav_msgs::msg::Goals goals_array; - goals_array.header.stamp = rclcpp::Time(BT::convertFromString(parts[0])); - goals_array.header.frame_id = BT::convertFromString(parts[1]); + geometry_msgs::msg::PoseStampedArray pose_stamped_array; + pose_stamped_array.header.stamp = rclcpp::Time(BT::convertFromString(parts[0])); + pose_stamped_array.header.frame_id = BT::convertFromString(parts[1]); for (size_t i = 2; i < parts.size(); i += 9) { geometry_msgs::msg::PoseStamped pose_stamped; pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString(parts[i])); @@ -161,9 +161,9 @@ inline nav_msgs::msg::Goals convertFromString(const StringView key) pose_stamped.pose.orientation.y = BT::convertFromString(parts[i + 6]); pose_stamped.pose.orientation.z = BT::convertFromString(parts[i + 7]); pose_stamped.pose.orientation.w = BT::convertFromString(parts[i + 8]); - goals_array.goals.push_back(pose_stamped); + pose_stamped_array.poses.push_back(pose_stamped); } - return goals_array; + return pose_stamped_array; } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index 17b2e1af987..b4e0dd43f94 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -80,7 +80,7 @@ class ComputePathThroughPosesAction { return providedBasicPorts( { - BT::InputPort( + BT::InputPort( "goals", "Destinations to plan through"), BT::InputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 8d636d84e45..9a579d2dfd5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -18,7 +18,7 @@ #include #include -#include "nav_msgs/msg/goals.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -73,7 +73,7 @@ class NavigateThroughPosesAction : public BtActionNode( + BT::InputPort( "goals", "Destinations to plan through"), BT::InputPort("behavior_tree", "Behavior tree to run"), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp index 7d3a4615bdb..5ed0e213ea7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "nav_msgs/msg/goals.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_service_node.hpp" #include "nav2_msgs/srv/get_costs.hpp" @@ -52,7 +52,7 @@ class RemoveInCollisionGoals : public BtServiceNode { return providedBasicPorts( { - BT::InputPort("input_goals", + BT::InputPort("input_goals", "Original goals to remove from"), BT::InputPort( "cost_threshold", 254.0, @@ -61,7 +61,7 @@ class RemoveInCollisionGoals : public BtServiceNode BT::InputPort( "consider_unknown_as_obstacle", false, "Whether to consider unknown cost as obstacle"), - BT::OutputPort("output_goals", + BT::OutputPort("output_goals", "Goals with in-collision goals removed"), }); } @@ -70,7 +70,7 @@ class RemoveInCollisionGoals : public BtServiceNode bool use_footprint_; bool consider_unknown_as_obstacle_; double cost_threshold_; - nav_msgs::msg::Goals input_goals_; + geometry_msgs::msg::PoseStampedArray input_goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index 4a17896f89c..ccb400cdab4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -19,7 +19,7 @@ #include #include -#include "nav_msgs/msg/goals.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "behaviortree_cpp/action_node.h" @@ -43,9 +43,9 @@ class RemovePassedGoals : public BT::ActionNodeBase static BT::PortsList providedPorts() { return { - BT::InputPort("input_goals", + BT::InputPort("input_goals", "Original goals to remove viapoints from"), - BT::OutputPort("output_goals", + BT::OutputPort("output_goals", "Goals with passed viapoints removed"), BT::InputPort("radius", 0.5, "radius to goal for it to be considered for removal"), BT::InputPort("robot_base_frame", "Robot base frame"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp index 67833a383b3..209958c38d4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp @@ -34,12 +34,7 @@ class AreErrorCodesPresent : public BT::ConditionNode const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf) { - std::vector error_codes_to_check_vector; - getInput("error_codes_to_check", error_codes_to_check_vector); //NOLINT - - error_codes_to_check_ = std::set( - error_codes_to_check_vector.begin(), - error_codes_to_check_vector.end()); + getInput>("error_codes_to_check", error_codes_to_check_); //NOLINT } AreErrorCodesPresent() = delete; @@ -60,7 +55,7 @@ class AreErrorCodesPresent : public BT::ConditionNode return { BT::InputPort("error_code", "The active error codes"), //NOLINT - BT::InputPort>("error_codes_to_check", "Error codes to check")//NOLINT + BT::InputPort>("error_codes_to_check", "Error codes to check")//NOLINT }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 3c5e9cc5f9c..5e730017d92 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" -#include "nav_msgs/msg/goals.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -59,7 +59,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -70,7 +70,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode bool first_time; rclcpp::Node::SharedPtr node_; geometry_msgs::msg::PoseStamped goal_; - nav_msgs::msg::Goals goals_; + geometry_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index 7f37723a2b3..4ea81d896b2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -19,7 +19,7 @@ #include #include "behaviortree_cpp/condition_node.h" -#include "nav_msgs/msg/goals.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree @@ -56,7 +56,7 @@ class GoalUpdatedCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -65,7 +65,7 @@ class GoalUpdatedCondition : public BT::ConditionNode private: geometry_msgs::msg::PoseStamped goal_; - nav_msgs::msg::Goals goals_; + geometry_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp index f24a7ff3d76..49215ae9a14 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -50,7 +50,7 @@ class GoalUpdatedController : public BT::DecoratorNode static BT::PortsList providedPorts() { return { - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -66,7 +66,7 @@ class GoalUpdatedController : public BT::DecoratorNode bool goal_was_updated_; geometry_msgs::msg::PoseStamped goal_; - nav_msgs::msg::Goals goals_; + geometry_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 71bcf8f5598..2bf6b43e9c6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -21,7 +21,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/goals.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "behaviortree_cpp/decorator_node.h" @@ -54,10 +54,10 @@ class GoalUpdater : public BT::DecoratorNode { return { BT::InputPort("input_goal", "Original Goal"), - BT::InputPort("input_goals", "Original Goals"), + BT::InputPort("input_goals", "Original Goals"), BT::OutputPort("output_goal", "Received Goal by subscription"), - BT::OutputPort("output_goals", + BT::OutputPort("output_goals", "Received Goals by subscription") }; } @@ -85,16 +85,16 @@ class GoalUpdater : public BT::DecoratorNode /** * @brief Callback function for goals update topic - * @param msg Shared pointer to nav_msgs::msg::Goals message + * @param msg Shared pointer to geometry_msgs::msg::PoseStampedArray message */ - void callback_updated_goals(const nav_msgs::msg::Goals::SharedPtr msg); + void callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg); rclcpp::Subscription::SharedPtr goal_sub_; - rclcpp::Subscription::SharedPtr goals_sub_; + rclcpp::Subscription::SharedPtr goals_sub_; geometry_msgs::msg::PoseStamped last_goal_received_; bool last_goal_received_set_{false}; - nav_msgs::msg::Goals last_goals_received_; + geometry_msgs::msg::PoseStampedArray last_goals_received_; bool last_goals_received_set_{false}; rclcpp::Node::SharedPtr node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 632a941acbe..3d1dcff2962 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -58,7 +58,7 @@ class SpeedController : public BT::DecoratorNode BT::InputPort("max_rate", 1.0, "Maximum rate"), BT::InputPort("min_speed", 0.0, "Minimum speed"), BT::InputPort("max_speed", 0.5, "Maximum speed"), - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -120,7 +120,7 @@ class SpeedController : public BT::DecoratorNode // current goal geometry_msgs::msg::PoseStamped goal_; - nav_msgs::msg::Goals goals_; + geometry_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index 68a83771cbe..76b50ac3f51 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -39,7 +39,7 @@ void RemoveInCollisionGoals::on_tick() getInput("input_goals", input_goals_); getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_); - if (input_goals_.goals.empty()) { + if (input_goals_.poses.empty()) { setOutput("output_goals", input_goals_); should_send_request_ = false; return; @@ -47,7 +47,7 @@ void RemoveInCollisionGoals::on_tick() request_ = std::make_shared(); request_->use_footprint = use_footprint_; - for (const auto & goal : input_goals_.goals) { + for (const auto & goal : input_goals_.poses) { request_->poses.push_back(goal); } } @@ -63,16 +63,16 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion( return BT::NodeStatus::FAILURE; } - nav_msgs::msg::Goals valid_goal_poses; + geometry_msgs::msg::PoseStampedArray valid_goal_poses; for (size_t i = 0; i < response->costs.size(); ++i) { if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || response->costs[i] < cost_threshold_) { - valid_goal_poses.goals.push_back(input_goals_.goals[i]); + valid_goal_poses.poses.push_back(input_goals_.poses[i]); } } // Inform if all goals have been removed - if (valid_goal_poses.goals.empty()) { + if (valid_goal_poses.poses.empty()) { RCLCPP_INFO( node_->get_logger(), "All goals are in collision and have been removed from the list"); diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index b3cc3a39ead..9b44a0635ef 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -49,10 +49,10 @@ inline BT::NodeStatus RemovePassedGoals::tick() initialize(); } - nav_msgs::msg::Goals goal_poses; + geometry_msgs::msg::PoseStampedArray goal_poses; getInput("input_goals", goal_poses); - if (goal_poses.goals.empty()) { + if (goal_poses.poses.empty()) { setOutput("output_goals", goal_poses); return BT::NodeStatus::SUCCESS; } @@ -61,21 +61,21 @@ inline BT::NodeStatus RemovePassedGoals::tick() geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( - current_pose, *tf_, goal_poses.goals[0].header.frame_id, robot_base_frame_, + current_pose, *tf_, goal_poses.poses[0].header.frame_id, robot_base_frame_, transform_tolerance_)) { return BT::NodeStatus::FAILURE; } double dist_to_goal; - while (goal_poses.goals.size() > 1) { - dist_to_goal = euclidean_distance(goal_poses.goals[0].pose, current_pose.pose); + while (goal_poses.poses.size() > 1) { + dist_to_goal = euclidean_distance(goal_poses.poses[0].pose, current_pose.pose); if (dist_to_goal > viapoint_achieved_radius_) { break; } - goal_poses.goals.erase(goal_poses.goals.begin()); + goal_poses.poses.erase(goal_poses.poses.begin()); } setOutput("output_goals", goal_poses); diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index 929f87399db..fcd3a665134 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -38,7 +38,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() return BT::NodeStatus::SUCCESS; } - nav_msgs::msg::Goals current_goals; + geometry_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index 488a7472d37..06ddf76ae95 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -33,7 +33,7 @@ BT::NodeStatus GoalUpdatedCondition::tick() return BT::NodeStatus::FAILURE; } - nav_msgs::msg::Goals current_goals; + geometry_msgs::msg::PoseStampedArray current_goals; geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goals", current_goals); BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index ad139506fa6..d78c5d1e1fd 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -44,7 +44,7 @@ BT::NodeStatus GoalUpdatedController::tick() setStatus(BT::NodeStatus::RUNNING); - nav_msgs::msg::Goals current_goals; + geometry_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index de969937704..4b970fbd051 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -72,7 +72,7 @@ void GoalUpdater::createROSInterfaces() goals_updater_topic_ = goals_updater_topic_new; rclcpp::SubscriptionOptions sub_option; sub_option.callback_group = callback_group_; - goals_sub_ = node_->create_subscription( + goals_sub_ = node_->create_subscription( goals_updater_topic_, rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goals, this, _1), @@ -87,7 +87,7 @@ inline BT::NodeStatus GoalUpdater::tick() } geometry_msgs::msg::PoseStamped goal; - nav_msgs::msg::Goals goals; + geometry_msgs::msg::PoseStampedArray goals; getInput("input_goal", goal); getInput("input_goals", goals); @@ -120,7 +120,7 @@ inline BT::NodeStatus GoalUpdater::tick() } if (last_goals_received_set_) { - if (last_goals_received_.goals.empty()) { + if (last_goals_received_.poses.empty()) { setOutput("output_goals", goals); } else if (last_goals_received_.header.stamp == rclcpp::Time(0)) { RCLCPP_WARN( @@ -154,7 +154,7 @@ GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::Shared } void -GoalUpdater::callback_updated_goals(const nav_msgs::msg::Goals::SharedPtr msg) +GoalUpdater::callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg) { last_goals_received_ = *msg; last_goals_received_set_ = true; diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index b6891108861..347f1ccd25c 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -66,7 +66,7 @@ inline BT::NodeStatus SpeedController::tick() first_tick_ = true; } - nav_msgs::msg::Goals current_goals; + geometry_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 550047e56e2..bb813205456 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -44,7 +44,7 @@ class ComputePathThroughPosesActionServer const auto goal = goal_handle->get_goal(); auto result = std::make_shared(); result->path.poses.resize(2); - result->path.poses[1].pose.position.x = goal->goals.goals[0].pose.position.x; + result->path.poses[1].pose.position.x = goal->goals.poses[0].pose.position.x; if (goal->use_start) { result->path.poses[0].pose.position.x = goal->start.pose.position.x; } else { @@ -137,9 +137,9 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav_msgs::msg::Goals goals; - goals.goals.resize(1); - goals.goals[0].pose.position.x = 1.0; + geometry_msgs::msg::PoseStampedArray goals; + goals.poses.resize(1); + goals.poses[0].pose.position.x = 1.0; config_->blackboard->set("goals", goals); // tick until node succeeds @@ -150,7 +150,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); - EXPECT_EQ(action_server_->getCurrentGoal()->goals.goals[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, 1.0); EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); @@ -166,7 +166,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal - goals.goals[0].pose.position.x = -2.5; + goals.poses[0].pose.position.x = -2.5; config_->blackboard->set("goals", goals); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { @@ -174,7 +174,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) } EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(action_server_->getCurrentGoal()->goals.goals[0].pose.position.x, -2.5); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, -2.5); EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); @@ -202,9 +202,9 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) config_->blackboard->set("start", start); // create new goal and set it on blackboard - nav_msgs::msg::Goals goals; - goals.goals.resize(1); - goals.goals[0].pose.position.x = 1.0; + geometry_msgs::msg::PoseStampedArray goals; + goals.poses.resize(1); + goals.poses[0].pose.position.x = 1.0; config_->blackboard->set("goals", goals); // tick until node succeeds @@ -215,7 +215,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); - EXPECT_EQ(action_server_->getCurrentGoal()->goals.goals[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, 1.0); EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0); EXPECT_TRUE(action_server_->getCurrentGoal()->use_start); EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); @@ -232,7 +232,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal and new start - goals.goals[0].pose.position.x = -2.5; + goals.poses[0].pose.position.x = -2.5; start.pose.position.x = -1.5; config_->blackboard->set("goals", goals); config_->blackboard->set("start", start); @@ -242,7 +242,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) } EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(action_server_->getCurrentGoal()->goals.goals[0].pose.position.x, -2.5); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, -2.5); EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); diff --git a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp index 2e9fcbceadd..3596af739c3 100644 --- a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp @@ -21,7 +21,7 @@ #include #include "nav_msgs/msg/path.hpp" -#include "nav_msgs/msg/goals.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "behaviortree_cpp/bt_factory.h" @@ -97,11 +97,11 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) // create new path and set it on blackboard nav_msgs::msg::Path path; - nav_msgs::msg::Goals goals; - goals.goals.resize(2); - goals.goals[0].pose.position.x = 1.0; - goals.goals[1].pose.position.x = 2.0; - path.poses = goals.goals; + geometry_msgs::msg::PoseStampedArray goals; + goals.poses.resize(2); + goals.poses[0].pose.position.x = 1.0; + goals.poses[1].pose.position.x = 2.0; + path.poses = goals.poses; path.header.frame_id = "test_frame_1"; config_->blackboard->set("path", path); diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index f73dca6ac28..cf0c1079068 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -74,7 +74,7 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test "wait_for_service_timeout", std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); - nav_msgs::msg::Goals poses; + geometry_msgs::msg::PoseStampedArray poses; config_->blackboard->set( "goals", poses); @@ -132,10 +132,10 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - nav_msgs::msg::Goals poses; - poses.goals.resize(1); - poses.goals[0].pose.position.x = -2.5; - poses.goals[0].pose.orientation.x = 1.0; + geometry_msgs::msg::PoseStampedArray poses; + poses.poses.resize(1); + poses.poses[0].pose.position.x = -2.5; + poses.poses[0].pose.orientation.x = 1.0; config_->blackboard->set("goals", poses); // tick until node succeeds diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp index 4a475b0fd8c..ea750fe4193 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -148,19 +148,19 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav_msgs::msg::Goals poses; - poses.goals.resize(4); - poses.goals[0].pose.position.x = 0.0; - poses.goals[0].pose.position.y = 0.0; + geometry_msgs::msg::PoseStampedArray poses; + poses.poses.resize(4); + poses.poses[0].pose.position.x = 0.0; + poses.poses[0].pose.position.y = 0.0; - poses.goals[1].pose.position.x = 0.5; - poses.goals[1].pose.position.y = 0.0; + poses.poses[1].pose.position.x = 0.5; + poses.poses[1].pose.position.y = 0.0; - poses.goals[2].pose.position.x = 1.0; - poses.goals[2].pose.position.y = 0.0; + poses.poses[2].pose.position.x = 1.0; + poses.poses[2].pose.position.y = 0.0; - poses.goals[3].pose.position.x = 2.0; - poses.goals[3].pose.position.y = 0.0; + poses.poses[3].pose.position.x = 2.0; + poses.poses[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -172,13 +172,13 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); // check that it removed the point in range - nav_msgs::msg::Goals output_poses; + geometry_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.goals.size(), 3u); - EXPECT_EQ(output_poses.goals[0], poses.goals[0]); - EXPECT_EQ(output_poses.goals[1], poses.goals[1]); - EXPECT_EQ(output_poses.goals[2], poses.goals[2]); + EXPECT_EQ(output_poses.poses.size(), 3u); + EXPECT_EQ(output_poses.poses[0], poses.poses[0]); + EXPECT_EQ(output_poses.poses[1], poses.poses[1]); + EXPECT_EQ(output_poses.poses[2], poses.poses[2]); } TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fail) @@ -195,19 +195,19 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fa tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav_msgs::msg::Goals poses; - poses.goals.resize(4); - poses.goals[0].pose.position.x = 0.0; - poses.goals[0].pose.position.y = 0.0; + geometry_msgs::msg::PoseStampedArray poses; + poses.poses.resize(4); + poses.poses[0].pose.position.x = 0.0; + poses.poses[0].pose.position.y = 0.0; - poses.goals[1].pose.position.x = 0.5; - poses.goals[1].pose.position.y = 0.0; + poses.poses[1].pose.position.x = 0.5; + poses.poses[1].pose.position.y = 0.0; - poses.goals[2].pose.position.x = 1.0; - poses.goals[2].pose.position.y = 0.0; + poses.poses[2].pose.position.x = 1.0; + poses.poses[2].pose.position.y = 0.0; - poses.goals[3].pose.position.x = 2.0; - poses.goals[3].pose.position.y = 0.0; + poses.poses[3].pose.position.x = 2.0; + poses.poses[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -219,14 +219,14 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fa // check that it failed and returned the original goals EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); - nav_msgs::msg::Goals output_poses; + geometry_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.goals.size(), 4u); - EXPECT_EQ(output_poses.goals[0], poses.goals[0]); - EXPECT_EQ(output_poses.goals[1], poses.goals[1]); - EXPECT_EQ(output_poses.goals[2], poses.goals[2]); - EXPECT_EQ(output_poses.goals[3], poses.goals[3]); + EXPECT_EQ(output_poses.poses.size(), 4u); + EXPECT_EQ(output_poses.poses[0], poses.poses[0]); + EXPECT_EQ(output_poses.poses[1], poses.poses[1]); + EXPECT_EQ(output_poses.poses[2], poses.poses[2]); + EXPECT_EQ(output_poses.poses[3], poses.poses[3]); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 74fd7c6fdad..9ed40d8bad7 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -114,19 +114,19 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav_msgs::msg::Goals poses; - poses.goals.resize(4); - poses.goals[0].pose.position.x = 0.0; - poses.goals[0].pose.position.y = 0.0; + geometry_msgs::msg::PoseStampedArray poses; + poses.poses.resize(4); + poses.poses[0].pose.position.x = 0.0; + poses.poses[0].pose.position.y = 0.0; - poses.goals[1].pose.position.x = 0.5; - poses.goals[1].pose.position.y = 0.0; + poses.poses[1].pose.position.x = 0.5; + poses.poses[1].pose.position.y = 0.0; - poses.goals[2].pose.position.x = 1.0; - poses.goals[2].pose.position.y = 0.0; + poses.poses[2].pose.position.x = 1.0; + poses.poses[2].pose.position.y = 0.0; - poses.goals[3].pose.position.x = 2.0; - poses.goals[3].pose.position.y = 0.0; + poses.poses[3].pose.position.x = 2.0; + poses.poses[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -136,12 +136,12 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) } // check that it removed the point in range - nav_msgs::msg::Goals output_poses; + geometry_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.goals.size(), 2u); - EXPECT_EQ(output_poses.goals[0], poses.goals[2]); - EXPECT_EQ(output_poses.goals[1], poses.goals[3]); + EXPECT_EQ(output_poses.poses.size(), 2u); + EXPECT_EQ(output_poses.poses[0], poses.poses[2]); + EXPECT_EQ(output_poses.poses[1], poses.poses[3]); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp index aa0372f49b3..111cda7d005 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp @@ -28,7 +28,7 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF void SetUp() { uint16_t error_code = ActionResult::NONE; - std::vector error_codes_to_check = {ActionResult::UNKNOWN}; //NOLINT + std::set error_codes_to_check = {ActionResult::UNKNOWN}; //NOLINT config_->blackboard->set("error_code", error_code); config_->blackboard->set("error_codes_to_check", error_codes_to_check); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp index 63b4cb336b0..d1c3c5e4207 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -32,8 +32,8 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree // Setting fake goals on blackboard geometry_msgs::msg::PoseStamped goal1; goal1.header.stamp = node_->now(); - nav_msgs::msg::Goals poses1; - poses1.goals.push_back(goal1); + geometry_msgs::msg::PoseStampedArray poses1; + poses1.poses.push_back(goal1); config_->blackboard->set("goal", goal1); config_->blackboard->set("goals", poses1); bt_node_ = std::make_shared( @@ -63,8 +63,8 @@ TEST_F(GoalUpdatedControllerTestFixture, test_behavior) // Creating updated fake-goals geometry_msgs::msg::PoseStamped goal2; goal2.header.stamp = node_->now(); - nav_msgs::msg::Goals poses2; - poses2.goals.push_back(goal2); + geometry_msgs::msg::PoseStampedArray poses2; + poses2.poses.push_back(goal2); // starting in idle EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index fa9cc46ab24..2f70061da7f 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -98,17 +98,17 @@ TEST_F(GoalUpdaterTestFixture, test_tick) // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; - nav_msgs::msg::Goals goals; + geometry_msgs::msg::PoseStampedArray goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; - goals.goals.push_back(goal); + goals.poses.push_back(goal); config_->blackboard->set("goal", goal); config_->blackboard->set("goals", goals); // tick tree without publishing updated goal and get updated_goal tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - nav_msgs::msg::Goals updated_goals; + geometry_msgs::msg::PoseStampedArray updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); } @@ -130,31 +130,31 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) auto goal_updater_pub = node_->create_publisher("goal_update", 10); auto goals_updater_pub = - node_->create_publisher("goals_update", 10); + node_->create_publisher("goals_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; - nav_msgs::msg::Goals goals; + geometry_msgs::msg::PoseStampedArray goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; goals.header.stamp = goal.header.stamp; - goals.goals.push_back(goal); + goals.poses.push_back(goal); config_->blackboard->set("goal", goal); config_->blackboard->set("goals", goals); // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update; - nav_msgs::msg::Goals goals_to_update; + geometry_msgs::msg::PoseStampedArray goals_to_update; goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0); goal_to_update.pose.position.x = 2.0; goals_to_update.header.stamp = goal_to_update.header.stamp; - goals_to_update.goals.push_back(goal_to_update); + goals_to_update.poses.push_back(goal_to_update); goal_updater_pub->publish(goal_to_update); goals_updater_pub->publish(goals_to_update); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - nav_msgs::msg::Goals updated_goals; + geometry_msgs::msg::PoseStampedArray updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); @@ -181,31 +181,31 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) auto goal_updater_pub = node_->create_publisher("goal_update", 10); auto goals_updater_pub = - node_->create_publisher("goals_update", 10); + node_->create_publisher("goals_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; - nav_msgs::msg::Goals goals; + geometry_msgs::msg::PoseStampedArray goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; - goals.goals.push_back(goal); + goals.poses.push_back(goal); config_->blackboard->set("goal", goal); config_->blackboard->set("goals", goals); // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update_1; - nav_msgs::msg::Goals goals_to_update_1; + geometry_msgs::msg::PoseStampedArray goals_to_update_1; goal_to_update_1.header.stamp = node_->now(); goal_to_update_1.pose.position.x = 2.0; goals_to_update_1.header.stamp = goal_to_update_1.header.stamp; - goals_to_update_1.goals.push_back(goal_to_update_1); + goals_to_update_1.poses.push_back(goal_to_update_1); geometry_msgs::msg::PoseStamped goal_to_update_2; - nav_msgs::msg::Goals goals_to_update_2; + geometry_msgs::msg::PoseStampedArray goals_to_update_2; goal_to_update_2.header.stamp = node_->now(); goal_to_update_2.pose.position.x = 3.0; goals_to_update_2.header.stamp = goal_to_update_2.header.stamp; - goals_to_update_2.goals.push_back(goal_to_update_2); + goals_to_update_2.poses.push_back(goal_to_update_2); goal_updater_pub->publish(goal_to_update_1); goals_updater_pub->publish(goals_to_update_1); @@ -213,7 +213,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) goals_updater_pub->publish(goals_to_update_2); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - nav_msgs::msg::Goals updated_goals; + geometry_msgs::msg::PoseStampedArray updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index 45b35f00daf..ae07ee3c21b 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -42,7 +42,7 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi goal.header.stamp = node_->now(); config_->blackboard->set("goal", goal); - nav_msgs::msg::Goals fake_poses; + geometry_msgs::msg::PoseStampedArray fake_poses; config_->blackboard->set("goals", fake_poses); // NOLINT config_->input_ports["min_rate"] = 0.1; diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 8d68a5a1c79..87527e05e8e 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -258,68 +258,68 @@ TEST(PoseStampedVectorPortTest, test_correct_syntax) EXPECT_EQ(values[1].pose.orientation.w, 14.0); } -TEST(GoalsArrayPortTest, test_wrong_syntax) +TEST(PoseStampedArrayPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + )"; BT::BehaviorTreeFactory factory; - factory.registerNodeType>( - "GoalsArrayPortTest"); + factory.registerNodeType>( + "PoseStampedArrayPortTest"); EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( - + )"; EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } -TEST(GoalsArrayPortTest, test_correct_syntax) +TEST(PoseStampedArrayPortTest, test_correct_syntax) { std::string xml_txt = R"( - + )"; BT::BehaviorTreeFactory factory; - factory.registerNodeType>( - "GoalsArrayPortTest"); + factory.registerNodeType>( + "PoseStampedArrayPortTest"); auto tree = factory.createTreeFromText(xml_txt); tree = factory.createTreeFromText(xml_txt); - nav_msgs::msg::Goals values; + geometry_msgs::msg::PoseStampedArray values; tree.rootNode()->getInput("test", values); - EXPECT_EQ(rclcpp::Time(values.goals[0].header.stamp).nanoseconds(), 0); - EXPECT_EQ(values.goals[0].header.frame_id, "map"); - EXPECT_EQ(values.goals[0].pose.position.x, 1.0); - EXPECT_EQ(values.goals[0].pose.position.y, 2.0); - EXPECT_EQ(values.goals[0].pose.position.z, 3.0); - EXPECT_EQ(values.goals[0].pose.orientation.x, 4.0); - EXPECT_EQ(values.goals[0].pose.orientation.y, 5.0); - EXPECT_EQ(values.goals[0].pose.orientation.z, 6.0); - EXPECT_EQ(values.goals[0].pose.orientation.w, 7.0); - EXPECT_EQ(rclcpp::Time(values.goals[1].header.stamp).nanoseconds(), 0); - EXPECT_EQ(values.goals[1].header.frame_id, "odom"); - EXPECT_EQ(values.goals[1].pose.position.x, 8.0); - EXPECT_EQ(values.goals[1].pose.position.y, 9.0); - EXPECT_EQ(values.goals[1].pose.position.z, 10.0); - EXPECT_EQ(values.goals[1].pose.orientation.x, 11.0); - EXPECT_EQ(values.goals[1].pose.orientation.y, 12.0); - EXPECT_EQ(values.goals[1].pose.orientation.z, 13.0); - EXPECT_EQ(values.goals[1].pose.orientation.w, 14.0); + EXPECT_EQ(rclcpp::Time(values.poses[0].header.stamp).nanoseconds(), 0); + EXPECT_EQ(values.poses[0].header.frame_id, "map"); + EXPECT_EQ(values.poses[0].pose.position.x, 1.0); + EXPECT_EQ(values.poses[0].pose.position.y, 2.0); + EXPECT_EQ(values.poses[0].pose.position.z, 3.0); + EXPECT_EQ(values.poses[0].pose.orientation.x, 4.0); + EXPECT_EQ(values.poses[0].pose.orientation.y, 5.0); + EXPECT_EQ(values.poses[0].pose.orientation.z, 6.0); + EXPECT_EQ(values.poses[0].pose.orientation.w, 7.0); + EXPECT_EQ(rclcpp::Time(values.poses[1].header.stamp).nanoseconds(), 0); + EXPECT_EQ(values.poses[1].header.frame_id, "odom"); + EXPECT_EQ(values.poses[1].pose.position.x, 8.0); + EXPECT_EQ(values.poses[1].pose.position.y, 9.0); + EXPECT_EQ(values.poses[1].pose.position.z, 10.0); + EXPECT_EQ(values.poses[1].pose.orientation.x, 11.0); + EXPECT_EQ(values.poses[1].pose.orientation.y, 12.0); + EXPECT_EQ(values.poses[1].pose.orientation.z, 13.0); + EXPECT_EQ(values.poses[1].pose.orientation.w, 14.0); } TEST(PathPortTest, test_wrong_syntax) diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index de68270e29b..38f7228c7b5 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav_msgs/msg/path.hpp" diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index bcadbafa10c..d4b319f125e 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -113,10 +113,10 @@ NavigateThroughPosesNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); - nav_msgs::msg::Goals goal_poses; + geometry_msgs::msg::PoseStampedArray goal_poses; [[maybe_unused]] auto res = blackboard->get(goals_blackboard_id_, goal_poses); - if (goal_poses.goals.size() == 0) { + if (goal_poses.poses.size() == 0) { bt_action_server_->publishFeedback(feedback_msg); return; } @@ -178,7 +178,7 @@ NavigateThroughPosesNavigator::onLoop() feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; - feedback_msg->number_of_poses_remaining = goal_poses.goals.size(); + feedback_msg->number_of_poses_remaining = goal_poses.poses.size(); bt_action_server_->publishFeedback(feedback_msg); } @@ -228,9 +228,9 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr return false; } - nav_msgs::msg::Goals goals_array = goal->poses; + geometry_msgs::msg::PoseStampedArray pose_stamped_array = goal->poses; int i = 0; - for (auto & goal_pose : goals_array.goals) { + for (auto & goal_pose : pose_stamped_array.poses) { if (!nav2_util::transformPoseInTargetFrame( goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, feedback_utils_.transform_tolerance)) @@ -246,11 +246,11 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr i++; } - if (goals_array.goals.size() > 0) { + if (pose_stamped_array.poses.size() > 0) { RCLCPP_INFO( logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)", - goals_array.goals.size(), goals_array.goals.back().pose.position.x, - goals_array.goals.back().pose.position.y); + pose_stamped_array.poses.size(), pose_stamped_array.poses.back().pose.position.x, + pose_stamped_array.poses.back().pose.position.y); } // Reset state for new action feedback @@ -259,8 +259,8 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard->set(goals_blackboard_id_, - std::move(goals_array)); + blackboard->set(goals_blackboard_id_, + std::move(pose_stamped_array)); return true; } diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 5dff3d7371c..b1438dc01f8 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -298,8 +298,6 @@ class Polygon std::string base_frame_id_; /// @brief Transform tolerance tf2::Duration transform_tolerance_; - /// @brief Collision monitor node's clock - rclcpp::Clock::SharedPtr node_clock_; // Visualization /// @brief Whether to publish the polygon diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 1e087070191..a528c74d640 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -39,8 +39,7 @@ Polygon::Polygon( : node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING), slowdown_ratio_(0.0), linear_limit_(0.0), angular_limit_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer), - base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance), - node_clock_(nullptr) + base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance) { RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str()); } @@ -52,7 +51,6 @@ Polygon::~Polygon() polygon_pub_.reset(); poly_.clear(); dyn_params_handler_.reset(); - node_clock_.reset(); } bool Polygon::configure() @@ -62,7 +60,6 @@ bool Polygon::configure() throw std::runtime_error{"Failed to lock node"}; } - node_clock_ = node->get_clock(); std::string polygon_sub_topic, polygon_pub_topic, footprint_topic; if (!getParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) { @@ -591,11 +588,9 @@ Polygon::dynamicParametersCallback( void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) { - RCLCPP_INFO_THROTTLE( + RCLCPP_INFO( logger_, - *node_clock_, - 2000, - "[%s]: Polygon shape update has arrived", + "[%s]: Polygon shape update has been arrived", polygon_name_.c_str()); updatePolygon(msg); } diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index d49b94c2873..30b90f4f4ab 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -114,7 +114,7 @@ bool SimpleGoalChecker::isGoalReached( double dyaw = angles::shortest_angular_distance( tf2::getYaw(query_pose.orientation), tf2::getYaw(goal_pose.orientation)); - return fabs(dyaw) <= yaw_goal_tolerance_; + return fabs(dyaw) < yaw_goal_tolerance_; } bool SimpleGoalChecker::getTolerances( diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp index 5faba1bdc79..303921bb17e 100644 --- a/nav2_controller/plugins/test/goal_checker.cpp +++ b/nav2_controller/plugins/test/goal_checker.cpp @@ -40,7 +40,6 @@ #include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "eigen3/Eigen/Geometry" using nav2_controller::SimpleGoalChecker; using nav2_controller::StoppedGoalChecker; @@ -238,108 +237,6 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params) EXPECT_EQ(pose_tol.position.y, 200.0); } -TEST(StoppedGoalChecker, is_reached) -{ - auto x = std::make_shared("goal_checker"); - - SimpleGoalChecker gc; - StoppedGoalChecker sgc; - auto costmap = std::make_shared("test_costmap"); - - sgc.initialize(x, "test", costmap); - gc.initialize(x, "test2", costmap); - geometry_msgs::msg::Pose goal_pose; - geometry_msgs::msg::Twist velocity; - geometry_msgs::msg::Pose current_pose; - - // Current linear x position is tolerance away from goal - current_pose.position.x = 0.25; - velocity.linear.x = 0.25; - EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current linear x speed exceeds tolerance - velocity.linear.x = 0.25 + std::numeric_limits::epsilon(); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current linear x position is further than tolerance away from goal - current_pose.position.x = 0.25 + std::numeric_limits::epsilon(); - velocity.linear.x = 0.25; - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - current_pose.position.x = 0.0; - velocity.linear.x = 0.0; - - // Current linear position is tolerance away from goal - current_pose.position.x = 0.25 / std::sqrt(2); - current_pose.position.y = 0.25 / std::sqrt(2); - velocity.linear.x = 0.25 / std::sqrt(2); - velocity.linear.y = 0.25 / std::sqrt(2); - EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current linear speed exceeds tolerance - velocity.linear.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); - velocity.linear.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current linear position is further than tolerance away from goal - current_pose.position.x = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); - current_pose.position.y = 0.25 / std::sqrt(2) + std::numeric_limits::epsilon(); - velocity.linear.x = 0.25 / std::sqrt(2); - velocity.linear.y = 0.25 / std::sqrt(2); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - current_pose.position.x = 0.0; - velocity.linear.x = 0.0; - - - // Current angular position is tolerance away from goal - auto quat = - (Eigen::AngleAxisd::Identity() * Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ())).coeffs(); - // epsilon for orientation is a lot bigger than double limit, probably from TF getYaw - auto quat_epsilon = - (Eigen::AngleAxisd::Identity() * - Eigen::AngleAxisd(0.25 + 1.0E-15, Eigen::Vector3d::UnitZ())).coeffs(); - - current_pose.orientation.z = quat[2]; - current_pose.orientation.w = quat[3]; - velocity.angular.z = 0.25; - EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current angular speed exceeds tolerance - velocity.angular.z = 0.25 + std::numeric_limits::epsilon(); - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity)); - sgc.reset(); - gc.reset(); - - // Current angular position is further than tolerance away from goal - current_pose.orientation.z = quat_epsilon[2]; - current_pose.orientation.w = quat_epsilon[3]; - velocity.angular.z = 0.25; - EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity)); - EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity)); -} - int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp index 4c5e0b28a90..228f51fe9c9 100644 --- a/nav2_controller/plugins/test/progress_checker.cpp +++ b/nav2_controller/plugins/test/progress_checker.cpp @@ -84,11 +84,12 @@ class TestLifecycleNode : public nav2_util::LifecycleNode } }; -bool checkMacro( +void checkMacro( nav2_core::ProgressChecker & pc, double x0, double y0, double theta0, double x1, double y1, double theta1, - int delay) + int delay, + bool expected_result) { pc.reset(); geometry_msgs::msg::PoseStamped pose0, pose1; @@ -100,7 +101,11 @@ bool checkMacro( pose1.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1); EXPECT_TRUE(pc.check(pose0)); rclcpp::sleep_for(std::chrono::milliseconds(delay)); - return pc.check(pose1); + if (expected_result) { + EXPECT_TRUE(pc.check(pose1)); + } else { + EXPECT_FALSE(pc.check(pose1)); + } } TEST(SimpleProgressChecker, progress_checker_reset) @@ -120,7 +125,7 @@ TEST(SimpleProgressChecker, unit_tests) SimpleProgressChecker pc; pc.initialize(x, "nav2_controller"); - double time_allowance = 0.1; + double time_allowance = 0.5; int half_time_allowance_ms = static_cast(time_allowance * 0.5 * 1000); int twice_time_allowance_ms = static_cast(time_allowance * 2.0 * 1000); @@ -142,83 +147,23 @@ TEST(SimpleProgressChecker, unit_tests) // BELOW time allowance (set to time_allowance) // no movement - EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms)); + checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true); // translation below required_movement_radius (default 0.5) - EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms)); - EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms)); + checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true); + checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true); // translation above required_movement_radius (default 0.5) - EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms)); - EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms)); + checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true); + checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true); // ABOVE time allowance (set to time_allowance) // no movement - EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); + checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false); // translation below required_movement_radius (default 0.5) - EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms)); - EXPECT_FALSE(checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms)); + checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false); + checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false); // translation above required_movement_radius (default 0.5) - EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms)); - EXPECT_TRUE(checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms)); -} - -TEST(SimpleProgressChecker, required_movement_radius) { - auto lifecycle_node = std::make_shared("progress_checker"); - - SimpleProgressChecker progress_checker; - progress_checker.initialize(lifecycle_node, "nav2_controller"); - - auto parameters_client = std::make_shared( - lifecycle_node->get_node_base_interface(), lifecycle_node->get_node_topics_interface(), - lifecycle_node->get_node_graph_interface(), - lifecycle_node->get_node_services_interface()); - - constexpr double radius = 0.25; - constexpr double time_allowance = 0.05; - constexpr double twice_time_allowance_ms = time_allowance * 2.0 * 1000; - auto results = parameters_client->set_parameters_atomically( - {rclcpp::Parameter("nav2_controller.required_movement_radius", radius), - rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)}); - rclcpp::spin_until_future_complete( - lifecycle_node->get_node_base_interface(), - results); - - EXPECT_EQ( - lifecycle_node->get_parameter("nav2_controller.required_movement_radius").as_double(), - radius); - - // ABOVE time allowance (set to time_allowance) - // no movement - EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); - // translation at required_movement_radius one axis - EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, radius, 0, 0, twice_time_allowance_ms)); - EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, 0, radius, 0, twice_time_allowance_ms)); - constexpr auto axis = radius / std::sqrt(2); - EXPECT_FALSE(checkMacro(progress_checker, 0, 0, 0, axis, axis, 0, twice_time_allowance_ms)); - // translation at required_movement_radius both axes - - // translation above required_movement_radius one axis - constexpr auto above = radius + std::numeric_limits::epsilon(); - EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, above, 0, 0, twice_time_allowance_ms)); - EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, above, 0, twice_time_allowance_ms)); - // translation at required_movement_radius both axes - constexpr auto above_axis = axis + std::numeric_limits::epsilon(); - EXPECT_TRUE( - checkMacro(progress_checker, 0, 0, 0, above_axis, above_axis, 0, twice_time_allowance_ms)); - - // Edge case, negative radius always true - results = parameters_client->set_parameters_atomically( - {rclcpp::Parameter("nav2_controller.required_movement_radius", -radius)}); - rclcpp::spin_until_future_complete( - lifecycle_node->get_node_base_interface(), - results); - EXPECT_EQ( - lifecycle_node->get_parameter("nav2_controller.required_movement_radius").as_double(), - -radius); - EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); - // translation at required_movement_radius one axis - EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, radius, 0, 0, twice_time_allowance_ms)); - EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, 0, radius, 0, twice_time_allowance_ms)); - EXPECT_TRUE(checkMacro(progress_checker, 0, 0, 0, axis, axis, 0, twice_time_allowance_ms)); + checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true); + checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true); } TEST(PoseProgressChecker, pose_progress_checker_reset) @@ -238,7 +183,7 @@ TEST(PoseProgressChecker, unit_tests) PoseProgressChecker rpc; rpc.initialize(x, "nav2_controller"); - double time_allowance = 0.1; + double time_allowance = 0.5; int half_time_allowance_ms = static_cast(time_allowance * 0.5 * 1000); int twice_time_allowance_ms = static_cast(time_allowance * 2.0 * 1000); @@ -260,35 +205,35 @@ TEST(PoseProgressChecker, unit_tests) // BELOW time allowance (set to time_allowance) // no movement - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms)); + checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true); // translation below required_movement_radius (default 0.5) - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms)); - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms)); + checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true); // rotation below required_movement_angle (default 0.5) - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms)); - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms)); + checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms, true); // translation above required_movement_radius (default 0.5) - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms)); - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms)); + checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true); // rotation above required_movement_angle (default 0.5) - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms)); - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms)); + checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms, true); // ABOVE time allowance (set to time_allowance) // no movement - EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms)); + checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false); // translation below required_movement_radius (default 0.5) - EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms)); - EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms)); + checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false); + checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false); // rotation below required_movement_angle (default 0.5) - EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms)); - EXPECT_FALSE(checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms)); + checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms, false); + checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms, false); // translation above required_movement_radius (default 0.5) - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms)); - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms)); + checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true); // rotation above required_movement_angle (default 0.5) - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms)); - EXPECT_TRUE(checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms)); + checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms, true); + checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms, true); } int main(int argc, char ** argv) 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..858d89b0aca 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -48,15 +48,7 @@ #pragma GCC diagnostic ignored "-Wreorder" #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" #include "sensor_msgs/msg/point_cloud.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp index 2e00a412637..2b34d409a50 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp @@ -20,7 +20,6 @@ #include #include #include - #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" @@ -28,6 +27,7 @@ #include "nav2_costmap_2d/observation_buffer.hpp" #include "nav2_costmap_2d/inflation_layer.hpp" #include "tf2_ros/message_filter.h" +#include "message_filters/subscriber.hpp" #include "pluginlib/class_loader.hpp" using nav2_costmap_2d::LETHAL_OBSTACLE; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp index 3f5fce1537f..8d39116c607 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -42,6 +42,7 @@ #include #include "map_msgs/msg/occupancy_grid_update.hpp" +#include "message_filters/subscriber.hpp" #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index 307385bd4a8..c938f30f55e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -43,10 +43,11 @@ #include #include "map_msgs/msg/occupancy_grid_update.hpp" -#include "rclcpp/rclcpp.hpp" +#include "message_filters/subscriber.hpp" #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" +#include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/footprint.hpp" namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 477020a3b65..ce66cf0c333 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -39,6 +39,7 @@ #define NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_ #include +#include "message_filters/subscriber.hpp" #include #include diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index a05548a8cde..51c018ae427 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -230,19 +230,8 @@ void ObstacleLayer::onInitialize() // create a callback for the topic if (data_type == "LaserScan") { - std::shared_ptr> sub; - - // For Jazzy compatibility - #if RCLCPP_VERSION_GTE(29, 0, 0) - sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); - #else - sub = std::make_shared>( - node, topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt); - #endif - sub->unsubscribe(); auto filter = std::make_shared>( @@ -270,19 +259,8 @@ void ObstacleLayer::onInitialize() observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05)); } else { - std::shared_ptr> sub; - - // For Jazzy compatibility - #if RCLCPP_VERSION_GTE(29, 0, 0) - sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); - #else - sub = std::make_shared>( - node, topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt); - #endif - sub->unsubscribe(); if (inf_is_valid) { diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index 757f679e85a..0e07373c338 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,5 +1,5 @@ #goal definition -nav_msgs/Goals goals +geometry_msgs/PoseStampedArray goals geometry_msgs/PoseStamped start string planner_id bool use_start # If false, use current robot pose as path start, if true, use start above instead diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index a4301f36e4a..0efffe433ec 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,6 +1,6 @@ #goal definition -nav_msgs/Goals poses +geometry_msgs/PoseStampedArray poses string behavior_tree --- #result definition diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index be65ffef9c8..933bd7664d1 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -394,7 +394,7 @@ void PlannerServer::computePlanThroughPoses() getPreemptedGoalIfRequested(action_server_poses_, goal); - if (goal->goals.goals.empty()) { + if (goal->goals.poses.empty()) { throw nav2_core::NoViapointsGiven("No viapoints given"); } @@ -409,7 +409,7 @@ void PlannerServer::computePlanThroughPoses() }; // Get consecutive paths through these points - for (unsigned int i = 0; i != goal->goals.goals.size(); i++) { + for (unsigned int i = 0; i != goal->goals.poses.size(); i++) { // Get starting point if (i == 0) { curr_start = start; @@ -419,7 +419,7 @@ void PlannerServer::computePlanThroughPoses() curr_start = concat_path.poses.back(); curr_start.header = concat_path.header; } - curr_goal = goal->goals.goals[i]; + curr_goal = goal->goals.poses[i]; // Transform them into the global frame if (!transformPosesToGlobalFrame(curr_start, curr_goal)) { diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 2239c47fe5d..b4c25d5fb3c 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -102,7 +102,7 @@ private Q_SLOTS: std::vector orientation); void startWaypointFollowing(std::vector poses); void startNavigation(geometry_msgs::msg::PoseStamped); - void startNavThroughPoses(nav_msgs::msg::Goals poses); + void startNavThroughPoses(geometry_msgs::msg::PoseStampedArray poses); using NavigationGoalHandle = rclcpp_action::ClientGoalHandle; using WaypointFollowerGoalHandle = @@ -196,8 +196,8 @@ private Q_SLOTS: QState * accumulated_wp_{nullptr}; QState * accumulated_nav_through_poses_{nullptr}; - nav_msgs::msg::Goals acummulated_poses_; - nav_msgs::msg::Goals store_poses_; + geometry_msgs::msg::PoseStampedArray acummulated_poses_; + geometry_msgs::msg::PoseStampedArray store_poses_; // Publish the visual markers with the waypoints void updateWpNavigationMarkers(); diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index a956108ce5f..4b3a5f49051 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -675,7 +675,7 @@ void Nav2Panel::loophandler() void Nav2Panel::handleGoalLoader() { - acummulated_poses_ = nav_msgs::msg::Goals(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); std::cout << "Loading Waypoints!" << std::endl; @@ -699,7 +699,7 @@ void Nav2Panel::handleGoalLoader() auto waypoint = waypoint_iter[it->first.as()]; auto pose = waypoint["pose"].as>(); auto orientation = waypoint["orientation"].as>(); - acummulated_poses_.goals.push_back(convert_to_msg(pose, orientation)); + acummulated_poses_.poses.push_back(convert_to_msg(pose, orientation)); } // Publishing Waypoint Navigation marker after loading wp's @@ -731,7 +731,7 @@ void Nav2Panel::handleGoalSaver() { // Check if the waypoints are accumulated - if (acummulated_poses_.goals.empty()) { + if (acummulated_poses_.poses.empty()) { std::cout << "No accumulated Points to Save!" << std::endl; return; } else { @@ -744,19 +744,19 @@ void Nav2Panel::handleGoalSaver() out << YAML::BeginMap; // Save WPs to data structure - for (unsigned int i = 0; i < acummulated_poses_.goals.size(); ++i) { + for (unsigned int i = 0; i < acummulated_poses_.poses.size(); ++i) { out << YAML::Key << "waypoint" + std::to_string(i); out << YAML::BeginMap; out << YAML::Key << "pose"; std::vector pose = - {acummulated_poses_.goals[i].pose.position.x, acummulated_poses_.goals[i].pose.position.y, - acummulated_poses_.goals[i].pose.position.z}; + {acummulated_poses_.poses[i].pose.position.x, acummulated_poses_.poses[i].pose.position.y, + acummulated_poses_.poses[i].pose.position.z}; out << YAML::Value << pose; out << YAML::Key << "orientation"; std::vector orientation = - {acummulated_poses_.goals[i].pose.orientation.w, acummulated_poses_.goals[i].pose.orientation.x, - acummulated_poses_.goals[i].pose.orientation.y, - acummulated_poses_.goals[i].pose.orientation.z}; + {acummulated_poses_.poses[i].pose.orientation.w, acummulated_poses_.poses[i].pose.orientation.x, + acummulated_poses_.poses[i].pose.orientation.y, + acummulated_poses_.poses[i].pose.orientation.z}; out << YAML::Value << orientation; out << YAML::EndMap; } @@ -837,10 +837,10 @@ Nav2Panel::onInitialize() // Clearing all the stored values once reaching the final goal if ( loop_count_ == stoi(nr_of_loops_->displayText().toStdString()) && - goal_index_ == static_cast(store_poses_.goals.size()) - 1 && + goal_index_ == static_cast(store_poses_.poses.size()) - 1 && msg->status_list.back().status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { - store_poses_ = nav_msgs::msg::Goals(); + store_poses_ = geometry_msgs::msg::PoseStampedArray(); waypoint_status_indicator_->clear(); loop_no_ = "0"; loop_count_ = 0; @@ -936,8 +936,8 @@ Nav2Panel::onCancel() &Nav2Panel::onCancelButtonPressed, this)); waypoint_status_indicator_->clear(); - store_poses_ = nav_msgs::msg::Goals(); - acummulated_poses_ = nav_msgs::msg::Goals(); + store_poses_ = geometry_msgs::msg::PoseStampedArray(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); } void Nav2Panel::onResumedWp() @@ -967,12 +967,12 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) pose.pose.position.z = 0.0; pose.pose.orientation = orientationAroundZAxis(theta); - if (store_poses_.goals.empty()) { + if (store_poses_.poses.empty()) { if (state_machine_.configuration().contains(accumulating_)) { waypoint_status_indicator_->clear(); - acummulated_poses_.goals.push_back(pose); + acummulated_poses_.poses.push_back(pose); } else { - acummulated_poses_ = nav_msgs::msg::Goals(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); updateWpNavigationMarkers(); std::cout << "Start navigation" << std::endl; startNavigation(pose); @@ -1031,7 +1031,7 @@ Nav2Panel::onCancelButtonPressed() void Nav2Panel::onAccumulatedWp() { - if (acummulated_poses_.goals.empty()) { + if (acummulated_poses_.poses.empty()) { state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); waypoint_status_indicator_->setText( " Note: Uh oh! Someone forgot to select the waypoints"); @@ -1053,7 +1053,7 @@ Nav2Panel::onAccumulatedWp() /** Making sure that the pose array does not get updated * between the process**/ - if (store_poses_.goals.empty()) { + if (store_poses_.poses.empty()) { std::cout << "Start waypoint" << std::endl; // Setting the final loop value on the text box for sanity @@ -1066,12 +1066,12 @@ Nav2Panel::onAccumulatedWp() if (store_initial_pose_) { try { init_transform = tf2_buffer_->lookupTransform( - acummulated_poses_.goals[0].header.frame_id, base_frame_, + acummulated_poses_.poses[0].header.frame_id, base_frame_, tf2::TimePointZero); } catch (const tf2::TransformException & ex) { RCLCPP_INFO( client_node_->get_logger(), "Could not transform %s to %s: %s", - acummulated_poses_.goals[0].header.frame_id.c_str(), base_frame_.c_str(), ex.what()); + acummulated_poses_.poses[0].header.frame_id.c_str(), base_frame_.c_str(), ex.what()); return; } @@ -1087,24 +1087,24 @@ Nav2Panel::onAccumulatedWp() initial_pose.pose.orientation.w = init_transform.transform.rotation.w; // inserting the acummulated pose - acummulated_poses_.goals.insert(acummulated_poses_.goals.begin(), initial_pose); + acummulated_poses_.poses.insert(acummulated_poses_.poses.begin(), initial_pose); updateWpNavigationMarkers(); initial_pose_stored_ = true; if (loop_count_ == 0) { goal_index_ = 1; } } else if (!store_initial_pose_ && initial_pose_stored_) { - acummulated_poses_.goals.erase( - acummulated_poses_.goals.begin(), - acummulated_poses_.goals.begin()); + acummulated_poses_.poses.erase( + acummulated_poses_.poses.begin(), + acummulated_poses_.poses.begin()); } } else { std::cout << "Resuming waypoint" << std::endl; } - startWaypointFollowing(acummulated_poses_.goals); + startWaypointFollowing(acummulated_poses_.poses); store_poses_ = acummulated_poses_; - acummulated_poses_ = nav_msgs::msg::Goals(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); } void @@ -1117,8 +1117,8 @@ Nav2Panel::onAccumulatedNTP() void Nav2Panel::onAccumulating() { - acummulated_poses_ = nav_msgs::msg::Goals(); - store_poses_ = nav_msgs::msg::Goals(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); + store_poses_ = geometry_msgs::msg::PoseStampedArray(); loop_count_ = 0; loop_no_ = "0"; initial_pose_stored_ = false; @@ -1255,7 +1255,7 @@ Nav2Panel::startWaypointFollowing(std::vector p } void -Nav2Panel::startNavThroughPoses(nav_msgs::msg::Goals poses) +Nav2Panel::startNavThroughPoses(geometry_msgs::msg::PoseStampedArray poses) { auto is_action_server_ready = nav_through_poses_action_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -1273,8 +1273,8 @@ Nav2Panel::startNavThroughPoses(nav_msgs::msg::Goals poses) RCLCPP_DEBUG( client_node_->get_logger(), "Sending a path of %zu waypoints:", - nav_through_poses_goal_.poses.goals.size()); - for (auto waypoint : nav_through_poses_goal_.poses.goals) { + nav_through_poses_goal_.poses.poses.size()); + for (auto waypoint : nav_through_poses_goal_.poses.poses) { RCLCPP_DEBUG( client_node_->get_logger(), "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); @@ -1385,14 +1385,14 @@ Nav2Panel::updateWpNavigationMarkers() auto marker_array = std::make_unique(); - for (size_t i = 0; i < acummulated_poses_.goals.size(); i++) { + for (size_t i = 0; i < acummulated_poses_.poses.size(); i++) { // Draw a green arrow at the waypoint pose visualization_msgs::msg::Marker arrow_marker; - arrow_marker.header = acummulated_poses_.goals[i].header; + arrow_marker.header = acummulated_poses_.poses[i].header; arrow_marker.id = getUniqueId(); arrow_marker.type = visualization_msgs::msg::Marker::ARROW; arrow_marker.action = visualization_msgs::msg::Marker::ADD; - arrow_marker.pose = acummulated_poses_.goals[i].pose; + arrow_marker.pose = acummulated_poses_.poses[i].pose; arrow_marker.scale.x = 0.3; arrow_marker.scale.y = 0.05; arrow_marker.scale.z = 0.02; @@ -1406,11 +1406,11 @@ Nav2Panel::updateWpNavigationMarkers() // Draw a red circle at the waypoint pose visualization_msgs::msg::Marker circle_marker; - circle_marker.header = acummulated_poses_.goals[i].header; + circle_marker.header = acummulated_poses_.poses[i].header; circle_marker.id = getUniqueId(); circle_marker.type = visualization_msgs::msg::Marker::SPHERE; circle_marker.action = visualization_msgs::msg::Marker::ADD; - circle_marker.pose = acummulated_poses_.goals[i].pose; + circle_marker.pose = acummulated_poses_.poses[i].pose; circle_marker.scale.x = 0.05; circle_marker.scale.y = 0.05; circle_marker.scale.z = 0.05; @@ -1424,11 +1424,11 @@ Nav2Panel::updateWpNavigationMarkers() // Draw the waypoint number visualization_msgs::msg::Marker marker_text; - marker_text.header = acummulated_poses_.goals[i].header; + marker_text.header = acummulated_poses_.poses[i].header; marker_text.id = getUniqueId(); marker_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_text.action = visualization_msgs::msg::Marker::ADD; - marker_text.pose = acummulated_poses_.goals[i].pose; + marker_text.pose = acummulated_poses_.poses[i].pose; marker_text.pose.position.z += 0.2; // draw it on top of the waypoint marker_text.scale.x = 0.07; marker_text.scale.y = 0.07; diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index ed277bb2e51..61182260692 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -619,7 +619,7 @@ def _getPathThroughPosesImpl( goal_msg.start = start goal_msg.goals.header.frame_id = 'map' goal_msg.goals.header.stamp = self.get_clock().now().to_msg() - goal_msg.goals.goals = goals + goal_msg.goals.poses = goals goal_msg.planner_id = planner_id goal_msg.use_start = use_start 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 f24729cd79d..8935cff9a22 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 @@ -132,7 +132,7 @@ def runNavigateAction(self, goal_msg = NavigateThroughPoses.Goal() goal_msg.poses.header.frame_id = 'map' goal_msg.poses.header.stamp = self.get_clock().now().to_msg() - goal_msg.poses.goals = [ + goal_msg.poses.poses = [ self.getStampedPoseMsg(self.goal_pose), self.getStampedPoseMsg(self.goal_pose), ] 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 6109223c82a..11624528010 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 @@ -90,7 +90,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): goal_msg = NavigateThroughPoses.Goal() goal_msg.poses.header.frame_id = 'map' goal_msg.poses.header.stamp = self.get_clock().now().to_msg() - goal_msg.poses.goals = [ + goal_msg.poses.poses = [ self.getStampedPoseMsg(self.goal_pose), self.getStampedPoseMsg(self.goal_pose), ] @@ -166,7 +166,7 @@ def runNavigatePreemptionAction(self, block): ) goal_msg = NavigateThroughPoses.Goal() - goal_msg.poses.goals = [self.getStampedPoseMsg(self.initial_pose)] + goal_msg.poses.poses = [self.getStampedPoseMsg(self.initial_pose)] self.info_msg('Sending goal request...') send_goal_future = self.action_client.send_goal_async(goal_msg) diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 7dd40fdddd8..e9624f36c84 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -45,23 +45,13 @@ target_link_libraries(${library_name} PUBLIC rclcpp::rclcpp rclcpp_action::rclcpp_action rclcpp_lifecycle::rclcpp_lifecycle + robot_localization::rl_lib tf2_ros::tf2_ros ) target_link_libraries(${library_name} PRIVATE rclcpp_components::component ) -# Extract the major version (first number before the dot) -string(REGEX MATCH "^[0-9]+" ROS2_MAJOR_VERSION "${rclcpp_VERSION}") -# Convert to integer -if(ROS2_MAJOR_VERSION GREATER_EQUAL 29) - message(STATUS "ROS2_MAJOR_VERSION (${ROS2_MAJOR_VERSION}) is greater than 29 (Kilted and newer).") - target_link_libraries(${library_name} PUBLIC robot_localization::rl_lib) -else() - message(STATUS "ROS2_MAJOR_VERSION (${ROS2_MAJOR_VERSION}) is NOT greater than 29 (Jazzy and older).") - ament_target_dependencies(${library_name} PUBLIC robot_localization) -endif() - add_executable(${executable_name} src/main.cpp ) From cf23375cc6005296f875f0afdda248b91e14602a Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Fri, 21 Mar 2025 21:40:25 +0000 Subject: [PATCH 13/14] Rm submodule Signed-off-by: Nils-ChristianIseke --- .pre-commit-config.yaml | 1 + ament_lint | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) delete mode 160000 ament_lint diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 918c86fc381..95cb7e12bac 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -27,6 +27,7 @@ repos: 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] diff --git a/ament_lint b/ament_lint deleted file mode 160000 index f14163fe2a8..00000000000 --- a/ament_lint +++ /dev/null @@ -1 +0,0 @@ -Subproject commit f14163fe2a8d91886fedfc59c5f4fcb6d4e9453a From 360e96fd952da6280583708d7c2d0862365e8d29 Mon Sep 17 00:00:00 2001 From: Nils-ChristianIseke Date: Sat, 22 Mar 2025 06:59:06 +0000 Subject: [PATCH 14/14] pre-commit run --all after merge Signed-off-by: Nils-ChristianIseke --- nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index 2efb08485bb..a588fdd63e2 100644 --- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -21,14 +21,13 @@ 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) + 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