diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 01731dda48c..0a64abd703a 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -14,6 +14,8 @@ // "--pid=host", // DDS discovery with host, without --network=host // "--privileged", // device access to host peripherals, e.g. USB // "--security-opt=seccomp=unconfined", // enable debugging, e.g. gdb + // "--volume=/tmp/.X11-unix:/tmp/.X11-unix", // X11 socket for GUI applications + // "--gpus=all" // access to all GPUs, e.g. for GPU-accelerated applications ], "workspaceFolder": "/opt/overlay_ws/src/navigation2", "workspaceMount": "source=${localWorkspaceFolder},target=${containerWorkspaceFolder},type=bind", @@ -23,6 +25,8 @@ "remoteEnv": { "OVERLAY_MIXINS": "release ccache lld", "CCACHE_DIR": "/tmp/.ccache" + // "QT_X11_NO_MITSHM": "1", // disable MIT-SHM for X11 forwarding + // "DISPLAY": "${localEnv:DISPLAY}", // X11 forwarding }, "mounts": [ { diff --git a/.github/mergify.yml b/.github/mergify.yml index f91d5df4d38..754975652a2 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,14 +8,14 @@ pull_request_rules: branches: - jazzy - - name: backport to iron at reviewers discretion + - name: backport to humble-main at reviewers discretion conditions: - base=main - - "label=backport-iron" + - "label=backport-humble-main" actions: backport: branches: - - iron + - humble_main - name: backport to humble at reviewers discretion conditions: @@ -26,14 +26,14 @@ pull_request_rules: branches: - humble - - name: backport to humble_main at reviewers discretion + - name: backport to kilted at reviewers discretion conditions: - base=main - - "label=backport-humble-main" + - "label=backport-kilted" actions: backport: branches: - - humble_main + - kilted - name: delete head branch after merge conditions: diff --git a/.github/workflows/build_main_against_distros.yml b/.github/workflows/build_main_against_distros.yml new file mode 100644 index 00000000000..5c5f58d9103 --- /dev/null +++ b/.github/workflows/build_main_against_distros.yml @@ -0,0 +1,49 @@ +name: Build Against Released Distributions + +on: + workflow_dispatch: + pull_request: + branches: + - main + +jobs: + build-docker: + runs-on: ubuntu-latest + + strategy: + fail-fast: false + matrix: + ros_distro: [jazzy, kilted] + + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + - name: Set up Docker build context + run: | + mkdir -p /tmp/docker_context/ws/src + cp -r . /tmp/docker_context/ws/src + echo 'FROM ghcr.io/ros-navigation/nav2_docker:${{ matrix.ros_distro }}-nightly + + RUN apt-get update && apt-get install -y \ + python3-pip \ + python3-colcon-common-extensions \ + python3-vcstool \ + git \ + curl \ + && rm -rf /var/lib/apt/lists/* + + WORKDIR /root/ws + + COPY ws /root/ws + + RUN apt-get update && rosdep update && \ + rosdep install --from-paths src --ignore-src -r -y \ + --skip-keys=slam_toolbox + + RUN . /opt/ros/${{ matrix.ros_distro }}/setup.sh && colcon build --packages-skip nav2_system_tests nav2_bringup nav2_simple_commander nav2_loopback_sim navigation2' > /tmp/docker_context/Dockerfile + + + - name: Build Docker image + run: | + docker build -t nav2-${{ matrix.ros_distro }}-main-compatibility /tmp/docker_context diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml index 8e914771645..5692ce8cf26 100644 --- a/.github/workflows/update_ci_image.yaml +++ b/.github/workflows/update_ci_image.yaml @@ -8,6 +8,7 @@ on: push: branches: - main + - kilted - jazzy - humble - humble_main diff --git a/.gitignore b/.gitignore index 203501ea402..bb346f49ba9 100644 --- a/.gitignore +++ b/.gitignore @@ -45,6 +45,7 @@ install __pycache__/ *.py[cod] .ipynb_checkpoints +.pytest_cache/ sphinx_doc/_build diff --git a/README.md b/README.md index 64125080c7f..d7e58d43dc2 100644 --- a/README.md +++ b/README.md @@ -113,39 +113,40 @@ If you use our work on VSLAM and formal comparisons for service robot needs, ple ## Build Status -| Package | humble Source | humble 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/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/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/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/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/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/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/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/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/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/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/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/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/) | [![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/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/) | [![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/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_loopback_sim | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_loopback_sim__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_loopback_sim__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_loopback_sim__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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/) | +| Package | humble Source | humble Debian | jazzy Source | jazzy Debian | kilted Source | kilted 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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__navigation2__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__navigation2__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__navigation2__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_amcl__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_amcl__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_amcl__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_behavior_tree__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_behavior_tree__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_behavior_tree__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_behaviors__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_behaviors__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_behaviors__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_bringup__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_bringup__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_bringup__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_bt_navigator__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_bt_navigator__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_bt_navigator__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_collision_monitor__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_collision_monitor__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_collision_monitor__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_common__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_common__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_common__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_constrained_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_constrained_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_constrained_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_core__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_core__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_core__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_costmap_2d__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_costmap_2d__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_costmap_2d__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/) | [![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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__opennav_docking__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__opennav_docking__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__opennav_docking__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_dwb_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_dwb_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_dwb_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/) | [![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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_graceful_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_graceful_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_graceful_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_lifecycle_manager__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_lifecycle_manager__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_lifecycle_manager__ubuntu_noble_amd64__binary/) | +| nav2_loopback_sim | N/A | N/A | [![Build Status](https://build.ros2.org/job/Jsrc_un__nav2_loopback_sim__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Jsrc_un__nav2_loopback_sim__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Jbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_loopback_sim__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_loopback_sim__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_loopback_sim__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_loopback_sim__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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_map_server__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_map_server__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_map_server__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_mppi_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_mppi_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_mppi_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_msgs__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_msgs__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_navfn_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_navfn_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_navfn_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_regulated_pure_pursuit_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_regulated_pure_pursuit_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_rotation_shim_controller__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_rotation_shim_controller__ubuntu_noble_amd64__binary/) | +| nav2_route | N/A | N/A | N/A | N/A | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_route__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_route__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_route__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_route__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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_rviz_plugins__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_rviz_plugins__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_rviz_plugins__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_simple_commander__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_simple_commander__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_simple_commander__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_smac_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_smac_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_smac_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_system_tests__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_system_tests__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_system_tests__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_theta_star_planner__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_theta_star_planner__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_theta_star_planner__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_util__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_util__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_util__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_velocity_smoother__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_velocity_smoother__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_velocity_smoother__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_voxel_grid__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_voxel_grid__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_voxel_grid__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_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/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/) | [![Build Status](https://build.ros2.org/job/Ksrc_un__nav2_waypoint_follower__ubuntu_noble__source/badge/icon)](https://build.ros2.org/job/Ksrc_un__nav2_waypoint_follower__ubuntu_noble__source/) | [![Build Status](https://build.ros2.org/job/Kbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Kbin_un64__nav2_waypoint_follower__ubuntu_noble_amd64__binary/) | diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index c7341a5985a..58b48246a9d 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -29,6 +29,7 @@ #include #include "message_filters/subscriber.hpp" +#include "rclcpp/version.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -171,8 +172,14 @@ class AmclNode : public nav2_util::LifecycleNode * @brief Initialize incoming data message subscribers and filters */ void initMessageFilters(); + + // To Support Kilted and Older from Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::unique_ptr> laser_scan_sub_; + #else std::unique_ptr> laser_scan_sub_; + #endif std::unique_ptr> laser_scan_filter_; message_filters::Connection laser_scan_connection_; diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 38da836d086..d7af79e860b 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.3.1 + 1.4.0

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 5dcc83eb93a..953e5e05509 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1182,6 +1182,9 @@ AmclNode::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { @@ -1520,9 +1523,15 @@ AmclNode::initMessageFilters() { auto sub_opt = rclcpp::SubscriptionOptions(); sub_opt.callback_group = callback_group_; + + #if RCLCPP_VERSION_GTE(29, 6, 0) + laser_scan_sub_ = std::make_unique>( + shared_from_this(), scan_topic_, rclcpp::SensorDataQoS(), sub_opt); + #else laser_scan_sub_ = std::make_unique>( shared_from_this(), scan_topic_, rmw_qos_profile_sensor_data, sub_opt); + #endif laser_scan_filter_ = std::make_unique>( *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 32e65a7b5b7..c99cbf2b9ac 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -187,6 +187,15 @@ class BtActionNode : public BT::ActionNodeBase return BT::NodeStatus::SUCCESS; } + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + virtual void on_timeout() + { + return; + } + /** * @brief The main override required by a BT action * @return BT::NodeStatus Status of tick execution @@ -231,6 +240,7 @@ class BtActionNode : public BT::ActionNodeBase "Timed out while waiting for action server to acknowledge goal request for %s", action_name_.c_str()); future_goal_handle_.reset(); + on_timeout(); return BT::NodeStatus::FAILURE; } } @@ -261,6 +271,7 @@ class BtActionNode : public BT::ActionNodeBase "Timed out while waiting for action server to acknowledge goal request for %s", action_name_.c_str()); future_goal_handle_.reset(); + on_timeout(); return BT::NodeStatus::FAILURE; } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp index f3f7e0794c0..f4a740abc84 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp @@ -66,6 +66,12 @@ class AssistedTeleopAction : public BtActionNode */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * @brief Function to read parameters and initialize class variables */ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp index c56da81de5d..6d258552a7f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp @@ -64,6 +64,12 @@ class ComputeAndTrackRouteAction : public BtActionNode */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * \brief Override required by the a BT action. Cancel the action and set the path output */ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp index 55a30e74c3b..2a2ac3948ca 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp @@ -88,6 +88,12 @@ class DriveOnHeadingAction : public BtActionNode */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * @brief Function to perform some user-defined operation after a timeout * waiting for a result that hasn't been received yet 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 fa175002a15..71c6597683f 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 @@ -67,6 +67,12 @@ class NavigateThroughPosesAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * @brief Function to read parameters and initialize class variables */ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp index f4e65a7bf82..866dc130901 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp @@ -50,6 +50,12 @@ class WaitAction : public BtActionNode */ void on_tick() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * @brief Function to read parameters and initialize class variables */ diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 851efab9c24..31da48c1868 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.3.1 + 1.4.0 Nav2 behavior tree wrappers, nodes, and utilities Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index 2c95dda33a7..3f293f588bf 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -69,6 +69,12 @@ BT::NodeStatus AssistedTeleopAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void AssistedTeleopAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 5e29107c9e4..d31ff9aecb1 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -78,6 +78,12 @@ BT::NodeStatus BackUpAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void BackUpAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp index cc6db6fbe8b..eb2edc19b76 100644 --- a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp @@ -75,6 +75,12 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void ComputeAndTrackRouteAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void ComputeAndTrackRouteAction::on_wait_for_result( std::shared_ptr/*feedback*/) { diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index 3b778903df5..72a62258599 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -66,6 +66,12 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void ComputePathThroughPosesAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void ComputePathThroughPosesAction::halt() { nav_msgs::msg::Path empty_path; diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index 41772aca79b..0d80527c913 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -82,6 +82,12 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void ComputePathToPoseAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void ComputePathToPoseAction::halt() { nav_msgs::msg::Path empty_path; diff --git a/nav2_behavior_tree/plugins/action/compute_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_route_action.cpp index cc16125c74b..6613d3e7b91 100644 --- a/nav2_behavior_tree/plugins/action/compute_route_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_route_action.cpp @@ -87,6 +87,12 @@ BT::NodeStatus ComputeRouteAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void ComputeRouteAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void ComputeRouteAction::halt() { resetPorts(); diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index e3a0fc02765..b612c16f86b 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -76,6 +76,12 @@ BT::NodeStatus DriveOnHeadingAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void DriveOnHeadingAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 875c1b71c44..e89a23e282a 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -58,6 +58,12 @@ BT::NodeStatus FollowPathAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void FollowPathAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::CONTROLLER_TIMED_OUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void FollowPathAction::on_wait_for_result( std::shared_ptr/*feedback*/) { diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp index be6272317ab..3b362d55300 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -61,6 +61,11 @@ BT::NodeStatus NavigateThroughPosesAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void NavigateThroughPosesAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index 4bfac9e8829..775314babc4 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -60,6 +60,12 @@ BT::NodeStatus NavigateToPoseAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void NavigateToPoseAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp index ebd72cbf137..cb420e0508e 100644 --- a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp @@ -65,6 +65,12 @@ BT::NodeStatus SmoothPathAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void SmoothPathAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index c41d2a97744..f5b1cb07fa9 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -68,6 +68,12 @@ BT::NodeStatus SpinAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void SpinAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index 4a357fd1b94..8d22313e04b 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -73,6 +73,12 @@ BT::NodeStatus WaitAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void WaitAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 2ef8566947b..5c983cb3554 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.3.1 + 1.4.0 Nav2 behavior server Carlos Orduno Steve Macenski diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 858e6a9784f..ba6fa035c11 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -36,6 +36,7 @@ def generate_launch_description() -> LaunchDescription: slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') keepout_mask_yaml_file = LaunchConfiguration('keepout_mask') + speed_mask_yaml_file = LaunchConfiguration('speed_mask') graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') @@ -45,15 +46,22 @@ def generate_launch_description() -> LaunchDescription: log_level = LaunchConfiguration('log_level') use_localization = LaunchConfiguration('use_localization') use_keepout_zones = LaunchConfiguration('use_keepout_zones') + use_speed_zones = LaunchConfiguration('use_speed_zones') # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + yaml_substitutions = { + 'KEEPOUT_ZONE_ENABLED': use_keepout_zones, + 'SPEED_ZONE_ENABLED': use_speed_zones, + } + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, + value_rewrites=yaml_substitutions, convert_types=True, ), allow_substs=True, @@ -80,6 +88,11 @@ def generate_launch_description() -> LaunchDescription: description='Full path to keepout mask yaml file to load' ) + declare_speed_mask_yaml_cmd = DeclareLaunchArgument( + 'speed_mask', default_value='', + description='Full path to speed mask yaml file to load' + ) + declare_graph_file_cmd = DeclareLaunchArgument( 'graph', default_value='', description='Path to the graph file to load' @@ -95,6 +108,11 @@ def generate_launch_description() -> LaunchDescription: description='Whether to enable keepout zones or not' ) + declare_use_speed_zones_cmd = DeclareLaunchArgument( + 'use_speed_zones', default_value='True', + description='Whether to enable speed zones or not' + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', @@ -189,6 +207,22 @@ def generate_launch_description() -> LaunchDescription: }.items(), ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'speed_zone_launch.py') + ), + condition=IfCondition(PythonExpression([use_speed_zones])), + launch_arguments={ + 'namespace': namespace, + 'speed_mask': speed_mask_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container', + }.items(), + ), + IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(launch_dir, 'navigation_launch.py') @@ -218,6 +252,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_keepout_mask_yaml_cmd) + ld.add_action(declare_speed_mask_yaml_cmd) ld.add_action(declare_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) @@ -227,6 +262,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_log_level_cmd) ld.add_action(declare_use_localization_cmd) ld.add_action(declare_use_keepout_zones_cmd) + ld.add_action(declare_use_speed_zones_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/launch/keepout_zone_launch.py b/nav2_bringup/launch/keepout_zone_launch.py index 58d5a5987bf..2a8a1925bd6 100644 --- a/nav2_bringup/launch/keepout_zone_launch.py +++ b/nav2_bringup/launch/keepout_zone_launch.py @@ -40,16 +40,21 @@ def generate_launch_description() -> LaunchDescription: use_keepout_zones = LaunchConfiguration('use_keepout_zones') log_level = LaunchConfiguration('log_level') - lifecycle_nodes = ['filter_mask_server', 'costmap_filter_info_server'] + lifecycle_nodes = ['keepout_filter_mask_server', 'keepout_costmap_filter_info_server'] # Map fully qualified names to relative ones so the node's namespace can be prepended. remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + yaml_substitutions = { + 'KEEPOUT_ZONE_ENABLED': use_keepout_zones, + } + configured_params = ParameterFile( RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites={}, + value_rewrites=yaml_substitutions, convert_types=True, ), allow_substs=True, @@ -116,7 +121,7 @@ def generate_launch_description() -> LaunchDescription: condition=IfCondition(use_keepout_zones), package='nav2_map_server', executable='map_server', - name='filter_mask_server', + name='keepout_filter_mask_server', output='screen', respawn=use_respawn, respawn_delay=2.0, @@ -128,7 +133,7 @@ def generate_launch_description() -> LaunchDescription: condition=IfCondition(use_keepout_zones), package='nav2_map_server', executable='costmap_filter_info_server', - name='costmap_filter_info_server', + name='keepout_costmap_filter_info_server', output='screen', respawn=use_respawn, respawn_delay=2.0, @@ -162,7 +167,7 @@ def generate_launch_description() -> LaunchDescription: ComposableNode( package='nav2_map_server', plugin='nav2_map_server::MapServer', - name='filter_mask_server', + name='keepout_filter_mask_server', parameters=[ configured_params, {'yaml_filename': keepout_mask_yaml_file} @@ -172,7 +177,7 @@ def generate_launch_description() -> LaunchDescription: ComposableNode( package='nav2_map_server', plugin='nav2_map_server::CostmapFilterInfoServer', - name='costmap_filter_info_server', + name='keepout_costmap_filter_info_server', parameters=[configured_params], remappings=remappings, ), diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 8d5e3e154c0..5ee6a365adc 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -58,7 +58,7 @@ def generate_launch_description() -> LaunchDescription: package='rviz2', executable='rviz2', namespace=namespace, - arguments=['-d', rviz_config_file], + arguments=['-d', rviz_config_file, '--ros-args', '--log-level', 'warn'], output='screen', parameters=[{'use_sim_time': use_sim_time}], remappings=[ diff --git a/nav2_bringup/launch/speed_zone_launch.py b/nav2_bringup/launch/speed_zone_launch.py new file mode 100644 index 00000000000..02f8254231b --- /dev/null +++ b/nav2_bringup/launch/speed_zone_launch.py @@ -0,0 +1,224 @@ +# Copyright (c) 2025 Leander Stephen Desouza +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node, SetParameter +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description() -> LaunchDescription: + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + speed_mask_yaml_file = LaunchConfiguration('speed_mask') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + use_speed_zones = LaunchConfiguration('use_speed_zones') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['speed_filter_mask_server', 'speed_costmap_filter_info_server'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] + + yaml_substitutions = { + 'SPEED_ZONE_ENABLED': use_speed_zones, + } + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + value_rewrites=yaml_substitutions, + convert_types=True, + ), + allow_substs=True, + ) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1' + ) + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', default_value='', description='Top-level namespace' + ) + + declare_speed_mask_yaml_cmd = DeclareLaunchArgument( + 'speed_mask', + default_value='', + description='Full path to speed mask yaml file to load', + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true', + ) + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes', + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', + default_value='False', + description='Use composed bringup if True', + ) + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', + default_value='nav2_container', + description='the name of container that nodes will load in if use composition', + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', + default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.', + ) + + declare_use_speed_zones_cmd = DeclareLaunchArgument( + 'use_speed_zones', default_value='True', + description='Whether to enable speed zones or not' + ) + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', description='log level' + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + Node( + condition=IfCondition(use_speed_zones), + package='nav2_map_server', + executable='map_server', + name='speed_filter_mask_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, {'yaml_filename': speed_mask_yaml_file}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + condition=IfCondition(use_speed_zones), + package='nav2_map_server', + executable='costmap_filter_info_server', + name='speed_costmap_filter_info_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_speed_zone', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}], + ), + ], + ) + # LoadComposableNode for map server twice depending if we should use the + # value of map from a CLI or launch default or user defined value in the + # yaml configuration file. They are separated since the conditions + # currently only work on the LoadComposableNodes commands and not on the + # ComposableNode node function itself + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + LoadComposableNodes( + target_container=container_name_full, + condition=IfCondition(use_speed_zones), + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='speed_filter_mask_server', + parameters=[ + configured_params, + {'yaml_filename': speed_mask_yaml_file} + ], + remappings=remappings, + ), + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::CostmapFilterInfoServer', + name='speed_costmap_filter_info_server', + parameters=[configured_params], + remappings=remappings, + ), + ], + ), + + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_speed_zone', + parameters=[ + {'autostart': autostart, 'node_names': lifecycle_nodes} + ], + ), + ], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_speed_mask_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_use_speed_zones_cmd) + ld.add_action(declare_log_level_cmd) + + # Add the actions to launch all of the map modifier nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_bringup/launch/tb3_loopback_simulation_launch.py b/nav2_bringup/launch/tb3_loopback_simulation_launch.py index 26c22a31d50..9f51e66c04b 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation_launch.py @@ -144,6 +144,8 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_respawn': use_respawn, 'use_localization': 'False', # Don't use SLAM, AMCL + 'use_keepout_zones': 'False', + 'use_speed_zones': 'False', }.items(), ) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index cc40ed90c56..e0a5f217646 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -195,6 +195,8 @@ def generate_launch_description() -> LaunchDescription: 'autostart': autostart, 'use_composition': use_composition, 'use_respawn': use_respawn, + 'use_keepout_zones': 'False', + 'use_speed_zones': 'False', }.items(), ) # The SDF file for the world is a xacro file because we wanted to diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index f92181332bf..f9197a3085c 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -55,6 +55,7 @@ def generate_launch_description() -> LaunchDescription: namespace = LaunchConfiguration('namespace') map_yaml_file = LaunchConfiguration('map') keepout_mask_yaml_file = LaunchConfiguration('keepout_mask') + speed_mask_yaml_file = LaunchConfiguration('speed_mask') graph_filepath = LaunchConfiguration('graph') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') @@ -62,6 +63,7 @@ def generate_launch_description() -> LaunchDescription: use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') use_keepout_zones = LaunchConfiguration('use_keepout_zones') + use_speed_zones = LaunchConfiguration('use_speed_zones') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -104,6 +106,12 @@ def generate_launch_description() -> LaunchDescription: description='Full path to keepout mask file to load', ) + declare_speed_mask_yaml_cmd = DeclareLaunchArgument( + 'speed_mask', + default_value=os.path.join(bringup_dir, 'maps', f'{MAP_TYPE}_speed.yaml'), + description='Full path to speed mask file to load', + ) + declare_graph_file_cmd = DeclareLaunchArgument( 'graph', default_value=os.path.join(bringup_dir, 'graphs', 'turtlebot4_graph.geojson'), @@ -144,6 +152,11 @@ def generate_launch_description() -> LaunchDescription: description='Whether to enable keepout zones or not' ) + declare_use_speed_zones_cmd = DeclareLaunchArgument( + 'use_speed_zones', default_value='True', + description='Whether to enable speed zones or not' + ) + declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), @@ -216,6 +229,7 @@ def generate_launch_description() -> LaunchDescription: 'slam': slam, 'map': map_yaml_file, 'keepout_mask': keepout_mask_yaml_file, + 'speed_mask': speed_mask_yaml_file, 'graph': graph_filepath, 'use_sim_time': use_sim_time, 'params_file': params_file, @@ -223,6 +237,7 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_respawn': use_respawn, 'use_keepout_zones': use_keepout_zones, + 'use_speed_zones': use_speed_zones, }.items(), ) @@ -282,6 +297,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_keepout_mask_yaml_cmd) + ld.add_action(declare_speed_mask_yaml_cmd) ld.add_action(declare_graph_file_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) @@ -298,6 +314,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_use_keepout_zones_cmd) + ld.add_action(declare_use_speed_zones_cmd) ld.add_action(set_env_vars_resources) ld.add_action(world_sdf_xacro) diff --git a/nav2_bringup/maps/depot_speed.pgm b/nav2_bringup/maps/depot_speed.pgm new file mode 100644 index 00000000000..c2e449b34e5 Binary files /dev/null and b/nav2_bringup/maps/depot_speed.pgm differ diff --git a/nav2_bringup/maps/depot_speed.yaml b/nav2_bringup/maps/depot_speed.yaml new file mode 100644 index 00000000000..314ede87c3f --- /dev/null +++ b/nav2_bringup/maps/depot_speed.yaml @@ -0,0 +1,7 @@ +image: depot_speed.pgm +mode: scale +resolution: 0.05 +origin: [0.0, 0.0, 0] +negate: 0 +occupied_thresh: 1.0 +free_thresh: 0.0 diff --git a/nav2_bringup/maps/warehouse_speed.pgm b/nav2_bringup/maps/warehouse_speed.pgm new file mode 100644 index 00000000000..1bb6828ff34 Binary files /dev/null and b/nav2_bringup/maps/warehouse_speed.pgm differ diff --git a/nav2_bringup/maps/warehouse_speed.yaml b/nav2_bringup/maps/warehouse_speed.yaml new file mode 100644 index 00000000000..0372ed53e1c --- /dev/null +++ b/nav2_bringup/maps/warehouse_speed.yaml @@ -0,0 +1,7 @@ +image: warehouse_speed.pgm +mode: scale +resolution: 0.03 +origin: [-15.1, -25, 0] +negate: 0 +occupied_thresh: 1.0 +free_thresh: 0.0 diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index da293f0997f..dc5ebc7bb19 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.3.1 + 1.4.0 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index bf9ee77364e..768baadedb5 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -95,6 +95,7 @@ controller_server: goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] use_realtime_priority: false + speed_limit_topic: "speed_limit" # Progress checker parameters progress_checker: @@ -227,8 +228,10 @@ local_costmap: filters: ["keepout_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" - enabled: True - filter_info_topic: "costmap_filter_info" + enabled: KEEPOUT_ZONE_ENABLED + filter_info_topic: "keepout_costmap_filter_info" + override_lethal_cost: True + lethal_override_cost: 200 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -276,11 +279,18 @@ global_costmap: resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - filters: ["keepout_filter"] + filters: ["keepout_filter", "speed_filter"] keepout_filter: plugin: "nav2_costmap_2d::KeepoutFilter" - enabled: True - filter_info_topic: "costmap_filter_info" + enabled: KEEPOUT_ZONE_ENABLED + filter_info_topic: "keepout_costmap_filter_info" + override_lethal_cost: True + lethal_override_cost: 200 + speed_filter: + plugin: "nav2_costmap_2d::SpeedFilter" + enabled: SPEED_ZONE_ENABLED + filter_info_topic: "speed_costmap_filter_info" + speed_limit_topic: "speed_limit" obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -319,19 +329,32 @@ map_server: # yaml_filename: "" service_introspection_mode: "disabled" -filter_mask_server: +keepout_filter_mask_server: ros__parameters: topic_name: "keepout_filter_mask" # yaml_filename: "" -costmap_filter_info_server: +keepout_costmap_filter_info_server: ros__parameters: type: 0 - filter_info_topic: "costmap_filter_info" + filter_info_topic: "keepout_costmap_filter_info" mask_topic: "keepout_filter_mask" base: 0.0 multiplier: 1.0 +speed_filter_mask_server: + ros__parameters: + topic_name: "speed_filter_mask" + # yaml_filename: "" + +speed_costmap_filter_info_server: + ros__parameters: + type: 1 + filter_info_topic: "speed_costmap_filter_info" + mask_topic: "speed_filter_mask" + base: 100.0 + multiplier: -1.0 + map_saver: ros__parameters: save_map_timeout: 5.0 @@ -426,6 +449,7 @@ route_server: # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson boundary_radius_to_achieve_node: 1.0 radius_to_achieve_node: 2.0 + smooth_corners: true operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"] ReroutingService: plugin: "nav2_route::ReroutingService" @@ -546,3 +570,9 @@ loopback_simulator: map_frame_id: "map" scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' update_duration: 0.02 + scan_range_min: 0.05 + scan_range_max: 30.0 + scan_angle_min: -3.1415 + scan_angle_max: 3.1415 + scan_angle_increment: 0.02617 + scan_use_inf: true diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index 71af24b27bb..eb8852ffff7 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -210,6 +210,29 @@ Visualization Manager: Value: map_updates Use Timestamp: false Value: true + - Alpha: 0.699999988079071 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: speed_filter_mask + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: speed_filter_mask_updates + Use Timestamp: false + Value: true - Alpha: 1 Class: nav2_rviz_plugins/ParticleCloud Color: 0; 180; 0 diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index fdc5a87bbed..d7a8e903bc3 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.3.1 + 1.4.0 Nav2 BT Navigator Server Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 22871152b78..bb166d9bbf4 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.3.1 + 1.4.0 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 472a7d255e3..420d0268bbf 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -548,13 +548,17 @@ bool CollisionMonitor::processStopSlowdownLimit( const double linear_vel = std::hypot(velocity.x, velocity.y); // absolute Velocity safe_vel; double ratio = 1.0; + + // Calculate the most restrictive ratio to preserve curvature if (linear_vel != 0.0) { - ratio = std::clamp(polygon->getLinearLimit() / linear_vel, 0.0, 1.0); + ratio = std::min(ratio, polygon->getLinearLimit() / linear_vel); + } + if (velocity.tw != 0.0) { + ratio = std::min(ratio, polygon->getAngularLimit() / std::abs(velocity.tw)); } - safe_vel.x = velocity.x * ratio; - safe_vel.y = velocity.y * ratio; - safe_vel.tw = std::clamp( - velocity.tw, -polygon->getAngularLimit(), polygon->getAngularLimit()); + ratio = std::clamp(ratio, 0.0, 1.0); + // Apply the same ratio to all components to preserve curvature + safe_vel = velocity * ratio; // Check that currently calculated velocity is safer than // chosen for previous shapes one if (safe_vel < robot_action.req_vel) { diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 1e087070191..aeda547fd46 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -578,7 +578,9 @@ Polygon::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); - + if(param_name.find(polygon_name_ + ".") != 0) { + continue; + } if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { if (param_name == polygon_name_ + "." + "enabled") { enabled_ = parameter.as_bool(); diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index e6e10b08639..50dc6e9ef96 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -121,7 +121,9 @@ Source::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); - + if(param_name.find(source_name_ + ".") != 0) { + continue; + } if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { if (param_name == source_name_ + "." + "enabled") { enabled_ = parameter.as_bool(); diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 0de57bd895e..2fda8571995 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -821,10 +821,12 @@ TEST_F(Tester, testProcessStopSlowdownLimit) publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2); - const double ratio = LINEAR_LIMIT / speed; + const double linear_ratio = LINEAR_LIMIT / speed; + const double angular_ratio = ANGULAR_LIMIT / 0.1; + const double ratio = std::min(linear_ratio, angular_ratio); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON); ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON); - ASSERT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1 * ratio, EPSILON); ASSERT_TRUE(waitActionState(500ms)); ASSERT_EQ(action_state_->action_type, LIMIT); ASSERT_EQ(action_state_->polygon_name, "Limit"); @@ -906,10 +908,12 @@ TEST_F(Tester, testPolygonSource) publishCmdVel(0.5, 0.2, 0.1); EXPECT_TRUE(waitCmdVel(500ms)); const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2); - const double ratio = LINEAR_LIMIT / speed; + const double linear_ratio = LINEAR_LIMIT / speed; + const double angular_ratio = ANGULAR_LIMIT / 0.1; + const double ratio = std::min(linear_ratio, angular_ratio); EXPECT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON); EXPECT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON); - EXPECT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.1 * ratio, EPSILON); EXPECT_TRUE(waitActionState(500ms)); EXPECT_EQ(action_state_->action_type, LIMIT); EXPECT_EQ(action_state_->polygon_name, "Limit"); diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt index 139835a896b..1f79afd2c96 100644 --- a/nav2_common/CMakeLists.txt +++ b/nav2_common/CMakeLists.txt @@ -15,3 +15,16 @@ install( DIRECTORY cmake DESTINATION share/${PROJECT_NAME} ) + +install( + DIRECTORY test + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_test REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(test_rewritten_yaml test/test_rewritten_yaml.py + ) +endif() diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 49ec39b5377..e475dc410fc 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -48,6 +48,7 @@ def __init__( param_rewrites: dict[str, launch.SomeSubstitutionsType], root_key: Optional[launch.SomeSubstitutionsType] = None, key_rewrites: Optional[dict[str, launch.SomeSubstitutionsType]] = None, + value_rewrites: Optional[dict[str, launch.SomeSubstitutionsType]] = None, convert_types: bool = False, ) -> None: super().__init__() @@ -58,6 +59,7 @@ def __init__( :param: param_rewrites mappings to replace :param: root_key if provided, the contents are placed under this key :param: key_rewrites keys of mappings to replace + :param: value_rewrites values to replace :param: convert_types whether to attempt converting the string to a number or boolean """ @@ -68,6 +70,7 @@ def __init__( normalize_to_list_of_substitutions(source_file) self.__param_rewrites = {} self.__key_rewrites = {} + self.__value_rewrites = {} self.__convert_types = convert_types self.__root_key = None for key in param_rewrites: @@ -79,6 +82,11 @@ def __init__( self.__key_rewrites[key] = normalize_to_list_of_substitutions( key_rewrites[key] ) + if value_rewrites is not None: + for value in value_rewrites: + self.__value_rewrites[value] = normalize_to_list_of_substitutions( + value_rewrites[value] + ) if root_key is not None: self.__root_key = normalize_to_list_of_substitutions(root_key) @@ -94,11 +102,15 @@ def describe(self) -> str: def perform(self, context: launch.LaunchContext) -> str: yaml_filename = launch.utilities.perform_substitutions(context, self.name) rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) - param_rewrites, keys_rewrites = self.resolve_rewrites(context) - data = yaml.safe_load(open(yaml_filename)) + param_rewrites, keys_rewrites, value_rewrites = self.resolve_rewrites(context) + + with open(yaml_filename, 'r') as yaml_file: + data = yaml.safe_load(yaml_file) + self.substitute_params(data, param_rewrites) self.add_params(data, param_rewrites) self.substitute_keys(data, keys_rewrites) + self.substitute_values(data, value_rewrites) if self.__root_key is not None: root_key = launch.utilities.perform_substitutions(context, self.__root_key) if root_key: @@ -108,7 +120,7 @@ def perform(self, context: launch.LaunchContext) -> str: return rewritten_yaml.name def resolve_rewrites(self, context: launch.LaunchContext) -> \ - tuple[dict[str, str], dict[str, str]]: + tuple[dict[str, str], dict[str, str], dict[str, str]]: resolved_params = {} for key in self.__param_rewrites: resolved_params[key] = launch.utilities.perform_substitutions( @@ -119,7 +131,12 @@ def resolve_rewrites(self, context: launch.LaunchContext) -> \ resolved_keys[key] = launch.utilities.perform_substitutions( context, self.__key_rewrites[key] ) - return resolved_params, resolved_keys + resolved_values = {} + for value in self.__value_rewrites: + resolved_values[value] = launch.utilities.perform_substitutions( + context, self.__value_rewrites[value] + ) + return resolved_params, resolved_keys, resolved_values def substitute_params(self, yaml: dict[str, YamlValue], param_rewrites: dict[str, str]) -> None: @@ -149,6 +166,24 @@ def add_params(self, yaml: dict[str, YamlValue], if 'ros__parameters' in yaml_keys: yaml = self.updateYamlPathVals(yaml, yaml_keys, new_val) + def substitute_values( + self, yaml: dict[str, YamlValue], + value_rewrites: dict[str, str]) -> None: + + def process_value(value: YamlValue) -> YamlValue: + if isinstance(value, dict): + for k, v in list(value.items()): + value[k] = process_value(v) + return value + elif isinstance(value, list): + return [process_value(v) for v in value] + elif str(value) in value_rewrites: + return self.convert(value_rewrites[str(value)]) + return value + + for key in list(yaml.keys()): + yaml[key] = process_value(yaml[key]) + def updateYamlPathVals( self, yaml: dict[str, YamlValue], yaml_key_list: list[str], rewrite_val: YamlValue) -> dict[str, YamlValue]: diff --git a/nav2_common/package.xml b/nav2_common/package.xml index f2cebe1c8f7..7aa6a3925eb 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.3.1 + 1.4.0 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 @@ -20,6 +20,10 @@ ament_cmake_core + ament_cmake_test + ament_cmake_pytest + python3-pytest + ament_cmake diff --git a/nav2_common/test/test_rewritten_yaml.py b/nav2_common/test/test_rewritten_yaml.py new file mode 100644 index 00000000000..2bf69b95f35 --- /dev/null +++ b/nav2_common/test/test_rewritten_yaml.py @@ -0,0 +1,133 @@ +# Copyright (c) 2025 Leander Stephen Desouza +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import tempfile +from typing import Generator + +import launch +from launch.substitutions import LaunchConfiguration +from nav2_common.launch import RewrittenYaml +import pytest +import yaml + + +class TestRewrittenYamlValueRewrites: + """Test that value rewrites work correctly in RewrittenYaml.""" + + @pytest.fixture(autouse=True) + def setup_teardown(self) -> Generator[None, None, None]: + # Create a temporary YAML file for testing + self.test_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) + self.test_yaml.write("""\ + param0: placeholder_bool + param1: placeholder_string + param2: placeholder_float + param3: placeholder_int + param4: placeholder_list + param5: placeholder_dict + nested: + param6: placeholder_bool + param7: placeholder_string + param8: placeholder_float + param9: placeholder_int + param10: placeholder_list + param11: placeholder_dict + other_value: 42 + list_values: + - placeholder_bool + - placeholder_string + - placeholder_float + - placeholder_int + - placeholder_list + - placeholder_dict + - normal_value + """) + self.test_yaml.close() + yield + os.unlink(self.test_yaml.name) + + def test_value_rewrites(self) -> None: + """Test that value rewrites work for various types.""" + # Set up launch configurations for our test values + launch_configurations = { + 'test_bool': 'true', + 'test_string': 'replaced_string', + 'test_float': '3.14', + 'test_int': '100', + 'test_list': '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]', + 'test_dict': ('{"key1": "value1", "key2": 2, "key3": 3.14, ' + '"key4": ["list_item1", "list_item2"]}') + } + context = launch.LaunchContext() + for key, value in launch_configurations.items(): + context.launch_configurations[key] = value + + value_rewrites = { + 'placeholder_bool': LaunchConfiguration('test_bool'), + 'placeholder_string': LaunchConfiguration('test_string'), + 'placeholder_float': LaunchConfiguration('test_float'), + 'placeholder_int': LaunchConfiguration('test_int'), + 'placeholder_list': LaunchConfiguration('test_list'), + 'placeholder_dict': LaunchConfiguration('test_dict'), + } + + rewriter = RewrittenYaml( + source_file=self.test_yaml.name, + param_rewrites={}, + value_rewrites=value_rewrites, + convert_types=True, + ) + output_file = rewriter.perform(context) + + try: + with open(output_file) as f: + result = yaml.safe_load(f) + + assert result['param0'] is True + assert result['param1'] == 'replaced_string' + assert result['param2'] == 3.14 + assert result['param3'] == 100 + assert result['param4'] == \ + '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]' + assert result['param5'] == \ + '{"key1": "value1", "key2": 2, "key3": 3.14, "key4": ["list_item1", "list_item2"]}' + + # Check nested values + assert result['nested']['param6'] is True + assert result['nested']['param7'] == 'replaced_string' + assert result['nested']['param8'] == 3.14 + assert result['nested']['param9'] == 100 + assert result['nested']['param10'] == \ + '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]' + assert result['nested']['param11'] == \ + '{"key1": "value1", "key2": 2, "key3": 3.14, "key4": ["list_item1", "list_item2"]}' + + # Check list values + assert result['list_values'][0] is True + assert result['list_values'][1] == 'replaced_string' + assert result['list_values'][2] == 3.14 + assert result['list_values'][3] == 100 + assert result['list_values'][4] == \ + '["string", 42, 2.718, ["sublist_item1", "sublist_item2"]]' + assert result['list_values'][5] == \ + '{"key1": "value1", "key2": 2, "key3": 3.14, "key4": ["list_item1", "list_item2"]}' + + # Check other values remain unchanged + assert result['nested']['other_value'] == 42 + assert result['list_values'][6] == 'normal_value' + + finally: + if os.path.exists(output_file): + os.unlink(output_file) diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index 0628a745261..09f4d27015c 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.3.1 + 1.4.0 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 7eb51ca459c..1532aecbf07 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.3.1 + 1.4.0 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp index 271e5635c07..1ac4d20e026 100644 --- a/nav2_controller/plugins/pose_progress_checker.cpp +++ b/nav2_controller/plugins/pose_progress_checker.cpp @@ -79,11 +79,14 @@ PoseProgressChecker::dynamicParametersCallback(std::vector pa { rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".required_movement_angle") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".required_movement_angle") { required_movement_angle_ = parameter.as_double(); } } diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index d49b94c2873..f4bd8173ac2 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -145,18 +145,20 @@ SimpleGoalChecker::dynamicParametersCallback(std::vector para { rcl_interfaces::msg::SetParametersResult result; for (auto & parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".xy_goal_tolerance") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".xy_goal_tolerance") { xy_goal_tolerance_ = parameter.as_double(); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; - } else if (name == plugin_name_ + ".yaw_goal_tolerance") { + } else if (param_name == plugin_name_ + ".yaw_goal_tolerance") { yaw_goal_tolerance_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".stateful") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".stateful") { stateful_ = parameter.as_bool(); } } diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 120b4320da1..001edbf5d62 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -98,13 +98,16 @@ SimpleProgressChecker::dynamicParametersCallback(std::vector { rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".required_movement_radius") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".required_movement_radius") { radius_ = parameter.as_double(); - } else if (name == plugin_name_ + ".movement_time_allowance") { + } else if (param_name == plugin_name_ + ".movement_time_allowance") { time_allowance_ = rclcpp::Duration::from_seconds(parameter.as_double()); } } diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index 179c698bd46..7408d4db78a 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -119,13 +119,16 @@ StoppedGoalChecker::dynamicParametersCallback(std::vector par { rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".rot_stopped_velocity") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".rot_stopped_velocity") { rot_stopped_velocity_ = parameter.as_double(); - } else if (name == plugin_name_ + ".trans_stopped_velocity") { + } else if (param_name == plugin_name_ + ".trans_stopped_velocity") { trans_stopped_velocity_ = parameter.as_double(); } } diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 06fd66ff364..9df9bc41c22 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -842,12 +842,12 @@ ControllerServer::dynamicParametersCallback(std::vector param rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); // If we are trying to change the parameter of a plugin we can just skip it at this point // as they handle parameter changes themselves and don't need to lock the mutex - if (name.find('.') != std::string::npos) { + if (param_name.find('.') != std::string::npos) { continue; } @@ -861,16 +861,14 @@ ControllerServer::dynamicParametersCallback(std::vector param return result; } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == "controller_frequency") { - controller_frequency_ = parameter.as_double(); - } else if (name == "min_x_velocity_threshold") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "min_x_velocity_threshold") { min_x_velocity_threshold_ = parameter.as_double(); - } else if (name == "min_y_velocity_threshold") { + } else if (param_name == "min_y_velocity_threshold") { min_y_velocity_threshold_ = parameter.as_double(); - } else if (name == "min_theta_velocity_threshold") { + } else if (param_name == "min_theta_velocity_threshold") { min_theta_velocity_threshold_ = parameter.as_double(); - } else if (name == "failure_tolerance") { + } else if (param_name == "failure_tolerance") { failure_tolerance_ = parameter.as_double(); } } diff --git a/nav2_controller/test/test_dynamic_parameters.cpp b/nav2_controller/test/test_dynamic_parameters.cpp index b8f03e19991..523991a8e12 100644 --- a/nav2_controller/test/test_dynamic_parameters.cpp +++ b/nav2_controller/test/test_dynamic_parameters.cpp @@ -62,8 +62,7 @@ TEST(WPTest, test_dynamic_parameters) controller->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("controller_frequency", 100.0), - rclcpp::Parameter("min_x_velocity_threshold", 100.0), + {rclcpp::Parameter("min_x_velocity_threshold", 100.0), rclcpp::Parameter("min_y_velocity_threshold", 100.0), rclcpp::Parameter("min_theta_velocity_threshold", 100.0), rclcpp::Parameter("failure_tolerance", 5.0)}); @@ -72,7 +71,6 @@ TEST(WPTest, test_dynamic_parameters) controller->get_node_base_interface(), results); - EXPECT_EQ(controller->get_parameter("controller_frequency").as_double(), 100.0); EXPECT_EQ(controller->get_parameter("min_x_velocity_threshold").as_double(), 100.0); EXPECT_EQ(controller->get_parameter("min_y_velocity_threshold").as_double(), 100.0); EXPECT_EQ(controller->get_parameter("min_theta_velocity_threshold").as_double(), 100.0); diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 55e94f1b9fc..044eb215676 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.3.1 + 1.4.0 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index e2225196d72..a705c74f99a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -102,6 +102,12 @@ class KeepoutFilter : public CostmapFilter nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; std::string global_frame_; // Frame of current layer (master_grid) + + bool override_lethal_cost_{false}; // If true, lethal cost will be overridden + unsigned char lethal_override_cost_{252}; // Value to override lethal cost with + bool last_pose_lethal_{false}; // If true, last pose was lethal + unsigned int lethal_state_update_min_x_{999999u}, lethal_state_update_min_y_{999999u}; + unsigned int lethal_state_update_max_x_{0u}, lethal_state_update_max_y_{0u}; }; } // namespace nav2_costmap_2d 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 1cdec808da7..16113d61b16 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -43,6 +43,7 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rclcpp/version.h" #include "laser_geometry/laser_geometry.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wreorder" @@ -234,8 +235,13 @@ class ObstacleLayer : public CostmapLayer /// @brief Used to project laser scans into point clouds laser_geometry::LaserProjection projector_; /// @brief Used for the observation message filters + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::vector> + observation_subscribers_; + #else std::vector>> observation_subscribers_; + #endif /// @brief Used to make sure that transforms are available for each sensor std::vector> observation_notifiers_; /// @brief Used to store observations from various sensors diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 9340bd404a8..9a7e1146cb8 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.3.1 + 1.4.0 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index ac01861ef67..0f24178e36d 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -74,6 +74,15 @@ void KeepoutFilter::initializeFilter( std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1)); global_frame_ = layered_costmap_->getGlobalFrameID(); + + declareParameter("override_lethal_cost", rclcpp::ParameterValue(false)); + node->get_parameter(name_ + "." + "override_lethal_cost", override_lethal_cost_); + declareParameter("lethal_override_cost", rclcpp::ParameterValue(MAX_NON_OBSTACLE)); + node->get_parameter(name_ + "." + "lethal_override_cost", lethal_override_cost_); + + // clamp lethal_override_cost_ in case if higher than MAX_NON_OBSTACLE is given + lethal_override_cost_ = \ + std::clamp(lethal_override_cost_, FREE_SPACE, MAX_NON_OBSTACLE); } void KeepoutFilter::filterInfoCallback( @@ -149,7 +158,7 @@ void KeepoutFilter::maskCallback( void KeepoutFilter::process( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j, - const geometry_msgs::msg::Pose2D & /*pose*/) + const geometry_msgs::msg::Pose2D & pose) { std::lock_guard guard(*getMutex()); @@ -244,10 +253,47 @@ void KeepoutFilter::process( } // unsigned<-signed conversions. - unsigned const int mg_min_x_u = static_cast(mg_min_x); - unsigned const int mg_min_y_u = static_cast(mg_min_y); - unsigned const int mg_max_x_u = static_cast(mg_max_x); - unsigned const int mg_max_y_u = static_cast(mg_max_y); + unsigned int mg_min_x_u = static_cast(mg_min_x); + unsigned int mg_min_y_u = static_cast(mg_min_y); + unsigned int mg_max_x_u = static_cast(mg_max_x); + unsigned int mg_max_y_u = static_cast(mg_max_y); + + // Let's find the pose's cost if we are allowed to override the lethal cost + bool is_pose_lethal = false; + if (override_lethal_cost_) { + geometry_msgs::msg::Pose2D mask_pose; + if (transformPose(global_frame_, pose, filter_mask_->header.frame_id, mask_pose)) { + unsigned int mask_robot_i, mask_robot_j; + if (worldToMask(filter_mask_, mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j)) { + auto data = getMaskCost(filter_mask_, mask_robot_i, mask_robot_j); + is_pose_lethal = (data == INSCRIBED_INFLATED_OBSTACLE || data == LETHAL_OBSTACLE); + if (is_pose_lethal) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 2000, + "KeepoutFilter: Pose is in keepout zone, reducing cost override to navigate out."); + } + } + } + + // If in lethal space or just exited lethal space, + // we need to update all possible spaces touched during this state + if (is_pose_lethal || (last_pose_lethal_ && !is_pose_lethal)) { + lethal_state_update_min_x_ = std::min(mg_min_x_u, lethal_state_update_min_x_); + mg_min_x_u = lethal_state_update_min_x_; + lethal_state_update_min_y_ = std::min(mg_min_y_u, lethal_state_update_min_y_); + mg_min_y_u = lethal_state_update_min_y_; + lethal_state_update_max_x_ = std::max(mg_max_x_u, lethal_state_update_max_x_); + mg_max_x_u = lethal_state_update_max_x_; + lethal_state_update_max_y_ = std::max(mg_max_y_u, lethal_state_update_max_y_); + mg_max_y_u = lethal_state_update_max_y_; + } else { + // If out of lethal space, reset managed lethal state sizes + lethal_state_update_min_x_ = master_grid.getSizeInCellsX(); + lethal_state_update_min_y_ = master_grid.getSizeInCellsY(); + lethal_state_update_max_x_ = 0u; + lethal_state_update_max_y_ = 0u; + } + } unsigned int i, j; // master_grid iterators unsigned int index; // corresponding index of master_grid @@ -284,12 +330,19 @@ void KeepoutFilter::process( if (data == NO_INFORMATION) { continue; } + if (data > old_data || old_data == NO_INFORMATION) { - master_array[index] = data; + if (override_lethal_cost_ && is_pose_lethal) { + master_array[index] = lethal_override_cost_; + } else { + master_array[index] = data; + } } } } } + + last_pose_lethal_ = is_pose_lethal; } void KeepoutFilter::resetFilter() diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index ac30accf244..225cd4062d0 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -68,8 +68,9 @@ void SpeedFilter::initializeFilter( std::string speed_limit_topic; declareParameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit")); node->get_parameter(name_ + "." + "speed_limit_topic", speed_limit_topic); + speed_limit_topic = joinWithParentNamespace(speed_limit_topic); - filter_info_topic_ = filter_info_topic; + filter_info_topic_ = joinWithParentNamespace(filter_info_topic); // Setting new costmap filter info subscriber RCLCPP_INFO( logger_, @@ -139,7 +140,7 @@ void SpeedFilter::filterInfoCallback( return; } - mask_topic_ = msg->filter_mask_topic; + mask_topic_ = joinWithParentNamespace(msg->filter_mask_topic); // Setting new filter mask subscriber RCLCPP_INFO( diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 6ce187a0b23..90dfbab319e 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -444,6 +444,9 @@ InflationLayer::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == name_ + "." + "inflation_radius" && diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 75dc2bcf217..e3181acdf78 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -46,6 +46,7 @@ #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" #include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_util/node_utils.hpp" #include "rclcpp/version.h" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer) @@ -100,6 +101,8 @@ void ObstacleLayer::onInitialize() node->get_parameter("track_unknown_space", track_unknown_space); node->get_parameter("transform_tolerance", transform_tolerance); node->get_parameter(name_ + "." + "observation_sources", topics_string); + double tf_filter_tolerance = nav2_util::declare_or_get_parameter(node, name_ + "." + + "tf_filter_tolerance", 0.05); int combination_method_param{}; node->get_parameter(name_ + "." + "combination_method", combination_method_param); @@ -231,13 +234,23 @@ void ObstacleLayer::onInitialize() // create a callback for the topic if (data_type == "LaserScan") { + // For Kilted and Older Support from Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::shared_ptr> sub; + #else std::shared_ptr> sub; + #endif - // For Jazzy compatibility - #if RCLCPP_VERSION_GTE(29, 0, 0) + // For Kilted compatibility in Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + sub = std::make_shared>( + node, topic, custom_qos_profile, sub_opt); + // For Jazzy compatibility in Message Filters API change + #elif RCLCPP_VERSION_GTE(29, 0, 0) sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); + // For Humble and Older compatibility in Message Filters API change #else sub = std::make_shared>( @@ -268,16 +281,27 @@ void ObstacleLayer::onInitialize() observation_subscribers_.push_back(sub); observation_notifiers_.push_back(filter); - observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds(0.05)); + observation_notifiers_.back()->setTolerance(rclcpp::Duration::from_seconds( + tf_filter_tolerance)); } else { + // For Kilted and Older Support from Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + std::shared_ptr> sub; + #else std::shared_ptr> sub; + #endif - // For Jazzy compatibility - #if RCLCPP_VERSION_GTE(29, 0, 0) + // For Kilted compatibility in Message Filters API change + #if RCLCPP_VERSION_GTE(29, 6, 0) + sub = std::make_shared>( + node, topic, custom_qos_profile, sub_opt); + // For Jazzy compatibility in Message Filters API change + #elif RCLCPP_VERSION_GTE(29, 0, 0) sub = std::make_shared>(node, topic, custom_qos_profile, sub_opt); + // For Humble and Older compatibility in Message Filters API change #else sub = std::make_shared>( @@ -326,6 +350,9 @@ ObstacleLayer::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == name_ + "." + "min_obstacle_height") { diff --git a/nav2_costmap_2d/plugins/plugin_container_layer.cpp b/nav2_costmap_2d/plugins/plugin_container_layer.cpp index 743a4751e1a..b8f50d01dee 100644 --- a/nav2_costmap_2d/plugins/plugin_container_layer.cpp +++ b/nav2_costmap_2d/plugins/plugin_container_layer.cpp @@ -217,6 +217,9 @@ rcl_interfaces::msg::SetParametersResult PluginContainerLayer::dynamicParameters for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } if (param_type == ParameterType::PARAMETER_INTEGER) { if (param_name == name_ + "." + "combination_method") { diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 4ef50c1f0d4..4d58dc2914f 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -485,6 +485,9 @@ StaticLayer::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } if (param_name == name_ + "." + "map_subscribe_transient_local" || param_name == name_ + "." + "map_topic" || diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index afd1ad06459..af1ac0a016f 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -496,6 +496,9 @@ VoxelLayer::dynamicParametersCallback( for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); + if (param_name.find(name_ + ".") != 0) { + continue; + } if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == name_ + "." + "max_obstacle_height") { diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index ae4de8f0c34..87a67a0758f 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -725,42 +725,46 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter std::lock_guard lock_reinit(_dynamic_parameter_mutex); for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } + - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == "robot_radius") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "robot_radius") { robot_radius_ = parameter.as_double(); // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); } - } else if (name == "footprint_padding") { + } else if (param_name == "footprint_padding") { footprint_padding_ = parameter.as_double(); padded_footprint_ = unpadded_footprint_; padFootprint(padded_footprint_, footprint_padding_); layered_costmap_->setFootprint(padded_footprint_); - } else if (name == "transform_tolerance") { + } else if (param_name == "transform_tolerance") { transform_tolerance_ = parameter.as_double(); - } else if (name == "publish_frequency") { + } else if (param_name == "publish_frequency") { map_publish_frequency_ = parameter.as_double(); if (map_publish_frequency_ > 0) { publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_); } else { publish_cycle_ = rclcpp::Duration(-1s); } - } else if (name == "resolution") { + } else if (param_name == "resolution") { resize_map = true; resolution_ = parameter.as_double(); - } else if (name == "origin_x") { + } else if (param_name == "origin_x") { resize_map = true; origin_x_ = parameter.as_double(); - } else if (name == "origin_y") { + } else if (param_name == "origin_y") { resize_map = true; origin_y_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_INTEGER) { - if (name == "width") { + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == "width") { if (parameter.as_int() > 0) { resize_map = true; map_width_meters_ = parameter.as_int(); @@ -771,7 +775,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter result.successful = false; return result; } - } else if (name == "height") { + } else if (param_name == "height") { if (parameter.as_int() > 0) { resize_map = true; map_height_meters_ = parameter.as_int(); @@ -783,14 +787,14 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter return result; } } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == "footprint") { + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == "footprint") { footprint_ = parameter.as_string(); std::vector new_footprint; if (makeFootprintFromString(footprint_, new_footprint)) { setRobotFootprint(new_footprint); } - } else if (name == "robot_base_frame") { + } else if (param_name == "robot_base_frame") { // First, make sure that the transform between the robot base frame // and the global frame is available std::string tf_error; diff --git a/nav2_docking/README.md b/nav2_docking/README.md index b063e72dd75..f246c0c2493 100644 --- a/nav2_docking/README.md +++ b/nav2_docking/README.md @@ -201,12 +201,15 @@ For debugging purposes, there are several publishers which can be used with RVIZ | controller_frequency | Control frequency (Hz) for vision-control loop | double | 50.0 | | initial_perception_timeout | Timeout (s) to wait to obtain initial perception of the dock | double | 5.0 | | wait_charge_timeout | Timeout (s) to wait to see if charging starts after docking | double | 5.0 | -| dock_approach_timeout | timeout (s) to attempt vision-control approach loop | double | 30.0 | +| dock_approach_timeout | Timeout (s) to attempt vision-control approach loop | double | 30.0 | +| rotate_to_dock_timeout | Timeout (s) to attempt rotate-to-dock loop | double | 10.0 | | undock_linear_tolerance | Tolerance (m) to exit the undocking control loop at staging pose | double | 0.05 | -| undock_angular_tolerance | Angular Tolerance (rad) to exist undocking loop at staging pose | double | 0.05 | +| undock_angular_tolerance | Angular tolerance (rad) to exit undocking loop at staging pose | double | 0.05 | | max_retries | Maximum number of retries to attempt | int | 3 | | base_frame | Robot's base frame for control law | string | "base_link" | | fixed_frame | Fixed frame to use, recommended to be a smooth odometry frame **not** map | string | "odom" | +| odom_topic | The topic to use for the odometry data | string | "odom" | +| rotation_angular_tolerance | Angular tolerance (rad) to exit the rotation loop when rotate_to_dock is enabled | double | 0.05 | | dock_prestaging_tolerance | L2 distance in X,Y,Theta from the staging pose to bypass navigation | double | 0.5 | | dock_plugins | A set of dock plugins to load | vector | N/A | | dock_database | The filepath to the dock database to use for this environment | string | N/A | @@ -220,6 +223,8 @@ For debugging purposes, there are several publishers which can be used with RVIZ | controller.v_linear_max | Maximum linear velocity (m/s) | double | 0.25 | | controller.v_angular_max | Maximum angular velocity (rad/s) produced by the control law | double | 0.75 | | controller.slowdown_radius | Radius (m) around the goal pose in which the robot will start to slow down | double | 0.25 | +| controller.rotate_to_heading_angular_vel | Angular velocity (rad/s) to rotate to the goal heading when rotate_to_dock is enabled | double | 1.0 | +| controller.rotate_to_heading_max_angular_accel | Maximum angular acceleration (rad/s^2) to rotate to the goal heading when rotate_to_dock is enabled | double | 3.2 | | controller.use_collision_detection | Whether to use collision detection to avoid obstacles | bool | true | | controller.costmap_topic | The topic to use for the costmap | string | "local_costmap/costmap_raw" | | controller.footprint_topic | The topic to use for the robot's footprint | string | "local_costmap/published_footprint" | @@ -251,6 +256,7 @@ Note: `dock_plugins` and either `docks` or `dock_database` are required. | staging_x_offset | Staging pose offset forward (negative) of dock pose (m) | double | -0.7 | | staging_yaw_offset | Staging pose angle relative to dock pose (rad) | double | 0.0 | | dock_direction | Whether the robot is docking with the dock forward or backward in motion | string | "forward" or "backward" | +| rotate_to_dock | Enables backward docking without requiring a sensor for detection during the final approach. When enabled, the robot approaches the staging pose facing forward with sensor coverage for dock detection; after detection, it rotates and backs into the dock using only the initially detected pose for dead reckoning. | bool | false | Note: The external detection rotation angles are setup to work out of the box with Apriltags detectors in `image_proc` and `isaac_ros`. diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp index 142b4beaf7d..90fb5cea38c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp @@ -67,6 +67,18 @@ class Controller const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool is_docking, bool backward = false); + /** + * @brief Perform a command for in-place rotation. + * @param angular_distance_to_heading Angular distance to goal. + * @param current_velocity Current angular velocity. + * @param dt Control loop duration [s]. + * @returns TwistStamped command for in-place rotation. + */ + geometry_msgs::msg::Twist computeRotateToHeadingCommand( + const double & angular_distance_to_heading, + const geometry_msgs::msg::Twist & current_velocity, + const double & dt); + protected: /** * @brief Check if a trajectory is collision free. @@ -110,6 +122,7 @@ class Controller std::unique_ptr control_law_; double k_phi_, k_delta_, beta_, lambda_; double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_; + double rotate_to_heading_angular_vel_, rotate_to_heading_max_angular_accel_; // The trajectory of the robot while dock / undock for visualization / debug purposes rclcpp::Publisher::SharedPtr trajectory_pub_; diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index 74d46f48f17..61edfc85888 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -27,6 +27,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_util/simple_action_server.hpp" #include "nav2_util/twist_publisher.hpp" +#include "nav_2d_utils/odom_subscriber.hpp" #include "opennav_docking/controller.hpp" #include "opennav_docking/utils.hpp" #include "opennav_docking/types.hpp" @@ -94,6 +95,12 @@ class DockingServer : public nav2_util::LifecycleNode */ bool approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward); + /** + * @brief Perform a pure rotation to dock orientation. + * @param dock_pose The target pose that will be used to rotate. + */ + void rotateToDock(const geometry_msgs::msg::PoseStamped & dock_pose); + /** * @brief Wait for charging to begin. * @param dock Dock instance, used to query isCharging(). @@ -236,6 +243,8 @@ class DockingServer : public nav2_util::LifecycleNode double wait_charge_timeout_; // Timeout to approach into the dock and reset its approach is retrying double dock_approach_timeout_; + // Timeout to rotate to the dock + double rotate_to_dock_timeout_; // When undocking, these are the tolerances for arriving at the staging pose double undock_linear_tolerance_, undock_angular_tolerance_; // Maximum number of times the robot will return to staging pose and retry docking @@ -248,11 +257,14 @@ class DockingServer : public nav2_util::LifecycleNode std::optional dock_backwards_; // The tolerance to the dock's staging pose not requiring navigation double dock_prestaging_tolerance_; + // Angular tolerance to exit the rotation loop when rotate_to_dock is enabled + double rotation_angular_tolerance_; // This is a class member so it can be accessed in publish feedback rclcpp::Time action_start_time_; std::unique_ptr vel_publisher_; + std::unique_ptr odom_sub_; std::unique_ptr docking_action_server_; std::unique_ptr undocking_action_server_; diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index bb802980419..5e8b0070525 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -2,7 +2,7 @@ opennav_docking - 1.3.1 + 1.4.0 A Task Server for robot charger docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp index 8128ba82253..62a87651bea 100644 --- a/nav2_docking/opennav_docking/src/controller.cpp +++ b/nav2_docking/opennav_docking/src/controller.cpp @@ -49,6 +49,10 @@ Controller::Controller( node, "controller.v_angular_max", rclcpp::ParameterValue(0.75)); nav2_util::declare_parameter_if_not_declared( node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.rotate_to_heading_max_angular_accel", rclcpp::ParameterValue(3.2)); nav2_util::declare_parameter_if_not_declared( node, "controller.use_collision_detection", rclcpp::ParameterValue(true)); nav2_util::declare_parameter_if_not_declared( @@ -95,6 +99,10 @@ Controller::Controller( configureCollisionChecker(node, costmap_topic, footprint_topic, transform_tolerance_); } + node->get_parameter("controller.rotate_to_heading_angular_vel", rotate_to_heading_angular_vel_); + node->get_parameter("controller.rotate_to_heading_max_angular_accel", + rotate_to_heading_max_angular_accel_); + trajectory_pub_ = node->create_publisher("docking_trajectory", 1); } @@ -117,6 +125,31 @@ bool Controller::computeVelocityCommand( return isTrajectoryCollisionFree(pose, is_docking, backward); } +geometry_msgs::msg::Twist Controller::computeRotateToHeadingCommand( + const double & angular_distance_to_heading, + const geometry_msgs::msg::Twist & current_velocity, + const double & dt) +{ + geometry_msgs::msg::Twist cmd_vel; + const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0; + const double angular_vel = sign * rotate_to_heading_angular_vel_; + const double min_feasible_angular_speed = + current_velocity.angular.z - rotate_to_heading_max_angular_accel_ * dt; + const double max_feasible_angular_speed = + current_velocity.angular.z + rotate_to_heading_max_angular_accel_ * dt; + cmd_vel.angular.z = + std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); + + // Check if we need to slow down to avoid overshooting + double max_vel_to_stop = + std::sqrt(2 * rotate_to_heading_max_angular_accel_ * fabs(angular_distance_to_heading)); + if (fabs(cmd_vel.angular.z) > max_vel_to_stop) { + cmd_vel.angular.z = sign * max_vel_to_stop; + } + + return cmd_vel; +} + bool Controller::isTrajectoryCollisionFree( const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward) { @@ -208,31 +241,37 @@ Controller::dynamicParametersCallback(std::vector parameters) rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { - if (name == "controller.k_phi") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find("controller.") != 0) { + continue; + } + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { + if (param_name == "controller.k_phi") { k_phi_ = parameter.as_double(); - } else if (name == "controller.k_delta") { + } else if (param_name == "controller.k_delta") { k_delta_ = parameter.as_double(); - } else if (name == "controller.beta") { + } else if (param_name == "controller.beta") { beta_ = parameter.as_double(); - } else if (name == "controller.lambda") { + } else if (param_name == "controller.lambda") { lambda_ = parameter.as_double(); - } else if (name == "controller.v_linear_min") { + } else if (param_name == "controller.v_linear_min") { v_linear_min_ = parameter.as_double(); - } else if (name == "controller.v_linear_max") { + } else if (param_name == "controller.v_linear_max") { v_linear_max_ = parameter.as_double(); - } else if (name == "controller.v_angular_max") { + } else if (param_name == "controller.v_angular_max") { v_angular_max_ = parameter.as_double(); - } else if (name == "controller.slowdown_radius") { + } else if (param_name == "controller.slowdown_radius") { slowdown_radius_ = parameter.as_double(); - } else if (name == "controller.projection_time") { + } else if (param_name == "controller.rotate_to_heading_angular_vel") { + rotate_to_heading_angular_vel_ = parameter.as_double(); + } else if (param_name == "controller.rotate_to_heading_max_angular_accel") { + rotate_to_heading_max_angular_accel_ = parameter.as_double(); + } else if (param_name == "controller.projection_time") { projection_time_ = parameter.as_double(); - } else if (name == "controller.simulation_time_step") { + } else if (param_name == "controller.simulation_time_step") { simulation_time_step_ = parameter.as_double(); - } else if (name == "controller.dock_collision_threshold") { + } else if (param_name == "controller.dock_collision_threshold") { dock_collision_threshold_ = parameter.as_double(); } diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index f0f0dc0f5f6..888b57d08b6 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -33,6 +33,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) declare_parameter("initial_perception_timeout", 5.0); declare_parameter("wait_charge_timeout", 5.0); declare_parameter("dock_approach_timeout", 30.0); + declare_parameter("rotate_to_dock_timeout", 10.0); declare_parameter("undock_linear_tolerance", 0.05); declare_parameter("undock_angular_tolerance", 0.05); declare_parameter("max_retries", 3); @@ -40,6 +41,8 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options) declare_parameter("fixed_frame", "odom"); declare_parameter("dock_backwards", rclcpp::PARAMETER_BOOL); declare_parameter("dock_prestaging_tolerance", 0.5); + declare_parameter("odom_topic", "odom"); + declare_parameter("rotation_angular_tolerance", 0.05); } nav2_util::CallbackReturn @@ -52,14 +55,18 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("initial_perception_timeout", initial_perception_timeout_); get_parameter("wait_charge_timeout", wait_charge_timeout_); get_parameter("dock_approach_timeout", dock_approach_timeout_); + get_parameter("rotate_to_dock_timeout", rotate_to_dock_timeout_); get_parameter("undock_linear_tolerance", undock_linear_tolerance_); get_parameter("undock_angular_tolerance", undock_angular_tolerance_); get_parameter("max_retries", max_retries_); get_parameter("base_frame", base_frame_); get_parameter("fixed_frame", fixed_frame_); get_parameter("dock_prestaging_tolerance", dock_prestaging_tolerance_); + get_parameter("rotation_angular_tolerance", rotation_angular_tolerance_); + RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_); + // Check the dock_backwards deprecated parameter bool dock_backwards = false; try { if (get_parameter("dock_backwards", dock_backwards)) { @@ -73,6 +80,11 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & state) vel_publisher_ = std::make_unique(node, "cmd_vel", 1); tf2_buffer_ = std::make_shared(node->get_clock()); + // Create odom subscriber for backward blind docking + std::string odom_topic; + get_parameter("odom_topic", odom_topic); + odom_sub_ = std::make_unique(node, odom_topic); + double action_server_result_timeout; nav2_util::declare_parameter_if_not_declared( node, "action_server_result_timeout", rclcpp::ParameterValue(10.0)); @@ -165,6 +177,7 @@ DockingServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) controller_.reset(); vel_publisher_.reset(); dock_backwards_.reset(); + odom_sub_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -252,6 +265,8 @@ void DockingServer::dockRobot() if (dock->plugin->isCharger() && (dock->plugin->isDocked() || dock->plugin->isCharging())) { RCLCPP_INFO( get_logger(), "Robot is already docked and/or charging (if applicable), no need to dock"); + result->success = true; + docking_action_server_->succeeded_current(result); return; } @@ -289,10 +304,22 @@ void DockingServer::dockRobot() dock_backwards_.value() : (dock->plugin->getDockDirection() == opennav_docking_core::DockDirection::BACKWARD); + // If we performed a rotation before docking backward, we must rotate the staging pose + // to match the robot orientation + auto staging_pose = dock->getStagingPose(); + if (dock->plugin->shouldRotateToDock()) { + staging_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(staging_pose.pose.orientation) + M_PI); + } + // Docking control loop: while not docked, run controller rclcpp::Time dock_contact_time; while (rclcpp::ok()) { try { + // Perform a 180º to face away from the dock if needed + if (dock->plugin->shouldRotateToDock()) { + rotateToDock(dock_pose); + } // Approach the dock using control law if (approachDock(dock, dock_pose, dock_backward)) { // We are docked, wait for charging to begin @@ -327,7 +354,7 @@ void DockingServer::dockRobot() } // Reset to staging pose to try again - if (!resetApproach(dock->getStagingPose(), dock_backward)) { + if (!resetApproach(staging_pose, dock_backward)) { // Cancelled, preempted, or shutting down stashDockData(goal->use_dock_id, dock, false); publishZeroVelocity(); @@ -424,6 +451,43 @@ void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseSta } } +void DockingServer::rotateToDock(const geometry_msgs::msg::PoseStamped & dock_pose) +{ + const double dt = 1.0 / controller_frequency_; + auto target_pose = dock_pose; + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(target_pose.pose.orientation) + M_PI); + + rclcpp::Rate loop_rate(controller_frequency_); + auto start = this->now(); + auto timeout = rclcpp::Duration::from_seconds(rotate_to_dock_timeout_); + + while (rclcpp::ok()) { + auto robot_pose = getRobotPoseInFrame(dock_pose.header.frame_id); + auto angular_distance_to_heading = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), tf2::getYaw(target_pose.pose.orientation)); + if (fabs(angular_distance_to_heading) < rotation_angular_tolerance_) { + break; + } + + auto current_vel = std::make_unique(); + current_vel->twist.angular.z = odom_sub_->getTwist().theta; + + auto command = std::make_unique(); + command->header = robot_pose.header; + command->twist = controller_->computeRotateToHeadingCommand( + angular_distance_to_heading, current_vel->twist, dt); + + vel_publisher_->publish(std::move(command)); + + if (this->now() - start > timeout) { + throw opennav_docking_core::FailedToControl("Timed out rotating to dock"); + } + + loop_rate.sleep(); + } +} + bool DockingServer::approachDock( Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose, bool backward) { @@ -447,7 +511,7 @@ bool DockingServer::approachDock( } // Update perception - if (!dock->plugin->getRefinedPose(dock_pose, dock->id)) { + if (!dock->plugin->getRefinedPose(dock_pose, dock->id) && !dock->plugin->shouldRotateToDock()) { throw opennav_docking_core::FailedToDetectDock("Failed dock detection"); } @@ -652,6 +716,13 @@ void DockingServer::undockRobot() geometry_msgs::msg::PoseStamped staging_pose = dock->getStagingPose(dock_pose.pose, dock_pose.header.frame_id); + // If we performed a rotation before docking backward, we must rotate the staging pose + // to match the robot orientation + if (dock->shouldRotateToDock()) { + staging_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(staging_pose.pose.orientation) + M_PI); + } + // Control robot to staging pose rclcpp::Time loop_start = this->now(); while (rclcpp::ok()) { @@ -684,8 +755,13 @@ void DockingServer::undockRobot() command->twist, staging_pose, undock_linear_tolerance_, undock_angular_tolerance_, false, !dock_backward)) { - RCLCPP_INFO(get_logger(), "Robot has reached staging pose"); + // Perform a 180º to the original staging pose + if (dock->shouldRotateToDock()) { + rotateToDock(staging_pose); + } + // Have reached staging_pose + RCLCPP_INFO(get_logger(), "Robot has reached staging pose"); vel_publisher_->publish(std::move(command)); if (!dock->isCharger() || dock->hasStoppedCharging()) { RCLCPP_INFO(get_logger(), "Robot has undocked!"); @@ -761,29 +837,34 @@ DockingServer::dynamicParametersCallback(std::vector paramete rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == "controller_frequency") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "controller_frequency") { controller_frequency_ = parameter.as_double(); - } else if (name == "initial_perception_timeout") { + } else if (param_name == "initial_perception_timeout") { initial_perception_timeout_ = parameter.as_double(); - } else if (name == "wait_charge_timeout") { + } else if (param_name == "wait_charge_timeout") { wait_charge_timeout_ = parameter.as_double(); - } else if (name == "undock_linear_tolerance") { + } else if (param_name == "undock_linear_tolerance") { undock_linear_tolerance_ = parameter.as_double(); - } else if (name == "undock_angular_tolerance") { + } else if (param_name == "undock_angular_tolerance") { undock_angular_tolerance_ = parameter.as_double(); + } else if (param_name == "rotation_angular_tolerance") { + rotation_angular_tolerance_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == "base_frame") { + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == "base_frame") { base_frame_ = parameter.as_string(); - } else if (name == "fixed_frame") { + } else if (param_name == "fixed_frame") { fixed_frame_ = parameter.as_string(); } - } else if (type == ParameterType::PARAMETER_INTEGER) { - if (name == "max_retries") { + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == "max_retries") { max_retries_ = parameter.as_int(); } } diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp index 10576a2afff..c11d3018a9e 100644 --- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp @@ -79,9 +79,11 @@ void SimpleChargingDock::configure( nav2_util::declare_parameter_if_not_declared( node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); - // Direction of docking + // Direction of docking and if we should rotate to dock nav2_util::declare_parameter_if_not_declared( node_, name + ".dock_direction", rclcpp::ParameterValue(std::string("forward"))); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".rotate_to_dock", rclcpp::ParameterValue(false)); node_->get_parameter(name + ".use_battery_status", use_battery_status_); node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_); @@ -110,6 +112,12 @@ void SimpleChargingDock::configure( throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"}; } + node_->get_parameter(name + ".rotate_to_dock", rotate_to_dock_); + if (rotate_to_dock_ && dock_direction_ != opennav_docking_core::DockDirection::BACKWARD) { + throw std::runtime_error{"Parameter rotate_to_dock is enabled but dock direction is not " + "backward. Please set dock direction to backward."}; + } + // Setup filter double filter_coef; node_->get_parameter(name + ".filter_coef", filter_coef); diff --git a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp index 54001bfe1b6..36b952b88ee 100644 --- a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp @@ -70,9 +70,11 @@ void SimpleNonChargingDock::configure( nav2_util::declare_parameter_if_not_declared( node_, name + ".staging_yaw_offset", rclcpp::ParameterValue(0.0)); - // Direction of docking + // Direction of docking and if we should rotate to dock nav2_util::declare_parameter_if_not_declared( node_, name + ".dock_direction", rclcpp::ParameterValue(std::string("forward"))); + nav2_util::declare_parameter_if_not_declared( + node_, name + ".rotate_to_dock", rclcpp::ParameterValue(false)); node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_); node_->get_parameter(name + ".external_detection_timeout", external_detection_timeout_); @@ -99,6 +101,12 @@ void SimpleNonChargingDock::configure( throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"}; } + node_->get_parameter(name + ".rotate_to_dock", rotate_to_dock_); + if (rotate_to_dock_ && dock_direction_ != opennav_docking_core::DockDirection::BACKWARD) { + throw std::runtime_error{"Parameter rotate_to_dock is enabled but dock direction is not " + "backward. Please set dock direction to backward."}; + } + // Setup filter double filter_coef; node_->get_parameter(name + ".filter_coef", filter_coef); diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt index d0ec76332c0..149e401b9f6 100644 --- a/nav2_docking/opennav_docking/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking/test/CMakeLists.txt @@ -63,8 +63,11 @@ target_link_libraries(test_simple_charging_dock ament_add_gtest(test_simple_non_charging_dock test_simple_non_charging_dock.cpp ) -ament_target_dependencies(test_simple_non_charging_dock - ${dependencies} +target_link_libraries(test_simple_non_charging_dock + ${geometry_msgs_TARGETS} + ${library_name} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) target_link_libraries(test_simple_non_charging_dock ${library_name} simple_non_charging_dock @@ -128,3 +131,9 @@ ament_add_pytest_test(test_docking_server_backward test_docking_server.py NON_CHARGING_DOCK=False BACKWARD=True ) + +ament_add_pytest_test(test_docking_server_backward_blind test_docking_server.py + ENV + NON_CHARGING_DOCK=False + BACKWARD_BLIND=True +) diff --git a/nav2_docking/opennav_docking/test/docking_params.yaml b/nav2_docking/opennav_docking/test/docking_params.yaml index 79bc784b292..adcb8f48019 100644 --- a/nav2_docking/opennav_docking/test/docking_params.yaml +++ b/nav2_docking/opennav_docking/test/docking_params.yaml @@ -9,6 +9,7 @@ docking_server: plugin: opennav_docking::SimpleChargingDock use_battery_status: True dock_direction: forward + rotate_to_dock: False staging_yaw_offset: 0.0 docks: [test_dock] test_dock: diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index 9d4f0b9f5ea..3dc51ad60a0 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -229,7 +229,9 @@ TEST(ControllerTests, DynamicParameters) { rclcpp::Parameter("controller.slowdown_radius", 8.0), rclcpp::Parameter("controller.projection_time", 9.0), rclcpp::Parameter("controller.simulation_time_step", 10.0), - rclcpp::Parameter("controller.dock_collision_threshold", 11.0)}); + rclcpp::Parameter("controller.dock_collision_threshold", 11.0), + rclcpp::Parameter("controller.rotate_to_heading_angular_vel", 12.0), + rclcpp::Parameter("controller.rotate_to_heading_max_angular_accel", 13.0)}); // Spin rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); @@ -246,6 +248,9 @@ TEST(ControllerTests, DynamicParameters) { EXPECT_EQ(node->get_parameter("controller.projection_time").as_double(), 9.0); EXPECT_EQ(node->get_parameter("controller.simulation_time_step").as_double(), 10.0); EXPECT_EQ(node->get_parameter("controller.dock_collision_threshold").as_double(), 11.0); + EXPECT_EQ(node->get_parameter("controller.rotate_to_heading_angular_vel").as_double(), 12.0); + EXPECT_EQ( + node->get_parameter("controller.rotate_to_heading_max_angular_accel").as_double(), 13.0); } TEST(ControllerTests, TFException) @@ -537,6 +542,71 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { collision_tester->deactivate(); } +TEST(ControllerTests, RotateToHeading) { + auto node = std::make_shared("test"); + + float rotate_to_heading_angular_vel = 1.0; + float rotate_to_heading_max_angular_accel = 3.2; + nav2_util::declare_parameter_if_not_declared( + node, "controller.rotate_to_heading_angular_vel", + rclcpp::ParameterValue(rotate_to_heading_angular_vel)); + nav2_util::declare_parameter_if_not_declared( + node, "controller.rotate_to_heading_max_angular_accel", + rclcpp::ParameterValue(rotate_to_heading_max_angular_accel)); + + auto controller = std::make_unique( + node, nullptr, "test_base_frame", "test_base_frame"); + + geometry_msgs::msg::Twist current_velocity; + double angular_distance_to_heading; + double dt = 0.1; + + // Case 1: Positive angular distance, within feasible range + angular_distance_to_heading = 0.5; + current_velocity.angular.z = 0.1; + auto cmd_vel = + controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt); + EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0); + EXPECT_GE(cmd_vel.angular.z, 0.0); + EXPECT_LE(cmd_vel.angular.z, rotate_to_heading_angular_vel); + + // Case 2: Negative angular distance, within feasible range + angular_distance_to_heading = -0.5; + current_velocity.angular.z = -0.1; + cmd_vel = + controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt); + EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0); + EXPECT_LE(cmd_vel.angular.z, 0.0); + EXPECT_GE(cmd_vel.angular.z, -rotate_to_heading_angular_vel); + + // Case 3: Positive angular distance, exceeding max feasible speed + angular_distance_to_heading = 1.0; + current_velocity.angular.z = 0.5; + cmd_vel = + controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt); + EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd_vel.angular.z, + current_velocity.angular.z + rotate_to_heading_max_angular_accel * dt); + + // Case 4: Negative angular distance, exceeding max feasible speed + angular_distance_to_heading = -1.0; + current_velocity.angular.z = -0.5; + cmd_vel = + controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt); + EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd_vel.angular.z, + current_velocity.angular.z - rotate_to_heading_max_angular_accel * dt); + + // Case 5: Zero angular distance + angular_distance_to_heading = 0.0; + current_velocity.angular.z = 0.0; + cmd_vel = + controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt); + EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd_vel.angular.z, 0.0); + + controller.reset(); +} } // namespace opennav_docking diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 6d1a8d6db4d..086d76c6b3a 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server.py +++ b/nav2_docking/opennav_docking/test/test_docking_server.py @@ -31,6 +31,7 @@ import launch_testing.util from nav2_common.launch import RewrittenYaml from nav2_msgs.action import DockRobot, NavigateToPose, UndockRobot +from nav_msgs.msg import Odometry import pytest import rclpy from rclpy.action.client import ActionClient @@ -67,6 +68,10 @@ def generate_test_description() -> LaunchDescription: param_substitutions.update({'dock_direction': 'backward'}) param_substitutions.update({'staging_yaw_offset': '3.14'}) + if os.getenv('BACKWARD_BLIND') == 'True': + param_substitutions.update({'dock_direction': 'backward'}) + param_substitutions.update({'rotate_to_dock': 'True'}) + configured_params = RewrittenYaml( source_file=params_file, root_key='', @@ -122,6 +127,8 @@ def setUp(self) -> None: # Latest command velocity self.command = Twist() self.node = rclpy.create_node('test_docking_server') + # Publish odometry + self.odom_pub = self.node.create_publisher(Odometry, 'odom', 10) def tearDown(self) -> None: self.node.destroy_node() @@ -150,6 +157,7 @@ def publish(self) -> None: t.transform.rotation.z = sin(self.theta / 2.0) t.transform.rotation.w = cos(self.theta / 2.0) self.tf_broadcaster.sendTransform(t) + self.publish_odometry(t) # Publish the battery state if we are using a charging dock if os.getenv('NON_CHARGING_DOCK') == 'False': b = BatteryState() @@ -159,6 +167,17 @@ def publish(self) -> None: b.current = -1.0 self.battery_state_pub.publish(b) + def publish_odometry(self, odom_to_base_link: TransformStamped) -> None: + odom = Odometry() + odom.header.stamp = self.node.get_clock().now().to_msg() + odom.header.frame_id = 'odom' + odom.child_frame_id = 'base_link' + odom.pose.pose.position.x = odom_to_base_link.transform.translation.x + odom.pose.pose.position.y = odom_to_base_link.transform.translation.y + odom.pose.pose.orientation = odom_to_base_link.transform.rotation + odom.twist.twist = self.command + self.odom_pub.publish(odom) + def action_feedback_callback(self, msg: DockRobot.Feedback) -> None: # Force the docking action to run a full recovery loop and then # make contact with the dock (based on pose of robot) before @@ -193,8 +212,13 @@ def test_docking_server(self) -> None: self.timer = self.node.create_timer(0.05, self.timer_callback) # Create action client - self.dock_action_client = ActionClient(self.node, DockRobot, 'dock_robot') - self.undock_action_client = ActionClient(self.node, UndockRobot, 'undock_robot') + self.dock_action_client: ActionClient[ + DockRobot.Goal, DockRobot.Result, DockRobot.Feedback + ] = ActionClient(self.node, DockRobot, 'dock_robot') + + self.undock_action_client: ActionClient[ + UndockRobot.Goal, UndockRobot.Result, UndockRobot.Feedback + ] = ActionClient(self.node, UndockRobot, 'undock_robot') # Subscribe to command velocity self.node.create_subscription( diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index a97448b2d7d..e0ace25ce47 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -247,13 +247,11 @@ TEST(DockingServerTests, testDynamicParams) rclcpp::Parameter("undock_angular_tolerance", 0.125), rclcpp::Parameter("base_frame", std::string("hi")), rclcpp::Parameter("fixed_frame", std::string("hi")), - rclcpp::Parameter("max_retries", 7)}); + rclcpp::Parameter("max_retries", 7), + rclcpp::Parameter("rotation_angular_tolerance", 0.42)}); - rclcpp::spin_until_future_complete( - node->get_node_base_interface(), - results); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); - EXPECT_EQ(node->get_parameter("controller_frequency").as_double(), 0.2); EXPECT_EQ(node->get_parameter("initial_perception_timeout").as_double(), 1.0); EXPECT_EQ(node->get_parameter("wait_charge_timeout").as_double(), 1.2); EXPECT_EQ(node->get_parameter("undock_linear_tolerance").as_double(), 0.25); @@ -261,6 +259,7 @@ TEST(DockingServerTests, testDynamicParams) EXPECT_EQ(node->get_parameter("base_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("fixed_frame").as_string(), std::string("hi")); EXPECT_EQ(node->get_parameter("max_retries").as_int(), 7); + EXPECT_EQ(node->get_parameter("rotation_angular_tolerance").as_double(), 0.42); node->on_deactivate(rclcpp_lifecycle::State()); node->on_cleanup(rclcpp_lifecycle::State()); diff --git a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp index ccd4825863b..14747ef1742 100644 --- a/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp @@ -363,6 +363,44 @@ TEST(SimpleChargingDockTests, GetDockDirection) dock.reset(); } +TEST(SimpleChargingDockTests, ShouldRotateToDock) +{ + auto node = std::make_shared("test"); + + // Case 1: Direction to BACKWARD and rotate_to_dock to true + node->declare_parameter("my_dock.dock_direction", rclcpp::ParameterValue("backward")); + node->declare_parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true)); + + auto dock = std::make_unique(); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), true); + dock->cleanup(); + + // Case 2: Direction to BACKWARD and rotate_to_dock to false + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false))); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), false); + + // Case 3: Direction to FORWARD and rotate_to_dock to true + node->set_parameter( + rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("forward"))); + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true))); + EXPECT_THROW(dock->configure(node, "my_dock", nullptr), std::runtime_error); + EXPECT_EQ(dock->shouldRotateToDock(), true); + dock->cleanup(); + + // Case 4: Direction to FORWARD and rotate_to_dock to false + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false))); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), false); + + dock->cleanup(); + dock.reset(); +} + } // namespace opennav_docking int main(int argc, char ** argv) diff --git a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp index 68bfc193379..fe274c756c9 100644 --- a/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp @@ -324,6 +324,44 @@ TEST(SimpleNonChargingDockTests, GetDockDirection) dock.reset(); } +TEST(SimpleChargingDockTests, ShouldRotateToDock) +{ + auto node = std::make_shared("test"); + + // Case 1: Direction to BACKWARD and rotate_to_dock to true + node->declare_parameter("my_dock.dock_direction", rclcpp::ParameterValue("backward")); + node->declare_parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true)); + + auto dock = std::make_unique(); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), true); + dock->cleanup(); + + // Case 2: Direction to BACKWARD and rotate_to_dock to false + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false))); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), false); + + // Case 3: Direction to FORWARD and rotate_to_dock to true + node->set_parameter( + rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("forward"))); + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true))); + EXPECT_THROW(dock->configure(node, "my_dock", nullptr), std::runtime_error); + EXPECT_EQ(dock->shouldRotateToDock(), true); + dock->cleanup(); + + // Case 4: Direction to FORWARD and rotate_to_dock to false + node->set_parameter( + rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false))); + EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr)); + EXPECT_EQ(dock->shouldRotateToDock(), false); + + dock->cleanup(); + dock.reset(); +} + } // namespace opennav_docking int main(int argc, char ** argv) diff --git a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp index 0a07e12e7f7..8f4a4afeff2 100644 --- a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp +++ b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp @@ -68,6 +68,12 @@ class DockRobotAction */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * \brief Override required by the a BT action. Cancel the action and set the path output */ diff --git a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp index 8674e477791..d0e78e3611f 100644 --- a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp +++ b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp @@ -68,6 +68,12 @@ class UndockRobotAction */ BT::NodeStatus on_cancelled() override; + /** + * @brief Function to perform work in a BT Node when the action server times out + * Such as setting the error code ID status to timed out for action clients. + */ + void on_timeout() override; + /** * \brief Override required by the a BT action. Cancel the action and set the path output */ diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 37785551165..d2071203bfe 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -2,7 +2,7 @@ opennav_docking_bt - 1.3.1 + 1.4.0 A set of BT nodes and XMLs for docking Steve Macenski Apache-2.0 diff --git a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp index be3e58b8c8c..35cea456b4f 100644 --- a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp +++ b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp @@ -67,6 +67,12 @@ BT::NodeStatus DockRobotAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void DockRobotAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void DockRobotAction::halt() { BtActionNode::halt(); diff --git a/nav2_docking/opennav_docking_bt/src/undock_robot.cpp b/nav2_docking/opennav_docking_bt/src/undock_robot.cpp index 0669bad92bd..400615e6f0d 100644 --- a/nav2_docking/opennav_docking_bt/src/undock_robot.cpp +++ b/nav2_docking/opennav_docking_bt/src/undock_robot.cpp @@ -58,6 +58,12 @@ BT::NodeStatus UndockRobotAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void UndockRobotAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void UndockRobotAction::halt() { BtActionNode::halt(); diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index ada25c9f474..8d4ace00eaa 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -136,11 +136,19 @@ class ChargingDock */ DockDirection getDockDirection() {return dock_direction_;} + /** + * @brief Determines whether the robot should rotate 180º to face away from the dock. + * For example, to perform a backward docking without detections. + * @return bool If the robot should rotate to face away from the dock. + */ + bool shouldRotateToDock() {return rotate_to_dock_;} + std::string getName() {return name_;} protected: std::string name_; DockDirection dock_direction_{DockDirection::UNKNOWN}; + bool rotate_to_dock_{false}; }; } // namespace opennav_docking_core diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index 2df639d5161..57a3d15b386 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -2,7 +2,7 @@ opennav_docking_core - 1.3.1 + 1.4.0 A set of headers for plugins core to the opennav docking framework Steve Macenski Apache-2.0 diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 3a9a7bf9ccc..695cebd19b7 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.3.1 + 1.4.0 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index ea3e167a2bb..8b55d813004 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.3.1 + 1.4.0 DWB core interfaces package Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 92d1f2df16f..a697e486021 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.3.1 + 1.4.0 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index e6ce9307a62..594c7619a41 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.3.1 + 1.4.0 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 3e0b0c941ec..ba51d4d06e1 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.3.1 + 1.4.0 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp index dd7d5ff1e64..d40f7f1b66e 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -177,43 +177,46 @@ KinematicsHandler::dynamicParametersCallback(std::vector para KinematicParameters kinematics(*kinematics_.load()); for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".min_vel_x") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".min_vel_x") { kinematics.min_vel_x_ = parameter.as_double(); - } else if (name == plugin_name_ + ".min_vel_y") { + } else if (param_name == plugin_name_ + ".min_vel_y") { kinematics.min_vel_y_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_vel_x") { + } else if (param_name == plugin_name_ + ".max_vel_x") { kinematics.max_vel_x_ = parameter.as_double(); kinematics.base_max_vel_x_ = kinematics.max_vel_x_; - } else if (name == plugin_name_ + ".max_vel_y") { + } else if (param_name == plugin_name_ + ".max_vel_y") { kinematics.max_vel_y_ = parameter.as_double(); kinematics.base_max_vel_y_ = kinematics.max_vel_y_; - } else if (name == plugin_name_ + ".max_vel_theta") { + } else if (param_name == plugin_name_ + ".max_vel_theta") { kinematics.max_vel_theta_ = parameter.as_double(); kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_; - } else if (name == plugin_name_ + ".min_speed_xy") { + } else if (param_name == plugin_name_ + ".min_speed_xy") { kinematics.min_speed_xy_ = parameter.as_double(); kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_; - } else if (name == plugin_name_ + ".max_speed_xy") { + } else if (param_name == plugin_name_ + ".max_speed_xy") { kinematics.max_speed_xy_ = parameter.as_double(); kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_; - } else if (name == plugin_name_ + ".min_speed_theta") { + } else if (param_name == plugin_name_ + ".min_speed_theta") { kinematics.min_speed_theta_ = parameter.as_double(); kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_; - } else if (name == plugin_name_ + ".acc_lim_x") { + } else if (param_name == plugin_name_ + ".acc_lim_x") { kinematics.acc_lim_x_ = parameter.as_double(); - } else if (name == plugin_name_ + ".acc_lim_y") { + } else if (param_name == plugin_name_ + ".acc_lim_y") { kinematics.acc_lim_y_ = parameter.as_double(); - } else if (name == plugin_name_ + ".acc_lim_theta") { + } else if (param_name == plugin_name_ + ".acc_lim_theta") { kinematics.acc_lim_theta_ = parameter.as_double(); - } else if (name == plugin_name_ + ".decel_lim_x") { + } else if (param_name == plugin_name_ + ".decel_lim_x") { kinematics.decel_lim_x_ = parameter.as_double(); - } else if (name == plugin_name_ + ".decel_lim_y") { + } else if (param_name == plugin_name_ + ".decel_lim_y") { kinematics.decel_lim_y_ = parameter.as_double(); - } else if (name == plugin_name_ + ".decel_lim_theta") { + } else if (param_name == plugin_name_ + ".decel_lim_theta") { kinematics.decel_lim_theta_ = parameter.as_double(); } } diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index f98c8473a88..7d8363bcfc2 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.3.1 + 1.4.0 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 9dbb53a1ccd..5f5ed5f97bc 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.3.1 + 1.4.0 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index f3dae14813b..060a6c2f701 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.3.1 + 1.4.0 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index b45196dcf5b..be7ee9c4806 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.3.1 + 1.4.0 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_graceful_controller/src/parameter_handler.cpp b/nav2_graceful_controller/src/parameter_handler.cpp index 7b2f7c42936..65fc260eb5f 100644 --- a/nav2_graceful_controller/src/parameter_handler.cpp +++ b/nav2_graceful_controller/src/parameter_handler.cpp @@ -135,45 +135,48 @@ ParameterHandler::dynamicParametersCallback(std::vector param std::lock_guard lock_reinit(mutex_); for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".transform_tolerance") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".transform_tolerance") { params_.transform_tolerance = parameter.as_double(); - } else if (name == plugin_name_ + ".min_lookahead") { + } else if (param_name == plugin_name_ + ".min_lookahead") { params_.min_lookahead = parameter.as_double(); - } else if (name == plugin_name_ + ".max_lookahead") { + } else if (param_name == plugin_name_ + ".max_lookahead") { params_.max_lookahead = parameter.as_double(); - } else if (name == plugin_name_ + ".k_phi") { + } else if (param_name == plugin_name_ + ".k_phi") { params_.k_phi = parameter.as_double(); - } else if (name == plugin_name_ + ".k_delta") { + } else if (param_name == plugin_name_ + ".k_delta") { params_.k_delta = parameter.as_double(); - } else if (name == plugin_name_ + ".beta") { + } else if (param_name == plugin_name_ + ".beta") { params_.beta = parameter.as_double(); - } else if (name == plugin_name_ + ".lambda") { + } else if (param_name == plugin_name_ + ".lambda") { params_.lambda = parameter.as_double(); - } else if (name == plugin_name_ + ".v_linear_min") { + } else if (param_name == plugin_name_ + ".v_linear_min") { params_.v_linear_min = parameter.as_double(); - } else if (name == plugin_name_ + ".v_linear_max") { + } else if (param_name == plugin_name_ + ".v_linear_max") { params_.v_linear_max = parameter.as_double(); params_.v_linear_max_initial = params_.v_linear_max; - } else if (name == plugin_name_ + ".v_angular_max") { + } else if (param_name == plugin_name_ + ".v_angular_max") { params_.v_angular_max = parameter.as_double(); params_.v_angular_max_initial = params_.v_angular_max; - } else if (name == plugin_name_ + ".v_angular_min_in_place") { + } else if (param_name == plugin_name_ + ".v_angular_min_in_place") { params_.v_angular_min_in_place = parameter.as_double(); - } else if (name == plugin_name_ + ".slowdown_radius") { + } else if (param_name == plugin_name_ + ".slowdown_radius") { params_.slowdown_radius = parameter.as_double(); - } else if (name == plugin_name_ + ".initial_rotation_tolerance") { + } else if (param_name == plugin_name_ + ".initial_rotation_tolerance") { params_.initial_rotation_tolerance = parameter.as_double(); - } else if (name == plugin_name_ + ".rotation_scaling_factor") { + } else if (param_name == plugin_name_ + ".rotation_scaling_factor") { params_.rotation_scaling_factor = parameter.as_double(); - } else if (name == plugin_name_ + ".in_place_collision_resolution") { + } else if (param_name == plugin_name_ + ".in_place_collision_resolution") { params_.in_place_collision_resolution = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".initial_rotation") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".initial_rotation") { if (parameter.as_bool() && params_.allow_backward) { RCLCPP_WARN( logger_, "Initial rotation and allow backward parameters are both true, " @@ -181,9 +184,9 @@ ParameterHandler::dynamicParametersCallback(std::vector param continue; } params_.initial_rotation = parameter.as_bool(); - } else if (name == plugin_name_ + ".prefer_final_rotation") { + } else if (param_name == plugin_name_ + ".prefer_final_rotation") { params_.prefer_final_rotation = parameter.as_bool(); - } else if (name == plugin_name_ + ".allow_backward") { + } else if (param_name == plugin_name_ + ".allow_backward") { if (params_.initial_rotation && parameter.as_bool()) { RCLCPP_WARN( logger_, "Initial rotation and allow backward parameters are both true, " @@ -191,7 +194,7 @@ ParameterHandler::dynamicParametersCallback(std::vector param continue; } params_.allow_backward = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_collision_detection") { + } else if (param_name == plugin_name_ + ".use_collision_detection") { params_.use_collision_detection = parameter.as_bool(); } } diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index f5c010075f9..f70a336d141 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.3.1 + 1.4.0 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_loopback_sim/README.md b/nav2_loopback_sim/README.md index fc160d91070..62104e05e9b 100644 --- a/nav2_loopback_sim/README.md +++ b/nav2_loopback_sim/README.md @@ -42,6 +42,13 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated na - `scan_publish_dur`: : The duration between publishing scan (default 0.1s -- 10hz) - `publish_map_odom_tf`: Whether or not to publish tf from `map_frame_id` to `odom_frame_id` (default `true`) - `publish_clock`: Whether or not to publish simulated clock to `/clock` (default `true`) +- `scan_range_min`: Minimum measurable distance from the scan in meters. Values below this are considered invalid (default: `0.05`) +- `scan_range_max`: Maximum measurable distance from the scan in meters. Values beyond this are out of range (default: `30.0`) +- `scan_angle_min`: Starting angle of the scan in radians (leftmost angle) (default: `-Ï€` / `-3.1415`) +- `scan_angle_max`: Ending angle of the scan in radians (rightmost angle) (default: `Ï€` / `3.1415`) +- `scan_angle_increment`: Angular resolution of the scan in radians (angle between consecutive measurements) (default: `0.02617`) +- `scan_use_inf`: Whether to use `inf` for out-of-range values. If `false`, uses `scan_range_max - 0.1` instead (default: `True`) + ### Topics diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 53d67540214..77a69143a9c 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -86,6 +86,30 @@ def __init__(self) -> None: self.declare_parameter('publish_clock', True) self.publish_clock = self.get_parameter('publish_clock').get_parameter_value().bool_value + self.declare_parameter('scan_range_min', 0.05) + self.scan_range_min = \ + self.get_parameter('scan_range_min').get_parameter_value().double_value + + self.declare_parameter('scan_range_max', 30.0) + self.scan_range_max = \ + self.get_parameter('scan_range_max').get_parameter_value().double_value + + self.declare_parameter('scan_angle_min', -math.pi) + self.scan_angle_min = \ + self.get_parameter('scan_angle_min').get_parameter_value().double_value + + self.declare_parameter('scan_angle_max', math.pi) + self.scan_angle_max = \ + self.get_parameter('scan_angle_max').get_parameter_value().double_value + + self.declare_parameter('scan_angle_increment', 0.0261) # 0.0261 rad = 1.5 degrees + self.scan_angle_increment = \ + self.get_parameter('scan_angle_increment').get_parameter_value().double_value + + self.declare_parameter('scan_use_inf', True) + self.use_inf = \ + self.get_parameter('scan_use_inf').get_parameter_value().bool_value + self.t_map_to_odom = TransformStamped() self.t_map_to_odom.header.frame_id = self.map_frame_id self.t_map_to_odom.child_frame_id = self.odom_frame_id @@ -235,14 +259,14 @@ def publishLaserScan(self) -> None: self.scan_msg = LaserScan() self.scan_msg.header.stamp = (self.get_clock().now()).to_msg() self.scan_msg.header.frame_id = self.scan_frame_id - self.scan_msg.angle_min = -math.pi - self.scan_msg.angle_max = math.pi + self.scan_msg.angle_min = self.scan_angle_min + self.scan_msg.angle_max = self.scan_angle_max # 1.5 degrees - self.scan_msg.angle_increment = 0.0261799 + self.scan_msg.angle_increment = self.scan_angle_increment self.scan_msg.time_increment = 0.0 self.scan_msg.scan_time = 0.1 - self.scan_msg.range_min = 0.05 - self.scan_msg.range_max = 30.0 + self.scan_msg.range_min = self.scan_range_min + self.scan_msg.range_max = self.scan_range_max num_samples = int( (self.scan_msg.angle_max - self.scan_msg.angle_min) / self.scan_msg.angle_increment) @@ -323,7 +347,10 @@ def getLaserPose(self) -> tuple[float, float, float]: def getLaserScan(self, num_samples: int) -> None: if self.map is None or self.initial_pose is None or self.mat_base_to_laser is None: - self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples + if self.use_inf: + self.scan_msg.ranges = [float('inf')] * num_samples + else: + self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples return x0, y0, theta = self.getLaserPose() @@ -332,7 +359,10 @@ def getLaserScan(self, num_samples: int) -> None: if not 0 < mx0 < self.map.info.width or not 0 < my0 < self.map.info.height: # outside map - self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples + if self.use_inf: + self.scan_msg.ranges = [float('inf')] * num_samples + else: + self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples return for i in range(num_samples): @@ -361,6 +391,8 @@ def getLaserScan(self, num_samples: int) -> None: break line_iterator.advance() + if self.scan_msg.ranges[i] == 0.0 and self.use_inf: + self.scan_msg.ranges[i] = float('inf') def main() -> None: diff --git a/nav2_loopback_sim/package.xml b/nav2_loopback_sim/package.xml index 9376cfb50c7..9d55a6d4837 100644 --- a/nav2_loopback_sim/package.xml +++ b/nav2_loopback_sim/package.xml @@ -2,7 +2,7 @@ nav2_loopback_sim - 1.3.1 + 1.4.0 A loopback simulator to replace physics simulation steve macenski Apache-2.0 @@ -12,6 +12,7 @@ nav_msgs tf_transformations tf2_ros + python3-transforms3d ament_copyright ament_flake8 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 1525e3f5ca8..9b4d47790b7 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.3.1 + 1.4.0 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 3b11644e19f..68db5b73719 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -73,7 +73,7 @@ This process is then repeated a number of times and returns a converged solution | max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. | | prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. | | transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with TF. | - | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | + | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. In addition, these cusping points will also be treated by the critics as local goals that the robot will attempt to reach. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | | inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. | | inversion_yaw_tolerance | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg. | diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp index 478bfd23492..42e98190d9a 100644 --- a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -79,7 +79,8 @@ void prepareAndRunBenchmark( printInfo(optimizer_settings, path_settings, critics); auto node = getDummyNode(optimizer_settings, critics); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); auto optimizer = getDummyOptimizer(node, costmap_ros, parameters_handler.get()); // evalControl args diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index c285a14b664..2646c98fd9e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -144,6 +144,7 @@ class CostCritic : public CriticFunction std::string inflation_layer_name_; unsigned int power_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp index 08ec354cf3b..58c108b21cd 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp @@ -46,6 +46,7 @@ class GoalAngleCritic : public CriticFunction float threshold_to_consider_{0}; unsigned int power_{0}; float weight_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp index 8fb8fb688ae..6b248f29945 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp @@ -46,6 +46,7 @@ class GoalCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; float threshold_to_consider_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index 31b6a3d0df8..23fb3db2ca0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -97,6 +97,7 @@ class ObstaclesCritic : public CriticFunction unsigned int power_{0}; float repulsion_weight_, critical_weight_{0}; + bool enforce_path_inversion_{false}; std::string inflation_layer_name_; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index f7ad2fda6a9..133447a8774 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -52,6 +52,7 @@ class PathAlignCritic : public CriticFunction bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index b137270aba9..85b9d48eb83 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -81,6 +81,7 @@ class PathAngleCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index 1ccd08c32fe..aacec055ccf 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -53,6 +53,7 @@ class PathFollowCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp index e17ad235e5c..811ba9ae9a6 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp @@ -45,6 +45,7 @@ class PreferForwardCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; float threshold_to_consider_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp index 3e316a567e0..bc40b194404 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp @@ -44,6 +44,7 @@ class TwirlingCritic : public CriticFunction protected: unsigned int power_{0}; float weight_{0}; + bool enforce_path_inversion_{false}; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index e0821e548d6..2ced1fe2fc4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -120,8 +120,9 @@ class Optimizer /** * @brief Reset the optimization problem to initial conditions + * @param Whether to reset the constraints to its base values */ - void reset(); + void reset(bool reset_dynamic_speed_limits = true); /** * @brief Get the motion model time step diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp index 4445186c24c..28f0eaf3997 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp @@ -45,7 +45,6 @@ class ParametersHandler rcl_interfaces::msg::SetParametersResult & result); using post_callback_t = void (); using pre_callback_t = void (); - /** * @brief Constructor for mppi::ParametersHandler */ @@ -56,7 +55,7 @@ class ParametersHandler * @param parent Weak ptr to node */ explicit ParametersHandler( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent); + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string & name); /** * @brief Destructor for mppi::ParametersHandler @@ -168,10 +167,12 @@ class ParametersHandler on_set_param_handler_; rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::string node_name_; + std::string name_; bool verbose_{false}; std::unordered_map> get_param_callbacks_; + std::unordered_map> get_pre_callbacks_; std::vector> pre_callbacks_; std::vector> post_callbacks_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 2ca44fd0137..40f825fd69c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -218,6 +218,49 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path) return result; } +/** + * @brief Get the last pose from a path + * @param path Reference to the path + * @return geometry_msgs::msg::Pose Last pose in the path + */ +inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path) +{ + const unsigned int path_last_idx = path.x.size() - 1; + + auto last_orientation = path.yaws(path_last_idx); + + tf2::Quaternion pose_orientation; + pose_orientation.setRPY(0.0, 0.0, last_orientation); + + geometry_msgs::msg::Pose pathPose; + pathPose.position.x = path.x(path_last_idx); + pathPose.position.y = path.y(path_last_idx); + pathPose.orientation.x = pose_orientation.x(); + pathPose.orientation.y = pose_orientation.y(); + pathPose.orientation.z = pose_orientation.z(); + pathPose.orientation.w = pose_orientation.w(); + + return pathPose; +} + +/** + * @brief Get the target pose to be evaluated by the critic + * @param data Data to use + * @param enforce_path_inversion True to return the cusp point (last pose of the path) + * instead of the original goal + * @return geometry_msgs::msg::Pose Target pose for the critic + */ +inline geometry_msgs::msg::Pose getCriticGoal( + const CriticData & data, + bool enforce_path_inversion) +{ + if (enforce_path_inversion) { + return getLastPathPose(data.path); + } else { + return data.goal; + } +} + /** * @brief Check if the robot pose is within the Goal Checker's tolerances to goal * @param global_checker Pointer to the goal checker diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 3ed96f7abdb..ed8332288bf 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.3.1 + 1.4.0 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 57effc952ee..9c6c7c209a5 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -31,7 +31,7 @@ void MPPIController::configure( costmap_ros_ = costmap_ros; tf_buffer_ = tf; name_ = name; - parameters_handler_ = std::make_unique(parent); + parameters_handler_ = std::make_unique(parent, name_); auto node = parent_.lock(); // Get high-level controller parameters @@ -86,7 +86,7 @@ void MPPIController::deactivate() void MPPIController::reset() { - optimizer_.reset(); + optimizer_.reset(false /*Don't reset zone-based speed limits between requests*/); } geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 0efbb7b24bb..cb7b39e856f 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -22,6 +22,9 @@ namespace mppi::critics void CostCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); @@ -132,6 +135,8 @@ void CostCritic::score(CriticData & data) return; } + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + // Setup cost information for various parts of the critic is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); auto * costmap = collision_checker_.getCostmap(); @@ -148,7 +153,7 @@ void CostCritic::score(CriticData & data) // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) { + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index aada6ff4920..161e2a4fddf 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -19,11 +19,12 @@ namespace mppi::critics void GoalAngleCritic::initialize() { - auto getParam = parameters_handler_->getParamGetter(name_); + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 3.0f); - getParam(threshold_to_consider_, "threshold_to_consider", 0.5f); RCLCPP_INFO( @@ -35,14 +36,19 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_ || !utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (!utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } - const auto goal_idx = data.path.x.size() - 1; - const float goal_yaw = data.path.yaws(goal_idx); + double goal_yaw = tf2::getYaw(goal.orientation); if (power_ > 1u) { data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 282ddc62850..7aa5f51aeff 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -21,6 +21,9 @@ namespace mppi::critics void GoalCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); @@ -34,14 +37,20 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_ || !utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (!utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } - const auto & goal_x = data.goal.position.x; - const auto & goal_y = data.goal.position.y; + auto goal_x = goal.position.x; + auto goal_y = goal.position.y; const auto delta_x = data.trajectories.x - goal_x; const auto delta_y = data.trajectories.y - goal_y; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index ee660a75815..deba6009421 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -23,6 +23,9 @@ namespace mppi::critics void ObstaclesCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion1", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); @@ -127,9 +130,11 @@ void ObstaclesCritic::score(CriticData & data) possible_collision_cost_ = findCircumscribedCost(costmap_ros_); } + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; - if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) { + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) { near_goal = true; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 71b05e0a7da..03da09b7bb5 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -19,6 +19,9 @@ namespace mppi::critics void PathAlignCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 10.0f); @@ -39,9 +42,15 @@ void PathAlignCritic::initialize() void PathAlignCritic::score(CriticData & data) { + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + // Don't apply close to goal, let the goal critics take over - if (!enabled_ || utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.goal)) + if (utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index a312b1e50ae..1374c8e749d 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -23,6 +23,7 @@ namespace mppi::critics void PathAngleCritic::initialize() { auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); float vx_min; getParentParam(vx_min, "vx_min", -0.35); if (fabs(vx_min) < 1e-6f) { // zero @@ -61,8 +62,14 @@ void PathAngleCritic::initialize() void PathAngleCritic::score(CriticData & data) { - if (!enabled_ || - utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 56059cc2811..4f8c1c70eec 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -21,6 +21,9 @@ namespace mppi::critics void PathFollowCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam( @@ -33,8 +36,15 @@ void PathFollowCritic::initialize() void PathFollowCritic::score(CriticData & data) { - if (!enabled_ || data.path.x.size() < 2 || - utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (data.path.x.size() < 2 || + utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index b5fb3880c14..a220cbe1fd9 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -21,6 +21,9 @@ namespace mppi::critics void PreferForwardCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); @@ -34,8 +37,14 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { - if (!enabled_ || utils::withinPositionGoalTolerance( - threshold_to_consider_, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, goal)) { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 06b7b70fc5f..c9cfc79233c 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -21,6 +21,9 @@ namespace mppi::critics void TwirlingCritic::initialize() { + auto getParentParam = parameters_handler_->getParamGetter(parent_name_); + getParentParam(enforce_path_inversion_, "enforce_path_inversion", false); + auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); @@ -32,8 +35,14 @@ void TwirlingCritic::initialize() void TwirlingCritic::score(CriticData & data) { - if (!enabled_ || - utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.goal)) + if (!enabled_) { + return; + } + + geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_); + + if (utils::withinPositionGoalTolerance( + data.goal_checker, data.state.pose.pose, goal)) { return; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index d841699642b..3b944fc9ecd 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -132,7 +132,7 @@ void Optimizer::setOffset(double controller_frequency) } } -void Optimizer::reset() +void Optimizer::reset(bool reset_dynamic_speed_limits) { state_.reset(settings_.batch_size, settings_.time_steps); control_sequence_.reset(settings_.time_steps); @@ -141,7 +141,9 @@ void Optimizer::reset() control_history_[2] = {0.0f, 0.0f, 0.0f}; control_history_[3] = {0.0f, 0.0f, 0.0f}; - settings_.constraints = settings_.base_constraints; + if (reset_dynamic_speed_limits) { + settings_.constraints = settings_.base_constraints; + } costs_.setZero(settings_.batch_size); generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index 34d7764e695..4d540ef1cae 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -18,12 +18,13 @@ namespace mppi { ParametersHandler::ParametersHandler( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent) + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string & name) { node_ = parent; auto node = node_.lock(); node_name_ = node->get_name(); logger_ = node->get_logger(); + name_ = name; } ParametersHandler::~ParametersHandler() @@ -57,23 +58,32 @@ ParametersHandler::dynamicParamsCallback( result.reason = ""; std::lock_guard lock(parameters_change_mutex_); - - for (auto & pre_cb : pre_callbacks_) { - pre_cb(); - } - + std::vector plugin_params; for (auto & param : parameters) { const std::string & param_name = param.get_name(); - - if (auto callback = get_param_callbacks_.find(param_name); - callback != get_param_callbacks_.end()) - { - callback->second(param, result); + if (param_name.find(name_ + ".") != 0) { + continue; } + plugin_params.push_back(param); } - for (auto & post_cb : post_callbacks_) { - post_cb(); + if (!plugin_params.empty()) { + for (auto & pre_cb : pre_callbacks_) { + pre_cb(); + } + + for (auto & param : plugin_params) { + const std::string & param_name = param.get_name(); + if (auto callback = get_param_callbacks_.find(param_name); + callback != get_param_callbacks_.end()) + { + callback->second(param, result); + } + } + + for (auto & post_cb : post_callbacks_) { + post_cb(); + } } if (!result.successful) { diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index 0c83a492e63..ae1362ff5f1 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -95,7 +95,8 @@ TEST(CriticManagerTests, BasicCriticOperations) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -134,7 +135,8 @@ TEST(CriticManagerTests, CriticLoadingTest) rclcpp::ParameterValue(std::vector{"ConstraintCritic", "PreferForwardCritic"})); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State state; costmap_ros->on_configure(state); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 563009976e3..7e0f61ccf9b 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -62,7 +62,8 @@ TEST(CriticTests, ConstraintsCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -130,7 +131,8 @@ TEST(CriticTests, ObstacleCriticMisalignedParams) { auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); auto getParam = param_handler.getParamGetter("critic"); bool consider_footprint; getParam(consider_footprint, "consider_footprint", true); @@ -150,7 +152,8 @@ TEST(CriticTests, ObstacleCriticAlignedParams) { auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); auto getParam = param_handler.getParamGetter("critic"); bool consider_footprint; getParam(consider_footprint, "consider_footprint", false); @@ -169,7 +172,8 @@ TEST(CriticTests, CostCriticMisAlignedParams) { auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; auto getParam = param_handler.getParamGetter("critic"); bool consider_footprint; @@ -189,7 +193,8 @@ TEST(CriticTests, CostCriticAlignedParams) { auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; auto getParam = param_handler.getParamGetter("critic"); bool consider_footprint; @@ -207,7 +212,8 @@ TEST(CriticTests, GoalAngleCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -240,6 +246,11 @@ TEST(CriticTests, GoalAngleCritic) path.y(9) = 0.0; path.yaws(9) = 3.14; goal.position.x = 10.0; + goal.position.y = 0.0; + goal.orientation.x = 0.0; + goal.orientation.y = 0.0; + goal.orientation.z = 1.0; + goal.orientation.w = 0.0; critic.score(data); EXPECT_NEAR(costs.sum(), 0, 1e-6); @@ -261,7 +272,8 @@ TEST(CriticTests, GoalCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -313,7 +325,8 @@ TEST(CriticTests, PathAngleCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -431,7 +444,8 @@ TEST(CriticTests, PreferForwardCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -487,7 +501,8 @@ TEST(CriticTests, TwirlingCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -551,7 +566,8 @@ TEST(CriticTests, PathFollowCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -602,7 +618,8 @@ TEST(CriticTests, PathAlignCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); @@ -712,7 +729,8 @@ TEST(CriticTests, VelocityDeadbandCritic) auto node = std::make_shared("my_node"); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); auto getParam = param_handler.getParamGetter("critic"); std::vector deadband_velocities_; getParam(deadband_velocities_, "deadband_velocities", std::vector{0.08, 0.08, 0.08}); diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 7babbd9f5db..28653ad50c2 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -127,7 +127,8 @@ TEST(MotionModelTests, AckermannTest) control_sequence.reset(timesteps); // populates with zeros state.reset(batches, timesteps); // populates with zeros auto node = std::make_shared("my_node"); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); std::unique_ptr model = std::make_unique(¶m_handler, node->get_name()); @@ -185,7 +186,8 @@ TEST(MotionModelTests, AckermannReversingTest) control_sequence2.reset(timesteps); // populates with zeros state.reset(batches, timesteps); // populates with zeros auto node = std::make_shared("my_node"); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); std::unique_ptr model = std::make_unique(¶m_handler, node->get_name()); diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index b9daf8af5f3..7533913d828 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -38,7 +38,8 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle) auto node = std::make_shared("node"); node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false)); - ParametersHandler handler(node); + std::string name = "test"; +ParametersHandler handler(node, name); generator.initialize(settings, false, "test_name", &handler); generator.reset(settings, false); @@ -50,7 +51,8 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) // Tests shuts down internal thread cleanly auto node = std::make_shared("node"); node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true)); - ParametersHandler handler(node); + std::string name = "test"; +ParametersHandler handler(node, name); NoiseGenerator generator; mppi::models::OptimizerSettings settings; settings.batch_size = 100; @@ -138,7 +140,8 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMainNoRegenerate) // This time with no regeneration of noises auto node = std::make_shared("node"); node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false)); - ParametersHandler handler(node); + std::string name = "test"; +ParametersHandler handler(node, name); NoiseGenerator generator; mppi::models::OptimizerSettings settings; settings.batch_size = 100; diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index 09551c387ae..e4339944ad3 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -66,7 +66,8 @@ TEST_P(OptimizerSuite, OptimizerTest) { printInfo(optimizer_settings, path_settings, critics); auto node = getDummyNode(optimizer_settings, critics); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); auto optimizer = getDummyOptimizer(node, costmap_ros, parameters_handler.get()); // evalControl args diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 26eb2fff83a..1c3732399e6 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -229,7 +229,8 @@ TEST(OptimizerTests, BasicInitializedFunctions) node->declare_parameter("mppic.ay_min", rclcpp::ParameterValue(3.0)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -266,7 +267,8 @@ TEST(OptimizerTests, TestOptimizerMotionModels) node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -298,7 +300,8 @@ TEST(OptimizerTests, setOffsetTests) node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -321,7 +324,8 @@ TEST(OptimizerTests, resetTests) node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -341,7 +345,8 @@ TEST(OptimizerTests, FallbackTests) node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -365,7 +370,8 @@ TEST(OptimizerTests, PrepareTests) node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -393,7 +399,8 @@ TEST(OptimizerTests, shiftControlSequenceTests) node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -436,7 +443,8 @@ TEST(OptimizerTests, SpeedLimitTests) node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -476,7 +484,8 @@ TEST(OptimizerTests, applyControlSequenceConstraintsTests) node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -537,7 +546,8 @@ TEST(OptimizerTests, updateStateVelocitiesTests) node->declare_parameter("mppic.az_max", rclcpp::ParameterValue(3.5)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -562,7 +572,8 @@ TEST(OptimizerTests, getControlFromSequenceAsTwistTests) node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -597,7 +608,8 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); @@ -663,7 +675,8 @@ TEST(OptimizerTests, TestGetters) node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string test = "test"; + ParametersHandler param_handler(node, test); rclcpp_lifecycle::State lstate; costmap_ros->on_configure(lstate); optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler); diff --git a/nav2_mppi_controller/test/parameter_handler_test.cpp b/nav2_mppi_controller/test/parameter_handler_test.cpp index 56917bbb3c0..74c26b63c49 100644 --- a/nav2_mppi_controller/test/parameter_handler_test.cpp +++ b/nav2_mppi_controller/test/parameter_handler_test.cpp @@ -29,8 +29,8 @@ class ParametersHandlerWrapper : public ParametersHandler ParametersHandlerWrapper() = default; explicit ParametersHandlerWrapper( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent) - : ParametersHandler(parent) {} + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name) + : ParametersHandler(parent, name) {} template auto asWrapped(rclcpp::Parameter parameter) @@ -44,17 +44,19 @@ using namespace mppi; // NOLINT TEST(ParameterHandlerTest, asTypeConversionTest) { ParametersHandlerWrapper a; - rclcpp::Parameter int_p("int_parameter", rclcpp::ParameterValue(1)); - rclcpp::Parameter double_p("double_parameter", rclcpp::ParameterValue(10.0)); - rclcpp::Parameter bool_p("bool_parameter", rclcpp::ParameterValue(false)); - rclcpp::Parameter string_p("string_parameter", rclcpp::ParameterValue(std::string("hello"))); + rclcpp::Parameter int_p("test.int_parameter", rclcpp::ParameterValue(1)); + rclcpp::Parameter double_p("test.double_parameter", rclcpp::ParameterValue(10.0)); + rclcpp::Parameter bool_p("test.bool_parameter", rclcpp::ParameterValue(false)); + rclcpp::Parameter string_p("test.string_parameter", rclcpp::ParameterValue(std::string("hello"))); - rclcpp::Parameter intv_p("intv_parameter", rclcpp::ParameterValue(std::vector{1})); + rclcpp::Parameter intv_p("test.intv_parameter", rclcpp::ParameterValue(std::vector{1})); rclcpp::Parameter doublev_p( - "doublev_parameter", rclcpp::ParameterValue(std::vector{10.0})); - rclcpp::Parameter boolv_p("boolv_parameter", rclcpp::ParameterValue(std::vector{false})); + "test.doublev_parameter", rclcpp::ParameterValue(std::vector{10.0})); + rclcpp::Parameter boolv_p("test.boolv_parameter", + rclcpp::ParameterValue(std::vector{false})); rclcpp::Parameter stringv_p( - "stringv_parameter", rclcpp::ParameterValue(std::vector{std::string("hello")})); + "test.stringv_parameter", + rclcpp::ParameterValue(std::vector{std::string("hello")})); EXPECT_EQ(a.asWrapped(int_p), 1); EXPECT_EQ(a.asWrapped(double_p), 10.0); @@ -89,15 +91,15 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest) dynamic_triggered = true; }; - rclcpp::Parameter random_param("blah_blah", rclcpp::ParameterValue(true)); - rclcpp::Parameter random_param2("use_sim_time", rclcpp::ParameterValue(true)); + rclcpp::Parameter random_param(".blah_blah", rclcpp::ParameterValue(true)); + rclcpp::Parameter random_param2(".use_sim_time", rclcpp::ParameterValue(true)); bool val = false; ParametersHandlerWrapper a; a.addPreCallback(preCb); a.addPostCallback(postCb); - a.addParamCallback("use_sim_time", dynamicCb); - a.setParamCallback(val, "blah_blah"); + a.addParamCallback(".use_sim_time", dynamicCb); + a.setParamCallback(val, ".blah_blah"); // Dynamic callback should not trigger, wrong parameter, but val should be updated a.dynamicParamsCallback(std::vector{random_param}); @@ -117,21 +119,22 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest) TEST(ParameterHandlerTest, GetSystemParamsTest) { auto node = std::make_shared("my_node"); + std::string name = "test"; node->declare_parameter("param1", rclcpp::ParameterValue(true)); - node->declare_parameter("ns.param2", rclcpp::ParameterValue(7)); + node->declare_parameter(name + ".param2", rclcpp::ParameterValue(7)); // Get parameters in global namespace and in subnamespaces - ParametersHandler handler(node); + ParametersHandler handler(node, name); auto getParameter = handler.getParamGetter(""); bool p1 = false; int p2 = 0; getParameter(p1, "param1", false); - getParameter(p2, "ns.param2", 0); + getParameter(p2, name + ".param2", 0); EXPECT_EQ(p1, true); EXPECT_EQ(p2, 7); // Get parameters in subnamespaces using name semantics of getter - auto getParameter2 = handler.getParamGetter("ns"); + auto getParameter2 = handler.getParamGetter(name); p2 = 0; getParameter2(p2, "param2", 0); EXPECT_EQ(p2, 7); @@ -141,16 +144,17 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) { auto node = std::make_shared("my_node"); - node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); - node->declare_parameter("static_int", rclcpp::ParameterValue(7)); - ParametersHandlerWrapper handler(node); + node->declare_parameter("test.dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("test.static_int", rclcpp::ParameterValue(7)); + std::string name = "test"; + ParametersHandlerWrapper handler(node, name); handler.start(); // Get parameters and check they have initial values auto getParameter = handler.getParamGetter(""); int p1 = 0, p2 = 0; - getParameter(p1, "dynamic_int", 0, ParameterType::Dynamic); - getParameter(p2, "static_int", 0, ParameterType::Static); + getParameter(p1, "test.dynamic_int", 0, ParameterType::Dynamic); + getParameter(p2, "test.static_int", 0, ParameterType::Static); EXPECT_EQ(p1, 7); EXPECT_EQ(p2, 7); @@ -164,8 +168,8 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) rec_param->set_parameters_atomically( { rclcpp::Parameter("my_node.verbose", true), - rclcpp::Parameter("dynamic_int", 10), - rclcpp::Parameter("static_int", 10) + rclcpp::Parameter("test.dynamic_int", 10), + rclcpp::Parameter("test.static_int", 10) }); auto rc = rclcpp::spin_until_future_complete( @@ -178,7 +182,7 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) EXPECT_FALSE(result.reason.empty()); EXPECT_EQ( result.reason, std::string("Rejected change to static parameter: ") + - "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}"); + "{\"name\": \"test.static_int\", \"type\": \"integer\", \"value\": \"10\"}"); // Now, only param1 should change, param 2 should be the same EXPECT_EQ(p1, 10); @@ -188,16 +192,17 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest) { auto node = std::make_shared("my_node"); - node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); - node->declare_parameter("static_int", rclcpp::ParameterValue(7)); - ParametersHandlerWrapper handler(node); + node->declare_parameter("test.dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("test.static_int", rclcpp::ParameterValue(7)); + std::string name = "test"; + ParametersHandlerWrapper handler(node, name); handler.start(); // Get parameters and check they have initial values auto getParameter = handler.getParamGetter(""); int p1 = 0, p2 = 0; - getParameter(p1, "dynamic_int", 0, ParameterType::Dynamic); - getParameter(p2, "static_int", 0, ParameterType::Static); + getParameter(p1, "test.dynamic_int", 0, ParameterType::Dynamic); + getParameter(p2, "test.static_int", 0, ParameterType::Static); EXPECT_EQ(p1, 7); EXPECT_EQ(p2, 7); @@ -211,8 +216,8 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest) rec_param->set_parameters_atomically( { // Don't set default param rclcpp::Parameter("my_node.verbose", false), - rclcpp::Parameter("dynamic_int", 10), - rclcpp::Parameter("static_int", 10) + rclcpp::Parameter("test.dynamic_int", 10), + rclcpp::Parameter("test.static_int", 10) }); auto rc = rclcpp::spin_until_future_complete( @@ -225,7 +230,7 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest) EXPECT_FALSE(result.reason.empty()); EXPECT_EQ( result.reason, std::string("Rejected change to static parameter: ") + - "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}"); + "{\"name\": \"test.static_int\", \"type\": \"integer\", \"value\": \"10\"}"); // Now, only param1 should change, param 2 should be the same EXPECT_EQ(p1, 10); @@ -236,9 +241,10 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest) { auto node = std::make_shared("my_node"); - node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); - node->declare_parameter("static_int", rclcpp::ParameterValue(7)); - ParametersHandlerWrapper handler(node); + node->declare_parameter("test.dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("test.static_int", rclcpp::ParameterValue(7)); + std::string name = "test"; + ParametersHandlerWrapper handler(node, name); handler.start(); // Set verbose true to get more information about bad parameter usage @@ -266,9 +272,9 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest) // Try to set some parameters that have not been declared via the service client result_future = rec_param->set_parameters_atomically( { - rclcpp::Parameter("static_int", 10), - rclcpp::Parameter("not_declared", true), - rclcpp::Parameter("not_declared2", true), + rclcpp::Parameter("test.static_int", 10), + rclcpp::Parameter("test.not_declared", true), + rclcpp::Parameter("test.not_declared2", true), }); rc = rclcpp::spin_until_future_complete( diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 96bf9f7a627..87e6e9ea58d 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -104,7 +104,8 @@ TEST(PathHandlerTests, TestBounds) auto results = costmap_ros->set_parameters_atomically( {rclcpp::Parameter("global_frame", "odom"), rclcpp::Parameter("robot_base_frame", "base_link")}); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State state; costmap_ros->on_configure(state); @@ -152,7 +153,8 @@ TEST(PathHandlerTests, TestTransforms) node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); auto costmap_ros = std::make_shared( "dummy_costmap", "", true); - ParametersHandler param_handler(node); + std::string name = "test"; + ParametersHandler param_handler(node, name); rclcpp_lifecycle::State state; costmap_ros->on_configure(state); diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index c867ec678b9..fea66927707 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -26,7 +26,8 @@ using namespace mppi; // NOLINT TEST(TrajectoryVisualizerTests, StateTransition) { auto node = std::make_shared("my_node"); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "map", parameters_handler.get()); @@ -38,7 +39,8 @@ TEST(TrajectoryVisualizerTests, StateTransition) TEST(TrajectoryVisualizerTests, VisPathRepub) { auto node = std::make_shared("my_node"); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); nav_msgs::msg::Path received_path; nav_msgs::msg::Path pub_path; pub_path.header.frame_id = "fake_frame"; @@ -61,7 +63,8 @@ TEST(TrajectoryVisualizerTests, VisPathRepub) TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) { auto node = std::make_shared("my_node"); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( @@ -123,7 +126,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) { auto node = std::make_shared("my_node"); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( @@ -150,7 +154,8 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) TEST(TrajectoryVisualizerTests, VisOptimalPath) { auto node = std::make_shared("my_node"); - auto parameters_handler = std::make_unique(node); + std::string name = "test"; + auto parameters_handler = std::make_unique(node, name); builtin_interfaces::msg::Time cmd_stamp; cmd_stamp.sec = 5; cmd_stamp.nanosec = 10; diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 9c5903fccd8..5734db444e4 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -633,6 +633,83 @@ TEST(UtilsTests, toTrajectoryMsgTest) EXPECT_EQ(trajectory_msg->points[4].time_from_start, rclcpp::Duration(4, 0)); } +TEST(UtilsTests, getLastPathPoseTest) +{ + nav_msgs::msg::Path path; + path.poses.resize(10); + path.poses[9].pose.position.x = 5.0; + path.poses[9].pose.position.y = 50.0; + path.poses[9].pose.orientation.x = 0.0; + path.poses[9].pose.orientation.y = 0.0; + path.poses[9].pose.orientation.z = 1.0; + path.poses[9].pose.orientation.w = 0.0; + + models::Path path_t = toTensor(path); + geometry_msgs::msg::Pose last_path_pose = utils::getLastPathPose(path_t); + + EXPECT_EQ(last_path_pose.position.x, 5); + EXPECT_EQ(last_path_pose.position.y, 50); + EXPECT_NEAR(last_path_pose.orientation.x, 0.0, 1e-3); + EXPECT_NEAR(last_path_pose.orientation.y, 0.0, 1e-3); + EXPECT_NEAR(last_path_pose.orientation.z, 1.0, 1e-3); + EXPECT_NEAR(last_path_pose.orientation.w, 0.0, 1e-3); +} + +TEST(UtilsTests, getCriticGoalTest) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 10.0; + pose.position.y = 1.0; + + nav_msgs::msg::Path path; + path.poses.resize(10); + path.poses[9].pose.position.x = 5.0; + path.poses[9].pose.position.y = 50.0; + path.poses[9].pose.orientation.x = 0.0; + path.poses[9].pose.orientation.y = 0.0; + path.poses[9].pose.orientation.z = 1.0; + path.poses[9].pose.orientation.w = 0.0; + + geometry_msgs::msg::Pose goal; + goal.position.x = 6.0; + goal.position.y = 60.0; + goal.orientation.x = 0.0; + goal.orientation.y = 0.0; + goal.orientation.z = 0.0; + goal.orientation.w = 1.0; + + // Create CriticData with state and goal initialized + models::State state; + state.pose.pose = pose; + models::Trajectories generated_trajectories; + models::Path path_t = toTensor(path); + Eigen::ArrayXf costs; + float model_dt; + CriticData data = { + state, generated_trajectories, path_t, goal, + costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; + + bool enforce_path_inversion = true; + geometry_msgs::msg::Pose target_goal = utils::getCriticGoal(data, enforce_path_inversion); + + EXPECT_EQ(target_goal.position.x, 5); + EXPECT_EQ(target_goal.position.y, 50); + EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.z, 1.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.w, 0.0, 1e-3); + + enforce_path_inversion = false; + target_goal = utils::getCriticGoal(data, enforce_path_inversion); + + EXPECT_EQ(target_goal.position.x, 6); + EXPECT_EQ(target_goal.position.y, 60); + EXPECT_NEAR(target_goal.orientation.x, 0.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.y, 0.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.z, 0.0, 1e-3); + EXPECT_NEAR(target_goal.orientation.w, 1.0, 1e-3); +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_msgs/action/DockRobot.action b/nav2_msgs/action/DockRobot.action index e3e2fbf78cb..ae70c33413a 100644 --- a/nav2_msgs/action/DockRobot.action +++ b/nav2_msgs/action/DockRobot.action @@ -21,6 +21,7 @@ uint16 FAILED_TO_STAGE=903 uint16 FAILED_TO_DETECT_DOCK=904 uint16 FAILED_TO_CONTROL=905 uint16 FAILED_TO_CHARGE=906 +uint16 TIMEOUT=907 uint16 UNKNOWN=999 bool success True # docking success status diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 8bbaa361e73..1d7a1951578 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -11,6 +11,7 @@ uint16 NONE=0 uint16 UNKNOWN=9100 uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=9101 uint16 TF_ERROR=9102 +uint16 TIMEOUT=9103 uint16 error_code string error_msg diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 6da35cee510..14272c5663a 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -10,6 +10,7 @@ uint16 NONE=0 uint16 UNKNOWN=9000 uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=9001 uint16 TF_ERROR=9002 +uint16 TIMEOUT=9003 uint16 error_code string error_msg diff --git a/nav2_msgs/action/UndockRobot.action b/nav2_msgs/action/UndockRobot.action index 61747830607..5bb882a2e41 100644 --- a/nav2_msgs/action/UndockRobot.action +++ b/nav2_msgs/action/UndockRobot.action @@ -16,6 +16,7 @@ float32 max_undocking_time 30.0 # Maximum time to undock uint16 NONE=0 uint16 DOCK_NOT_VALID=902 uint16 FAILED_TO_CONTROL=905 +uint16 TIMEOUT=907 uint16 UNKNOWN=999 bool success True # docking success status diff --git a/nav2_msgs/action/Wait.action b/nav2_msgs/action/Wait.action index 1496e3371ed..c6b20d4f93d 100644 --- a/nav2_msgs/action/Wait.action +++ b/nav2_msgs/action/Wait.action @@ -8,6 +8,7 @@ string error_msg uint16 NONE=0 uint16 UNKNOWN=740 +uint16 TIMEOUT=741 --- #feedback definition builtin_interfaces/Duration time_left diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index c9e806f194e..12a085fbf88 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.3.1 + 1.4.0 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 9ad576e5ab2..94e70c2f61c 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.3.1 + 1.4.0 Nav2 NavFn planner Steve Macenski Carlos Orduno diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index e01261434d1..e42c26202a5 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -525,19 +525,21 @@ NavfnPlanner::dynamicParametersCallback(std::vector parameter { rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == name_ + ".tolerance") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if(param_name.find(name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + ".tolerance") { tolerance_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == name_ + ".use_astar") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + ".use_astar") { use_astar_ = parameter.as_bool(); - } else if (name == name_ + ".allow_unknown") { + } else if (param_name == name_ + ".allow_unknown") { allow_unknown_ = parameter.as_bool(); - } else if (name == name_ + ".use_final_approach_orientation") { + } else if (param_name == name_ + ".use_final_approach_orientation") { use_final_approach_orientation_ = parameter.as_bool(); } } diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 26de6cfa13a..68d97a5c0d0 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.3.1 + 1.4.0 Nav2 planner server package Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 7d29119f31b..e36602a02fd 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -732,11 +732,14 @@ PlannerServer::dynamicParametersCallback(std::vector paramete std::lock_guard lock(dynamic_params_lock_); rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == "expected_planner_frequency") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "expected_planner_frequency") { if (parameter.as_double() > 0) { max_planner_duration_ = 1 / parameter.as_double(); } else { diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index f49f9b31420..2caec97ff32 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -91,6 +91,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `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) | +| `min_distance_to_obstacle` | The shortest distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. It is limited to maximum distance of lookahead distance selected. | Example fully-described XML with default parameter values: diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 765a0e85dc0..e3090df2cef 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -42,6 +42,7 @@ struct Parameters double min_approach_linear_velocity; double approach_velocity_scaling_dist; double max_allowed_time_to_collision_up_to_carrot; + double min_distance_to_obstacle; bool use_regulated_linear_velocity_scaling; bool use_cost_regulated_linear_velocity_scaling; double cost_scaling_dist; @@ -61,6 +62,7 @@ struct Parameters bool interpolate_curvature_after_goal; bool use_collision_detection; double transform_tolerance; + bool stateful; }; /** diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index df3e5ea587e..2d766ea967f 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -220,6 +220,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool cancelling_ = false; bool finished_cancelling_ = false; bool is_rotating_to_heading_ = false; + bool has_reached_xy_tolerance_ = false; std::shared_ptr> global_path_pub_; std::shared_ptr> diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index ff4105b62e7..41b2bade1ee 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.3.1 + 1.4.0 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp index ac654fcab76..89a5a2a1bff 100644 --- a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp @@ -92,8 +92,16 @@ bool CollisionChecker::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); // only forward simulate within time requested + double max_allowed_time_to_collision_check = params_->max_allowed_time_to_collision_up_to_carrot; + if (params_->min_distance_to_obstacle > 0.0) { + max_allowed_time_to_collision_check = std::max( + params_->max_allowed_time_to_collision_up_to_carrot, + params_->min_distance_to_obstacle / std::max(std::abs(linear_vel), + params_->min_approach_linear_velocity) + ); + } int i = 1; - while (i * projection_time < params_->max_allowed_time_to_collision_up_to_carrot) { + while (i * projection_time < max_allowed_time_to_collision_check) { i++; // apply velocity at curr_pose over distance diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 984d3f31af6..5a346c23c1d 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -61,6 +61,9 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_distance_to_obstacle", + rclcpp::ParameterValue(-1.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( @@ -101,6 +104,8 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true)); node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); params_.base_desired_linear_vel = params_.desired_linear_vel; @@ -129,6 +134,9 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", params_.max_allowed_time_to_collision_up_to_carrot); + node->get_parameter( + plugin_name_ + ".min_distance_to_obstacle", + params_.min_distance_to_obstacle); node->get_parameter( plugin_name_ + ".use_regulated_linear_velocity_scaling", params_.use_regulated_linear_velocity_scaling); @@ -181,6 +189,7 @@ ParameterHandler::ParameterHandler( node->get_parameter( plugin_name_ + ".use_collision_detection", params_.use_collision_detection); + node->get_parameter(plugin_name_ + ".stateful", params_.stateful); if (params_.inflation_cost_scaling_factor <= 0.0) { RCLCPP_WARN( @@ -209,11 +218,15 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpda rcl_interfaces::msg::SetParametersResult result; result.successful = true; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".inflation_cost_scaling_factor" && parameter.as_double() <= 0.0) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".inflation_cost_scaling_factor" && + parameter.as_double() <= 0.0) + { RCLCPP_WARN( logger_, "The value inflation_cost_scaling_factor is incorrectly set, " "it should be >0. Ignoring parameter update."); @@ -222,11 +235,11 @@ rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpda RCLCPP_WARN( logger_, "The value of parameter '%s' is incorrectly set to %f, " "it should be >=0. Ignoring parameter update.", - name.c_str(), parameter.as_double()); + param_name.c_str(), parameter.as_double()); result.successful = false; } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".allow_reversing") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".allow_reversing") { if (params_.use_rotate_to_heading && parameter.as_bool()) { RCLCPP_WARN( logger_, "Both use_rotate_to_heading and allow_reversing " @@ -245,73 +258,71 @@ ParameterHandler::updateParametersCallback( rcl_interfaces::msg::SetParametersResult result; std::lock_guard lock_reinit(mutex_); - for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + for (const auto & parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".inflation_cost_scaling_factor") { - if (parameter.as_double() <= 0.0) { - RCLCPP_WARN( - logger_, "The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Ignoring parameter update."); - continue; - } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") { params_.inflation_cost_scaling_factor = parameter.as_double(); - } else if (name == plugin_name_ + ".desired_linear_vel") { + } else if (param_name == plugin_name_ + ".desired_linear_vel") { params_.desired_linear_vel = parameter.as_double(); params_.base_desired_linear_vel = parameter.as_double(); - } else if (name == plugin_name_ + ".lookahead_dist") { + } else if (param_name == plugin_name_ + ".lookahead_dist") { params_.lookahead_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".max_lookahead_dist") { + } else if (param_name == plugin_name_ + ".max_lookahead_dist") { params_.max_lookahead_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".min_lookahead_dist") { + } else if (param_name == plugin_name_ + ".min_lookahead_dist") { params_.min_lookahead_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".lookahead_time") { + } else if (param_name == plugin_name_ + ".lookahead_time") { params_.lookahead_time = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { + } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") { params_.rotate_to_heading_angular_vel = parameter.as_double(); - } else if (name == plugin_name_ + ".min_approach_linear_velocity") { + } else if (param_name == plugin_name_ + ".min_approach_linear_velocity") { params_.min_approach_linear_velocity = parameter.as_double(); - } else if (name == plugin_name_ + ".curvature_lookahead_dist") { + } else if (param_name == plugin_name_ + ".curvature_lookahead_dist") { params_.curvature_lookahead_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { + } else if (param_name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double(); - } else if (name == plugin_name_ + ".cost_scaling_dist") { + } else if (param_name == plugin_name_ + ".min_distance_to_obstacle") { + params_.min_distance_to_obstacle = parameter.as_double(); + } else if (param_name == plugin_name_ + ".cost_scaling_dist") { params_.cost_scaling_dist = parameter.as_double(); - } else if (name == plugin_name_ + ".cost_scaling_gain") { + } else if (param_name == plugin_name_ + ".cost_scaling_gain") { params_.cost_scaling_gain = parameter.as_double(); - } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") { + } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_radius") { params_.regulated_linear_scaling_min_radius = parameter.as_double(); - } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") { + } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_speed") { params_.regulated_linear_scaling_min_speed = parameter.as_double(); - } else if (name == plugin_name_ + ".max_angular_accel") { + } else if (param_name == plugin_name_ + ".max_angular_accel") { params_.max_angular_accel = parameter.as_double(); - } else if (name == plugin_name_ + ".cancel_deceleration") { + } else if (param_name == plugin_name_ + ".cancel_deceleration") { params_.cancel_deceleration = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { + } else if (param_name == plugin_name_ + ".rotate_to_heading_min_angle") { params_.rotate_to_heading_min_angle = parameter.as_double(); - } else if (name == plugin_name_ + ".transform_tolerance") { + } else if (param_name == plugin_name_ + ".transform_tolerance") { params_.transform_tolerance = parameter.as_double(); - } else if (name == plugin_name_ + ".max_robot_pose_search_dist") { + } else if (param_name == plugin_name_ + ".max_robot_pose_search_dist") { params_.max_robot_pose_search_dist = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { params_.use_velocity_scaled_lookahead_dist = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { + } else if (param_name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { params_.use_regulated_linear_velocity_scaling = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_fixed_curvature_lookahead") { + } else if (param_name == plugin_name_ + ".use_fixed_curvature_lookahead") { params_.use_fixed_curvature_lookahead = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { + } else if (param_name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_collision_detection") { + } else if (param_name == plugin_name_ + ".use_collision_detection") { params_.use_collision_detection = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_rotate_to_heading") { + } else if (param_name == plugin_name_ + ".stateful") { + params_.stateful = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".use_rotate_to_heading") { params_.use_rotate_to_heading = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_cancel_deceleration") { + } else if (param_name == plugin_name_ + ".use_cancel_deceleration") { params_.use_cancel_deceleration = parameter.as_bool(); - } else if (name == plugin_name_ + ".allow_reversing") { + } else if (param_name == plugin_name_ + ".allow_reversing") { if (params_.use_rotate_to_heading && parameter.as_bool()) { RCLCPP_WARN( logger_, "Both use_rotate_to_heading and allow_reversing " @@ -319,7 +330,7 @@ ParameterHandler::updateParametersCallback( continue; } params_.allow_reversing = parameter.as_bool(); - } else if (name == plugin_name_ + ".interpolate_curvature_after_goal") { + } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") { params_.interpolate_curvature_after_goal = parameter.as_bool(); } } diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 49617ea558e..2f43f427f2f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -320,8 +320,21 @@ bool RegulatedPurePursuitController::shouldRotateToGoalHeading( const geometry_msgs::msg::PoseStamped & carrot_pose) { // Whether we should rotate robot to goal heading - double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - return params_->use_rotate_to_heading && dist_to_goal < goal_dist_tol_; + if (!params_->use_rotate_to_heading) { + return false; + } + + double dist_to_goal = std::hypot( + carrot_pose.pose.position.x, carrot_pose.pose.position.y); + + if (params_->stateful) { + if (!has_reached_xy_tolerance_ && dist_to_goal < goal_dist_tol_) { + has_reached_xy_tolerance_ = true; + } + return has_reached_xy_tolerance_; + } + + return dist_to_goal < goal_dist_tol_; } void RegulatedPurePursuitController::rotateToHeading( @@ -466,6 +479,7 @@ void RegulatedPurePursuitController::applyConstraints( void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) { + has_reached_xy_tolerance_ = false; path_handler_->setPlan(path); } @@ -493,6 +507,7 @@ void RegulatedPurePursuitController::reset() { cancelling_ = false; finished_cancelling_ = false; + has_reached_xy_tolerance_ = false; } double RegulatedPurePursuitController::findVelocitySignChange( diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index a46f0e741f1..c6c0f32ca40 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -498,8 +498,14 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) TEST(RegulatedPurePursuitTest, rotateTests) { + // -------------------------- + // Non-Stateful Configuration + // -------------------------- auto ctrl = std::make_shared(); auto node = std::make_shared("testRPP"); + nav2_util::declare_parameter_if_not_declared( + node, "PathFollower.stateful", rclcpp::ParameterValue(false)); + std::string name = "PathFollower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); @@ -565,6 +571,27 @@ TEST(RegulatedPurePursuitTest, rotateTests) curr_speed.angular.z = 1.0; ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); EXPECT_NEAR(ang_v, 0.84, 0.01); + + // ----------------------- + // Stateful Configuration + // ----------------------- + node->set_parameter( + rclcpp::Parameter("PathFollower.stateful", true)); + + ctrl->configure(node, name, tf, costmap); + + // Start just outside tolerance + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.26; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false); + + // Enter tolerance (should set internal flag) + carrot.pose.position.y = 0.24; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + // Move outside tolerance again - still expect true (due to persistent state) + carrot.pose.position.y = 0.26; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); } TEST(RegulatedPurePursuitTest, applyConstraints) @@ -693,6 +720,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) rclcpp::Parameter("test.rotate_to_heading_angular_vel", 18.0), rclcpp::Parameter("test.min_approach_linear_velocity", 1.0), rclcpp::Parameter("test.max_allowed_time_to_collision_up_to_carrot", 2.0), + rclcpp::Parameter("test.min_distance_to_obstacle", 2.0), rclcpp::Parameter("test.cost_scaling_dist", 2.0), rclcpp::Parameter("test.cost_scaling_gain", 4.0), rclcpp::Parameter("test.regulated_linear_scaling_min_radius", 10.0), @@ -705,7 +733,8 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) rclcpp::Parameter("test.use_cost_regulated_linear_velocity_scaling", false), rclcpp::Parameter("test.inflation_cost_scaling_factor", 1.0), rclcpp::Parameter("test.allow_reversing", false), - rclcpp::Parameter("test.use_rotate_to_heading", false)}); + rclcpp::Parameter("test.use_rotate_to_heading", false), + rclcpp::Parameter("test.stateful", false)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -721,6 +750,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) EXPECT_EQ( node->get_parameter( "test.max_allowed_time_to_collision_up_to_carrot").as_double(), 2.0); + EXPECT_EQ(node->get_parameter("test.min_distance_to_obstacle").as_double(), 2.0); EXPECT_EQ(node->get_parameter("test.cost_scaling_dist").as_double(), 2.0); EXPECT_EQ(node->get_parameter("test.cost_scaling_gain").as_double(), 4.0); EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_radius").as_double(), 10.0); @@ -736,6 +766,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) "test.use_cost_regulated_linear_velocity_scaling").as_bool(), false); EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false); EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.stateful").as_bool(), false); // Should fail auto results2 = rec_param->set_parameters_atomically( diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 9e450f75736..686c6915731 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.3.1 + 1.4.0 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index e6e435adbf5..b964cf0166b 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -411,29 +411,32 @@ RotationShimController::dynamicParametersCallback(std::vector std::lock_guard lock_reinit(mutex_); for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".angular_dist_threshold") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".angular_dist_threshold") { angular_dist_threshold_ = parameter.as_double(); - } else if (name == plugin_name_ + ".forward_sampling_distance") { + } else if (param_name == plugin_name_ + ".forward_sampling_distance") { forward_sampling_distance_ = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { + } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") { rotate_to_heading_angular_vel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_angular_accel") { + } else if (param_name == plugin_name_ + ".max_angular_accel") { max_angular_accel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".simulate_ahead_time") { + } else if (param_name == plugin_name_ + ".simulate_ahead_time") { simulate_ahead_time_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".rotate_to_goal_heading") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".rotate_to_goal_heading") { rotate_to_goal_heading_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".rotate_to_heading_once") { + } else if (param_name == plugin_name_ + ".rotate_to_heading_once") { rotate_to_heading_once_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".closed_loop") { + } else if (param_name == plugin_name_ + ".closed_loop") { closed_loop_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_path_orientations") { + } else if (param_name == plugin_name_ + ".use_path_orientations") { use_path_orientations_ = parameter.as_bool(); } } diff --git a/nav2_route/README.md b/nav2_route/README.md index a434276bff8..c0eb510b1db 100644 --- a/nav2_route/README.md +++ b/nav2_route/README.md @@ -114,6 +114,8 @@ route_server: path_density: 0.05 # Density of points for generating the dense nav_msgs/Path from route (m) max_iterations: 0 # Maximum number of search iterations, if 0, uses maximum possible max_planning_time: 2.0 # Maximum planning time (seconds) + smooth_corners: true # Whether to smooth corners formed by adjacent edges or not + smoothing_radius: 1.0 # Radius of corner to fit into the corner costmap_topic: 'global_costmap/costmap_raw' # Costmap topic when enable_nn_search is enabled. May also be used by the collision monitor operation and/or the costmap edge scorer if using the same topic to share resources. graph_file_loader: "GeoJsonGraphFileLoader" # Name of default file loader diff --git a/nav2_route/include/nav2_route/corner_smoothing.hpp b/nav2_route/include/nav2_route/corner_smoothing.hpp new file mode 100644 index 00000000000..f14d2f43a41 --- /dev/null +++ b/nav2_route/include/nav2_route/corner_smoothing.hpp @@ -0,0 +1,176 @@ +// Copyright (c) 2025, Polymath Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_ROUTE__CORNER_SMOOTHING_HPP_ +#define NAV2_ROUTE__CORNER_SMOOTHING_HPP_ + +#include +#include +#include + +#include "nav2_route/types.hpp" +#include "nav2_route/utils.hpp" + +namespace nav2_route +{ +/** + * @class nav2_route::CornerArc + * @brief A class used to smooth corners defined by the edges and nodes + * of the route graph. Used with path converter to output a corner smoothed plan + */ +class CornerArc +{ +public: + /** + * @brief A constructor for nav2_route::CornerArc + * @param start start coordinate of corner to be smoothed + * @param corner corner coordinate of corner to be smoothed + * @param end end coordinate of corner to be smoothed + * @param minimum_radius smoothing radius to fit to the corner + */ + CornerArc( + const Coordinates & start, const Coordinates & corner, const Coordinates & end, + float minimum_radius) + { + start_edge_length_ = hypotf(corner.x - start.x, corner.y - start.y); + end_edge_length_ = hypotf(end.x - corner.x, end.y - corner.y); + + // invalid scenario would cause equations to blow up + if (start_edge_length_ == 0.0 || end_edge_length_ == 0.0) {return;} + + float angle = getAngleBetweenEdges(start, corner, end); + + // cannot smooth a 0 (back and forth) or 180 (straight line) angle + if (std::abs(angle) < 1E-6 || std::abs(angle - M_PI) < 1E-6) {return;} + + float tangent_length = minimum_radius / (std::tan(std::fabs(angle) / 2)); + + if (tangent_length < start_edge_length_ && tangent_length < end_edge_length_) { + std::vector start_edge_unit_tangent = + {(start.x - corner.x) / start_edge_length_, (start.y - corner.y) / start_edge_length_}; + std::vector end_edge_unit_tangent = + {(end.x - corner.x) / end_edge_length_, (end.y - corner.y) / end_edge_length_}; + + float bisector_x = start_edge_unit_tangent[0] + end_edge_unit_tangent[0]; + float bisector_y = start_edge_unit_tangent[1] + end_edge_unit_tangent[1]; + float bisector_magnitude = std::sqrt(bisector_x * bisector_x + bisector_y * bisector_y); + + std::vector unit_bisector = + {bisector_x / bisector_magnitude, bisector_y / bisector_magnitude}; + + start_coordinate_.x = corner.x + start_edge_unit_tangent[0] * tangent_length; + start_coordinate_.y = corner.y + start_edge_unit_tangent[1] * tangent_length; + + end_coordinate_.x = corner.x + end_edge_unit_tangent[0] * tangent_length; + end_coordinate_.y = corner.y + end_edge_unit_tangent[1] * tangent_length; + + float bisector_length = minimum_radius / std::sin(angle / 2); + + circle_center_coordinate_.x = corner.x + unit_bisector[0] * bisector_length; + circle_center_coordinate_.y = corner.y + unit_bisector[1] * bisector_length; + + valid_corner_ = true; + } + } + + /** + * @brief A destructor for nav2_route::CornerArc + */ + ~CornerArc() = default; + + /** + * @brief interpolates the arc for a path of certain density + * @param max_angle_resolution Resolution to interpolate path to + * @param poses Pose output + */ + void interpolateArc( + const float & max_angle_resolution, + std::vector & poses) + { + std::vector r_start{start_coordinate_.x - circle_center_coordinate_.x, + start_coordinate_.y - circle_center_coordinate_.y}; + std::vector r_end{end_coordinate_.x - circle_center_coordinate_.x, + end_coordinate_.y - circle_center_coordinate_.y}; + float cross = r_start[0] * r_end[1] - r_start[1] * r_end[0]; + float dot = r_start[0] * r_end[0] + r_start[1] * r_end[1]; + float signed_angle = std::atan2(cross, dot); + // lower limit for N is 1 to protect against divide by 0 + int N = std::max(1, static_cast(std::ceil(std::abs(signed_angle) / max_angle_resolution))); + float angle_resolution = signed_angle / N; + + float x, y; + for (int i = 0; i <= N; i++) { + float angle = i * angle_resolution; + x = circle_center_coordinate_.x + + (r_start[0] * std::cos(angle) - r_start[1] * std::sin(angle)); + y = circle_center_coordinate_.y + + (r_start[0] * std::sin(angle) + r_start[1] * std::cos(angle)); + poses.push_back(utils::toMsg(x, y)); + } + } + + /** + * @brief return if a valid corner arc (one that doesn't overrun the edge lengths) is generated + * @return if the a corner was able to be generated + */ + bool isCornerValid() const {return valid_corner_;} + + /** + * @brief return the start coordinate of the corner arc + * @return start coordinate of the arc + */ + Coordinates getCornerStart() const {return start_coordinate_;} + + /** + * @brief return the end coordinate of the corner arc + * @return end coordinate of the arc + */ + Coordinates getCornerEnd() const {return end_coordinate_;} + +protected: + /** + * @brief find the signed angle between a corner generated by 3 points + * @param start start coordinate of corner to be smoothed + * @param corner corner coordinate of corner to be smoothed + * @param end end coordinate of corner to be smoothed + * @return signed angle of the corner + */ + float getAngleBetweenEdges( + const Coordinates & start, const Coordinates & corner, + const Coordinates & end) + { + float start_dx = start.x - corner.x; + float start_dy = start.y - corner.y; + + float end_dx = end.x - corner.x; + float end_dy = end.y - corner.y; + + float angle = + acos((start_dx * end_dx + start_dy * end_dy) / (start_edge_length_ * end_edge_length_)); + + return angle; + } + +private: + bool valid_corner_{false}; + float start_edge_length_; + float end_edge_length_; + Coordinates start_coordinate_; + Coordinates end_coordinate_; + Coordinates circle_center_coordinate_; +}; + +} // namespace nav2_route + +#endif // NAV2_ROUTE__CORNER_SMOOTHING_HPP_ diff --git a/nav2_route/include/nav2_route/path_converter.hpp b/nav2_route/include/nav2_route/path_converter.hpp index 90b61898160..c4454f768ba 100644 --- a/nav2_route/include/nav2_route/path_converter.hpp +++ b/nav2_route/include/nav2_route/path_converter.hpp @@ -26,6 +26,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_route/types.hpp" #include "nav2_route/utils.hpp" +#include "nav2_route/corner_smoothing.hpp" namespace nav2_route { @@ -80,7 +81,10 @@ class PathConverter protected: rclcpp_lifecycle::LifecyclePublisher::SharedPtr path_pub_; + rclcpp::Logger logger_{rclcpp::get_logger("PathConverter")}; float density_; + float smoothing_radius_; + bool smooth_corners_; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp index 562f5b67339..a2886347f8b 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/goal_orientation_scorer.hpp @@ -26,7 +26,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "angles/angles.h" namespace nav2_route diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp index 41d92970704..a0b2e132d02 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/start_pose_orientation_scorer.hpp @@ -27,7 +27,7 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2/utils.h" +#include "tf2/utils.hpp" #include "angles/angles.h" namespace nav2_route diff --git a/nav2_route/include/nav2_route/types.hpp b/nav2_route/include/nav2_route/types.hpp index dc31c5e01da..0f021a0e23f 100644 --- a/nav2_route/include/nav2_route/types.hpp +++ b/nav2_route/include/nav2_route/types.hpp @@ -138,6 +138,8 @@ struct DirectionalEdge EdgeCost edge_cost; // Cost information associated with edge Metadata metadata; // Any metadata stored in the graph file of interest Operations operations; // Operations to perform related to the edge + + float getEdgeLength(); }; typedef DirectionalEdge * EdgePtr; @@ -194,6 +196,13 @@ struct Node } }; +inline float DirectionalEdge::getEdgeLength() +{ + return hypotf( + end->coords.x - start->coords.x, + end->coords.y - start->coords.y); +} + /** * @struct nav2_route::Route * @brief An ordered set of nodes and edges corresponding to the planned route diff --git a/nav2_route/src/path_converter.cpp b/nav2_route/src/path_converter.cpp index 38adea20c25..85607a5491b 100644 --- a/nav2_route/src/path_converter.cpp +++ b/nav2_route/src/path_converter.cpp @@ -30,9 +30,16 @@ void PathConverter::configure(nav2_util::LifecycleNode::SharedPtr node) nav2_util::declare_parameter_if_not_declared( node, "path_density", rclcpp::ParameterValue(0.05)); density_ = static_cast(node->get_parameter("path_density").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "smoothing_radius", rclcpp::ParameterValue(1.0)); + smoothing_radius_ = static_cast(node->get_parameter("smoothing_radius").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "smooth_corners", rclcpp::ParameterValue(false)); + smooth_corners_ = node->get_parameter("smooth_corners").as_bool(); path_pub_ = node->create_publisher("plan", 1); path_pub_->on_activate(); + logger_ = node->get_logger(); } nav_msgs::msg::Path PathConverter::densify( @@ -54,17 +61,50 @@ nav_msgs::msg::Path PathConverter::densify( interpolateEdge(start.x, start.y, end.x, end.y, path.poses); } - // Fill in path via route edges - for (unsigned int i = 0; i != route.edges.size(); i++) { - const EdgePtr edge = route.edges[i]; - const Coordinates & start = edge->start->coords; - const Coordinates & end = edge->end->coords; - interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + Coordinates start; + Coordinates end; + + if (!route.edges.empty()) { + start = route.edges[0]->start->coords; + + // Fill in path via route edges + for (unsigned int i = 0; i < route.edges.size() - 1; i++) { + const EdgePtr edge = route.edges[i]; + const EdgePtr & next_edge = route.edges[i + 1]; + end = edge->end->coords; + + CornerArc corner_arc(start, end, next_edge->end->coords, smoothing_radius_); + if (corner_arc.isCornerValid() && smooth_corners_) { + // if an arc exists, end of the first edge is the start of the arc + end = corner_arc.getCornerStart(); + + // interpolate to start of arc + interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + + // interpolate arc + corner_arc.interpolateArc(density_ / smoothing_radius_, path.poses); + + // new start of next edge is end of smoothing arc + start = corner_arc.getCornerEnd(); + } else { + if (smooth_corners_) { + RCLCPP_WARN( + logger_, "Unable to smooth corner between edge %i and edge %i", edge->edgeid, + next_edge->edgeid); + } + interpolateEdge(start.x, start.y, end.x, end.y, path.poses); + start = end; + } + } } if (route.edges.empty()) { path.poses.push_back(utils::toMsg(route.start_node->coords.x, route.start_node->coords.y)); } else { + interpolateEdge( + start.x, start.y, route.edges.back()->end->coords.x, + route.edges.back()->end->coords.y, path.poses); + path.poses.push_back( utils::toMsg(route.edges.back()->end->coords.x, route.edges.back()->end->coords.y)); } diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index 186c93dbfca..6ff732b35e4 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -127,3 +127,11 @@ ament_add_gtest(test_goal_intent_search target_link_libraries(test_goal_intent_search ${library_name} ) + +# Test corner smoothing +ament_add_gtest(test_corner_smoothing + test_corner_smoothing.cpp +) +target_link_libraries(test_corner_smoothing + ${library_name} +) diff --git a/nav2_route/test/test_corner_smoothing.cpp b/nav2_route/test/test_corner_smoothing.cpp new file mode 100644 index 00000000000..928ab07f3cd --- /dev/null +++ b/nav2_route/test/test_corner_smoothing.cpp @@ -0,0 +1,115 @@ +// Copyright (c) 2025, Polymath Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_route/corner_smoothing.hpp" +// #include "nav2_route/types.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +using namespace nav2_route; // NOLINT + +TEST(CornerSmoothingTest, test_corner_smoothing) +{ + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 1; + test_node2.coords.x = 10.0; + test_node2.coords.y = 0.0; + test_node3.nodeid = 2; + test_node3.coords.x = 10.0; + test_node3.coords.y = 10.0; + + double smoothing_radius = 2.0; + + CornerArc corner_arc(test_node1.coords, test_node2.coords, test_node3.coords, smoothing_radius); + + Coordinates start = corner_arc.getCornerStart(); + Coordinates end = corner_arc.getCornerEnd(); + + EXPECT_TRUE(corner_arc.isCornerValid()); + EXPECT_EQ(start.x, 8.0); + EXPECT_EQ(start.y, 0.0); + EXPECT_EQ(end.x, 10.0); + EXPECT_EQ(end.y, 2.0); +} + +TEST(LargeRadiusTest, test_large_radius_smoothing) +{ + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 1; + test_node2.coords.x = 10.0; + test_node2.coords.y = 0.0; + test_node3.nodeid = 2; + test_node3.coords.x = 10.0; + test_node3.coords.y = 10.0; + + double smoothing_radius = 20.0; + + CornerArc corner_arc(test_node1.coords, test_node2.coords, test_node3.coords, smoothing_radius); + + EXPECT_FALSE(corner_arc.isCornerValid()); +} + +TEST(ColinearSmoothingTest, test_colinear_smoothing) +{ + Node test_node1, test_node2, test_node3; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + test_node2.nodeid = 1; + test_node2.coords.x = 10.0; + test_node2.coords.y = 0.0; + test_node3.nodeid = 2; + test_node3.coords.x = 20.0; + test_node3.coords.y = 0.0; + + double smoothing_radius = 2.0; + + CornerArc corner_arc(test_node1.coords, test_node2.coords, test_node3.coords, smoothing_radius); + + EXPECT_FALSE(corner_arc.isCornerValid()); +} + +TEST(DegeneratePointsTest, test_degenerate_points_smoothing) +{ + Node test_node1; + test_node1.nodeid = 0; + test_node1.coords.x = 0.0; + test_node1.coords.y = 0.0; + + double smoothing_radius = 2.0; + + CornerArc corner_arc(test_node1.coords, test_node1.coords, test_node1.coords, smoothing_radius); + + EXPECT_FALSE(corner_arc.isCornerValid()); +} diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 0c509957230..5360d4750ce 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.3.1 + 1.4.0 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 47539607ebb..1f83425b064 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -54,12 +54,12 @@ void CostmapCostTool::onInitialize() rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); local_cost_client_ = std::make_shared>( - "/local_costmap/get_cost_local_costmap", + "local_costmap/get_cost_local_costmap", node, false /* Does not create and spin an internal executor*/); global_cost_client_ = std::make_shared>( - "/global_costmap/get_cost_global_costmap", + "global_costmap/get_cost_global_costmap", node, false /* Does not create and spin an internal executor*/); } diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 080f5d93e73..e2293165f1b 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.3.1 + 1.4.0 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index b0e3bab0188..a4720b3c5b2 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -56,7 +56,6 @@ class AStarAlgorithm typedef typename NodeVector::iterator NeighborIterator; typedef std::function NodeGetter; typedef GoalManager GoalManagerT; - typedef std::vector> GoalStateVector; /** diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 0eecb78185e..0c5cdede0bd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -15,6 +15,10 @@ #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ +#include +#include +#include + #include #include #include @@ -61,7 +65,33 @@ class AnalyticExpansion Coordinates proposed_coords; }; - typedef std::vector AnalyticExpansionNodes; + /** + * @struct AnalyticExpansionNodes + * @brief Analytic expansion nodes and associated metadata + * + * This structure holds a collection of analytic expansion nodes and the number of direction + * changes encountered during the expansion. + */ + struct AnalyticExpansionNodes + { + AnalyticExpansionNodes() = default; + + void add( + NodePtr & node, + Coordinates & initial_coords, + Coordinates & proposed_coords) + { + nodes.emplace_back(node, initial_coords, proposed_coords); + } + + void setDirectionChanges(int changes) + { + direction_changes = changes; + } + + std::vector nodes; + int direction_changes{0}; + }; /** * @brief Constructor for analytic expansion object @@ -112,14 +142,15 @@ class AnalyticExpansion /** * @brief Refined analytic path from the current node to the goal - * @param current_node The node to start the analytic path from + * @param node The node to start the analytic path from. Node head may + * change as a result of refinement * @param goal_node The goal node to plan to * @param getter The function object that gets valid nodes from the graph * @param analytic_nodes The set of analytic nodes to refine * @return The score of the refined path */ float refineAnalyticPath( - const NodePtr & current_node, + NodePtr & node, const NodePtr & goal_node, const NodeGetter & getter, AnalyticExpansionNodes & analytic_nodes); @@ -135,6 +166,13 @@ class AnalyticExpansion const NodePtr & node, const NodePtr & goal, const AnalyticExpansionNodes & expanded_nodes); + /** + * @brief Counts the number of direction changes in a Reeds-Shepp path + * @param path The Reeds-Shepp path to count direction changes in + * @return The number of direction changes in the path + */ + int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path); + /** * @brief Takes an expanded nodes to clean up, if necessary, of any state * information that may be polluting it from a prior search iteration diff --git a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp index 826fe9d47e5..317a67de933 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp @@ -53,7 +53,8 @@ class GoalManager GoalManager() : _goals_set(NodeSet()), _goals_state(GoalStateVector()), - _goals_coordinate(CoordinateVector()) + _goals_coordinate(CoordinateVector()), + _ref_goal_coord(Coordinates()) { } @@ -72,15 +73,12 @@ class GoalManager } /** - * @brief Sets the internal goals' state list to the provided goals. - * @param goals Vector of goals state to set, + * @brief Adds goal to the goal vector + *@param goal Reference to the NodePtr */ - - void setGoalStates( - GoalStateVector & goals_state) + void addGoal(NodePtr & goal) { - clear(); - _goals_state = goals_state; + _goals_state.push_back({goal, true}); } /** @@ -128,6 +126,11 @@ class GoalManager GridCollisionChecker * collision_checker, const bool & traverse_unknown) { + // Make sure that there was a goal clear before this was run + if (!_goals_set.empty() || !_goals_coordinate.empty()) { + throw std::runtime_error("Goal set should be cleared before calling " + "removeinvalidgoals"); + } for (unsigned int i = 0; i < _goals_state.size(); i++) { if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) || tolerance > 0.001) @@ -146,7 +149,7 @@ class GoalManager * @param node Node pointer to check. * @return if node matches any goal in the goal set. */ - inline bool isGoal(NodePtr & node) + inline bool isGoal(const NodePtr & node) { return _goals_set.find(node) != _goals_set.end(); } @@ -178,11 +181,36 @@ class GoalManager return _goals_coordinate; } + /** + * @brief Set the Reference goal coordinate + * @param coord Coordinates to set as Reference goal + */ + inline void setRefGoalCoordinates(const Coordinates & coord) + { + _ref_goal_coord = coord; + } + + /** + * @brief Checks whether the Reference goal coordinate has changed. + * @param coord Coordinates to compare with the current Reference goal coordinate. + * @return true if the Reference goal coordinate has changed, false otherwise. + */ + inline bool hasGoalChanged(const Coordinates & coord) + { + /** + * Note: This function checks if the goal has changed. This has to be done with + * the coordinates not the Node pointer because the Node pointer + * can be reused for different goals, but the coordinates will always + * be unique for each goal. + */ + return _ref_goal_coord != coord; + } + protected: NodeSet _goals_set; GoalStateVector _goals_state; CoordinateVector _goals_coordinate; - NodePtr primary_goal; + Coordinates _ref_goal_coord; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 96f40f47664..0b50c0e15fd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -128,6 +128,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _planned_footprints_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _smoothed_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; std::mutex _mutex; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index 6db78ddccaf..8cead0a0145 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -118,6 +118,8 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner bool _debug_visualizations; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _planned_footprints_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _smoothed_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; GoalHeadingMode _goal_heading_mode; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index b0ccaa8bb29..f71b4a932f6 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -188,11 +188,10 @@ struct MotionPrimitive MotionPoses poses; }; - /** - * @struct nav2_smac_planner::GoalState - * @brief A struct to store the goal state - */ - +/** + * @struct nav2_smac_planner::GoalState + * @brief A struct to store the goal state + */ template struct GoalState { diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index d73d553496b..8303ffc165f 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.3.1 + 1.4.0 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 2762ae37c8a..98d040bd9cc 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -198,17 +198,16 @@ void AStarAlgorithm::setGoal( if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } - - NodePtr goal = addToGraph( + _goal_manager.clear(); + auto goal = addToGraph( Node2D::getIndex( static_cast(mx), static_cast(my), getSizeX())); goal->setPose(Node2D::Coordinates(mx, my)); - GoalStateVector goals_state; - goals_state.push_back({goal, true}); - _goal_manager.setGoalStates(goals_state); + _goal_manager.addGoal(goal); + _coarse_search_resolution = 1; } @@ -223,34 +222,58 @@ void AStarAlgorithm::setGoal( // Default to minimal resolution unless overridden for ALL_DIRECTION _coarse_search_resolution = 1; - unsigned int num_bins = NodeT::motion_table.num_angle_quantization; - GoalStateVector goals_state; + _goal_manager.clear(); + Coordinates ref_goal_coord(mx, my, static_cast(dim_3)); + + if (!_search_info.cache_obstacle_heuristic || + _goal_manager.hasGoalChanged(ref_goal_coord)) + { + if (!_start) { + throw std::runtime_error("Start must be set before goal."); + } + NodeT::resetObstacleHeuristic( + _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); + } + + _goal_manager.setRefGoalCoordinates(ref_goal_coord); + + unsigned int num_bins = NodeT::motion_table.num_angle_quantization; // set goal based on heading mode switch (goal_heading_mode) { case GoalHeadingMode::DEFAULT: { // add a single goal node with single heading - auto goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); - goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(dim_3))); - goals_state.push_back({goal, true}); + auto goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); + _goal_manager.addGoal(goal); break; } case GoalHeadingMode::BIDIRECTIONAL: { // Add two goals, one for each direction // add goal in original direction - auto goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); - goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(dim_3))); - goals_state.push_back({goal, true}); + auto goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); + _goal_manager.addGoal(goal); // Add goal node in opposite (180°) direction unsigned int opposite_heading = (dim_3 + (num_bins / 2)) % num_bins; - auto opposite_goal = addToGraph(NodeT::getIndex(mx, my, opposite_heading)); - opposite_goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(opposite_heading))); - goals_state.push_back({opposite_goal, true}); + auto opposite_goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + opposite_heading)); + opposite_goal->setPose( + typename NodeT::Coordinates(mx, my, static_cast(opposite_heading))); + _goal_manager.addGoal(opposite_goal); break; } @@ -260,43 +283,19 @@ void AStarAlgorithm::setGoal( // Add goal nodes for all headings for (unsigned int i = 0; i < num_bins; ++i) { - auto goal = addToGraph(NodeT::getIndex(mx, my, i)); - goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(i))); - goals_state.push_back({goal, true}); + auto goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + i)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(i))); + _goal_manager.addGoal(goal); } break; } case GoalHeadingMode::UNKNOWN: throw std::runtime_error("Goal heading is UNKNOWN."); } - - // check if we need to reset the obstacle heuristic, we only need to - // check that the x and y component has changed - const auto & previous_goals = _goal_manager.getGoalsState(); - - // Lambda to check if goal has changed - auto goalHasChanged = [&]() -> bool { - if (previous_goals.empty()) { - return true; - } - - const auto & prev = previous_goals.front().goal; - const auto & curr = goals_state.front().goal; - return (prev->pose.x != curr->pose.x) || (prev->pose.y != curr->pose.y); - }; - - if (!_search_info.cache_obstacle_heuristic || goalHasChanged()) { - if (!_start) { - throw std::runtime_error("Start must be set before goal."); - } - - NodeT::resetObstacleHeuristic( - _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); - } - - // assign the goals state - _goal_manager.setGoalStates(goals_state); } template diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index da46f781975..13df5feb6af 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -12,10 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include -#include -#include - #include #include #include @@ -64,8 +60,9 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic NodeT::getCoords( current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); - AnalyticExpansionNodes current_best_analytic_nodes = {}; + AnalyticExpansionNodes current_best_analytic_nodes; NodePtr current_best_goal = nullptr; + NodePtr current_best_node = nullptr; float current_best_score = std::numeric_limits::max(); closest_distance = std::min( @@ -95,15 +92,17 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic getAnalyticPath( current_node, current_goal_node, getter, current_node->motion_table.state_space); - if (!analytic_nodes.empty()) { + if (!analytic_nodes.nodes.empty()) { found_valid_expansion = true; - bool score = refineAnalyticPath( - current_node, current_goal_node, getter, analytic_nodes); + NodePtr node = current_node; + float score = refineAnalyticPath( + node, current_goal_node, getter, analytic_nodes); // Update the best score if we found a better path if (score < current_best_score) { current_best_analytic_nodes = analytic_nodes; current_best_goal = current_goal_node; current_best_score = score; + current_best_node = node; } } } @@ -115,23 +114,25 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic getAnalyticPath( current_node, current_goal_node, getter, current_node->motion_table.state_space); - if (!analytic_nodes.empty()) { - bool score = refineAnalyticPath( - current_node, current_goal_node, getter, analytic_nodes); + if (!analytic_nodes.nodes.empty()) { + NodePtr node = current_node; + float score = refineAnalyticPath( + node, current_goal_node, getter, analytic_nodes); // Update the best score if we found a better path if (score < current_best_score) { current_best_analytic_nodes = analytic_nodes; current_best_goal = current_goal_node; current_best_score = score; + current_best_node = node; } } } } } - if (!current_best_analytic_nodes.empty()) { + if (!current_best_analytic_nodes.nodes.empty()) { return setAnalyticPath( - current_node, current_best_goal, + current_best_node, current_best_goal, current_best_analytic_nodes); } analytic_iterations--; @@ -141,6 +142,28 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic return NodePtr(nullptr); } +template +int AnalyticExpansion::countDirectionChanges( + const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path) +{ + const double * lengths = path.length_; + int changes = 0; + int last_dir = 0; + for (int i = 0; i < 5; ++i) { + if (lengths[i] == 0.0) { + continue; + } + + int currentDirection = (lengths[i] > 0.0) ? 1 : -1; + if (last_dir != 0 && currentDirection != last_dir) { + ++changes; + } + last_dir = currentDirection; + } + + return changes; +} + template typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( const NodePtr & node, @@ -158,6 +181,12 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansiondistance(from(), to()); + auto rs_state_space = dynamic_cast(state_space.get()); + int direction_changes = 0; + if (rs_state_space) { + direction_changes = countDirectionChanges(rs_state_space->reedsShepp(from.get(), to.get())); + } + // A move of sqrt(2) is guaranteed to be in a new cell static const float sqrt_2 = sqrtf(2.0f); @@ -174,7 +203,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion reals; double theta; @@ -209,7 +238,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(proposed_coordinates); if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { // Save the node, and its previous coordinates in case we need to abort - possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates); + possible_nodes.add(next, initial_node_coords, proposed_coordinates); node_costs.emplace_back(next->getCost()); prev = next; } else { @@ -259,7 +288,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(node_pose.initial_coords); } @@ -268,17 +297,17 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion float AnalyticExpansion::refineAnalyticPath( - const NodePtr & current_node, + NodePtr & node, const NodePtr & goal_node, const NodeGetter & getter, AnalyticExpansionNodes & analytic_nodes) { - NodePtr node = current_node; NodePtr test_node = node; AnalyticExpansionNodes refined_analytic_nodes; for (int i = 0; i < 8; i++) { @@ -295,9 +324,13 @@ float AnalyticExpansion::refineAnalyticPath( getAnalyticPath( test_node, goal_node, getter, test_node->motion_table.state_space); - if (refined_analytic_nodes.empty()) { + if (refined_analytic_nodes.nodes.empty()) { break; } + if (refined_analytic_nodes.direction_changes > analytic_nodes.direction_changes) { + // If the direction changes are worse, we don't want to use this path + continue; + } analytic_nodes = refined_analytic_nodes; node = test_node; } else { @@ -310,7 +343,7 @@ float AnalyticExpansion::refineAnalyticPath( // higher than the minimum turning radius and use the best solution based on // a scoring function similar to that used in traversal cost estimation. auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.size() < 2) { + if (expansion.nodes.size() < 2) { return std::numeric_limits::max(); } @@ -318,18 +351,19 @@ float AnalyticExpansion::refineAnalyticPath( float normalized_cost = 0.0; // Analytic expansions are consistently spaced const float distance = hypotf( - expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, - expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); - const float & weight = expansion[0].node->motion_table.cost_penalty; - for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { + expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x, + expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y); + const float & weight = expansion.nodes[0].node->motion_table.cost_penalty; + for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) { normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function + // Search's Traversal Cost Function score += distance * (1.0 + weight * normalized_cost); } return score; }; - float best_score = scoringFn(analytic_nodes); + float original_score = scoringFn(analytic_nodes); + float best_score = original_score; float score = std::numeric_limits::max(); float min_turn_rad = node->motion_table.min_turning_radius; const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius @@ -343,7 +377,21 @@ float AnalyticExpansion::refineAnalyticPath( } refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); score = scoringFn(refined_analytic_nodes); - if (score <= best_score) { + + // Normal scoring: prioritize lower cost as long as not more directional changes + if (score <= best_score && + refined_analytic_nodes.direction_changes <= analytic_nodes.direction_changes) + { + analytic_nodes = refined_analytic_nodes; + best_score = score; + continue; + } + + // Special case: If we have a better score than original (only) and less directional changes + // the path quality is still better than the original and is less operationally complex + if (score <= original_score && + refined_analytic_nodes.direction_changes < analytic_nodes.direction_changes) + { analytic_nodes = refined_analytic_nodes; best_score = score; } @@ -361,7 +409,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalytic _detached_nodes.clear(); // Legitimate final path - set the parent relationships, states, and poses NodePtr prev = node; - for (const auto & node_pose : expanded_nodes) { + for (const auto & node_pose : expanded_nodes.nodes) { auto n = node_pose.node; cleanNode(n); if (n->getIndex() != goal_node->getIndex()) { @@ -407,7 +455,7 @@ getAnalyticPath( template<> float AnalyticExpansion::refineAnalyticPath( - const NodePtr &, + NodePtr &, const NodePtr &, const NodeGetter &, AnalyticExpansionNodes &) diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 3936fc7157d..1210bc1619c 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -362,34 +362,36 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete bool reinit_downsampler = false; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == _name + ".tolerance") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if(param_name.find(_name + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == _name + ".tolerance") { _tolerance = static_cast(parameter.as_double()); - } else if (name == _name + ".cost_travel_multiplier") { + } else if (param_name == _name + ".cost_travel_multiplier") { reinit_a_star = true; _search_info.cost_penalty = parameter.as_double(); - } else if (name == _name + ".max_planning_time") { + } else if (param_name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == _name + ".downsample_costmap") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == _name + ".downsample_costmap") { reinit_downsampler = true; _downsample_costmap = parameter.as_bool(); - } else if (name == _name + ".allow_unknown") { + } else if (param_name == _name + ".allow_unknown") { reinit_a_star = true; _allow_unknown = parameter.as_bool(); - } else if (name == _name + ".use_final_approach_orientation") { + } else if (param_name == _name + ".use_final_approach_orientation") { _use_final_approach_orientation = parameter.as_bool(); } - } else if (type == ParameterType::PARAMETER_INTEGER) { - if (name == _name + ".downsampling_factor") { + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == _name + ".downsampling_factor") { reinit_downsampler = true; _downsampling_factor = parameter.as_int(); - } else if (name == _name + ".max_iterations") { + } else if (param_name == _name + ".max_iterations") { reinit_a_star = true; _max_iterations = parameter.as_int(); if (_max_iterations <= 0) { @@ -398,7 +400,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".max_on_approach_iterations") { + } else if (param_name == _name + ".max_on_approach_iterations") { reinit_a_star = true; _max_on_approach_iterations = parameter.as_int(); if (_max_on_approach_iterations <= 0) { @@ -407,7 +409,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".terminal_checking_interval") { + } else if (param_name == _name + ".terminal_checking_interval") { reinit_a_star = true; _terminal_checking_interval = parameter.as_int(); } diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 22feccffabd..d6f4ec26fb8 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -294,6 +294,9 @@ void SmacPlannerHybrid::configure( _expansions_publisher = node->create_publisher("expansions", 1); _planned_footprints_publisher = node->create_publisher( "planned_footprints", 1); + _smoothed_footprints_publisher = + node->create_publisher( + "smoothed_footprints", 1); } RCLCPP_INFO( @@ -314,6 +317,7 @@ void SmacPlannerHybrid::activate() if (_debug_visualizations) { _expansions_publisher->on_activate(); _planned_footprints_publisher->on_activate(); + _smoothed_footprints_publisher->on_activate(); } if (_costmap_downsampler) { _costmap_downsampler->on_activate(); @@ -342,6 +346,7 @@ void SmacPlannerHybrid::deactivate() if (_debug_visualizations) { _expansions_publisher->on_deactivate(); _planned_footprints_publisher->on_deactivate(); + _smoothed_footprints_publisher->on_deactivate(); } if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); @@ -367,8 +372,11 @@ void SmacPlannerHybrid::cleanup() _costmap_downsampler.reset(); } _raw_plan_publisher.reset(); - _expansions_publisher.reset(); - _planned_footprints_publisher.reset(); + if (_debug_visualizations) { + _expansions_publisher.reset(); + _planned_footprints_publisher.reset(); + _smoothed_footprints_publisher.reset(); + } } nav_msgs::msg::Path SmacPlannerHybrid::createPlan( @@ -522,9 +530,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (_debug_visualizations) { // Publish expansions for debug + auto now = _clock->now(); geometry_msgs::msg::PoseArray msg; geometry_msgs::msg::Pose msg_pose; - msg.header.stamp = _clock->now(); + msg.header.stamp = now; msg.header.frame_id = _global_frame; for (auto & e : *expansions) { msg_pose.position.x = std::get<0>(e); @@ -534,19 +543,20 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } _expansions_publisher->publish(msg); - // plot footprint path planned for debug if (_planned_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _planned_footprints_publisher->publish(std::move(marker_array)); + + // Publish smoothed footprints for debug + marker_array = std::make_unique(); for (size_t i = 0; i < plan.poses.size(); i++) { const std::vector edge = transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); - marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now())); - } - - if (marker_array->markers.empty()) { - visualization_msgs::msg::Marker clear_all_marker; - clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array->markers.push_back(clear_all_marker); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); } _planned_footprints_publisher->publish(std::move(marker_array)); } @@ -574,6 +584,27 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( " milliseconds to smooth path." << std::endl; #endif + if (_debug_visualizations) { + if (_smoothed_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _smoothed_footprints_publisher->publish(std::move(marker_array)); + + // Publish smoothed footprints for debug + marker_array = std::make_unique(); + auto now = _clock->now(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); + } + _smoothed_footprints_publisher->publish(std::move(marker_array)); + } + } + return plan; } @@ -589,19 +620,21 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para bool reinit_smoother = false; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == _name + ".max_planning_time") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if(param_name.find(_name + ".") != 0 && param_name != "resolution") { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); - } else if (name == _name + ".tolerance") { + } else if (param_name == _name + ".tolerance") { _tolerance = static_cast(parameter.as_double()); - } else if (name == _name + ".lookup_table_size") { + } else if (param_name == _name + ".lookup_table_size") { reinit_a_star = true; _lookup_table_size = parameter.as_double(); - } else if (name == _name + ".minimum_turning_radius") { + } else if (param_name == _name + ".minimum_turning_radius") { reinit_a_star = true; if (_smoother) { reinit_smoother = true; @@ -614,29 +647,29 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para } _minimum_turning_radius_global_coords = static_cast(parameter.as_double()); - } else if (name == _name + ".reverse_penalty") { + } else if (param_name == _name + ".reverse_penalty") { reinit_a_star = true; _search_info.reverse_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".change_penalty") { + } else if (param_name == _name + ".change_penalty") { reinit_a_star = true; _search_info.change_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".non_straight_penalty") { + } else if (param_name == _name + ".non_straight_penalty") { reinit_a_star = true; _search_info.non_straight_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".cost_penalty") { + } else if (param_name == _name + ".cost_penalty") { reinit_a_star = true; _search_info.cost_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".analytic_expansion_ratio") { + } else if (param_name == _name + ".analytic_expansion_ratio") { reinit_a_star = true; _search_info.analytic_expansion_ratio = static_cast(parameter.as_double()); - } else if (name == _name + ".analytic_expansion_max_length") { + } else if (param_name == _name + ".analytic_expansion_max_length") { reinit_a_star = true; _search_info.analytic_expansion_max_length = static_cast(parameter.as_double()) / _costmap->getResolution(); - } else if (name == _name + ".analytic_expansion_max_cost") { + } else if (param_name == _name + ".analytic_expansion_max_cost") { reinit_a_star = true; _search_info.analytic_expansion_max_cost = static_cast(parameter.as_double()); - } else if (name == "resolution") { + } else if (param_name == "resolution") { // Special case: When the costmap's resolution changes, need to reinitialize // the controller to have new resolution information RCLCPP_INFO(_logger, "Costmap resolution changed. Reinitializing SmacPlannerHybrid."); @@ -645,35 +678,35 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para reinit_downsampler = true; reinit_smoother = true; } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == _name + ".downsample_costmap") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == _name + ".downsample_costmap") { reinit_downsampler = true; _downsample_costmap = parameter.as_bool(); - } else if (name == _name + ".allow_unknown") { + } else if (param_name == _name + ".allow_unknown") { reinit_a_star = true; _allow_unknown = parameter.as_bool(); - } else if (name == _name + ".cache_obstacle_heuristic") { + } else if (param_name == _name + ".cache_obstacle_heuristic") { reinit_a_star = true; _search_info.cache_obstacle_heuristic = parameter.as_bool(); - } else if (name == _name + ".allow_primitive_interpolation") { + } else if (param_name == _name + ".allow_primitive_interpolation") { _search_info.allow_primitive_interpolation = parameter.as_bool(); reinit_a_star = true; - } else if (name == _name + ".smooth_path") { + } else if (param_name == _name + ".smooth_path") { if (parameter.as_bool()) { reinit_smoother = true; } else { _smoother.reset(); } - } else if (name == _name + ".analytic_expansion_max_cost_override") { + } else if (param_name == _name + ".analytic_expansion_max_cost_override") { _search_info.analytic_expansion_max_cost_override = parameter.as_bool(); reinit_a_star = true; } - } else if (type == ParameterType::PARAMETER_INTEGER) { - if (name == _name + ".downsampling_factor") { + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == _name + ".downsampling_factor") { reinit_a_star = true; reinit_downsampler = true; _downsampling_factor = parameter.as_int(); - } else if (name == _name + ".max_iterations") { + } else if (param_name == _name + ".max_iterations") { reinit_a_star = true; _max_iterations = parameter.as_int(); if (_max_iterations <= 0) { @@ -682,7 +715,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".max_on_approach_iterations") { + } else if (param_name == _name + ".max_on_approach_iterations") { reinit_a_star = true; _max_on_approach_iterations = parameter.as_int(); if (_max_on_approach_iterations <= 0) { @@ -691,10 +724,10 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".terminal_checking_interval") { + } else if (param_name == _name + ".terminal_checking_interval") { reinit_a_star = true; _terminal_checking_interval = parameter.as_int(); - } else if (name == _name + ".angle_quantization_bins") { + } else if (param_name == _name + ".angle_quantization_bins") { reinit_collision_checker = true; reinit_a_star = true; int angle_quantizations = parameter.as_int(); @@ -708,7 +741,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para ); _coarse_search_resolution = 1; } - } else if (name == _name + ".coarse_search_resolution") { + } else if (param_name == _name + ".coarse_search_resolution") { _coarse_search_resolution = parameter.as_int(); if (_coarse_search_resolution <= 0) { RCLCPP_WARN( @@ -726,8 +759,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _coarse_search_resolution = 1; } } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == _name + ".motion_model_for_search") { + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == _name + ".motion_model_for_search") { reinit_a_star = true; _motion_model = fromString(parameter.as_string()); if (_motion_model == MotionModel::UNKNOWN) { @@ -737,7 +770,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", _motion_model_for_search.c_str()); } - } else if (name == _name + ".goal_heading_mode") { + } else if (param_name == _name + ".goal_heading_mode") { std::string goal_heading_type = parameter.as_string(); GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); RCLCPP_INFO( diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index b771c2cd3dd..4968068e534 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -56,7 +56,6 @@ void SmacPlannerLattice::configure( _costmap_ros = costmap_ros; _name = name; _global_frame = costmap_ros->getGlobalFrameID(); - _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerLattice", name.c_str()); @@ -238,10 +237,15 @@ void SmacPlannerLattice::configure( _smoother->initialize(_metadata.min_turning_radius); } + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + if (_debug_visualizations) { _expansions_publisher = node->create_publisher("expansions", 1); _planned_footprints_publisher = node->create_publisher( "planned_footprints", 1); + _smoothed_footprints_publisher = + node->create_publisher( + "smoothed_footprints", 1); } RCLCPP_INFO( @@ -262,6 +266,7 @@ void SmacPlannerLattice::activate() if (_debug_visualizations) { _expansions_publisher->on_activate(); _planned_footprints_publisher->on_activate(); + _smoothed_footprints_publisher->on_activate(); } auto node = _node.lock(); // Add callback for dynamic parameters @@ -278,6 +283,7 @@ void SmacPlannerLattice::deactivate() if (_debug_visualizations) { _expansions_publisher->on_deactivate(); _planned_footprints_publisher->on_deactivate(); + _smoothed_footprints_publisher->on_deactivate(); } // shutdown dyn_param_handler auto node = _node.lock(); @@ -296,6 +302,11 @@ void SmacPlannerLattice::cleanup() _a_star.reset(); _smoother.reset(); _raw_plan_publisher.reset(); + if (_debug_visualizations) { + _expansions_publisher.reset(); + _planned_footprints_publisher.reset(); + _smoothed_footprints_publisher.reset(); + } } nav_msgs::msg::Path SmacPlannerLattice::createPlan( @@ -390,9 +401,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( _tolerance / static_cast(_costmap->getResolution()), cancel_checker, expansions.get())) { if (_debug_visualizations) { + auto now = _clock->now(); geometry_msgs::msg::PoseArray msg; geometry_msgs::msg::Pose msg_pose; - msg.header.stamp = _clock->now(); + msg.header.stamp = now; msg.header.frame_id = _global_frame; for (auto & e : *expansions) { msg_pose.position.x = std::get<0>(e); @@ -441,10 +453,11 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } if (_debug_visualizations) { + auto now = _clock->now(); // Publish expansions for debug geometry_msgs::msg::PoseArray msg; geometry_msgs::msg::Pose msg_pose; - msg.header.stamp = _clock->now(); + msg.header.stamp = now; msg.header.frame_id = _global_frame; for (auto & e : *expansions) { msg_pose.position.x = std::get<0>(e); @@ -454,19 +467,20 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } _expansions_publisher->publish(msg); - // plot footprint path planned for debug if (_planned_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _planned_footprints_publisher->publish(std::move(marker_array)); + + // Publish smoothed footprints for debug + marker_array = std::make_unique(); for (size_t i = 0; i < plan.poses.size(); i++) { const std::vector edge = transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); - marker_array->markers.push_back(createMarker(edge, i, _global_frame, _clock->now())); - } - - if (marker_array->markers.empty()) { - visualization_msgs::msg::Marker clear_all_marker; - clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array->markers.push_back(clear_all_marker); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); } _planned_footprints_publisher->publish(std::move(marker_array)); } @@ -494,6 +508,27 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( " milliseconds to smooth path." << std::endl; #endif + if (_debug_visualizations) { + if (_smoothed_footprints_publisher->get_subscription_count() > 0) { + // Clear all markers first + auto marker_array = std::make_unique(); + visualization_msgs::msg::Marker clear_all_marker; + clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array->markers.push_back(clear_all_marker); + _smoothed_footprints_publisher->publish(std::move(marker_array)); + + // Publish smoothed footprints for debug + marker_array = std::make_unique(); + auto now = _clock->now(); + for (size_t i = 0; i < plan.poses.size(); i++) { + const std::vector edge = + transformFootprintToEdges(plan.poses[i].pose, _costmap_ros->getRobotFootprint()); + marker_array->markers.push_back(createMarker(edge, i, _global_frame, now)); + } + _smoothed_footprints_publisher->publish(std::move(marker_array)); + } + } + return plan; } @@ -507,66 +542,68 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par bool reinit_smoother = false; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == _name + ".max_planning_time") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if(param_name.find(_name + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == _name + ".max_planning_time") { reinit_a_star = true; _max_planning_time = parameter.as_double(); - } else if (name == _name + ".tolerance") { + } else if (param_name == _name + ".tolerance") { _tolerance = static_cast(parameter.as_double()); - } else if (name == _name + ".lookup_table_size") { + } else if (param_name == _name + ".lookup_table_size") { reinit_a_star = true; _lookup_table_size = parameter.as_double(); - } else if (name == _name + ".reverse_penalty") { + } else if (param_name == _name + ".reverse_penalty") { reinit_a_star = true; _search_info.reverse_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".change_penalty") { + } else if (param_name == _name + ".change_penalty") { reinit_a_star = true; _search_info.change_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".non_straight_penalty") { + } else if (param_name == _name + ".non_straight_penalty") { reinit_a_star = true; _search_info.non_straight_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".cost_penalty") { + } else if (param_name == _name + ".cost_penalty") { reinit_a_star = true; _search_info.cost_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".rotation_penalty") { + } else if (param_name == _name + ".rotation_penalty") { reinit_a_star = true; _search_info.rotation_penalty = static_cast(parameter.as_double()); - } else if (name == _name + ".analytic_expansion_ratio") { + } else if (param_name == _name + ".analytic_expansion_ratio") { reinit_a_star = true; _search_info.analytic_expansion_ratio = static_cast(parameter.as_double()); - } else if (name == _name + ".analytic_expansion_max_length") { + } else if (param_name == _name + ".analytic_expansion_max_length") { reinit_a_star = true; _search_info.analytic_expansion_max_length = static_cast(parameter.as_double()) / _costmap->getResolution(); - } else if (name == _name + ".analytic_expansion_max_cost") { + } else if (param_name == _name + ".analytic_expansion_max_cost") { reinit_a_star = true; _search_info.analytic_expansion_max_cost = static_cast(parameter.as_double()); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == _name + ".allow_unknown") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == _name + ".allow_unknown") { reinit_a_star = true; _allow_unknown = parameter.as_bool(); - } else if (name == _name + ".cache_obstacle_heuristic") { + } else if (param_name == _name + ".cache_obstacle_heuristic") { reinit_a_star = true; _search_info.cache_obstacle_heuristic = parameter.as_bool(); - } else if (name == _name + ".allow_reverse_expansion") { + } else if (param_name == _name + ".allow_reverse_expansion") { reinit_a_star = true; _search_info.allow_reverse_expansion = parameter.as_bool(); - } else if (name == _name + ".smooth_path") { + } else if (param_name == _name + ".smooth_path") { if (parameter.as_bool()) { reinit_smoother = true; } else { _smoother.reset(); } - } else if (name == _name + ".analytic_expansion_max_cost_override") { + } else if (param_name == _name + ".analytic_expansion_max_cost_override") { _search_info.analytic_expansion_max_cost_override = parameter.as_bool(); reinit_a_star = true; } - } else if (type == ParameterType::PARAMETER_INTEGER) { - if (name == _name + ".max_iterations") { + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == _name + ".max_iterations") { reinit_a_star = true; _max_iterations = parameter.as_int(); if (_max_iterations <= 0) { @@ -575,7 +612,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".max_on_approach_iterations") { + } else if (param_name == _name + ".max_on_approach_iterations") { reinit_a_star = true; _max_on_approach_iterations = parameter.as_int(); if (_max_on_approach_iterations <= 0) { @@ -584,10 +621,10 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } - } else if (name == _name + ".terminal_checking_interval") { + } else if (param_name == _name + ".terminal_checking_interval") { reinit_a_star = true; _terminal_checking_interval = parameter.as_int(); - } else if (name == _name + ".coarse_search_resolution") { + } else if (param_name == _name + ".coarse_search_resolution") { _coarse_search_resolution = parameter.as_int(); if (_coarse_search_resolution <= 0) { RCLCPP_WARN( @@ -605,8 +642,8 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _coarse_search_resolution = 1; } } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == _name + ".lattice_filepath") { + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == _name + ".lattice_filepath") { reinit_a_star = true; if (_smoother) { reinit_smoother = true; @@ -622,7 +659,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par ); _coarse_search_resolution = 1; } - } else if (name == _name + ".goal_heading_mode") { + } else if (param_name == _name + ".goal_heading_mode") { std::string goal_heading_type = parameter.as_string(); GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); RCLCPP_INFO( diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 3d64ffbc2d2..701e7ad01ac 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -141,11 +141,13 @@ TEST(AStarTest, test_a_star_2d) int dummy_int2 = 0; EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {}, nullptr, dummy_int1, dummy_int2), nullptr); - EXPECT_EQ(expander.refineAnalyticPath(nullptr, nullptr, nullptr, + + nav2_smac_planner::Node2D * start = nullptr; + EXPECT_EQ(expander.refineAnalyticPath(start, nullptr, nullptr, analytic_expansion_nodes), std::numeric_limits::max()); nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr); - EXPECT_EQ(expected_nodes.size(), 0); + EXPECT_EQ(expected_nodes.nodes.size(), 0); delete costmapA; } diff --git a/nav2_smac_planner/test/test_goal_manager.cpp b/nav2_smac_planner/test/test_goal_manager.cpp index cbd17b87895..80f71b3a688 100644 --- a/nav2_smac_planner/test/test_goal_manager.cpp +++ b/nav2_smac_planner/test/test_goal_manager.cpp @@ -25,11 +25,11 @@ using namespace nav2_smac_planner; // NOLINT -using GoalManagerHybrid = GoalManager; -using NodePtr = NodeHybrid *; +using NodeT = NodeHybrid; +using NodePtr = NodeT *; +using GoalManagerHybrid = GoalManager; using NodeVector = GoalManagerHybrid::NodeVector; using CoordinateVector = GoalManagerHybrid::CoordinateVector; -using GoalStateVector = GoalManagerHybrid::GoalStateVector; TEST(GoalManagerTest, test_goal_manager) { @@ -66,12 +66,9 @@ TEST(GoalManagerTest, test_goal_manager) pose_a->setPose(NodeHybrid::Coordinates(0, 0, 0)); pose_b->setPose(NodeHybrid::Coordinates(0, 0, 10)); - GoalStateVector goals_state = { - {pose_a, true}, - {pose_b, true} - }; + goal_manager.addGoal(pose_a); + goal_manager.addGoal(pose_b); - goal_manager.setGoalStates(goals_state); EXPECT_FALSE(goal_manager.goalsIsEmpty()); EXPECT_EQ(goal_manager.getGoalsState().size(), 2u); EXPECT_TRUE(goal_manager.getGoalsSet().empty()); @@ -86,20 +83,21 @@ TEST(GoalManagerTest, test_goal_manager) } // Test is goal - EXPECT_TRUE(goal_manager.isGoal(goals_state[0].goal)); - EXPECT_TRUE(goal_manager.isGoal(goals_state[1].goal)); + EXPECT_TRUE(goal_manager.isGoal(pose_a)); + EXPECT_TRUE(goal_manager.isGoal(pose_b)); // Re-populate and validate reset state - goal_manager.setGoalStates(goals_state); + goal_manager.clear(); + goal_manager.addGoal(pose_a); + goal_manager.addGoal(pose_b); EXPECT_EQ(goal_manager.getGoalsSet().size(), 0); EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 0); // Add invalid goal - NodeHybrid pose_c(50); - pose_c.setPose(NodeHybrid::Coordinates(50, 50, 0)); // inside lethal zone - goals_state.push_back({&pose_c, true}); + NodePtr pose_c = new NodeHybrid(50); + pose_c->setPose(NodeHybrid::Coordinates(50, 50, 0)); // inside lethal zone - goal_manager.setGoalStates(goals_state); + goal_manager.addGoal(pose_c); EXPECT_EQ(goal_manager.getGoalsState().size(), 3); // Tolerance is high, one goal is invalid @@ -113,7 +111,11 @@ TEST(GoalManagerTest, test_goal_manager) } // Test with low tolerance, expect invalid goal to be filtered out - goal_manager.setGoalStates(goals_state); + goal_manager.clear(); + goal_manager.addGoal(pose_a); + goal_manager.addGoal(pose_b); + goal_manager.addGoal(pose_c); + int low_tolerance = 0.0f; goal_manager.removeInvalidGoals(low_tolerance, checker.get(), allow_unknow); @@ -122,7 +124,7 @@ TEST(GoalManagerTest, test_goal_manager) // expect last goal to be invalid for (const auto & goal_state : goal_manager.getGoalsState()) { - if (goal_state.goal == goals_state[2].goal) { + if (goal_state.goal == pose_c) { EXPECT_FALSE(goal_state.is_valid); } else { EXPECT_TRUE(goal_state.is_valid); @@ -130,16 +132,15 @@ TEST(GoalManagerTest, test_goal_manager) } // Prepare goals for expansion - GoalStateVector expansion_goals; + goal_manager.clear(); unsigned int test_goal_size = 16; for (unsigned int i = 0; i < test_goal_size; ++i) { - auto goal = new NodeHybrid(i); + NodePtr goal = new NodeHybrid(i); goal->setPose(NodeHybrid::Coordinates(i, i, 0)); - expansion_goals.push_back({goal, true}); + goal_manager.addGoal(goal); } - goal_manager.setGoalStates(expansion_goals); goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); NodeVector coarse_check_goals; @@ -158,6 +159,14 @@ TEST(GoalManagerTest, test_goal_manager) EXPECT_EQ(coarse_check_goals.size(), 4u); // indices 0, 4, 8, 12 EXPECT_EQ(fine_check_goals.size(), 12u); + + // expect throw if we try to call removeinvalidgoals twice + // without clearing the goal manager first + EXPECT_THROW(goal_manager.removeInvalidGoals( + tolerance, checker.get(), allow_unknow), + std::runtime_error + ); + delete costmapA; nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 7523ffc0aa1..72daaa67bd2 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.3.1 + 1.4.0 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 684dfd6800f..7a8261b63d7 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.3.1 + 1.4.0 A sets of system-level tests for Nav2 usually involving full robot simulation Carlos Orduno Apache-2.0 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 14e3737c710..50f8450b08b 100755 --- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -50,7 +50,12 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites={}, convert_types=True + source_file=params_file, root_key='', param_rewrites={}, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, + convert_types=True ) return LaunchDescription( 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 eb5b5a07e76..60fa938e673 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 @@ -44,6 +44,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) 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 d635adf9539..07daadc6ce1 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 @@ -44,6 +44,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) 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 5c48acad34f..0d7c9acfd34 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 @@ -44,6 +44,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) 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 c2956735ae3..b7f5009b9a5 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -53,7 +53,12 @@ def generate_launch_description() -> LaunchDescription: # Replace the `use_astar` setting on the params file configured_params = RewrittenYaml( - source_file=params_file, root_key='', param_rewrites={}, convert_types=True + source_file=params_file, root_key='', param_rewrites={}, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, + convert_types=True ) return LaunchDescription( 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 cf2eedb5623..c85b8966709 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -147,6 +147,7 @@ def generate_launch_description() -> LaunchDescription: 'namespace': '', 'map': map_yaml_file, 'use_keepout_zones': 'False', + 'use_speed_zones': 'False', 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 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 dae001a4603..cbe30268c92 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -144,6 +144,8 @@ def generate_launch_description() -> LaunchDescription: launch_arguments={ 'namespace': '', 'map': map_yaml_file, + 'use_keepout_zones': 'False', + 'use_speed_zones': 'False', 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, diff --git a/nav2_system_tests/src/route/test_route_launch.py b/nav2_system_tests/src/route/test_route_launch.py index 752470fb815..e475e84437a 100755 --- a/nav2_system_tests/src/route/test_route_launch.py +++ b/nav2_system_tests/src/route/test_route_launch.py @@ -73,6 +73,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) 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 46654ffac52..861308108a0 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 @@ -59,6 +59,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True, ) 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 9e5c8ccd3e6..cbb793118f9 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_launch.py +++ b/nav2_system_tests/src/waypoint_follower/test_case_launch.py @@ -57,6 +57,10 @@ def generate_launch_description() -> LaunchDescription: source_file=params_file, root_key='', param_rewrites=param_substitutions, + value_rewrites={ + 'KEEPOUT_ZONE_ENABLED': 'False', + 'SPEED_ZONE_ENABLED': 'False', + }, convert_types=True) context = LaunchContext() diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 0da357b5815..a8860994253 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.3.1 + 1.4.0 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index df90b2b4aff..202516daa9d 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -238,26 +238,28 @@ ThetaStarPlanner::dynamicParametersCallback(std::vector param { rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_INTEGER) { - if (name == name_ + ".how_many_corners") { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if(param_name.find(name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == name_ + ".how_many_corners") { planner_->how_many_corners_ = parameter.as_int(); } - if (name == name_ + ".terminal_checking_interval") { + if (param_name == name_ + ".terminal_checking_interval") { planner_->terminal_checking_interval_ = parameter.as_int(); } - } else if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == name_ + ".w_euc_cost") { + } else if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + ".w_euc_cost") { planner_->w_euc_cost_ = parameter.as_double(); - } else if (name == name_ + ".w_traversal_cost") { + } else if (param_name == name_ + ".w_traversal_cost") { planner_->w_traversal_cost_ = parameter.as_double(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == name_ + ".use_final_approach_orientation") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + ".use_final_approach_orientation") { use_final_approach_orientation_ = parameter.as_bool(); - } else if (name == name_ + ".allow_unknown") { + } else if (param_name == name_ + ".allow_unknown") { planner_->allow_unknown_ = parameter.as_bool(); } } diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 74f0df2dc60..13bdf907739 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -81,25 +81,26 @@ rclcpp::Node::SharedPtr generate_internal_node(const std::string & prefix = ""); */ std::string time_to_string(size_t len); +using ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor; + /// Declares static ROS2 parameter and sets it to a given value if it was not already declared /* Declares static ROS2 parameter and sets it to a given value * if it was not already declared. * * \param[in] node A node in which given parameter to be declared - * \param[in] param_name The name of parameter + * \param[in] parameter_name The name of parameter * \param[in] default_value Parameter value to initialize with * \param[in] parameter_descriptor Parameter descriptor (optional) */ template void declare_parameter_if_not_declared( NodeT node, - const std::string & param_name, + const std::string & parameter_name, const rclcpp::ParameterValue & default_value, - const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor()) + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) { - if (!node->has_parameter(param_name)) { - node->declare_parameter(param_name, default_value, parameter_descriptor); + if (!node->has_parameter(parameter_name)) { + node->declare_parameter(parameter_name, default_value, parameter_descriptor); } } @@ -107,21 +108,135 @@ void declare_parameter_if_not_declared( /* Declares static ROS2 parameter with given type if it was not already declared. * * \param[in] node A node in which given parameter to be declared + * \param[in] parameter_name Name of the parameter * \param[in] param_type The type of parameter - * \param[in] default_value Parameter value to initialize with * \param[in] parameter_descriptor Parameter descriptor (optional) */ template void declare_parameter_if_not_declared( NodeT node, - const std::string & param_name, + const std::string & parameter_name, const rclcpp::ParameterType & param_type, - const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor()) + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) +{ + if (!node->has_parameter(parameter_name)) { + node->declare_parameter(parameter_name, param_type, parameter_descriptor); + } +} + +/// Declares a parameter with the specified type if it was not already declared +/// and returns its value if available. +/* Declares a parameter with the specified type if it was not already declared. + * If the parameter was overridden, its value is returned, otherwise an + * rclcpp::exceptions::InvalidParameterValueException is thrown + * + * \param[in] node A node in which given parameter to be declared + * \param[in] parameter_name Name of the parameter + * \param[in] param_type The type of parameter + * \param[in] parameter_descriptor Parameter descriptor (optional) + * \return The value of the parameter or an exception + */ +template +ParameterT declare_or_get_parameter( + NodeT node, + const std::string & parameter_name, + const rclcpp::ParameterType & param_type, + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) +{ + if (node->has_parameter(parameter_name)) { + return node->get_parameter(parameter_name).template get_value(); + } + auto parameter = node->declare_parameter(parameter_name, param_type, parameter_descriptor); + if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { + std::string description = "Parameter " + parameter_name + " not in overrides"; + throw rclcpp::exceptions::InvalidParameterValueException(description.c_str()); + } + return parameter.template get(); +} + +using NodeParamInterfacePtr = rclcpp::node_interfaces::NodeParametersInterface::SharedPtr; + +/// If the parameter is already declared, returns its value, +/// otherwise declares it and returns the default value +/** + * If the parameter is already declared, returns its value, + * otherwise declares it and returns the default value. + * The method can optionally print a warning or throw when the override is missing. + * + * \param[in] logger Node logging interface + * \param[in] param_interface Node parameter interface + * \param[in] parameter_name Name of the parameter + * \param[in] default_value Default value of the parameter + * \param[in] warn_if_no_override If true, prints a warning whenever the parameter override is missing + * \param[in] strict_param_loading If true, throws an InvalidParameterValueException if the parameter override is missing + * \param[in] parameter_descriptor Optional parameter descriptor + * \return The value of the param from the override if existent, otherwise the default value + */ +template +ParamType declare_or_get_parameter( + const rclcpp::Logger & logger, NodeParamInterfacePtr param_interface, + const std::string & parameter_name, const ParamType & default_value, + bool warn_if_no_override = false, bool strict_param_loading = false, + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) { - if (!node->has_parameter(param_name)) { - node->declare_parameter(param_name, param_type, parameter_descriptor); + if (param_interface->has_parameter(parameter_name)) { + rclcpp::Parameter param(parameter_name, default_value); + param_interface->get_parameter(parameter_name, param); + return param.get_value(); } + + auto return_value = param_interface + ->declare_parameter(parameter_name, rclcpp::ParameterValue{default_value}, + parameter_descriptor) + .get(); + + const bool no_param_override = param_interface->get_parameter_overrides().find(parameter_name) == + param_interface->get_parameter_overrides().end(); + if (no_param_override) { + if (warn_if_no_override) { + RCLCPP_WARN_STREAM( + logger, + "Failed to get param " << parameter_name << " from overrides, using default value."); + } + if (strict_param_loading) { + std::string description = "Parameter " + parameter_name + + " not in overrides and strict_param_loading is True"; + throw rclcpp::exceptions::InvalidParameterValueException(description.c_str()); + } + } + + return return_value; +} + +/// If the parameter is already declared, returns its value, +/// otherwise declares it and returns the default value +/** + * If the parameter is already declared, returns its value, + * otherwise declares it and returns the default value. + * The method can be configured to print a warn message or throw an InvalidParameterValueException + * when the override is missing by enabling the parameters warn_on_missing_params + * or strict_param_loading respectively. + * + * \param[in] node Pointer to a node object + * \param[in] parameter_name Name of the parameter + * \param[in] default_value Default value of the parameter + * \param[in] parameter_descriptor Optional parameter descriptor + * \return The value of the param from the override if existent, otherwise the default value + */ +template +ParamType declare_or_get_parameter( + NodeT node, const std::string & parameter_name, + const ParamType & default_value, + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()) +{ + declare_parameter_if_not_declared(node, "warn_on_missing_params", rclcpp::ParameterValue(false)); + bool warn_if_no_override{false}; + node->get_parameter("warn_on_missing_params", warn_if_no_override); + declare_parameter_if_not_declared(node, "strict_param_loading", rclcpp::ParameterValue(false)); + bool strict_param_loading{false}; + node->get_parameter("strict_param_loading", strict_param_loading); + return declare_or_get_parameter(node->get_logger(), node->get_node_parameters_interface(), + parameter_name, default_value, warn_if_no_override, strict_param_loading, parameter_descriptor); } /// Gets the type of plugin for the selected node and its plugin diff --git a/nav2_util/package.xml b/nav2_util/package.xml index bdc150c4120..97583144f86 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.3.1 + 1.4.0 Nav2 utilities Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index d7ffb3cf5d5..c43ba85c7ee 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -25,6 +25,7 @@ using nav2_util::generate_internal_node; using nav2_util::add_namespaces; using nav2_util::time_to_string; using nav2_util::declare_parameter_if_not_declared; +using nav2_util::declare_or_get_parameter; using nav2_util::get_plugin_type_param; TEST(SanitizeNodeName, SanitizeNodeName) @@ -78,6 +79,50 @@ TEST(DeclareParameterIfNotDeclared, DeclareParameterIfNotDeclared) ASSERT_EQ(param, "fred"); } +TEST(DeclareOrGetParam, DeclareOrGetParam) +{ + auto node = std::make_shared("test_node"); + + // test declared parameter + node->declare_parameter("foobar", "foo"); + std::string param = declare_or_get_parameter(node, "foobar", std::string{"bar"}); + EXPECT_EQ(param, "foo"); + node->get_parameter("foobar", param); + EXPECT_EQ(param, "foo"); + + // test undeclared parameter + node->set_parameter(rclcpp::Parameter("warn_on_missing_params", true)); + int int_param = declare_or_get_parameter(node, "waldo", 3); + EXPECT_EQ(int_param, 3); + + // test unknown parameter with strict_param_loading enabled + bool got_exception{false}; + node->set_parameter(rclcpp::Parameter("strict_param_loading", true)); + try { + declare_or_get_parameter(node, "burpy", true); + } catch (const rclcpp::exceptions::InvalidParameterValueException & exc) { + got_exception = true; + } + EXPECT_TRUE(got_exception); + // The parameter is anyway declared with the default val and subsequent calls won't fail + EXPECT_TRUE(declare_or_get_parameter(node, "burpy", true)); + + // test declaration by type of existing param + int_param = declare_or_get_parameter(node, "waldo", + rclcpp::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(int_param, 3); + + // test declaration by type of non existing param + got_exception = false; + try { + int_param = declare_or_get_parameter(node, "wololo", + rclcpp::ParameterType::PARAMETER_INTEGER); + } catch (const rclcpp::exceptions::InvalidParameterValueException & exc) { + got_exception = true; + } + EXPECT_TRUE(got_exception); +} + TEST(GetPluginTypeParam, GetPluginTypeParam) { ::testing::FLAGS_gtest_death_test_style = "threadsafe"; diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 5163de4f846..f2a0c2cd61b 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.3.1 + 1.4.0 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 3e2fb782c81..cfce20042ff 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -397,11 +397,14 @@ VelocitySmoother::dynamicParametersCallback(std::vector param result.successful = true; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == "smoothing_frequency") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "smoothing_frequency") { smoothing_frequency_ = parameter.as_double(); if (timer_) { timer_->cancel(); @@ -412,22 +415,23 @@ VelocitySmoother::dynamicParametersCallback(std::vector param timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); - } else if (name == "velocity_timeout") { + } else if (param_name == "velocity_timeout") { velocity_timeout_ = rclcpp::Duration::from_seconds(parameter.as_double()); - } else if (name == "odom_duration") { + } else if (param_name == "odom_duration") { odom_duration_ = parameter.as_double(); odom_smoother_ = std::make_unique( shared_from_this(), odom_duration_, odom_topic_); } - } else if (type == ParameterType::PARAMETER_DOUBLE_ARRAY) { + } else if (param_type == ParameterType::PARAMETER_DOUBLE_ARRAY) { if (parameter.as_double_array().size() != 3) { - RCLCPP_WARN(get_logger(), "Invalid size of parameter %s. Must be size 3", name.c_str()); + RCLCPP_WARN(get_logger(), "Invalid size of parameter %s. Must be size 3", + param_name.c_str()); result.successful = false; break; } - if (name == "max_velocity") { + if (param_name == "max_velocity") { for (unsigned int i = 0; i != 3; i++) { if (parameter.as_double_array()[i] < 0.0) { RCLCPP_WARN( @@ -439,7 +443,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param if (result.successful) { max_velocities_ = parameter.as_double_array(); } - } else if (name == "min_velocity") { + } else if (param_name == "min_velocity") { for (unsigned int i = 0; i != 3; i++) { if (parameter.as_double_array()[i] > 0.0) { RCLCPP_WARN( @@ -451,7 +455,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param if (result.successful) { min_velocities_ = parameter.as_double_array(); } - } else if (name == "max_accel") { + } else if (param_name == "max_accel") { for (unsigned int i = 0; i != 3; i++) { if (parameter.as_double_array()[i] < 0.0) { RCLCPP_WARN( @@ -463,7 +467,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param if (result.successful) { max_accels_ = parameter.as_double_array(); } - } else if (name == "max_decel") { + } else if (param_name == "max_decel") { for (unsigned int i = 0; i != 3; i++) { if (parameter.as_double_array()[i] > 0.0) { RCLCPP_WARN( @@ -475,11 +479,11 @@ VelocitySmoother::dynamicParametersCallback(std::vector param if (result.successful) { max_decels_ = parameter.as_double_array(); } - } else if (name == "deadband_velocity") { + } else if (param_name == "deadband_velocity") { deadband_velocities_ = parameter.as_double_array(); } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == "feedback") { + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == "feedback") { if (parameter.as_string() == "OPEN_LOOP") { open_loop_ = true; odom_smoother_.reset(); @@ -494,7 +498,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param result.successful = false; break; } - } else if (name == "odom_topic") { + } else if (param_name == "odom_topic") { odom_topic_ = parameter.as_string(); odom_smoother_ = std::make_unique( diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 748944d74e2..ef9eedd9b42 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.3.1 + 1.4.0 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index aeb3d44009d..409e652d8ff 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.3.1 + 1.4.0 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index dcf025aa3ce..a33c7aab9e3 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -474,15 +474,18 @@ WaypointFollower::dynamicParametersCallback(std::vector param rcl_interfaces::msg::SetParametersResult result; for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find('.') != std::string::npos) { + continue; + } - if (type == ParameterType::PARAMETER_INTEGER) { - if (name == "loop_rate") { + if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == "loop_rate") { loop_rate_ = parameter.as_int(); } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == "stop_on_failure") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == "stop_on_failure") { stop_on_failure_ = parameter.as_bool(); } } diff --git a/navigation2/package.xml b/navigation2/package.xml index 325943f50d3..6aa4209831c 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.3.1 + 1.4.0 ROS2 Navigation Stack diff --git a/tools/underlay.repos b/tools/underlay.repos index 0918320f01f..fc372b54c67 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -31,10 +31,6 @@ repositories: # type: git # url: https://github.com/cra-ros-pkg/robot_localization.git # version: ros2 - ros-navigation/nav2_minimal_turtlebot_simulation: - type: git - url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git - version: main ros/common_interfaces: type: git url: https://github.com/ros2/common_interfaces.git @@ -43,3 +39,7 @@ repositories: type: git url: https://github.com/ros2/launch.git version: humble + ros-navigation/nav2_minimal_turtlebot_simulation: + type: git + url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git + version: main diff --git a/tools/update_readme_table.py b/tools/update_readme_table.py index d36b65eac72..5ca0a2d1d84 100755 --- a/tools/update_readme_table.py +++ b/tools/update_readme_table.py @@ -18,8 +18,8 @@ import requests # Global information about current distributions, shouldn't need to update -OSs = {'humble': 'jammy', 'jazzy': 'noble'} -Prefixs = {'humble': 'H', 'jazzy': 'J'} +OSs = {'humble': 'jammy', 'jazzy': 'noble', 'kilted': 'noble'} +Prefixs = {'humble': 'H', 'jazzy': 'J', 'kilted': 'K'} # Set your packages here Packages = [ @@ -47,6 +47,7 @@ 'nav2_planner', 'nav2_regulated_pure_pursuit_controller', 'nav2_rotation_shim_controller', + 'nav2_route', 'nav2_rviz_plugins', 'nav2_simple_commander', 'nav2_smac_planner', @@ -60,7 +61,7 @@ ] # Set which distributions you care about -Distros = ['humble', 'jazzy'] +Distros = ['humble', 'jazzy', 'kilted'] def getSrcPath(package: str, prefix: str, OS: str) -> str: @@ -78,6 +79,7 @@ def createPreamble(Distros: list[str]) -> str: table = '| Package | ' for distro in Distros: table += distro + ' Source | ' + distro + ' Debian | ' + table = table[:-1] # Remove the last space table += '\n' table += '| :---: |' for distro in Distros: