diff --git a/.circleci/config.yml b/.circleci/config.yml index 723f94c4fdb..1cd0a121f87 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -2,6 +2,26 @@ version: 2.1 _commands: common_commands: &common_commands + ccache_stats: + description: "CCache Stats" + parameters: + workspace: + type: string + when: + type: string + default: on_success + steps: + - run: + name: CCache Stats + working_directory: << parameters.workspace >> + environment: + CCACHE_DIR: << parameters.workspace >>/.ccache + command: | + ccache -s # show stats + ccache -z # zero stats + ccache -V # show version + ccache -p # show config + when: << parameters.when >> restore_from_cache: description: "Restore From Cache" parameters: @@ -12,11 +32,17 @@ _commands: steps: - restore_cache: name: Restore Cache << parameters.key >> - key: "<< parameters.key >>-v1\ - -{{ arch }}\ - -{{ .Branch }}\ - -{{ .Environment.CIRCLE_PR_NUMBER }}\ - -{{ checksum \"<< parameters.workspace >>/checksum.txt\" }}" + keys: + - "<< parameters.key >>-v1\ + -{{ arch }}\ + -{{ .Branch }}\ + -{{ .Environment.CIRCLE_PR_NUMBER }}\ + -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" + - "<< parameters.key >>-v1\ + -{{ arch }}\ + -main\ + -\ + -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" save_to_cache: description: "Save To Cache" parameters: @@ -36,10 +62,13 @@ _commands: -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ - -{{ checksum \"<< parameters.workspace >>/checksum.txt\" }}\ + -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}\ -{{ epoch }}" paths: - - << parameters.path >> + - << parameters.path >>/.ccache + - << parameters.path >>/build + - << parameters.path >>/install + - << parameters.path >>/log when: << parameters.when >> install_dependencies: description: "Install Dependencies" @@ -54,28 +83,23 @@ _commands: working_directory: << parameters.workspace >> command: | . << parameters.underlay >>/install/setup.sh - cat << parameters.underlay >>/checksum.txt > checksum.txt - vcs export --exact src | \ - (echo vcs_export && cat) >> checksum.txt - sha256sum $PWD/checksum.txt >> checksum.txt - apt-get update - rosdep update - - # workarround for OMPL and rosdep - # https://github.com/ompl/ompl/issues/753 - # Prevent searching $ROS_WS/install given it's too big for rosdep - - if [ "$ROS_WS" == "<< parameters.underlay >>" ]; then - underlay_ws="" - else - underlay_ws=<< parameters.underlay >>/src + AMENT_PREFIX_PATH=$(echo "$AMENT_PREFIX_PATH" | \ + sed -e 's|:/opt/ros/'$ROS_DISTRO'$||') + if [ "$AMENT_PREFIX_PATH" == "/opt/ros/$ROS_DISTRO" ] + then + unset AMENT_PREFIX_PATH fi - echo underlay_ws = $underlay_ws + cat << parameters.underlay >>/lockfile.txt > lockfile.txt + vcs export --exact << parameters.underlay >>/src | \ + (echo vcs_export && cat) >> lockfile.txt + sha256sum $PWD/lockfile.txt >> lockfile.txt + + apt-get update + rosdep update --rosdistro $ROS_DISTRO dependencies=$( rosdep install -q -y \ --from-paths src \ - $underlay_ws \ --ignore-src \ --skip-keys " \ slam_toolbox \ @@ -84,9 +108,9 @@ _commands: awk '$1 ~ /^resolution\:/' | \ awk -F'[][]' '{print $2}' | \ tr -d \, | xargs -n1 | sort -u | xargs) - dpkg -s $dependencies | \ - (echo workspace_dependencies && cat) >> checksum.txt - sha256sum $PWD/checksum.txt >> checksum.txt + dpkg --list dpkg $dependencies | \ + (echo workspace_dependencies && cat) >> lockfile.txt + sha256sum $PWD/lockfile.txt >> lockfile.txt setup_workspace: description: "Setup Workspace" parameters: @@ -103,33 +127,78 @@ _commands: type: boolean steps: - store_artifacts: - path: << parameters.workspace >>/checksum.txt + path: << parameters.workspace >>/lockfile.txt - restore_from_cache: key: << parameters.key >> workspace: << parameters.workspace >> - when: condition: << parameters.build >> steps: + - ccache_stats: + workspace: << parameters.workspace >> + when: always - run: name: Build Workspace | << parameters.workspace >> working_directory: << parameters.workspace >> + environment: + CCACHE_DIR: << parameters.workspace >>/.ccache command: | - BUILD_UNFINISHED=$(colcon list --packages-skip-build-finished) - BUILD_FAILED=$(colcon list --packages-select-build-failed) - if [ -n "$BUILD_UNFINISHED" ] || [ -n "$BUILD_FAILED" ] + colcon cache lock + + BUILD_UNFINISHED=$( + colcon list \ + --names-only \ + --packages-skip-build-finished \ + | xargs) + echo BUILD_UNFINISHED: $BUILD_UNFINISHED + + BUILD_FAILED=$( + colcon list \ + --names-only \ + --packages-select-build-failed \ + | xargs) + echo BUILD_FAILED: $BUILD_FAILED + + BUILD_INVALID=$( + colcon list \ + --names-only \ + --packages-select-cache-invalid \ + --packages-select-cache-key build \ + | xargs) + echo BUILD_INVALID: $BUILD_INVALID + + BUILD_PACKAGES="" + if [ -n "$BUILD_UNFINISHED" ] || \ + [ -n "$BUILD_FAILED" ] || \ + [ -n "$BUILD_INVALID" ] then - . << parameters.underlay >>/install/setup.sh - rm -rf build install log - colcon build \ - --symlink-install \ - --mixin << parameters.mixins >> - else - echo "Skipping Build" + BUILD_PACKAGES=$( + colcon list \ + --names-only \ + --packages-above \ + $BUILD_UNFINISHED \ + $BUILD_FAILED \ + $BUILD_INVALID \ + | xargs) fi + echo BUILD_PACKAGES: $BUILD_PACKAGES + + colcon clean packages --yes \ + --packages-select ${BUILD_PACKAGES} \ + --base-select install + + . << parameters.underlay >>/install/setup.sh + colcon build \ + --packages-select ${BUILD_PACKAGES} \ + --mixin << parameters.mixins >> + - ccache_stats: + workspace: << parameters.workspace >> + when: always - save_to_cache: key: << parameters.key >> path: << parameters.workspace >> workspace: << parameters.workspace >> + when: always - run: name: Copy Build Logs working_directory: << parameters.workspace >> @@ -140,28 +209,76 @@ _commands: test_workspace: description: "Test Workspace" parameters: + key: + type: string workspace: type: string + cache_test: + type: boolean steps: - run: name: Test Workspace | << parameters.workspace >> working_directory: << parameters.workspace >> command: | - . install/setup.sh + TEST_FAILURES=$( + colcon list \ + --names-only \ + --packages-select-test-failures \ + | xargs) + echo TEST_FAILURES: $TEST_FAILURES + + TEST_INVALID=$( + colcon list \ + --names-only \ + --packages-select-cache-invalid \ + --packages-select-cache-key test \ + | xargs) + echo TEST_INVALID: $TEST_INVALID + + TEST_PACKAGES="" + if [ -n "$TEST_FAILURES" ] || \ + [ -n "$TEST_INVALID" ] + then + TEST_PACKAGES=$( + colcon list \ + --names-only \ + --packages-above \ + $TEST_FAILURES \ + $TEST_INVALID) + fi + if ( ! << parameters.cache_test >> ) + then + TEST_PACKAGES=$( + colcon list \ + --names-only) + fi TEST_PACKAGES=$( - colcon list --names-only | \ - circleci tests split \ - --split-by=timings \ - --timings-type=classname \ - --show-counts | \ - xargs) + echo $TEST_PACKAGES \ + | circleci tests split \ + --split-by=timings \ + --timings-type=classname \ + --show-counts \ + | xargs) + echo TEST_PACKAGES: $TEST_PACKAGES + + colcon clean packages --yes \ + --packages-select ${TEST_PACKAGES} \ + --base-select test_result + + . install/setup.sh set -o xtrace colcon test \ - --packages-select ${TEST_PACKAGES} \ - --retest-until-pass ${RETEST_UNTIL_PASS} \ - --ctest-args --test-regex "test_.*" + --packages-select ${TEST_PACKAGES} colcon test-result \ --verbose + - when: + condition: << parameters.cache_test >> + steps: + - save_to_cache: + key: << parameters.key >> + path: << parameters.workspace >> + workspace: << parameters.workspace >> + when: always - run: name: Copy Test Logs working_directory: << parameters.workspace >> @@ -169,44 +286,28 @@ _commands: when: always - store_artifacts: path: << parameters.workspace >>/log/test - - run: - name: Copy Test Results - working_directory: << parameters.workspace >> - command: | - mkdir test_results/ - cp -rH build/*/test_results/* test_results - when: always - store_test_results: path: << parameters.workspace >>/test_results - store_artifacts: path: << parameters.workspace >>/test_results - trigger_dockerhub_url: - description: "Trigger Dockerhub URL" - parameters: - data: - type: string - steps: - - run: - command: | - curl -H "Content-Type: application/json" \ - --data '<< parameters.data >>' \ - -X POST ${DOCKERHUB_TRIGGER_URL} _steps: pre_checkout: &pre_checkout run: name: Pre Checkout command: | - mkdir -p $ROS_WS && cd $ROS_WS + mkdir -p $ROS_WS/src && cd $ROS_WS ln -s /opt/ros/$ROS_DISTRO install + echo $CACHE_NONCE | \ - (echo cache_nonce && cat) >> checksum.txt - sha256sum $PWD/checksum.txt >> checksum.txt + (echo cache_nonce && cat) >> lockfile.txt + sha256sum $PWD/lockfile.txt >> lockfile.txt + TZ=utc stat -c '%y' /ros_entrypoint.sh | \ - (echo ros_entrypoint && cat) >> checksum.txt - sha256sum $PWD/checksum.txt >> checksum.txt + (echo ros_entrypoint && cat) >> lockfile.txt + sha256sum $PWD/lockfile.txt >> lockfile.txt + rm -rf $OVERLAY_WS/* - mv ~/.ccache /mnt/ramdisk/.ccache on_checkout: &on_checkout checkout: path: src/navigation2 @@ -214,15 +315,14 @@ _steps: run: name: Post Checkout command: | + cp $OVERLAY_WS/src/navigation2/.circleci/defaults.yaml $COLCON_DEFAULTS_FILE if ! cmp \ $OVERLAY_WS/src/navigation2/tools/underlay.repos \ $UNDERLAY_WS/underlay.repos >/dev/null 2>&1 then - echo "Cleaning Underlay" - rm -rf $UNDERLAY_WS/* + echo "Importing Underlay" cp $OVERLAY_WS/src/navigation2/tools/underlay.repos \ $UNDERLAY_WS/underlay.repos - mkdir -p $UNDERLAY_WS/src vcs import $UNDERLAY_WS/src \ < $UNDERLAY_WS/underlay.repos fi @@ -230,18 +330,6 @@ _steps: install_dependencies: underlay: /opt/ros_ws workspace: /opt/underlay_ws - restore_ccache: &restore_ccache - restore_from_cache: - key: ccache - workspace: /opt/underlay_ws - ccache_stats: &ccache_stats - run: - name: CCache Stats - command: | - ccache -s # show stats - ccache -z # zero stats - ccache -V # show version - ccache -p # show config setup_underlay_workspace: &setup_underlay_workspace setup_workspace: &setup_workspace_underlay key: underlay_ws @@ -266,18 +354,11 @@ _steps: setup_workspace: <<: *setup_workspace_overlay build: false - store_ccache_logs: &store_ccache_logs - store_artifacts: - path: /tmp/ccache.log - save_ccache: &save_ccache - save_to_cache: - key: ccache - workspace: /opt/underlay_ws - path: /mnt/ramdisk/.ccache - when: always test_overlay_workspace: &test_overlay_workspace test_workspace: + key: overlay_ws workspace: /opt/overlay_ws + cache_test: << parameters.cache_test >> collect_overlay_coverage: &collect_overlay_coverage run: name: Collect Code Coverage @@ -289,21 +370,15 @@ _steps: name: Upload Code Coverage working_directory: /opt/overlay_ws command: | - curl -s https://codecov.io/bash | bash -s -- \ - -f "lcov/project_coverage.info" \ + curl -s https://codecov.io/bash > codecov + codecov_version=$(grep -o 'VERSION=\"[0-9\.]*\"' codecov | cut -d'"' -f2) + shasum -a 512 -c <(curl -s "https://raw.githubusercontent.com/codecov/codecov-bash/${codecov_version}/SHA512SUM" | grep -w "codecov") + bash codecov \ + -f "lcov/total_coverage.info" \ -R "src/navigation2" \ - -t "${CODECOV_TOKEN}" \ - -n "${CIRCLE_BUILD_NUM}" \ -F "project" \ -Z || echo 'Codecov upload failed' when: always - trigger_dockerhub_build: &trigger_dockerhub_build - trigger_dockerhub_url: - data: | - { - "source_type": "Branch", - "source_name": "main" - } commands: <<: *common_commands @@ -317,18 +392,12 @@ commands: description: "Setup Dependencies" steps: - *install_underlay_dependencies - - *restore_ccache - - *ccache_stats - *setup_underlay_workspace - - *ccache_stats - *install_overlay_dependencies build_source: description: "Build Source" steps: - *setup_overlay_workspace - - *store_ccache_logs - - *ccache_stats - - *save_ccache restore_build: description: "Restore Build" steps: @@ -339,6 +408,9 @@ commands: - *restore_overlay_workspace test_build: description: "Test Build" + parameters: + cache_test: + type: boolean steps: - *test_overlay_workspace report_coverage: @@ -346,52 +418,47 @@ commands: steps: - *collect_overlay_coverage - *upload_overlay_coverage - trigger_dockerhub: - description: "Trigger DockerHub" - steps: - - *trigger_dockerhub_build _environments: common_environment: &common_environment ROS_WS: "/opt/ros_ws" UNDERLAY_WS: "/opt/underlay_ws" OVERLAY_WS: "/opt/overlay_ws" - CCACHE_DIR: "/mnt/ramdisk/.ccache" + UNDERLAY_MIXINS: "release ccache" CCACHE_LOGFILE: "/tmp/ccache.log" CCACHE_MAXSIZE: "200M" - MAKEFLAGS: "-j 1 -l 2" - RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED: "0" - RETEST_UNTIL_PASS: "2" + MAKEFLAGS: "-j 2 -l 2 " + COLCON_DEFAULTS_FILE: "/tmp/defaults.yaml" + RCUTILS_LOGGING_BUFFERED_STREAM: "0" + RCUTILS_LOGGING_USE_STDOUT: "0" DEBIAN_FRONTEND: "noninteractive" executors: debug_exec: docker: - - image: rosplanning/navigation2:main.debug + - image: rosplanning/navigation2:main working_directory: /opt/overlay_ws environment: <<: *common_environment CACHE_NONCE: "Debug" OVERLAY_MIXINS: "debug ccache coverage-gcc" - UNDERLAY_MIXINS: "release ccache" release_exec: docker: - - image: rosplanning/navigation2:main.release + - image: rosplanning/navigation2:main working_directory: /opt/overlay_ws environment: <<: *common_environment CACHE_NONCE: "Release" OVERLAY_MIXINS: "release ccache" - UNDERLAY_MIXINS: "release ccache" - python_exec: - docker: - - image: circleci/python _jobs: job_test: &job_test parameters: + cache_test: + type: boolean + default: false rmw: - default: "rmw_fastrtps_cpp" + default: "rmw_cyclonedds_cpp" type: string parallelism: 1 environment: @@ -412,18 +479,16 @@ jobs: executor: debug_exec steps: - restore_build - - test_build + - test_build: + cache_test: << parameters.cache_test >> - report_coverage release_test: <<: *job_test executor: release_exec steps: - restore_build - - test_build - rebuild_dockerhub: - executor: python_exec - steps: - - trigger_dockerhub + - test_build: + cache_test: << parameters.cache_test >> workflows: version: 2 @@ -433,10 +498,12 @@ workflows: - debug_test: requires: - debug_build + cache_test: true - release_build - release_test: requires: - release_build + cache_test: true nightly: jobs: - debug_build @@ -450,7 +517,7 @@ workflows: matrix: parameters: rmw: - # - rmw_connext_cpp + - rmw_connextdds - rmw_cyclonedds_cpp - rmw_fastrtps_cpp triggers: @@ -460,13 +527,3 @@ workflows: branches: only: - main - dockerhub: - jobs: - - rebuild_dockerhub - triggers: - - schedule: - cron: "0 7 * * *" - filters: - branches: - only: - - main diff --git a/.circleci/defaults.yaml b/.circleci/defaults.yaml new file mode 100644 index 00000000000..e0d946f78ee --- /dev/null +++ b/.circleci/defaults.yaml @@ -0,0 +1,16 @@ +_common: &common + "test-result-base": "test_results" + +"clean.packages": + <<: *common +"build": + <<: *common + "executor": "parallel" + "parallel-workers": 2 + "symlink-install": true +"test": + <<: *common + "executor": "parallel" + "parallel-workers": 1 +"test-result": + <<: *common diff --git a/.dockerhub/debug/Dockerfile b/.dockerhub/builder/Dockerfile similarity index 100% rename from .dockerhub/debug/Dockerfile rename to .dockerhub/builder/Dockerfile diff --git a/.dockerhub/debug/hooks/build b/.dockerhub/builder/hooks/build similarity index 69% rename from .dockerhub/debug/hooks/build rename to .dockerhub/builder/hooks/build index 671d5e3f3d7..8687b011e9c 100755 --- a/.dockerhub/debug/hooks/build +++ b/.dockerhub/builder/hooks/build @@ -1,15 +1,14 @@ #!/bin/bash set -ex -export FROM_IMAGE=osrf/ros2:nightly +export FROM_IMAGE=osrf/ros2:testing export FAIL_ON_BUILD_FAILURE="" export UNDERLAY_MIXINS="release ccache" -export OVERLAY_MIXINS="debug ccache coverage-gcc" docker build \ + --target builder \ --tag ${IMAGE_NAME} \ --build-arg FROM_IMAGE \ --build-arg FAIL_ON_BUILD_FAILURE \ --build-arg UNDERLAY_MIXINS \ - --build-arg OVERLAY_MIXINS \ --file ../../Dockerfile ../../. diff --git a/.dockerhub/distro.Dockerfile b/.dockerhub/distro.Dockerfile index 47296be46b5..6a7ab190b2e 100644 --- a/.dockerhub/distro.Dockerfile +++ b/.dockerhub/distro.Dockerfile @@ -6,14 +6,14 @@ # # Example build command: # export DOCKER_BUILDKIT=1 -# export FROM_IMAGE="ros:foxy" +# export FROM_IMAGE="ros:rolling" # export OVERLAY_MIXINS="release ccache" -# docker build -t nav2:foxy \ +# docker build -t nav2:rolling \ # --build-arg FROM_IMAGE \ # --build-arg OVERLAY_MIXINS \ # -f distro.Dockerfile ../ -ARG FROM_IMAGE=ros:foxy +ARG FROM_IMAGE=ros:rolling ARG OVERLAY_WS=/opt/overlay_ws # multi-stage for caching diff --git a/.dockerhub/release/Dockerfile b/.dockerhub/release/Dockerfile deleted file mode 120000 index 36c49d2a30c..00000000000 --- a/.dockerhub/release/Dockerfile +++ /dev/null @@ -1 +0,0 @@ -../../Dockerfile \ No newline at end of file diff --git a/.dockerhub/release/hooks/build b/.dockerhub/release/hooks/build deleted file mode 100755 index 50bc737ca60..00000000000 --- a/.dockerhub/release/hooks/build +++ /dev/null @@ -1,15 +0,0 @@ -#!/bin/bash -set -ex - -export FROM_IMAGE=osrf/ros2:nightly-rmw-nonfree -export FAIL_ON_BUILD_FAILURE="" -export UNDERLAY_MIXINS="release ccache" -export OVERLAY_MIXINS="release ccache" - -docker build \ - --tag ${IMAGE_NAME} \ - --build-arg FROM_IMAGE \ - --build-arg FAIL_ON_BUILD_FAILURE \ - --build-arg UNDERLAY_MIXINS \ - --build-arg OVERLAY_MIXINS \ - --file ../../Dockerfile ../../. diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index da6919c3e55..697c37e530e 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -35,3 +35,11 @@ * I see alot of redundancy in this package, we might want to add a function `bool XYZ()` to reduce clutter * I tested on a differential drive robot, but there might be issues turning near corners on an omnidirectional platform --> + +#### For Maintainers: +- [ ] Check that any new parameters added are updated in navigation.ros.org +- [ ] Check that any significant change is added to the migration guide +- [ ] Check that any new functions have Doxygen added +- [ ] Check that any new features have test coverage +- [ ] Check that any new plugins is added to the plugins page +- [ ] If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists diff --git a/.github/workflows/trigger_dockerhub.yaml b/.github/workflows/trigger_dockerhub.yaml new file mode 100644 index 00000000000..f4ba5d032c5 --- /dev/null +++ b/.github/workflows/trigger_dockerhub.yaml @@ -0,0 +1,52 @@ +--- +name: Trigger Docker Hub + +on: + schedule: + # 7am UTC, 12am PDT + - cron: '0 7 * * *' + push: + branches: + - main + paths: + - '**/package.xml' + - '**/*.repos' + +jobs: + rebuild_dockerhub: + name: Trigger Docker Hub + runs-on: ubuntu-latest + container: + image: rosplanning/navigation2:main + steps: + - name: "Initialize environment" + run: | + echo "TRIGGER=false" >> $GITHUB_ENV + - name: "Check apt updates" + if: ${{ github.event_name == 'schedule' }} + env: + SOURCELIST: sources.list.d/ros2.list + run: | + apt-get update \ + -o Dir::Etc::sourcelist="${SOURCELIST}" + apt-get --simulate upgrade \ + -o Dir::Etc::sourcelist="${SOURCELIST}" \ + | grep "0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded." \ + && echo "No apt updates" || echo "TRIGGER=true" >> $GITHUB_ENV + - name: "Check package updates" + if: ${{ github.event_name == 'push' }} + run: | + echo "TRIGGER=true" >> $GITHUB_ENV + - name: "Trigger Dockerhub URL" + if: ${{ fromJSON(env.TRIGGER) }} + env: + DATA: | + { + "source_type": "Branch", + "source_name": "main" + } + run: | + echo ${DATA} \ + | curl -H "Content-Type: application/json" \ + --data @- \ + -X POST ${{ secrets.DOCKERHUB_TRIGGER_URL }} diff --git a/.gitignore b/.gitignore index 173558e8c0e..88c775d303c 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,7 @@ __pycache__/ *.py[cod] sphinx_doc/_build + +# CLion artifacts +.idea +cmake-build-debug/ diff --git a/Dockerfile b/Dockerfile index ae2b22bb023..5ea809319ff 100644 --- a/Dockerfile +++ b/Dockerfile @@ -2,11 +2,11 @@ # Build context must be the /navigation2 root folder for COPY. # Example build command: # export UNDERLAY_MIXINS="debug ccache" -# export OVERLAY_MIXINS="debug ccache coverage" +# export OVERLAY_MIXINS="debug ccache coverage-gcc" # docker build -t nav2:latest \ # --build-arg UNDERLAY_MIXINS \ # --build-arg OVERLAY_MIXINS ./ -ARG FROM_IMAGE=osrf/ros2:nightly +ARG FROM_IMAGE=osrf/ros2:testing ARG UNDERLAY_WS=/opt/underlay_ws ARG OVERLAY_WS=/opt/overlay_ws @@ -17,8 +17,7 @@ FROM $FROM_IMAGE AS cacher ARG UNDERLAY_WS WORKDIR $UNDERLAY_WS/src COPY ./tools/underlay.repos ../ -RUN vcs import ./ < ../underlay.repos && \ - find ./ -name ".git" | xargs rm -rf +RUN vcs import ./ < ../underlay.repos # copy overlay source ARG OVERLAY_WS @@ -27,25 +26,45 @@ COPY ./ ./navigation2 # copy manifests for caching WORKDIR /opt -RUN mkdir -p /tmp/opt && \ - find ./ -name "package.xml" | \ - xargs cp --parents -t /tmp/opt && \ - find ./ -name "COLCON_IGNORE" | \ - xargs cp --parents -t /tmp/opt || true +RUN find . -name "src" -type d \ + -mindepth 1 -maxdepth 2 -printf '%P\n' \ + | xargs -I % mkdir -p /tmp/opt/% && \ + find . -name "package.xml" \ + | xargs cp --parents -t /tmp/opt && \ + find . -name "COLCON_IGNORE" \ + | xargs cp --parents -t /tmp/opt || true # multi-stage for building FROM $FROM_IMAGE AS builder + +# config dependencies install ARG DEBIAN_FRONTEND=noninteractive +RUN echo '\ +APT::Install-Recommends "0";\n\ +APT::Install-Suggests "0";\n\ +' > /etc/apt/apt.conf.d/01norecommend # install CI dependencies -RUN apt-get update && apt-get install -q -y \ +ARG RTI_NC_LICENSE_ACCEPTED=yes +RUN apt-get update && \ + apt-get upgrade -y && \ + apt-get install -y \ ccache \ lcov \ + python3-pip \ + ros-$ROS_DISTRO-rmw-fastrtps-cpp \ + ros-$ROS_DISTRO-rmw-connextdds \ + ros-$ROS_DISTRO-rmw-cyclonedds-cpp \ + && pip3 install \ + fastcov \ + git+https://github.com/ruffsl/colcon-cache.git@c1cedadc1ac6131fe825d075526ed4ae8e1b473c \ + git+https://github.com/ruffsl/colcon-clean.git@87dee2dd1e47c2b97ac6d8300f76e3f607d19ef6 \ && rosdep update \ && rm -rf /var/lib/apt/lists/* # install underlay dependencies ARG UNDERLAY_WS +ENV UNDERLAY_WS $UNDERLAY_WS WORKDIR $UNDERLAY_WS COPY --from=cacher /tmp/$UNDERLAY_WS ./ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ @@ -61,7 +80,9 @@ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ COPY --from=cacher $UNDERLAY_WS ./ ARG UNDERLAY_MIXINS="release ccache" ARG FAIL_ON_BUILD_FAILURE=True +ARG CCACHE_DIR=".ccache" RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ + colcon cache lock && \ colcon build \ --symlink-install \ --mixin $UNDERLAY_MIXINS \ @@ -70,30 +91,32 @@ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ # install overlay dependencies ARG OVERLAY_WS +ENV OVERLAY_WS $OVERLAY_WS WORKDIR $OVERLAY_WS COPY --from=cacher /tmp/$OVERLAY_WS ./ RUN . $UNDERLAY_WS/install/setup.sh && \ apt-get update && rosdep install -q -y \ --from-paths src \ - $UNDERLAY_WS/src \ --skip-keys " \ slam_toolbox \ "\ --ignore-src \ && rm -rf /var/lib/apt/lists/* +# multi-stage for testing +FROM builder AS tester + # build overlay source COPY --from=cacher $OVERLAY_WS ./ ARG OVERLAY_MIXINS="release ccache" RUN . $UNDERLAY_WS/install/setup.sh && \ + colcon cache lock && \ colcon build \ --symlink-install \ --mixin $OVERLAY_MIXINS \ || ([ -z "$FAIL_ON_BUILD_FAILURE" ] || exit 1) # source overlay from entrypoint -ENV UNDERLAY_WS $UNDERLAY_WS -ENV OVERLAY_WS $OVERLAY_WS RUN sed --in-place \ 's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \ /ros_entrypoint.sh diff --git a/Doxyfile b/Doxyfile index 834f11b1a1e..aa34c7acb22 100644 --- a/Doxyfile +++ b/Doxyfile @@ -771,7 +771,7 @@ WARN_LOGFILE = # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING # Note: If this tag is empty the current directory is searched. -INPUT = src +INPUT = . # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses diff --git a/README.md b/README.md index f77efb8ae63..867df8828a4 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Navigation 2 +# Nav2 [![Build Status](https://img.shields.io/docker/pulls/rosplanning/navigation2.svg?maxAge=2592000)](https://hub.docker.com/r/rosplanning/navigation2) [![Build Status](https://img.shields.io/docker/cloud/build/rosplanning/navigation2.svg?label=docker%20build)](https://hub.docker.com/r/rosplanning/navigation2) [![codecov](https://codecov.io/gh/ros-planning/navigation2/branch/main/graph/badge.svg)](https://codecov.io/gh/ros-planning/navigation2)

@@ -7,14 +7,16 @@ For detailed instructions on how to: - [Getting Started](https://navigation.ros.org/getting_started/index.html) +- [Concepts](https://navigation.ros.org/concepts/index.html) - [Build](https://navigation.ros.org/build_instructions/index.html#build) - [Install](https://navigation.ros.org/build_instructions/index.html#install) -- [Tutorials](https://navigation.ros.org/tutorials/index.html) +- [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html) - [Configure](https://navigation.ros.org/configuration/index.html) - [Navigation Plugins](https://navigation.ros.org/plugins/index.html) +- [Migration Guides](https://navigation.ros.org/migration/index.html) - [Contribute](https://navigation.ros.org/contribute/index.html) -Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://navigation2.slack.com). +Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). ## Citation @@ -36,31 +38,33 @@ please cite this work in your papers! ## Build Status -| Service | Dashing | Foxy | Main | +| Service | Foxy | Galactic | Main | | :---: | :---: | :---: | :---: | -| ROS Build Farm | [![Build Status](http://build.ros2.org/job/Ddev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Ddev__navigation2__ubuntu_bionic_amd64/) | [![Build Status](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/badge/icon)](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/) | N/A | +| ROS Build Farm | [![Build Status](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/badge/icon)](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/) | [![Build Status](http://build.ros2.org/job/Gdev__navigation2__ubuntu_focal_amd64/badge/icon)](http://build.ros2.org/job/Gdev__navigation2__ubuntu_focal_amd64/) | N/A | | Circle CI | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/main) | -| Package | Dashing Source | Dashing Debian | Eloquent Source | Eloquent Debian | Foxy Source | Foxy Debian | -| :---: | :---: | :---: | :---: | :---: | :---: | :---: | -| Navigation2 | [![Build Status](http://build.ros2.org/job/Dsrc_uB__navigation2__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__navigation2__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__navigation2__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__navigation2__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__navigation2__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__navigation2__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__navigation2__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__navigation2__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | -| nav2_amcl | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_amcl__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_amcl__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_amcl__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_amcl__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_amcl__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_amcl__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_amcl__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_amcl__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/) | -| nav2_behavior_tree | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_behavior_tree__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_behavior_tree__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_behavior_tree__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_behavior_tree__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_behavior_tree__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_behavior_tree__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_behavior_tree__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_behavior_tree__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/) | -| nav2_bringup | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_bringup__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_bringup__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_bringup__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_bringup__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_bringup__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_bringup__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_bringup__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_bringup__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/) | -| nav2_bt_navigator | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_bt_navigator__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_bt_navigator__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_bt_navigator__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_bt_navigator__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_bt_navigator__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_bt_navigator__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_bt_navigator__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_bt_navigator__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/) | -| nav2_common | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_common__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_common__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_common__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_common__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_common__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_common__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_common__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_common__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/) | -| nav2_controller | N/A | N/A | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_controller__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_controller__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_controller__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_controller__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/) | -| nav2_core | N/A | N/A | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_core__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_core__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_core__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_core__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/) | -| nav2_costmap_2d | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_costmap_2d__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_costmap_2d__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_costmap_2d__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_costmap_2d__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_costmap_2d__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_costmap_2d__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_costmap_2d__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_costmap_2d__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/) | -| nav2_dwb_controller | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_dwb_controller__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_dwb_controller__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_dwb_controller__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_dwb_controller__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_dwb_controller__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_dwb_controller__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_dwb_controller__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_dwb_controller__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/) | -| nav2_lifecycle_manager | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_lifecycle_manager__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_lifecycle_manager__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_lifecycle_manager__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_lifecycle_manager__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_lifecycle_manager__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_lifecycle_manager__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_lifecycle_manager__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_lifecycle_manager__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/) | -| nav2_map_server | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_map_server__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_map_server__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_map_server__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_map_server__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_map_server__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_map_server__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_map_server__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_map_server__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/) | -| nav2_msgs | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_msgs__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_msgs__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_msgs__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_msgs__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_msgs__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_msgs__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_msgs__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_msgs__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/) | -| nav2_navfn_planner | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_navfn_planner__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_navfn_planner__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_navfn_planner__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_navfn_planner__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_navfn_planner__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_navfn_planner__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_navfn_planner__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_navfn_planner__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/) | -| nav2_planner | N/A | N/A | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_planner__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_planner__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_planner__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_planner__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/) | -| nav2_recoveries | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_recoveries__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_recoveries__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_recoveries__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_recoveries__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_recoveries__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_recoveries__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_recoveries__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_recoveries__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/) | -| nav2_rviz_plugins | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_rviz_plugins__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_rviz_plugins__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_rviz_plugins__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_rviz_plugins__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_rviz_plugins__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_rviz_plugins__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_rviz_plugins__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_rviz_plugins__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/) -| nav2_system_tests | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_system_tests__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_system_tests__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_system_tests__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_system_tests__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_system_tests__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_system_tests__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_system_tests__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_system_tests__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/) | -| nav2_util | [![Build Status](http://build.ros2.org/job/Dsrc_uB__nav2_util__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Dsrc_uB__nav2_util__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Dbin_uB64__nav2_util__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Dbin_uB64__nav2_util__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_util__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_util__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_util__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_util__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/) | -| nav2_waypoint_follower | N/A | N/A | [![Build Status](http://build.ros2.org/job/Esrc_uB__nav2_waypoint_follower__ubuntu_bionic__source/badge/icon)](http://build.ros2.org/job/Esrc_uB__nav2_waypoint_follower__ubuntu_bionic__source/) | [![Build Status](http://build.ros2.org/job/Ebin_uB64__nav2_waypoint_follower__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros2.org/job/Ebin_uB64__nav2_waypoint_follower__ubuntu_bionic_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/) | +| Package | Foxy Source | Foxy Debian | Galactic Source | Galactic Debian | +| :---: | :---: | :---: | :---: | :---: | +| Navigation2 | [![Build Status](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__navigation2__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__navigation2__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__navigation2__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__navigation2__ubuntu_focal_amd64__binary/) | +| nav2_amcl | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_amcl__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_amcl__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_amcl__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_amcl__ubuntu_focal_amd64__binary/) | +| nav2_behavior_tree | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_behavior_tree__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_behavior_tree__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_behavior_tree__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_behavior_tree__ubuntu_focal_amd64__binary/) | +| nav2_bringup | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bringup__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_bringup__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_bringup__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_bringup__ubuntu_focal_amd64__binary/) | +| nav2_bt_navigator | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_bt_navigator__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_bt_navigator__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_bt_navigator__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_bt_navigator__ubuntu_focal_amd64__binary/) | +| nav2_common | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_common__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_common__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_common__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_common__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_common__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_common__ubuntu_focal_amd64__binary/) | +| nav2_controller | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_controller__ubuntu_focal_amd64__binary/) | +| nav2_core | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_core__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_core__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_core__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_core__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_core__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_core__ubuntu_focal_amd64__binary/) | +| nav2_costmap_2d | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_costmap_2d__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_costmap_2d__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_costmap_2d__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_costmap_2d__ubuntu_focal_amd64__binary/) | +| nav2_dwb_controller | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_dwb_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_dwb_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_dwb_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_dwb_controller__ubuntu_focal_amd64__binary/) | +| nav2_lifecycle_manager | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_lifecycle_manager__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_lifecycle_manager__ubuntu_focal_amd64__binary/) | +| nav2_map_server | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_map_server__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_map_server__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_map_server__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_map_server__ubuntu_focal_amd64__binary/) | +| nav2_msgs | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_msgs__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_msgs__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_msgs__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_msgs__ubuntu_focal_amd64__binary/) | +| nav2_navfn_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_navfn_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_navfn_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_navfn_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_navfn_planner__ubuntu_focal_amd64__binary/) | +| nav2_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_planner__ubuntu_focal_amd64__binary/) | +| nav2_recoveries | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_recoveries__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_recoveries__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_recoveries__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_recoveries__ubuntu_focal_amd64__binary/) | +| nav2_regulated_pure_pursuit | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Fbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_regulated_pure_pursuit_controller__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/badge/icon)](https://build.ros2.org/job/Gbin_uF64__nav2_regulated_pure_pursuit_controller__ubuntu_focal_amd64__binary/) | +| nav2_rviz_plugins | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_rviz_plugins__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_rviz_plugins__ubuntu_focal_amd64__binary/) +| nav2_smac_planner | [![Build Status](http://build.ros2.org/job/Fsrc_uF__smac_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__smac_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__smac_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__smac_planner__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_smac_planner__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_smac_planner__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_smac_planner__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_smac_planner__ubuntu_focal_amd64__binary/) | +| nav2_system_tests | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_system_tests__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_system_tests__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_system_tests__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_system_tests__ubuntu_focal_amd64__binary/) | +| nav2_util | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_util__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_util__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_util__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_util__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_util__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_util__ubuntu_focal_amd64__binary/) | +| nav2_waypoint_follower | [![Build Status](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Fsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Fbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/) | [![Build Status](http://build.ros2.org/job/Gsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/badge/icon)](http://build.ros2.org/job/Gsrc_uF__nav2_waypoint_follower__ubuntu_focal__source/) | [![Build Status](http://build.ros2.org/job/Gbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/badge/icon)](http://build.ros2.org/job/Gbin_uF64__nav2_waypoint_follower__ubuntu_focal_amd64__binary/) | diff --git a/codecov.yml b/codecov.yml index 202b5fdbbca..b30da128979 100644 --- a/codecov.yml +++ b/codecov.yml @@ -1,5 +1,6 @@ fixes: - "src/navigation2/::" + - "install/::" ignore: - "*/**/test/*" # ignore package test directories, e.g. nav2_dwb_controller/costmap_queue/tests diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md deleted file mode 100644 index aa23d7b1f93..00000000000 --- a/doc/parameters/param_list.md +++ /dev/null @@ -1,852 +0,0 @@ -*NOTE: <> means plugin are namespaced by a name/id parameterization. The bracketed names may change due to your application configuration* - -# bt_navigator - -| Parameter | Default | Description | -| ----------| --------| ------------| -| default_bt_xml_filename | N/A | path to the default behavior tree XML description | -| enable_groot_monitoring | True | enable Groot live monitoring of the behavior tree | -| groot_zmq_publisher_port | 1666 | change port of the zmq publisher needed for groot | -| groot_zmq_server_port | 1667 | change port of the zmq server needed for groot | -| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries | -| transform_tolerance | 0.1 | TF transform tolerance | -| global_frame | "map" | Reference frame | -| robot_base_frame | "base_link" | Robot base frame | -| odom_topic | "odom" | Odometry topic | - -# costmaps - -## Node: costmap_2d_ros - -Namespace: /parent_ns/local_ns - -| Parameter | Default | Description | -| ----------| --------| ------------| -| always_send_full_costmap | false | Whether to send full costmap every update, rather than updates | -| footprint_padding | 0.01 | Amount to pad footprint (m) | -| footprint | "[]" | Ordered set of footprint points, must be closed set | -| global_frame | "map" | Reference frame | -| height | 5 | Height of costmap (m) | -| width | 5 | Width of costmap (m) | -| lethal_cost_threshold | 100 | Minimum cost of an occupancy grid map to be considered a lethal obstacle | -| map_topic | "parent_namespace/map" | Topic of map from map_server or SLAM | -| observation_sources | [""] | List of sources of sensors, to be used if not specified in plugin specific configurations | -| origin_x | 0.0 | X origin of the costmap relative to width (m) | -| origin_y | 0.0 | Y origin of the costmap relative to height (m) | -| publish_frequency | 1.0 | Frequency to publish costmap to topic | -| resolution | 0.1 | Resolution of 1 pixel of the costmap, in meters | -| robot_base_frame | "base_link" | Robot base frame | -| robot_radius| 0.1 | Robot radius to use, if footprint coordinates not provided | -| rolling_window | false | Whether costmap should roll with robot base frame | -| track_unknown_space | false | If false, treats unknown space as free space, else as unknown space | -| transform_tolerance | 0.3 | TF transform tolerance | -| trinary_costmap | true | If occupancy grid map should be interpreted as only 3 values (free, occupied, unknown) or with its stored values | -| unknown_cost_value | 255 | Cost of unknown space if tracking it | -| update_frequency | 5.0 | Costmap update frequency | -| use_maximum | false | whether when combining costmaps to use the maximum cost or override | -| plugins | {"static_layer", "obstacle_layer", "inflation_layer"} | List of mapped plugin names for parameter namespaces and names | -| clearable_layers | ["obstacle_layer", "voxel_layer", "range_layer"] | Layers that may be cleared using the clearing service | - -**NOTE:** When `plugins` parameter is overridden, each plugin namespace defined in the list needs to have a `plugin` parameter defining the type of plugin to be loaded in the namespace. - -For example: - -```yaml -local_costmap: - ros__parameters: - plugins: ["obstacle_layer", "voxel_layer", "sonar_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - sonar_layer: - plugin: "nav2_costmap_2d::RangeSensorLayer" - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" -``` - -When `plugins` parameter is not overridden, the following default plugins are loaded: - -| Namespace | Plugin | -| ----------| --------| -| "static_layer" | "nav2_costmap_2d::StaticLayer" | -| "obstacle_layer" | "nav2_costmap_2d::ObstacleLayer" | -| "inflation_layer" | "nav2_costmap_2d::InflationLayer" | - -## static_layer plugin - -* ``: Name corresponding to the `nav2_costmap_2d::StaticLayer` plugin. This name gets defined in `plugins`, default value is `static_layer` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.enabled | true | Whether it is enabled | -| ``.subscribe_to_updates | false | Subscribe to static map updates after receiving first | -| ``.map_subscribe_transient_local | true | QoS settings for map topic | -| ``.transform_tolerance | 0.0 | TF tolerance | -| ``.map_topic | "" | Name of the map topic to subscribe to (empty means use the map_topic defined by `costmap_2d_ros`) | - -## inflation_layer plugin - -* ``: Name corresponding to the `nav2_costmap_2d::InflationLayer` plugin. This name gets defined in `plugins`, default value is `inflation_layer` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.enabled | true | Whether it is enabled | -| ``.inflation_radius | 0.55 | Radius to inflate costmap around lethal obstacles | -| ``.cost_scaling_factor | 10.0 | Exponential decay factor across inflation radius | -| ``.inflate_unknown | false | Whether to inflate unknown cells as if lethal | -| ``.inflate_around_unknown | false | Whether to inflate unknown cells | - -## obstacle_layer plugin - -* ``: Name corresponding to the `nav2_costmap_2d::ObstacleLayer` plugin. This name gets defined in `plugins`, default value is `obstacle_layer` -* ``: Name of a source provided in ```.observation_sources` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.enabled | true | Whether it is enabled | -| ``.footprint_clearing_enabled | true | Clear any occupied cells under robot footprint | -| ``.max_obstacle_height | 2.0 | Maximum height to add return to occupancy grid | -| ``.combination_method | 1 | Enum for method to add data to master costmap, default to maximum | -| ``.observation_sources | "" | namespace of sources of data | -| ``.topic | "" | Topic of data | -| ``.sensor_frame | "" | frame of sensor, to use if not provided by message | -| ``.observation_persistence | 0.0 | How long to store messages in a buffer to add to costmap before removing them (s) | -| ``.expected_update_rate | 0.0 | Expected rate to get new data from sensor | -| ``.data_type | "LaserScan" | Data type of input, LaserScan or PointCloud2 | -| ``.min_obstacle_height | 0.0 | Minimum height to add return to occupancy grid | -| ``.max_obstacle_height | 0.0 | Maximum height to add return to occupancy grid for this source | -| ``.inf_is_valid | false | Are infinite returns from laser scanners valid measurements | -| ``.marking | true | Whether source should mark in costmap | -| ``.clearing | false | Whether source should raytrace clear in costmap | -| ``.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap | -| ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | - -## range_sensor_layer plugin - -* ``: Name corresponding to the `nav2_costmap_2d::RangeSensorLayer` plugin. This name gets defined in `plugins`. - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.enabled | true | Whether it is enabled | -| ``.topics | [""] | Range topics to subscribe to | -| ``.phi | 1.2 | Phi value | -| ``.inflate_cone | 1.0 | Inflate the triangular area covered by the sensor (percentage) | -| ``.no_readings_timeout | 0.0 | No Readings Timeout | -| ``.clear_threshold | 0.2 | Probability below which cells are marked as free | -| ``.mark_threshold | 0.8 | Probability above which cells are marked as occupied | -| ``.clear_on_max_reading | false | Clear on max reading | -| ``.input_sensor_type | ALL | Input sensor type either ALL (automatic selection), VARIABLE (min range != max range), or FIXED (min range == max range) | - -## voxel_layer plugin - -* ``: Name corresponding to the `nav2_costmap_2d::VoxelLayer` plugin. This name gets defined in `plugins` -* ``: Name of a source provided in ``.observation_sources` - -*Note*: These parameters will only get declared if a `` name such as `voxel_layer` is appended to `plugins` parameter and `"nav2_costmap_2d::VoxelLayer"` is appended to its `plugin` name parameter. - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.enabled | true | Whether it is enabled | -| ``.footprint_clearing_enabled | true | Clear any occupied cells under robot footprint | -| ``.max_obstacle_height | 2.0 | Maximum height to add return to occupancy grid | -| ``.z_voxels | 10 | Number of voxels high to mark, maximum 16| -| ``.origin_z | 0.0 | Where to start marking voxels (m) | -| ``.z_resolution | 0.2 | Resolution of voxels in height (m) | -| ``.unknown_threshold | 15 | Minimum number of empty voxels in a column to mark as unknown in 2D occupancy grid | -| ``.mark_threshold | 0 | Minimum number of voxels in a column to mark as occupied in 2D occupancy grid | -| ``.combination_method | 1 | Enum for method to add data to master costmap, default to maximum | -| ``.publish_voxel_map | false | Whether to publish 3D voxel grid, computationally expensive | -| ``.observation_sources | "" | namespace of sources of data | -| ``.topic | "" | Topic of data | -| ``.sensor_frame | "" | frame of sensor, to use if not provided by message | -| ``.observation_persistence | 0.0 | How long to store messages in a buffer to add to costmap before removing them (s) | -| ``.expected_update_rate | 0.0 | Expected rate to get new data from sensor | -| ``.data_type | "LaserScan" | Data type of input, LaserScan or PointCloud2 | -| ``.min_obstacle_height | 0.0 | Minimum height to add return to occupancy grid | -| ``.max_obstacle_height | 0.0 | Maximum height to add return to occupancy grid for this source | -| ``.inf_is_valid | false | Are infinite returns from laser scanners valid measurements | -| ``.marking | true | Whether source should mark in costmap | -| ``.clearing | false | Whether source should raytrace clear in costmap | -| ``.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap | -| ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | - -## keepout filter - -* ``: Name corresponding to the `nav2_costmap_2d::KeepoutFilter` plugin. This name gets defined in `plugins`. - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.enabled | true | Whether it is enabled | -| ``.filter_info_topic | N/A | Name of the incoming CostmapFilterInfo topic having filter-related information | -| ``.transform_tolerance | 0.1 | TF tolerance | - -## speed filter - -* ``: Name corresponding to the `nav2_costmap_2d::SpeedFilter` plugin. This name gets defined in `plugins`. - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.enabled | true | Whether it is enabled | -| ``.filter_info_topic | N/A | Name of the incoming CostmapFilterInfo topic having filter-related information | -| ``.speed_limit_topic | "speed_limit" | Topic to publish speed limit to | -| ``.transform_tolerance | 0.1 | TF tolerance | - -# controller_server - -| Parameter | Default | Description | -| ----------| --------| ------------| -| controller_frequency | 20.0 | Frequency to run controller | -| progress_checker_plugin | "progress_checker" | Plugin used by the controller to check whether the robot has at least covered a set distance/displacement in a set amount of time, thus checking the progress of the robot. | -| `.plugin` | "nav2_controller::SimpleProgressChecker" | Default plugin | -| goal_checker_plugin | "goal_checker" | Check if the goal has been reached | -| `.plugin` | "nav2_controller::SimpleGoalChecker" | Default plugin | -| controller_plugins | ["FollowPath"] | List of mapped names for controller plugins for processing requests and parameters | -| `.plugin` | "dwb_core::DWBLocalPlanner" | Default plugin | -| min_x_velocity_threshold | 0.0001 | Minimum X velocity to use (m/s) | -| min_y_velocity_threshold | 0.0001 | Minimum Y velocity to use (m/s) | -| min_theta_velocity_threshold | 0.0001 | Minimum angular velocity to use (rad/s) | -| speed_limit_topic | "speed_limit" | Speed limiting topic name to subscribe | - -**NOTE:** When `controller_plugins` parameter is overridden, each plugin namespace defined in the list needs to have a `plugin` parameter defining the type of plugin to be loaded in the namespace. - -For example: - -```yaml -controller_server: - ros__parameters: - controller_plugins: ["FollowPath"] - FollowPath: - plugin: "dwb_core::DWBLocalPlanner" -``` - -When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parameters are not overridden, the following default plugins are loaded: - -| Namespace | Plugin | -| ----------| --------| -| "FollowPath" | "dwb_core::DWBLocalPlanner" | -| "progress_checker" | "nav2_controller::SimpleProgressChecker" | -| "goal_checker" | "nav2_controller::SimpleGoalChecker" | - -## simple_progress_checker plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.required_movement_radius | 0.5 | Minimum distance to count as progress (m) | -| ``.movement_time_allowance | 10.0 | Maximum time allowence for progress to happen (s) | - - -## simple_goal_checker plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) | -| ``.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) | -| ``.stateful | true | Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes | - -## stopped_goal_checker plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) | -| ``.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) | - -# dwb_controller - -* ``: DWB plugin name defined in `controller_plugin_ids` in the controller_server parameters - -## dwb_local_planner - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.critics | N/A | List of critic plugins to use | -| ``.default_critic_namespaces | ["dwb_critics"] | Namespaces to load critics in | -| ``.prune_plan | true | Whether to prune the path of old, passed points | -| ``.prune_distance | 1.0 | Distance (m) to prune backward until | -| ``.debug_trajectory_details | false | Publish debug information | -| ``.trajectory_generator_name | "dwb_plugins::StandardTrajectoryGenerator" | Trajectory generator plugin name | -| ``.transform_tolerance | 0.1 | TF transform tolerance | -| ``.short_circuit_trajectory_evaluation | true | Stop evaluating scores after best score is found | -| ``.path_distance_bias | N/A | Old version of `PathAlign.scale`, use that instead | -| ``.goal_distance_bias | N/A | Old version of `GoalAlign.scale`, use that instead | -| ``.occdist_scale | N/A | Old version of `ObstacleFootprint.scale`, use that instead | -| ``.max_scaling_factor | N/A | Old version of `ObstacleFootprint.max_scaling_factor`, use that instead | -| ``.scaling_speed | N/A | Old version of `ObstacleFootprint.scaling_speed`, use that instead | -| ``.PathAlign.scale | 32.0 | Scale for path align critic, overriding local default | -| ``.GoalAlign.scale | 24.0 | Scale for goal align critic, overriding local default | -| ``.PathDist.scale | 32.0 | Scale for path distance critic, overriding local default | -| ``.GoalDist.scale | 24.0 | Scale for goal distance critic, overriding local default | - -## publisher - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.publish_evaluation |true | Whether to publish the local plan evaluation | -| ``.publish_global_plan | true | Whether to publish the global plan | -| ``.publish_transformed_plan | true | Whether to publish the global plan in the odometry frame | -| ``.publish_local_plan | true | Whether to publish the local planner's plan | -| ``.publish_trajectories | true | Whether to publish debug trajectories | -| ``.publish_cost_grid_pc | false | Whether to publish the cost grid | -| ``.marker_lifetime | 0.1 | How long for the marker to remain | - -## oscillation TrajectoryCritic - -* ``: oscillation critic name defined in `.critics` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.``.oscillation_reset_dist | 0.05 | Minimum distance to move to reset oscillation watchdog (m) | -| ``.``.oscillation_reset_angle | 0.2 | Minimum angular distance to move to reset watchdog (rad) | -| ``.``.oscillation_reset_time | -1 | Duration when a reset may be called. If -1, cannot be reset. | -| ``.``.x_only_threshold | 0.05 | Threshold to check in the X velocity direction | -| ``.``.scale | 1.0 | Weighed scale for critic | - -## kinematic_parameters - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.max_vel_theta | 0.0 | Maximum angular velocity (rad/s) | -| ``.min_speed_xy | 0.0 | Minimum translational speed (m/s) | -| ``.max_speed_xy | 0.0 | Maximum translational speed (m/s) | -| ``.min_speed_theta | 0.0 | Minimum angular speed (rad/s) | -| ``.min_vel_x | 0.0 | Minimum velocity X (m/s) | -| ``.min_vel_y | 0.0 | Minimum velocity Y (m/s) | -| ``.max_vel_x | 0.0 | Maximum velocity X (m/s) | -| ``.max_vel_y | 0.0 | Maximum velocity Y (m/s) | -| ``.acc_lim_x | 0.0 | Maximum acceleration X (m/s^2) | -| ``.acc_lim_y | 0.0 | Maximum acceleration Y (m/s^2) | -| ``.acc_lim_theta | 0.0 | Maximum acceleration rotation (rad/s^2) | -| ``.decel_lim_x | 0.0 | Maximum deceleration X (m/s^2) | -| ``.decel_lim_y | 0.0 | Maximum deceleration Y (m/s^2) | -| ``.decel_lim_theta | 0.0 | Maximum deceleration rotation (rad/s^2) | - -## xy_theta_iterator - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.vx_samples | 20 | Number of velocity samples in the X velocity direction | -| ``.vy_samples | 5 | Number of velocity samples in the Y velocity direction | -| ``.vtheta_samples | 20 | Number of velocity samples in the angular directions | - -## base_obstacle TrajectoryCritic - -* ``: base_obstacle critic name defined in `.critics` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.``.sum_scores | false | Whether to allow for scores to be sumed up | -| ``.``.scale | 1.0 | Weight scale | - -## obstacle_footprint TrajectoryCritic - -* ``: obstacle_footprint critic name defined in `.critics` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.``.sum_scores | false | Whether to allow for scores to be sumed up | -| ``.``.scale | 1.0 | Weight scale | - -## prefer_forward TrajectoryCritic - -* ``: prefer_forward critic name defined in `.critics` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.``.penalty | 1.0 | Penalty to apply to backward motion | -| ``.``.strafe_x | 0.1 | Minimum X velocity before penalty | -| ``.``.strafe_theta | 0.2 | Minimum angular velocity before penalty | -| ``.``.theta_scale | 10.0 | Weight for angular velocity component | -| ``.``.scale | 1.0 | Weight scale | - -## twirling TrajectoryCritic - -* ``: twirling critic name defined in `.critics` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.``.scale | 0.0 | Weight scale | - -## goal_dist TrajectoryCritic - -| ``.``.aggregation_type | "last" | last, sum, or product combination methods | -| ``.``.scale | 1.0 | Weight scale | - -## goal_align TrajectoryCritic - -* ``: goal_align critic name defined in `.critics` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.``.forward_point_distance | 0.325 | Point in front of robot to look ahead to compute angular change from | -| ``.``.scale | 1.0 | Weight scale | -| ``.``.aggregation_type | "last" | last, sum, or product combination methods | - -## map_grid TrajectoryCritic - -* ``: map_grid critic name defined in `.critics` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.``.aggregation_type | "last" | last, sum, or product combination methods | -| ``.``.scale | 1.0 | Weight scale | - -## path_dist TrajectoryCritic - -* ``: path_dist critic name defined in `.critics` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.``.aggregation_type | "last" | last, sum, or product combination methods | -| ``.``.scale | 1.0 | Weight scale | - -## path_align TrajectoryCritic - -* ``: path_align critic name defined in `.critics` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.``.forward_point_distance | 0.325 | Point in front of robot to look ahead to compute angular change from | -| ``.``.scale | 1.0 | Weight scale | -| ``.``.aggregation_type | "last" | last, sum, or product combination methods | - -## rotate_to_goal TrajectoryCritic - -* ``: rotate_to_goal critic name defined in `.critics` - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) | -| ``.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) | -| ``.slowing_factor | 5.0 | Factor to slow robot motion by while rotating to goal | -| ``.lookahead_time | -1 | If > 0, amount of time to look forward for a collision for. | -| ``.``.scale | 1.0 | Weight scale | - -## standard_traj_generator plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.sim_time | 1.7 | Time to simulate ahead by (s) | -| ``.discretize_by_time | false | If true, forward simulate by time. If False, forward simulate by linear and angular granularity | -| ``.time_granularity | 0.5 | Time ahead to project | -| ``.linear_granularity | 0.5 | Linear distance forward to project | -| ``.angular_granularity | 0.025 | Angular distance to project | -| ``.include_last_point | true | Whether to include the last pose in the trajectory | - -## limited_accel_generator plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.sim_time | N/A | Time to simulate ahead by (s) | - -# lifecycle_manager - -| Parameter | Default | Description | -| ----------| --------| ------------| -| node_names | N/A | Ordered list of node names to bringup through lifecycle transition | -| autostart | false | Whether to transition nodes to active state on startup | -| bond_timeout | 4.0 | Timeout for bond to fail if no heartbeat can be found, in seconds. If set to 0, it will be disabled. Must be larger than 0.3s for stable bringup. | - -# map_server - -## map_saver - -| Parameter | Default | Description | -| ----------| --------| ------------| -| save_map_timeout | 2.0 | Timeout to attempt to save map with (s) | -| free_thresh_default | 0.25 | Free space maximum threshold for occupancy grid | -| occupied_thresh_default | 0.65 | Occupied space minimum threshhold for occupancy grid | -| map_subscribe_transient_local | true | Use transient local QoS profile for incoming map subscription | - -## map_server - -| Parameter | Default | Description | -| ----------| --------| ------------| -| yaml_filename | N/A | Path to map yaml file | -| topic_name | "map" | topic to publish loaded map to | -| frame_id | "map" | Frame to publish loaded map in | - -## costmap_filter_info_server - -| Parameter | Default | Description | -| ----------| --------| ------------| -| type | 0 | Type of costmap filter used. This is an enum for the type of filter this should be interpreted as. We provide the following pre-defined types: 0 - keepout zones / preferred lanes filter; 1 - speed filter, speed limit is specified in % of maximum speed; 2 - speed filter, speed limit is specified in absolute value (not implemented yet) | -| filter_info_topic | "costmap_filter_info" | Topic to publish costmap filter information to | -| mask_topic | "filter_mask" | Topic to publish filter mask to. The value of this parameter should be in accordance with `topic_name` parameter of `map_server` tuned to filter mask publishing | -| base | 0.0 | Base of `OccupancyGrid` mask value -> filter space value linear conversion which is being proceeded as: `filter_space_value = base + multiplier * mask_value` | -| multiplier | 1.0 | Multiplier of `OccupancyGrid` mask value -> filter space value linear conversion which is being proceeded as: `filter_space_value = base + multiplier * mask_value` | - -# planner_server - -| Parameter | Default | Description | -| ----------| --------| ------------| -| planner_plugins | ["GridBased"] | List of Mapped plugin names for parameters and processing requests | -| expected_planner_frequency | 20.0 | Expected planner frequency. If the current frequency is less than the expected frequency, display the warning message | - -**NOTE:** When `planner_plugins` parameter is overridden, each plugin namespace defined in the list needs to have a `plugin` parameter defining the type of plugin to be loaded in the namespace. - -For example: - -```yaml -planner_server: - ros__parameters: - planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" -``` - -When `planner_plugins` parameter is not overridden, the following default plugins are loaded: - -| Namespace | Plugin | -| ----------| --------| -| "GridBased" | "nav2_navfn_planner/NavfnPlanner" | - -# navfn_planner - -* ``: Corresponding planner plugin ID for this type - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.tolerance | 0.5 | Tolerance in meters between requested goal pose and end of path | -| ``.use_astar | false | Whether to use A*, if false, uses Dijstra's expansion | -| ``.allow_unknown | true | Whether to allow planning in unknown space | - -# smac_planner - -* ``: Corresponding planner plugin ID for this type - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.tolerance | 0.5 | Tolerance in meters between requested goal pose and end of path | -| ``.downsample_costmap | false | Whether to downsample costmap | -| ``.downsampling_factor | 1 | Factor to downsample costmap by | -| ``.allow_unknown | true | whether to allow traversing in unknown space | -| ``.max_iterations | -1 | Number of iterations before failing, disabled by -1 | -| ``.max_on_approach_iterations | 1000 | Iterations after within threshold before returning approximate path with best heuristic | -| ``.max_planning_time | 5.0 | Maximum planning time in s | -| ``.smooth_path | false | Whether to smooth path with CG smoother | -| ``.motion_model_for_search | DUBIN | Motion model to search with. Options for SE2: DUBIN, REEDS_SHEPP. 2D: MOORE, VON_NEUMANN | -| ``.angle_quantization_bins | 1 | Number of angle quantization bins for SE2 node | -| ``.minimum_turning_radius | 0.20 | Minimum turning radius in m of vehicle or desired path | -| ``.reverse_penalty | 2.0 | Penalty to apply to SE2 node if in reverse direction | -| ``.change_penalty | 0.5 | Penalty to apply to SE2 node if changing direction | -| ``.non_straight_penalty | 1.1 | Penalty to apply to SE2 node if non-straight direction | -| ``.cost_penalty | 1.2 | Penalty to apply to SE2 node for cost at pose | -| ``.analytic_expansion_ratio | 2.0 | For SE2 nodes the planner will attempt an analytic path expansion every N iterations, where N = closest_distance_to_goal / analytic_expansion_ratio. Higher ratios result in more frequent expansions | -| ``.smoother.smoother.w_curve | 1.5 | Smoother weight on curvature of path | -| ``.smoother.smoother.w_dist | 0.0 | Smoother weight on distance from original path | -| ``.smoother.smoother.w_smooth | 15000 | Smoother weight on distance between nodes | -| ``.smoother.smoother.w_cost | 1.5 | Smoother weight on costmap cost | -| ``.smoother.smoother.cost_scaling_factor | 10.0 | Inflation layer's scale factor | -| ``.smoother.optimizer.max_time | 0.10 | Maximum time to spend smoothing, in seconds | -| ``.smoother.optimizer.max_iterations | 500 | Maximum number of iterations to spend smoothing | -| ``.smoother.optimizer.debug_optimizer | false | Whether to print debug info from Ceres | -| ``.smoother.optimizer.gradient_tol | 1e-10 | Minimum change in gradient to terminate smoothing | -| ``.smoother.optimizer.fn_tol | 1e-7 | Minimum change in function to terminate smoothing | -| ``.smoother.optimizer.param_tol | 1e-15 | Minimum change in parameter block to terminate smoothing | - -| ``.smoother.optimizer.advanced.min_line_search_step_size | 1e-20 | Terminate smoothing iteration if change in parameter block less than this | -| ``.smoother.optimizer.advanced.max_num_line_search_step_size_iterations | 50 | Maximum iterations for line search | -| ``.smoother.optimizer.advanced.line_search_sufficient_function_decrease | 1e-20 | Function decrease amount to terminate current line search iteration | -| ``.smoother.optimizer.advanced.max_num_line_search_direction_restarts | 10 | Maximum umber of restarts of line search when over-estimating | -| ``.smoother.optimizer.advanced.max_line_search_step_expansion | 50 | Step size multiplier at each iteration of line search | - -# waypoint_follower - -| Parameter | Default | Description | -| ----------| --------| ------------| -| stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. | -| loop_rate | 20 | Rate to check for results from current navigation task | -| waypoint_task_executor_plugin | `waypoint_task_executor` | Name of plugin to be loaded for executing waypoint tasks.| - -## WaitAtWaypoint plugin - -* ``: Name corresponding to the `nav2_waypoint_follower::WaitAtWaypoint` plugin. - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.enabled | true | Whether it is enabled | -| ``.waypoint_pause_duration | 0 | Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | - -## PhotoAtWaypoint plugin - -* ``: Name corresponding to the `nav2_waypoint_follower::PhotoAtWaypoint` plugin. - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.enabled | true | Whether it is enabled | -| ``.image_topic | "/camera/color/image_raw" | Camera image topic name to susbcribe | -| ``.save_dir | "/tmp/waypoint_images" | Path to directory to save taken photos | -| ``.image_format | "png" | Desired image format A few other options; "jpeg" , "jpg", "pgm", "tiff" | - -## InputAtWaypoint plugin - -* ``: Name corresponding to the `nav2_waypoint_follower::InputAtWaypoint` plugin. - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.timeout | 10.0 | Amount of time in seconds to wait for user input before moving on to the next waypoint. | -| ``.input_topic | "input_at_waypoint/input" | Topic input is published to to indicate to move to the next waypoint, in `std_msgs/Empty`. | - -# recoveries - -## recovery_server - -| Parameter | Default | Description | -| ----------| --------| ------------| -| costmap_topic | "local_costmap/costmap_raw" | Raw costmap topic for collision checking | -| footprint_topic | "local_costmap/published_footprint" | Topic for footprint in the costmap frame | -| cycle_frequency | 10.0 | Frequency to run recovery plugins | -| transform_tolerance | 0.1 | TF transform tolerance | -| global_frame | "odom" | Reference frame | -| robot_base_frame | "base_link" | Robot base frame | -| recovery_plugins | {"spin", "backup", "wait"}| List of plugin names to use, also matches action server names | - -**NOTE:** When `recovery_plugins` parameter is overridden, each plugin namespace defined in the list needs to have a `plugin` parameter defining the type of plugin to be loaded in the namespace. - -For example: - -```yaml -recoveries_server: - ros__parameters: - recovery_plugins: ["spin", "backup", "wait"] - spin: - plugin: "nav2_recoveries/Spin" - backup: - plugin: "nav2_recoveries/BackUp" - wait: - plugin: "nav2_recoveries/Wait" -``` - -When `recovery_plugins` parameter is not overridden, the following default plugins are loaded: - -| Namespace | Plugin | -| ----------| --------| -| "spin" | "nav2_recoveries/Spin" | -| "backup" | "nav2_recoveries/BackUp" | -| "wait" | "nav2_recoveries/Wait" | - -## spin plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| simulate_ahead_time | 2.0 | Time to look ahead for collisions (s) | -| max_rotational_vel | 1.0 | Maximum rotational velocity (rad/s) | -| min_rotational_vel | 0.4 | Minimum rotational velocity (rad/s) | -| rotational_acc_lim | 3.2 | maximum rotational acceleration (rad/s^2) | - -## backup plugin - -| Parameter | Default | Description | -| ----------| --------| ------------| -| simulate_ahead_time | 2.0 | Time to look ahead for collisions (s) | - -# AMCL - -| Parameter | Default | Description | -| ----------| --------| ------------| -| alpha1 | 0.2 | Expected process noise in odometry's rotation estimate from rotation | -| alpha2 | 0.2 | Expected process noise in odometry's rotation estimate from translation | -| alpha3 | 0.2 | Expected process noise in odometry's translation estimate from translation | -| alpha4 | 0.2 | Expected process noise in odometry's translation estimate from rotation | -| alpha5 | 0.2 | For Omni models only: translation noise | -| base_frame_id | "base_footprint" | Base frame | -| beam_skip_distance | 0.5 | Ignore beams that most particles disagree with in Likelihood field model. Maximum distance to consider skipping for (m) | -| beam_skip_error_threshold | 0.9 | Percentage of beams after not matching map to force full update due to bad convergance | -| beam_skip_threshold | 0.3 | Percentage of beams required to skip | -| do_beamskip | false | Whether to do beam skipping in Likelihood field model. | -| global_frame_id | "map" | The name of the coordinate frame published by the localization system | -| lambda_short | 0.1 | Exponential decay parameter for z_short part of model | -| laser_likelihood_max_dist | 2.0 | Maximum distance to do obstacle inflation on map, for use in likelihood_field model | -| laser_max_range | 100.0 | Maximum scan range to be considered, -1.0 will cause the laser's reported maximum range to be used | -| laser_min_range | -1 | Minimum scan range to be considered, -1.0 will cause the laser's reported minimum range to be used | -| laser_model_type | "likelihood_field" | Which model to use, either beam, likelihood_field, or likelihood_field_prob. Same as likelihood_field but incorporates the beamskip feature, if enabled | -| set_initial_pose | false | Causes AMCL to set initial pose from the initial_pose* parameters instead of waiting for the initial_pose message | -| initial_pose.x | 0.0 | X coordinate of the initial robot pose in the map frame | -| initial_pose.y | 0.0 | Y coordinate of the initial robot pose in the map frame | -| initial_pose.z | 0.0 | Z coordinate of the initial robot pose in the map frame | -| initial_pose.yaw | 0.0 | Yaw of the initial robot pose in the map frame | -| max_beams | 60 | How many evenly-spaced beams in each scan to be used when updating the filter | -| max_particles | 2000 | Maximum allowed number of particles | -| min_particles | 500 | Minimum allowed number of particles | -| odom_frame_id | "odom" | Which frame to use for odometry | -| pf_err | 0.05 | Particle Filter population error | -| pf_z | 0.99 | Particle filter population density | -| recovery_alpha_fast | 0.0 | Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001| -| resample_interval | 1 | Number of filter updates required before resampling | -| robot_model_type | "differential" | | -| save_pose_rate | 0.5 | Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used on subsequent runs to initialize the filter (-1.0 to disable) | -| sigma_hit | 0.2 | Standard deviation for Gaussian model used in z_hit part of the model. | -| tf_broadcast | true | Set this to false to prevent amcl from publishing the transform between the global frame and the odometry frame | -| transform_tolerance | 1.0 | Time with which to post-date the transform that is published, to indicate that this transform is valid into the future | -| update_min_a | 0.2 | Rotational movement required before performing a filter update | -| update_min_d | 0.25 | Translational movement required before performing a filter update | -| z_hit | 0.5 | Mixture weight for z_hit part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | -| z_max | 0.05 | Mixture weight for z_max part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | -| z_rand | 0.5 | Mixture weight for z_rand part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | -| z_short | 0.05 | Mixture weight for z_short part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | -| always_reset_initial_pose | false | Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter (with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the last known pose to initialize | -| scan_topic | scan | Topic to subscribe to in order to receive the laser scan for localization | -| map_topic | map | Topic to subscribe to in order to receive the map for localization | - ---- - -# Behavior Tree Nodes - -## Actions - -### BT Node BackUp - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| backup_dist | -0.15 | Total distance to backup | -| backup_speed | 0.025 | Backup speed | -| server_name | N/A | Action server name | -| server_timeout | 10 | Action server timeout (ms) | - -### BT Node ClearEntireCostmap - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| service_name | N/A | Server name | -| server_timeout | 10 | Action server timeout (ms) | - -### BT Node ComputePathToPose - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| goal | N/A | Goal pose | -| planner_id | N/A | Mapped name to the planner plugin type to use, e.g. GridBased | -| server_name | N/A | Action server name | -| server_timeout | 10 | Action server timeout (ms) | - -| Output Port | Default | Description | -| ----------- | ------- | ----------- | -| path | N/A | Path created by action server | - -### BT Node FollowPath - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| path | N/A | Path to follow | -| controller_id | N/A | Mapped name of the controller plugin type to use, e.g. FollowPath | -| server_name | N/A | Action server name | -| server_timeout | 10 | Action server timeout (ms) | - -### BT Node NavigateToPose - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| goal | N/A | Goal | -| server_name | N/A | Action server name | -| server_timeout | 10 | Action server timeout (ms) | - -### BT Node ReinitializeGlobalLocalization - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| service_name | N/A | Server name | -| server_timeout | 10 | Action server timeout (ms) | - -### BT Node Spin - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| spin_dist | 1.57 | Spin distance (radians) | -| server_name | N/A | Action server name | -| server_timeout | 10 | Action server timeout (ms) | - -### BT Node Wait - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| wait_duration | 1 | Wait time (s) | -| server_name | N/A | Action server name | -| server_timeout | 10 | Action server timeout (ms) | - -### BT Node TruncatePath - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| input_path | N/A | Path to be truncated | -| output_path | N/A | Path truncated | -| distance | 1.0 | Distance (m) to cut from last pose | - -## Conditions - -### BT Node DistanceTraveled - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| distance | 1.0 | Distance in meters after which the node should return success | -| global_frame | "map" | Reference frame | -| robot_base_frame | "base_link" | Robot base frame | - -### BT Node GoalReached - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| goal | N/A | Destination to check | -| global_frame | "map" | Reference frame | -| robot_base_frame | "base_link" | Robot base frame | - -### BT Node IsBatteryLow - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| min_battery | N/A | Minimum battery percentage/voltage | -| battery_topic | "/battery_status" | Battery topic | -| is_voltage | false | If true voltage will be used to check for low battery | - -### BT Node TimeExpired - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| seconds | 1.0 | Number of seconds after which node returns success | - -### BT Node TransformAvailable - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| child | "" | Child frame for transform | -| parent | "" | parent frame for transform | - -## Controls - -### BT Node RecoveryNode - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| number_of_retries | 1 | Number of retries | - -## Decorators - -### BT Node DistanceController - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| distance | 1.0 | Distance (m) | -| global_frame | "map" | Reference frame | -| robot_base_frame | "base_link" | Robot base frame | - -### BT Node RateController - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| hz | 10.0 | Rate to throttle | - -### BT Node SpeedController - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| min_rate | 0.1 | Minimum rate (hz) | -| max_rate | 1.0 | Maximum rate (hz) | -| min_speed | 0.0 | Minimum speed (m/s) | -| max_speed | 0.5 | Maximum speed (m/s) | -| filter_duration | 0.3 | Duration (secs) for velocity smoothing filter | - -### BT Node GoalUpdater - -| Input Port | Default | Description | -| ---------- | ------- | ----------- | -| input_goal | N/A | The reference goal | -| output_goal | N/A | The reference goal, or a newer goal received by subscription | diff --git a/doc/use_cases/README.md b/doc/use_cases/README.md index 14ea423c0e9..47d6ee56885 100644 --- a/doc/use_cases/README.md +++ b/doc/use_cases/README.md @@ -1,7 +1,7 @@ # Target Use Cases -The Navigation2 system is targeting the following use cases: +The Nav2 system is targeting the following use cases: [2D Indoor Navigation](indoor_navigation_use_case.md) - example: Warehouse / Logistics robot diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index dc86baaf6e1..a527a80a523 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -28,7 +28,6 @@ #include #include -#include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "message_filters/subscriber.h" #include "nav2_util/lifecycle_node.hpp" @@ -52,19 +51,42 @@ namespace nav2_amcl { - +/* + * @class AmclNode + * @brief ROS wrapper for AMCL + */ class AmclNode : public nav2_util::LifecycleNode { public: + /* + * @brief AMCL constructor + */ AmclNode(); + /* + * @brief AMCL destructor + */ ~AmclNode(); protected: - // Implement the lifecycle interface + /* + * @brief Lifecycle configure + */ nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /* + * @brief Lifecycle activate + */ nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /* + * @brief Lifecycle deactivate + */ nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /* + * @brief Lifecycle cleanup + */ nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /* + * @brief Lifecycle shutdown + */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't @@ -80,11 +102,30 @@ class AmclNode : public nav2_util::LifecycleNode } amcl_hyp_t; // Map-related + /* + * @brief Get new map from ROS topic to localize in + * @param msg Map message + */ void mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + /* + * @brief Handle a new map message + * @param msg Map message + */ void handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg); + /* + * @brief Creates lookup table of free cells in map + */ void createFreeSpaceVector(); + /* + * @brief Frees allocated map related memory + */ void freeMapDependentMemory(); map_t * map_{nullptr}; + /* + * @brief Convert an occupancy grid map to an AMCL map + * @param map_msg Map message + * @return pointer to map for AMCL to use + */ map_t * convertMap(const nav_msgs::msg::OccupancyGrid & map_msg); bool first_map_only_{true}; std::atomic first_map_received_{false}; @@ -96,6 +137,9 @@ class AmclNode : public nav2_util::LifecycleNode #endif // Transforms + /* + * @brief Initialize required ROS transformations + */ void initTransforms(); std::shared_ptr tf_broadcaster_; std::shared_ptr tf_listener_; @@ -103,29 +147,49 @@ class AmclNode : public nav2_util::LifecycleNode bool sent_first_transform_{false}; bool latest_tf_valid_{false}; tf2::Transform latest_tf_; + /* + * @brief Wait for transformation required to operate (laser to base) + */ void waitForTransforms(); // Message filters + /* + * @brief Initialize incoming data message subscribers and filters + */ void initMessageFilters(); std::unique_ptr> laser_scan_sub_; std::unique_ptr> laser_scan_filter_; message_filters::Connection laser_scan_connection_; // Publishers and subscribers + /* + * @brief Initialize pub subs of AMCL + */ void initPubSub(); rclcpp::Subscription::ConstSharedPtr initial_pose_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr pose_pub_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr particlecloud_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr particle_cloud_pub_; + /* + * @brief Handle with an initial pose estimate is received + */ void initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + /* + * @brief Handle when a laser scan is received + */ void laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan); // Services and service callbacks + /* + * @brief Initialize state services + */ void initServices(); rclcpp::Service::SharedPtr global_loc_srv_; + /* + * @brief Service callback for a global relocalization request + */ void globalLocalizationCallback( const std::shared_ptr request_header, const std::shared_ptr request, @@ -133,6 +197,9 @@ class AmclNode : public nav2_util::LifecycleNode // Let amcl update samples without requiring motion rclcpp::Service::SharedPtr nomotion_update_srv_; + /* + * @brief Request an AMCL update even though the robot hasn't moved + */ void nomotionUpdateCallback( const std::shared_ptr request_header, const std::shared_ptr request, @@ -142,12 +209,18 @@ class AmclNode : public nav2_util::LifecycleNode std::atomic force_update_{false}; // Odometry + /* + * @brief Initialize odometry + */ void initOdometry(); std::unique_ptr motion_model_; geometry_msgs::msg::PoseStamped latest_odom_pose_; geometry_msgs::msg::PoseWithCovarianceStamped last_published_pose_; double init_pose_[3]; // Initial robot pose double init_cov_[3]; + /* + * @brief Get robot pose in odom frame using TF + */ bool getOdomPose( // Helper to get odometric pose from transform system geometry_msgs::msg::PoseStamped & pose, @@ -156,8 +229,13 @@ class AmclNode : public nav2_util::LifecycleNode std::atomic first_pose_sent_; // Particle filter + /* + * @brief Initialize particle filter + */ void initParticleFilter(); - // Pose-generating function used to uniformly distribute particles over the map + /* + * @brief Pose-generating function used to uniformly distribute particles over the map + */ static pf_vector_t uniformPoseGenerator(void * arg); pf_t * pf_{nullptr}; bool pf_init_; @@ -165,41 +243,78 @@ class AmclNode : public nav2_util::LifecycleNode int resample_count_{0}; // Laser scan related + /* + * @brief Initialize laser scan + */ void initLaserScan(); + /* + * @brief Create a laser object + */ nav2_amcl::Laser * createLaserObject(); int scan_error_count_{0}; std::vector lasers_; std::vector lasers_update_; std::map frame_to_laser_; rclcpp::Time last_laser_received_ts_; + /* + * @brief Check if a laser has been received + */ void checkLaserReceived(); std::chrono::seconds laser_check_interval_; // TODO(mjeronimo): not initialized - + /* + * @brief Check if sufficient time has elapsed to get an update + */ bool checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time last_time); rclcpp::Time last_time_printed_msg_; - + /* + * @brief Add a new laser scanner if a new one is received in the laser scallbacks + */ bool addNewScanner( int & laser_index, const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan, const std::string & laser_scan_frame_id, geometry_msgs::msg::PoseStamped & laser_pose); + /* + * @brief Whether the pf needs to be updated + */ bool shouldUpdateFilter(const pf_vector_t pose, pf_vector_t & delta); + /* + * @brief Update the PF + */ bool updateFilter( const int & laser_index, const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan, const pf_vector_t & pose); + /* + * @brief Publish particle cloud + */ void publishParticleCloud(const pf_sample_set_t * set); + /* + * @brief Get the current state estimat hypothesis from the particle cloud + */ bool getMaxWeightHyp( std::vector & hyps, amcl_hyp_t & max_weight_hyps, int & max_weight_hyp); + /* + * @brief Publish robot pose in map frame from AMCL + */ void publishAmclPose( const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan, const std::vector & hyps, const int & max_weight_hyp); + /* + * @brief Determine TF transformation from map to odom + */ void calculateMaptoOdomTransform( const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan, const std::vector & hyps, const int & max_weight_hyp); + /* + * @brief Publish TF transformation from map to odom + */ void sendMapToOdomTransform(const tf2::TimePoint & transform_expiration); + /* + * @brief Handle a new pose estimate callback + */ void handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg); bool init_pose_received_on_inactive{false}; bool initial_pose_is_known_{false}; @@ -210,7 +325,9 @@ class AmclNode : public nav2_util::LifecycleNode double initial_pose_z_; double initial_pose_yaw_; - // Node parameters (initialized via initParameters) + /* + * @brief Get ROS parameters for node + */ void initParameters(); double alpha1_; double alpha2_; diff --git a/nav2_amcl/include/nav2_amcl/angleutils.hpp b/nav2_amcl/include/nav2_amcl/angleutils.hpp index d3f8332c5b3..84de30f78db 100644 --- a/nav2_amcl/include/nav2_amcl/angleutils.hpp +++ b/nav2_amcl/include/nav2_amcl/angleutils.hpp @@ -27,10 +27,26 @@ namespace nav2_amcl { +/* + * @class angleutils + * @brief Some utilities for working with angles + */ class angleutils { public: + /* + * @brief Normalize angles + * @brief z Angle to normalize + * @return normalized angle + */ static double normalize(double z); + + /* + * @brief Find minimum distance between 2 angles + * @brief a Angle 1 + * @brief b Angle 2 + * @return normalized angle difference + */ static double angle_diff(double a, double b); }; diff --git a/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp b/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp index 77fc388914a..14e68aff53f 100644 --- a/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp +++ b/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp @@ -24,21 +24,61 @@ namespace nav2_amcl { +/** + * @class nav2_amcl::MotionModel + * @brief An abstract motion model class + */ class MotionModel { public: virtual ~MotionModel() = default; + + /** + * @brief Update on new odometry data + * @param pf The particle filter to update + * @param pose pose of robot in odometry update + * @param delta change in pose in odometry update + */ virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta) = 0; + /** + * @brief An factory to create motion models + * @param type Type of motion model to create in factory + * @param alpha1 error parameters, see documentation + * @param alpha2 error parameters, see documentation + * @param alpha3 error parameters, see documentation + * @param alpha4 error parameters, see documentation + * @param alpha5 error parameters, see documentation + * @return MotionModel A pointer to the motion model it created + */ static MotionModel * createMotionModel( std::string & type, double alpha1, double alpha2, double alpha3, double alpha4, double alpha5); }; +/** + * @class nav2_amcl::OmniMotionModel + * @brief An Omnidirectional motion model class + */ class OmniMotionModel : public MotionModel { public: + /** + * @brief An omni constructor + * @param alpha1 error parameters, see documentation + * @param alpha2 error parameters, see documentation + * @param alpha3 error parameters, see documentation + * @param alpha4 error parameters, see documentation + * @param alpha5 error parameters, see documentation + */ OmniMotionModel(double alpha1, double alpha2, double alpha3, double alpha4, double alpha5); + + /** + * @brief Update on new odometry data + * @param pf The particle filter to update + * @param pose pose of robot in odometry update + * @param delta change in pose in odometry update + */ void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta); private: @@ -49,10 +89,28 @@ class OmniMotionModel : public MotionModel double alpha5_; }; +/** + * @class nav2_amcl::DifferentialMotionModel + * @brief An differential drive motion model class + */ class DifferentialMotionModel : public MotionModel { public: + /** + * @brief A diff drive constructor + * @param alpha1 error parameters, see documentation + * @param alpha2 error parameters, see documentation + * @param alpha3 error parameters, see documentation + * @param alpha4 error parameters, see documentation + */ DifferentialMotionModel(double alpha1, double alpha2, double alpha3, double alpha4); + + /** + * @brief Update on new odometry data + * @param pf The particle filter to update + * @param pose pose of robot in odometry update + * @param delta change in pose in odometry update + */ void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta); private: diff --git a/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp b/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp index 2b5beaea93d..edc41241284 100644 --- a/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp +++ b/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp @@ -29,12 +29,37 @@ namespace nav2_amcl // Forward declarations class LaserData; +/* + * @class Laser + * @brief Base class for laser sensor models + */ class Laser { public: + /** + * @brief A Laser constructor + * @param max_beams number of beams to use + * @param map Map pointer to use + */ Laser(size_t max_beams, map_t * map); + + /* + * @brief Laser destructor + */ virtual ~Laser(); + + /* + * @brief Run a sensor update on laser + * @param pf Particle filter to use + * @param data Laser data to use + * @return if it was succesful + */ virtual bool sensorUpdate(pf_t * pf, LaserData * data) = 0; + + /* + * @brief Set the laser pose from an update + * @param laser_pose Pose of the laser + */ void SetLaserPose(pf_vector_t & laser_pose); protected: @@ -42,6 +67,11 @@ class Laser double z_rand_; double sigma_hit_; + /* + * @brief Reallocate weights + * @param max_samples Max number of samples + * @param max_obs number of observations + */ void reallocTempData(int max_samples, int max_obs); map_t * map_; pf_vector_t laser_pose_; @@ -51,11 +81,22 @@ class Laser double ** temp_obs_; }; +/* + * @class LaserData + * @brief Class of laser data to process + */ class LaserData { public: Laser * laser; + + /* + * @brief LaserData constructor + */ LaserData() {ranges = NULL;} + /* + * @brief LaserData destructor + */ virtual ~LaserData() {delete[] ranges;} public: @@ -64,13 +105,26 @@ class LaserData double(*ranges)[2]; }; - +/* + * @class BeamModel + * @brief Beam model laser sensor + */ class BeamModel : public Laser { public: + /* + * @brief BeamModel constructor + */ BeamModel( double z_hit, double z_short, double z_max, double z_rand, double sigma_hit, double lambda_short, double chi_outlier, size_t max_beams, map_t * map); + + /* + * @brief Run a sensor update on laser + * @param pf Particle filter to use + * @param data Laser data to use + * @return if it was succesful + */ bool sensorUpdate(pf_t * pf, LaserData * data); private: @@ -81,29 +135,69 @@ class BeamModel : public Laser double chi_outlier_; }; +/* + * @class LikelihoodFieldModel + * @brief likelihood field model laser sensor + */ class LikelihoodFieldModel : public Laser { public: + /* + * @brief BeamModel constructor + */ LikelihoodFieldModel( double z_hit, double z_rand, double sigma_hit, double max_occ_dist, size_t max_beams, map_t * map); + + /* + * @brief Run a sensor update on laser + * @param pf Particle filter to use + * @param data Laser data to use + * @return if it was succesful + */ bool sensorUpdate(pf_t * pf, LaserData * data); private: + /* + * @brief Perform the update function + * @param data Laser data to use + * @param pf Particle filter to use + * @return if it was succesful + */ static double sensorFunction(LaserData * data, pf_sample_set_t * set); }; +/* + * @class LikelihoodFieldModelProb + * @brief likelihood prob model laser sensor + */ class LikelihoodFieldModelProb : public Laser { public: + /* + * @brief BeamModel constructor + */ LikelihoodFieldModelProb( double z_hit, double z_rand, double sigma_hit, double max_occ_dist, bool do_beamskip, double beam_skip_distance, double beam_skip_threshold, double beam_skip_error_threshold, size_t max_beams, map_t * map); + + /* + * @brief Run a sensor update on laser + * @param pf Particle filter to use + * @param data Laser data to use + * @return if it was succesful + */ bool sensorUpdate(pf_t * pf, LaserData * data); private: + /* + * @brief Perform the update function + * @param data Laser data to use + * @param pf Particle filter to use + * @return if it was succesful + */ static double sensorFunction(LaserData * data, pf_sample_set_t * set); bool do_beamskip_; double beam_skip_distance_; diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 4eb1fced5c9..27237a063d2 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.4.3 + 1.0.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 fe42cd49086..0e4fc209119 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -35,7 +35,7 @@ #include "nav2_util/string_utils.hpp" #include "nav2_amcl/sensors/laser/laser.hpp" #include "tf2/convert.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/LinearMath/Transform.h" #include "tf2_ros/buffer.h" #include "tf2_ros/message_filter.h" @@ -274,14 +274,8 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/) // Lifecycle publishers must be explicitly activated pose_pub_->on_activate(); - particlecloud_pub_->on_activate(); particle_cloud_pub_->on_activate(); - RCLCPP_WARN( - get_logger(), - "Publishing the particle cloud as geometry_msgs/PoseArray msg is deprecated, " - "will be published as nav2_msgs/ParticleCloud in the future"); - first_pose_sent_ = false; // Keep track of whether we're in the active state. We won't @@ -318,7 +312,6 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // Lifecycle publishers must be explicitly deactivated pose_pub_->on_deactivate(); - particlecloud_pub_->on_deactivate(); particle_cloud_pub_->on_deactivate(); // destroy bond connection @@ -354,7 +347,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // PubSub pose_pub_.reset(); - particlecloud_pub_.reset(); particle_cloud_pub_.reset(); // Odometry @@ -873,19 +865,15 @@ AmclNode::publishParticleCloud(const pf_sample_set_t * set) cloud_with_weights_msg->header.frame_id = global_frame_id_; cloud_with_weights_msg->particles.resize(set->sample_count); - auto cloud_msg = std::make_unique(); - cloud_msg->header.stamp = this->now(); - cloud_msg->header.frame_id = global_frame_id_; - cloud_msg->poses.resize(set->sample_count); for (int i = 0; i < set->sample_count; i++) { - cloud_msg->poses[i].position.x = set->samples[i].pose.v[0]; - cloud_msg->poses[i].position.y = set->samples[i].pose.v[1]; - cloud_msg->poses[i].position.z = 0; - cloud_msg->poses[i].orientation = orientationAroundZAxis(set->samples[i].pose.v[2]); - cloud_with_weights_msg->particles[i].pose = (*cloud_msg).poses[i]; + cloud_with_weights_msg->particles[i].pose.position.x = set->samples[i].pose.v[0]; + cloud_with_weights_msg->particles[i].pose.position.y = set->samples[i].pose.v[1]; + cloud_with_weights_msg->particles[i].pose.position.z = 0; + cloud_with_weights_msg->particles[i].pose.orientation = orientationAroundZAxis( + set->samples[i].pose.v[2]); cloud_with_weights_msg->particles[i].weight = set->samples[i].weight; } - particlecloud_pub_->publish(std::move(cloud_msg)); + particle_cloud_pub_->publish(std::move(cloud_with_weights_msg)); } @@ -1264,10 +1252,6 @@ AmclNode::initPubSub() { RCLCPP_INFO(get_logger(), "initPubSub"); - particlecloud_pub_ = create_publisher( - "particlecloud", - rclcpp::SensorDataQoS()); - particle_cloud_pub_ = create_publisher( "particle_cloud", rclcpp::SensorDataQoS()); diff --git a/nav2_amcl/src/map/map_cspace.cpp b/nav2_amcl/src/map/map_cspace.cpp index 6dca9e89645..ce66baf1a56 100644 --- a/nav2_amcl/src/map/map_cspace.cpp +++ b/nav2_amcl/src/map/map_cspace.cpp @@ -25,6 +25,10 @@ #include #include "nav2_amcl/map/map.hpp" +/* + * @class CellData + * @brief Data about map cells + */ class CellData { public: @@ -33,9 +37,16 @@ class CellData unsigned int src_i_, src_j_; }; +/* + * @class CachedDistanceMap + * @brief Cached map with distances + */ class CachedDistanceMap { public: + /* + * @brief CachedDistanceMap constructor + */ CachedDistanceMap(double scale, double max_dist) : distances_(NULL), scale_(scale), max_dist_(max_dist) { @@ -48,6 +59,10 @@ class CachedDistanceMap } } } + + /* + * @brief CachedDistanceMap destructor + */ ~CachedDistanceMap() { if (distances_) { @@ -63,7 +78,9 @@ class CachedDistanceMap int cell_radius_; }; - +/* + * @brief operator< + */ bool operator<(const CellData & a, const CellData & b) { return a.map_->cells[MAP_INDEX( @@ -71,6 +88,12 @@ bool operator<(const CellData & a, const CellData & b) a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist; } +/* + * @brief get_distance_map + * @param scale of cost information wrt distance + * @param max_dist Maximum distance to cache from occupied information + * @return Pointer to cached distance map + */ CachedDistanceMap * get_distance_map(double scale, double max_dist) { @@ -86,6 +109,9 @@ get_distance_map(double scale, double max_dist) return cdm; } +/* + * @brief enqueue cell data for caching + */ void enqueue( map_t * map, int i, int j, int src_i, int src_j, @@ -119,7 +145,11 @@ void enqueue( marked[MAP_INDEX(map, i, j)] = 1; } -// Update the cspace distance values +/* + * @brief Update the cspace distance values + * @param map Map to update + * @param max_occ_distance Maximum distance for occpuancy interest + */ void map_update_cspace(map_t * map, double max_occ_dist) { unsigned char * marked; diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index c92b06228fb..9de0751529a 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -54,6 +54,9 @@ ament_target_dependencies(${library_name} add_library(nav2_compute_path_to_pose_action_bt_node SHARED plugins/action/compute_path_to_pose_action.cpp) list(APPEND plugin_libs nav2_compute_path_to_pose_action_bt_node) +add_library(nav2_compute_path_through_poses_action_bt_node SHARED plugins/action/compute_path_through_poses_action.cpp) +list(APPEND plugin_libs nav2_compute_path_through_poses_action_bt_node) + add_library(nav2_follow_path_action_bt_node SHARED plugins/action/follow_path_action.cpp) list(APPEND plugin_libs nav2_follow_path_action_bt_node) @@ -117,12 +120,30 @@ list(APPEND plugin_libs nav2_recovery_node_bt_node) add_library(nav2_navigate_to_pose_action_bt_node SHARED plugins/action/navigate_to_pose_action.cpp) list(APPEND plugin_libs nav2_navigate_to_pose_action_bt_node) +add_library(nav2_navigate_through_poses_action_bt_node SHARED plugins/action/navigate_through_poses_action.cpp) +list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node) + +add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp) +list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node) + add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp) list(APPEND plugin_libs nav2_pipeline_sequence_bt_node) add_library(nav2_round_robin_node_bt_node SHARED plugins/control/round_robin_node.cpp) list(APPEND plugin_libs nav2_round_robin_node_bt_node) +add_library(nav2_single_trigger_bt_node SHARED plugins/decorator/single_trigger_node.cpp) +list(APPEND plugin_libs nav2_single_trigger_bt_node) + +add_library(nav2_planner_selector_bt_node SHARED plugins/action/planner_selector_node.cpp) +list(APPEND plugin_libs nav2_planner_selector_bt_node) + +add_library(nav2_controller_selector_bt_node SHARED plugins/action/controller_selector_node.cpp) +list(APPEND plugin_libs nav2_controller_selector_bt_node) + +add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp) +list(APPEND plugin_libs nav2_goal_checker_selector_bt_node) + foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 2a9cfac472b..4d74a901f09 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -67,6 +67,8 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a |----------|:-------------|------| | Backup | Action | Invokes the BackUp ROS2 action server, which causes the robot to back up to a specific pose. This is used in nav2 Behavior Trees as a recovery behavior. The nav2_recoveries module implements the BackUp action server. | | ComputePathToPose | Action | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. The server address can be remapped using the `server_name` input port. | +| ComputePathThroughPoses | Action | Invokes the ComputePathThroughPoses ROS2 action server, which is implemented by the nav2_planner module. The server address can be remapped using the `server_name` input port. | +| RemovePassedGoals | Action | Removes `goals` from a vector of intermediary goal poses which the robot is either approaching or has passed within a tolerance of to prevent replanning to initial points. | | FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the controller plugin modules loaded. The server address can be remapped using the `server_name` input port. | | GoalReached | Condition | Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, otherwise it returns FAILURE. | | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. | @@ -74,11 +76,20 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a | GoalUpdated | Condition | Checks if the global navigation goal has changed in the blackboard. Returns failure if the goal is the same, if it changes, it returns success. | | IsBatteryLow | Condition | Checks if battery is low by subscribing to a sensor_msgs/BatteryState topic and checking if battery voltage/percentage is below a specified minimum value. | | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. | +| NavigateThroughPoses | Action | Invokes the NavigateThroughPoses ROS2 action server, which is implemented by the bt_navigator module. | | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | | DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `DistanceController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | | SpeedController | Decorator | A node that controls the tick rate for its child based on the current robot speed. This decorator offers the most flexibility as the user can set the minimum/maximum tick rate which is adjusted according to the current robot speed. | +| SingleTrigger | Decorator | A node that triggers its child node(s) only a single time. It returns `FAILURE` in all other ticks. | | RecoveryNode | Control | The RecoveryNode is a control flow node with two children. It returns SUCCESS if and only if the first child returns SUCCESS. The second child will be executed only if the first child returns FAILURE. If the second child SUCCEEDS, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning FAILURE. In nav2, the RecoveryNode is included in Behavior Trees to implement recovery actions upon failures. -| Spin | Action | Invokes the Spin ROS2 action server, which is implemented by the nav2_recoveries module. This action is using in nav2 Behavior Trees as a recovery behavior. | +| Spin | Action | Invokes the Spin ROS2 action server, which is implemented by the nav2_recoveries module. This action is used in nav2 Behavior Trees as a recovery behavior. | | PipelineSequence | Control | Ticks the first child till it succeeds, then ticks the first and second children till the second one succeeds. It then ticks the first, second, and third children until the third succeeds, and so on, and so on. If at any time a child returns RUNNING, that doesn't change the behavior. If at any time a child returns FAILURE, that stops all children and returns FAILURE overall.| +| ClearEntireCostmap | Action | Invokes the ClearEntireCostmap ROS2 service server of costmap_2d_ros. The service address needs to be specified using the `service_name` input port, typically `local_costmap/clear_entirely_local_costmap` or `global_costmap/clear_entirely_global_costmap`. This action is used in nav2 Behavior Trees as a recovery behavior. | +| ClearCostmapExceptRegion | Action | Invokes the ClearCostmapExceptRegion ROS2 service server of costmap_2d_ros. It will clear all the costmap apart a square area centered on the robot of side size equal to the `reset_distance` input port. The service address needs to be specified using the `service_name` input port, typically `local_costmap/clear_except_local_costmap` or `global_costmap/clear_except_global_costmap`. This action is used in nav2 Behavior Trees as a recovery behavior. | +| ClearCostmapAroundRobot | Action | Invokes the ClearCostmapAroundRobot ROS2 service server of costmap_2d_ros. It will only clear the costmap on a square area centered on the robot of side size equal to the `reset_distance` input port. The service address needs to be specified using the `service_name` input port, typically `local_costmap/clear_around_local_costmap` or `global_costmap/clear_around_global_costmap`. This action is used in nav2 Behavior Trees as a recovery behavior. | +| PlannerSelector | Action | The PlannerSelector behavior is used to switch the planner that will be used by the planner server. It subscribes to a topic "planner_selector" to get the decision about what planner must be used. It is usually used before of the ComputePathToPoseAction. The selected_planner output port is passed to planner_id input port of the ComputePathToPoseAction. +| ControllerSelector | Action | The ControllerSelector behavior is used to switch the controller that will be used by the controller server. It subscribes to a topic "controller_selector" to get the decision about what controller must be used. It is usually used before of the FollowPath. The selected_controller output port is passed to controller_id input port of the FollowPath. +| GoalCheckerSelector | Action | The GoalCheckerSelector behavior is used to switch the current goal checker of the controller_server. It subscribes to a topic "goal_checker_selector" to get the decision about what goal_checker must be used. It is usually used before of the FollowPath. The selected_goal_checker output port is passed to goal_checker_id input port of the FollowPath. + For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/bt_basics/ diff --git a/nav2_behavior_tree/groot_instructions.md b/nav2_behavior_tree/groot_instructions.md index b1189a6ba6b..7ca0ac70ada 100644 --- a/nav2_behavior_tree/groot_instructions.md +++ b/nav2_behavior_tree/groot_instructions.md @@ -7,6 +7,6 @@ To visualize the behavior trees using Groot: 1. Open Groot in editor mode 2. Select the `Load palette from file` option (import button) near the top left corner. -3. Open the file `/path/to/navigation2/nav2_behavior_tree/nav2_tree_nodes.xml` to import all the behavior tree nodes used for navigation. +3. Open the file `/path/to/nav2/nav2_behavior_tree/nav2_tree_nodes.xml` to import all the behavior tree nodes used for navigation. 4. Select `Load tree` option near the top left corner 5. Browse the tree you want to visualize the select ok. diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 61816a99504..f5aab180c59 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -29,43 +29,89 @@ namespace nav2_behavior_tree { +/** + * @enum nav2_behavior_tree::BtStatus + * @brief An enum class representing BT execution status + */ enum class BtStatus { SUCCEEDED, FAILED, CANCELED }; +/** + * @class nav2_behavior_tree::BehaviorTreeEngine + * @brief A class to create and handle behavior trees + */ class BehaviorTreeEngine { public: + /** + * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine + * @param plugin_libraries vector of BT plugin library names to load + */ explicit BehaviorTreeEngine(const std::vector & plugin_libraries); virtual ~BehaviorTreeEngine() {} + /** + * @brief Function to execute a BT at a specific rate + * @param tree BT to execute + * @param onLoop Function to execute on each iteration of BT execution + * @param cancelRequested Function to check if cancel was requested during BT execution + * @param loopTimeout Time period for each iteration of BT execution + * @return nav2_behavior_tree::BtStatus Status of BT execution + */ BtStatus run( BT::Tree * tree, std::function onLoop, std::function cancelRequested, std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10)); + /** + * @brief Function to create a BT from a XML string + * @param xml_string XML string representing BT + * @param blackboard Blackboard for BT + * @return BT::Tree Created behavior tree + */ BT::Tree createTreeFromText( const std::string & xml_string, BT::Blackboard::Ptr blackboard); + /** + * @brief Function to create a BT from an XML file + * @param file_path Path to BT XML file + * @param blackboard Blackboard for BT + * @return BT::Tree Created behavior tree + */ BT::Tree createTreeFromFile( const std::string & file_path, BT::Blackboard::Ptr blackboard); + /** + * @brief Add groot monitor to publish BT status changes + * @param tree BT to monitor + * @param publisher_port ZMQ publisher port for the Groot monitor + * @param server_port ZMQ server port for the Groot monitor + * @param max_msg_per_second Maximum number of messages that can be sent per second + */ void addGrootMonitoring( BT::Tree * tree, uint16_t publisher_port, uint16_t server_port, uint16_t max_msg_per_second = 25); + /** + * @brief Reset groot monitor + */ void resetGrootMonitor(); - // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state + /** + * @brief Function to explicitly reset all BT nodes to initial state + * @param root_node Pointer to BT root node + */ void haltAllActions(BT::TreeNode * root_node); protected: // The factory that will be used to dynamically construct the behavior tree BT::BehaviorTreeFactory factory_; - std::unique_ptr groot_monitor_; + + static inline std::unique_ptr groot_monitor_; }; } // namespace nav2_behavior_tree 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 77aa8836d44..f47cc73d371 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 @@ -26,10 +26,20 @@ namespace nav2_behavior_tree { +/** + * @brief Abstract class representing an action based BT node + * @tparam ActionT Type of action + */ template class BtActionNode : public BT::ActionNodeBase { public: + /** + * @brief A nav2_behavior_tree::BtActionNode constructor + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ BtActionNode( const std::string & xml_tag_name, const std::string & action_name, @@ -37,8 +47,14 @@ class BtActionNode : public BT::ActionNodeBase : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) { node_ = config().blackboard->template get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); // Get the required items from the blackboard + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); @@ -54,7 +70,7 @@ class BtActionNode : public BT::ActionNodeBase createActionClient(action_name_); // Give the derive class a chance to do any initialization - RCLCPP_INFO(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str()); + RCLCPP_DEBUG(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str()); } BtActionNode() = delete; @@ -63,19 +79,26 @@ class BtActionNode : public BT::ActionNodeBase { } - // Create instance of an action server + /** + * @brief Create instance of an action client + * @param action_name Action name to create client for + */ void createActionClient(const std::string & action_name) { // Now that we have the ROS node to use, create the action client for this BT action - action_client_ = rclcpp_action::create_client(node_, action_name); + action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_); // Make sure the server is actually there before continuing - RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); + RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); action_client_->wait_for_action_server(); } - // Any subclass of BtActionNode that accepts parameters must provide a providedPorts method - // and call providedBasicPorts in it. + /** + * @brief Any subclass of BtActionNode that accepts parameters must provide a + * providedPorts method and call providedBasicPorts in it. + * @param addition Additional ports to add to BT port list + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedBasicPorts(BT::PortsList addition) { BT::PortsList basic = { @@ -87,6 +110,10 @@ class BtActionNode : public BT::ActionNodeBase return basic; } + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts({}); @@ -95,39 +122,54 @@ class BtActionNode : public BT::ActionNodeBase // Derived classes can override any of the following methods to hook into the // processing for the action: on_tick, on_wait_for_result, and on_success - // Could do dynamic checks, such as getting updates to values on the blackboard + /** + * @brief Function to perform some user-defined operation on tick + * Could do dynamic checks, such as getting updates to values on the blackboard + */ virtual void on_tick() { } - // There can be many loop iterations per tick. Any opportunity to do something after - // a timeout waiting for a result that hasn't been received yet + /** + * @brief Function to perform some user-defined operation after a timeout + * waiting for a result that hasn't been received yet + */ virtual void on_wait_for_result() { } - // Called upon successful completion of the action. A derived class can override this - // method to put a value on the blackboard, for example. + /** + * @brief Function to perform some user-defined operation upon successful + * completion of the action. Could put a value on the blackboard. + * @return BT::NodeStatus Returns SUCCESS by default, user may override return another value + */ virtual BT::NodeStatus on_success() { return BT::NodeStatus::SUCCESS; } - // Called when a the action is aborted. By default, the node will return FAILURE. - // The user may override it to return another value, instead. + /** + * @brief Function to perform some user-defined operation whe the action is aborted. + * @return BT::NodeStatus Returns FAILURE by default, user may override return another value + */ virtual BT::NodeStatus on_aborted() { return BT::NodeStatus::FAILURE; } - // Called when a the action is cancelled. By default, the node will return SUCCESS. - // The user may override it to return another value, instead. + /** + * @brief Function to perform some user-defined operation when the action is cancelled. + * @return BT::NodeStatus Returns SUCCESS by default, user may override return another value + */ virtual BT::NodeStatus on_cancelled() { return BT::NodeStatus::SUCCESS; } - // The main override required by a BT action + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override { // first step to be done only at the beginning of the Action @@ -138,7 +180,26 @@ class BtActionNode : public BT::ActionNodeBase // user defined callback on_tick(); - on_new_goal_received(); + send_new_goal(); + } + + // if new goal was sent and action server has not yet responded + // check the future goal handle + if (future_goal_handle_) { + auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + if (!is_future_goal_handle_complete(elapsed)) { + // return RUNNING if there is still some time before timeout happens + if (elapsed < server_timeout_) { + return BT::NodeStatus::RUNNING; + } + // if server has taken more time to respond than the specified timeout value return FAILURE + RCLCPP_WARN( + node_->get_logger(), + "Timed out while waiting for action server to acknowledge goal request for %s", + action_name_.c_str()); + future_goal_handle_.reset(); + return BT::NodeStatus::FAILURE; + } } // The following code corresponds to the "RUNNING" loop @@ -151,10 +212,22 @@ class BtActionNode : public BT::ActionNodeBase goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) { goal_updated_ = false; - on_new_goal_received(); + send_new_goal(); + auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + if (!is_future_goal_handle_complete(elapsed)) { + if (elapsed < server_timeout_) { + return BT::NodeStatus::RUNNING; + } + RCLCPP_WARN( + node_->get_logger(), + "Timed out while waiting for action server to acknowledge goal request for %s", + action_name_.c_str()); + future_goal_handle_.reset(); + return BT::NodeStatus::FAILURE; + } } - rclcpp::spin_some(node_); + callback_group_executor_.spin_some(); // check if, after invoking spin_some(), we finally received the result if (!goal_result_available_) { @@ -163,28 +236,37 @@ class BtActionNode : public BT::ActionNodeBase } } + BT::NodeStatus status; switch (result_.code) { case rclcpp_action::ResultCode::SUCCEEDED: - return on_success(); + status = on_success(); + break; case rclcpp_action::ResultCode::ABORTED: - return on_aborted(); + status = on_aborted(); + break; case rclcpp_action::ResultCode::CANCELED: - return on_cancelled(); + status = on_cancelled(); + break; default: throw std::logic_error("BtActionNode::Tick: invalid status value"); } + + goal_handle_.reset(); + return status; } - // The other (optional) override required by a BT action. In this case, we - // make sure to cancel the ROS2 action if it is still running. + /** + * @brief The other (optional) override required by a BT action. In this case, we + * make sure to cancel the ROS2 action if it is still running. + */ void halt() override { if (should_cancel_goal()) { auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) != + if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( @@ -197,6 +279,10 @@ class BtActionNode : public BT::ActionNodeBase } protected: + /** + * @brief Function to check if current goal should be cancelled + * @return bool True if current goal should be cancelled, false otherwise + */ bool should_cancel_goal() { // Shut the node down if it is currently running @@ -204,7 +290,12 @@ class BtActionNode : public BT::ActionNodeBase return false; } - rclcpp::spin_some(node_); + // No need to cancel the goal if goal handle is invalid + if (!goal_handle_) { + return false; + } + + callback_group_executor_.spin_some(); auto status = goal_handle_->get_status(); // Check if the goal is still executing @@ -212,13 +303,23 @@ class BtActionNode : public BT::ActionNodeBase status == action_msgs::msg::GoalStatus::STATUS_EXECUTING; } - - void on_new_goal_received() + /** + * @brief Function to send new goal to action server + */ + void send_new_goal() { goal_result_available_ = false; auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); send_goal_options.result_callback = [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { + if (future_goal_handle_) { + RCLCPP_DEBUG( + node_->get_logger(), + "Goal result for %s available, but it hasn't received the goal response yet. " + "It's probably a goal result for the last goal request", action_name_.c_str()); + return; + } + // TODO(#1652): a work around until rcl_action interface is updated // if goal ids are not matched, the older goal call this callback so ignore the result // if matched, it must be processed (including aborted) @@ -228,20 +329,53 @@ class BtActionNode : public BT::ActionNodeBase } }; - auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); + future_goal_handle_ = std::make_shared< + std::shared_future::SharedPtr>>( + action_client_->async_send_goal(goal_, send_goal_options)); + time_goal_sent_ = node_->now(); + } + + /** + * @brief Function to check if the action server acknowledged a new goal + * @param elapsed Duration since the last goal was sent and future goal handle has not completed. + * After waiting for the future to complete, this value is incremented with the timeout value. + * @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise + */ + bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed) + { + auto remaining = server_timeout_ - elapsed; + + // server has already timed out, no need to sleep + if (remaining <= std::chrono::milliseconds(0)) { + future_goal_handle_.reset(); + return false; + } - if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != - rclcpp::FutureReturnCode::SUCCESS) - { + auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; + auto result = + callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout); + elapsed += timeout; + + if (result == rclcpp::FutureReturnCode::INTERRUPTED) { + future_goal_handle_.reset(); throw std::runtime_error("send_goal failed"); } - goal_handle_ = future_goal_handle.get(); - if (!goal_handle_) { - throw std::runtime_error("Goal was rejected by the action server"); + if (result == rclcpp::FutureReturnCode::SUCCESS) { + goal_handle_ = future_goal_handle_->get(); + future_goal_handle_.reset(); + if (!goal_handle_) { + throw std::runtime_error("Goal was rejected by the action server"); + } + return true; } + + return false; } + /** + * @brief Function to increment recovery count on blackboard if this node wraps a recovery + */ void increment_recovery_count() { int recovery_count = 0; @@ -262,10 +396,20 @@ class BtActionNode : public BT::ActionNodeBase // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; // The timeout value while waiting for response from a server when a // new action goal is sent or canceled std::chrono::milliseconds server_timeout_; + + // The timeout value for BT loop execution + std::chrono::milliseconds bt_loop_duration_; + + // To track the action server acknowledgement when a new goal is sent + std::shared_ptr::SharedPtr>> + future_goal_handle_; + rclcpp::Time time_goal_sent_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp new file mode 100644 index 00000000000..ec977944418 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -0,0 +1,249 @@ +// Copyright (c) 2020 Sarthak Mittal +// +// 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_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_ +#define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_behavior_tree/behavior_tree_engine.hpp" +#include "nav2_behavior_tree/ros_topic_logger.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/simple_action_server.hpp" + +namespace nav2_behavior_tree +{ +/** + * @class nav2_behavior_tree::BtActionServer + * @brief An action server that uses behavior tree to execute an action + */ +template +class BtActionServer +{ +public: + using ActionServer = nav2_util::SimpleActionServer; + + typedef std::function OnGoalReceivedCallback; + typedef std::function OnLoopCallback; + typedef std::function OnPreemptCallback; + typedef std::function OnCompletionCallback; + + /** + * @brief A constructor for nav2_behavior_tree::BtActionServer class + */ + explicit BtActionServer( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & action_name, + const std::vector & plugin_lib_names, + const std::string & default_bt_xml_filename, + OnGoalReceivedCallback on_goal_received_callback, + OnLoopCallback on_loop_callback, + OnPreemptCallback on_preempt_callback, + OnCompletionCallback on_completion_callback); + + /** + * @brief A destructor for nav2_behavior_tree::BtActionServer class + */ + ~BtActionServer(); + + /** + * @brief Configures member variables + * Initializes action server for, builds behavior tree from xml file, + * and calls user-defined onConfigure. + * @return bool true on SUCCESS and false on FAILURE + */ + bool on_configure(); + + /** + * @brief Activates action server + * @return bool true on SUCCESS and false on FAILURE + */ + bool on_activate(); + + /** + * @brief Deactivates action server + * @return bool true on SUCCESS and false on FAILURE + */ + bool on_deactivate(); + + /** + * @brief Resets member variables + * @return bool true on SUCCESS and false on FAILURE + */ + bool on_cleanup(); + + /** + * @brief Replace current BT with another one + * @param bt_xml_filename The file containing the new BT, uses default filename if empty + * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false + * if something went wrong, and previous BT is maintained + */ + bool loadBehaviorTree(const std::string & bt_xml_filename = ""); + + /** + * @brief Getter function for BT Blackboard + * @return BT::Blackboard::Ptr Shared pointer to current BT blackboard + */ + BT::Blackboard::Ptr getBlackboard() const + { + return blackboard_; + } + + /** + * @brief Getter function for current BT XML filename + * @return string Containing current BT XML filename + */ + std::string getCurrentBTFilename() const + { + return current_bt_xml_filename_; + } + + /** + * @brief Getter function for default BT XML filename + * @return string Containing default BT XML filename + */ + std::string getDefaultBTFilename() const + { + return default_bt_xml_filename_; + } + + /** + * @brief Wrapper function to accept pending goal if a preempt has been requested + * @return Shared pointer to pending action goal + */ + const std::shared_ptr acceptPendingGoal() + { + return action_server_->accept_pending_goal(); + } + + /** + * @brief Wrapper function to terminate pending goal if a preempt has been requested + */ + void terminatePendingGoal() + { + action_server_->terminate_pending_goal(); + } + + /** + * @brief Wrapper function to get current goal + * @return Shared pointer to current action goal + */ + const std::shared_ptr getCurrentGoal() const + { + return action_server_->get_current_goal(); + } + + /** + * @brief Wrapper function to get pending goal + * @return Shared pointer to pending action goal + */ + const std::shared_ptr getPendingGoal() const + { + return action_server_->get_pending_goal(); + } + + /** + * @brief Wrapper function to publish action feedback + */ + void publishFeedback(typename std::shared_ptr feedback) + { + action_server_->publish_feedback(feedback); + } + + /** + * @brief Getter function for the current BT tree + * @return BT::Tree Current behavior tree + */ + BT::Tree getTree() const + { + return tree_; + } + + /** + * @brief Function to halt the current tree. It will interrupt the execution of RUNNING nodes + * by calling their halt() implementation (only for Async nodes that may return RUNNING) + */ + void haltTree() + { + tree_.rootNode()->halt(); + } + +protected: + /** + * @brief Action server callback + */ + void executeCallback(); + + // Action name + std::string action_name_; + + // Our action server implements the template action + std::shared_ptr action_server_; + + // Behavior Tree to be executed when goal is received + BT::Tree tree_; + + // The blackboard shared by all of the nodes in the tree + BT::Blackboard::Ptr blackboard_; + + // The XML file that cointains the Behavior Tree to create + std::string current_bt_xml_filename_; + std::string default_bt_xml_filename_; + + // The wrapper class for the BT functionality + std::unique_ptr bt_; + + // Libraries to pull plugins (BT Nodes) from + std::vector plugin_lib_names_; + + // A regular, non-spinning ROS node that we can use for calls to the action client + rclcpp::Node::SharedPtr client_node_; + + // Parent node + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + + // Clock + rclcpp::Clock::SharedPtr clock_; + + // Logger + rclcpp::Logger logger_{rclcpp::get_logger("BtActionServer")}; + + // To publish BT logs + std::unique_ptr topic_logger_; + + // Duration for each iteration of BT execution + std::chrono::milliseconds bt_loop_duration_; + + // Default timeout value while waiting for response from a server + std::chrono::milliseconds default_server_timeout_; + + // Parameters for Groot monitoring + bool enable_groot_monitoring_; + int groot_zmq_publisher_port_; + int groot_zmq_server_port_; + + // User-provided callbacks + OnGoalReceivedCallback on_goal_received_callback_; + OnLoopCallback on_loop_callback_; + OnPreemptCallback on_preempt_callback_; + OnCompletionCallback on_completion_callback_; +}; + +} // namespace nav2_behavior_tree + +#include // NOLINT(build/include_order) +#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp new file mode 100644 index 00000000000..c6a1479cda8 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -0,0 +1,265 @@ +// Copyright (c) 2020 Sarthak Mittal +// +// 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_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ +#define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_behavior_tree/bt_action_server.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +namespace nav2_behavior_tree +{ + +template +BtActionServer::BtActionServer( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & action_name, + const std::vector & plugin_lib_names, + const std::string & default_bt_xml_filename, + OnGoalReceivedCallback on_goal_received_callback, + OnLoopCallback on_loop_callback, + OnPreemptCallback on_preempt_callback, + OnCompletionCallback on_completion_callback) +: action_name_(action_name), + default_bt_xml_filename_(default_bt_xml_filename), + plugin_lib_names_(plugin_lib_names), + node_(parent), + on_goal_received_callback_(on_goal_received_callback), + on_loop_callback_(on_loop_callback), + on_preempt_callback_(on_preempt_callback), + on_completion_callback_(on_completion_callback) +{ + auto node = node_.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + // Declare this node's parameters + if (!node->has_parameter("bt_loop_duration")) { + node->declare_parameter("bt_loop_duration", 10); + } + if (!node->has_parameter("default_server_timeout")) { + node->declare_parameter("default_server_timeout", 20); + } + if (!node->has_parameter("enable_groot_monitoring")) { + node->declare_parameter("enable_groot_monitoring", false); + } + if (!node->has_parameter("groot_zmq_publisher_port")) { + node->declare_parameter("groot_zmq_publisher_port", 1666); + } + if (!node->has_parameter("groot_zmq_server_port")) { + node->declare_parameter("groot_zmq_server_port", 1667); + } +} + +template +BtActionServer::~BtActionServer() +{} + +template +bool BtActionServer::on_configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // use suffix '_rclcpp_node' to keep parameter file consistency #1773 + auto options = rclcpp::NodeOptions().arguments( + {"--ros-args", + "-r", std::string("__node:=") + std::string(node->get_name()) + action_name_ + "_rclcpp_node", + "--"}); + // Support for handling the topic-based goal pose from rviz + client_node_ = std::make_shared("_", options); + + action_server_ = std::make_shared( + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + action_name_, std::bind(&BtActionServer::executeCallback, this)); + + // Get parameter for monitoring with Groot via ZMQ Publisher + node->get_parameter("enable_groot_monitoring", enable_groot_monitoring_); + node->get_parameter("groot_zmq_publisher_port", groot_zmq_publisher_port_); + node->get_parameter("groot_zmq_server_port", groot_zmq_server_port_); + + // Get parameters for BT timeouts + int timeout; + node->get_parameter("bt_loop_duration", timeout); + bt_loop_duration_ = std::chrono::milliseconds(timeout); + node->get_parameter("default_server_timeout", timeout); + default_server_timeout_ = std::chrono::milliseconds(timeout); + + // Create the class that registers our custom nodes and executes the BT + bt_ = std::make_unique(plugin_lib_names_); + + // Create the blackboard that will be shared by all of the nodes in the tree + blackboard_ = BT::Blackboard::create(); + + // Put items on the blackboard + blackboard_->set("node", client_node_); // NOLINT + blackboard_->set("server_timeout", default_server_timeout_); // NOLINT + blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT + + return true; +} + +template +bool BtActionServer::on_activate() +{ + if (!loadBehaviorTree(default_bt_xml_filename_)) { + RCLCPP_ERROR(logger_, "Error loading XML file: %s", default_bt_xml_filename_.c_str()); + return false; + } + action_server_->activate(); + return true; +} + +template +bool BtActionServer::on_deactivate() +{ + action_server_->deactivate(); + return true; +} + +template +bool BtActionServer::on_cleanup() +{ + client_node_.reset(); + action_server_.reset(); + topic_logger_.reset(); + plugin_lib_names_.clear(); + current_bt_xml_filename_.clear(); + blackboard_.reset(); + bt_->haltAllActions(tree_.rootNode()); + bt_->resetGrootMonitor(); + bt_.reset(); + return true; +} + +template +bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filename) +{ + // Empty filename is default for backward compatibility + auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; + + // Use previous BT if it is the existing one + if (current_bt_xml_filename_ == filename) { + RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded"); + return true; + } + + // if a new tree is created, than the ZMQ Publisher must be destroyed + bt_->resetGrootMonitor(); + + // Read the input BT XML from the specified file into a string + std::ifstream xml_file(filename); + + if (!xml_file.good()) { + RCLCPP_ERROR(logger_, "Couldn't open input XML file: %s", filename.c_str()); + return false; + } + + auto xml_string = std::string( + std::istreambuf_iterator(xml_file), + std::istreambuf_iterator()); + + // Create the Behavior Tree from the XML input + tree_ = bt_->createTreeFromText(xml_string, blackboard_); + topic_logger_ = std::make_unique(client_node_, tree_); + + current_bt_xml_filename_ = filename; + + // Enable monitoring with Groot + if (enable_groot_monitoring_) { + // optionally add max_msg_per_second = 25 (default) here + try { + bt_->addGrootMonitoring(&tree_, groot_zmq_publisher_port_, groot_zmq_server_port_); + } catch (const std::logic_error & e) { + RCLCPP_ERROR(logger_, "ZMQ already enabled, Error: %s", e.what()); + } + } + + return true; +} + +template +void BtActionServer::executeCallback() +{ + if (!on_goal_received_callback_(action_server_->get_current_goal())) { + action_server_->terminate_current(); + return; + } + + auto is_canceling = [&]() { + if (action_server_ == nullptr) { + RCLCPP_DEBUG(logger_, "Action server unavailable. Canceling."); + return true; + } + if (!action_server_->is_server_active()) { + RCLCPP_DEBUG(logger_, "Action server is inactive. Canceling."); + return true; + } + return action_server_->is_cancel_requested(); + }; + + auto on_loop = [&]() { + if (action_server_->is_preempt_requested() && on_preempt_callback_) { + on_preempt_callback_(action_server_->get_pending_goal()); + } + topic_logger_->flush(); + on_loop_callback_(); + }; + + // Execute the BT that was previously created in the configure step + nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling, bt_loop_duration_); + + // Make sure that the Bt is not in a running state from a previous execution + // note: if all the ControlNodes are implemented correctly, this is not needed. + bt_->haltAllActions(tree_.rootNode()); + + // Give server an opportunity to populate the result message or simple give + // an indication that the action is complete. + auto result = std::make_shared(); + on_completion_callback_(result); + + switch (rc) { + case nav2_behavior_tree::BtStatus::SUCCEEDED: + RCLCPP_INFO(logger_, "Goal succeeded"); + action_server_->succeeded_current(result); + break; + + case nav2_behavior_tree::BtStatus::FAILED: + RCLCPP_ERROR(logger_, "Goal failed"); + action_server_->terminate_current(result); + break; + + case nav2_behavior_tree::BtStatus::CANCELED: + RCLCPP_INFO(logger_, "Goal canceled"); + action_server_->terminate_all(result); + break; + } +} + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp index f16e61ec99e..c109c4f0e29 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp @@ -30,6 +30,11 @@ namespace BT // in our BT XML files. They parse the strings in the XML into their corresponding // data type. +/** + * @brief Parse XML string to geometry_msgs::msg::Point + * @param key XML string + * @return geometry_msgs::msg::Point + */ template<> inline geometry_msgs::msg::Point convertFromString(const StringView key) { @@ -46,6 +51,11 @@ inline geometry_msgs::msg::Point convertFromString(const StringView key) } } +/** + * @brief Parse XML string to geometry_msgs::msg::Quaternion + * @param key XML string + * @return geometry_msgs::msg::Quaternion + */ template<> inline geometry_msgs::msg::Quaternion convertFromString(const StringView key) { @@ -63,6 +73,11 @@ inline geometry_msgs::msg::Quaternion convertFromString(const StringView key) } } +/** + * @brief Parse XML string to geometry_msgs::msg::PoseStamped + * @param key XML string + * @return geometry_msgs::msg::PoseStamped + */ template<> inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) { @@ -85,6 +100,11 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) } } +/** + * @brief Parse XML string to std::chrono::milliseconds + * @param key XML string + * @return std::chrono::milliseconds + */ template<> inline std::chrono::milliseconds convertFromString(const StringView key) { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 8931783df6c..5317b9136ea 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -26,36 +26,54 @@ namespace nav2_behavior_tree { +/** + * @brief Abstract class representing a service based BT node + * @tparam ServiceT Type of service + */ template -class BtServiceNode : public BT::SyncActionNode +class BtServiceNode : public BT::ActionNodeBase { public: + /** + * @brief A nav2_behavior_tree::BtServiceNode constructor + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ BtServiceNode( const std::string & service_node_name, const BT::NodeConfiguration & conf) - : BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name) + : BT::ActionNodeBase(service_node_name, conf), service_node_name_(service_node_name) { node_ = config().blackboard->template get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); // Get the required items from the blackboard + bt_loop_duration_ = + config().blackboard->template get("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); - service_client_ = node_->create_client(service_name_); + service_client_ = node_->create_client( + service_name_, + rmw_qos_profile_services_default, + callback_group_); // Make a request for the service without parameter request_ = std::make_shared(); // Make sure the server is actually there before continuing - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "Waiting for \"%s\" service", service_name_.c_str()); service_client_->wait_for_service(); - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "\"%s\" BtServiceNode initialized", service_node_name_.c_str()); } @@ -66,8 +84,12 @@ class BtServiceNode : public BT::SyncActionNode { } - // Any subclass of BtServiceNode that accepts parameters must provide a providedPorts method - // and call providedBasicPorts in it. + /** + * @brief Any subclass of BtServiceNode that accepts parameters must provide a + * providedPorts method and call providedBasicPorts in it. + * @param addition Additional ports to add to BT port list + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedBasicPorts(BT::PortsList addition) { BT::PortsList basic = { @@ -79,50 +101,94 @@ class BtServiceNode : public BT::SyncActionNode return basic; } + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts({}); } - // The main override required by a BT service + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override { - on_tick(); - auto future_result = service_client_->async_send_request(request_); - return check_future(future_result); + if (!request_sent_) { + on_tick(); + future_result_ = service_client_->async_send_request(request_); + sent_time_ = node_->now(); + request_sent_ = true; + } + return check_future(); + } + + /** + * @brief The other (optional) override required by a BT service. + */ + void halt() override + { + request_sent_ = false; + setStatus(BT::NodeStatus::IDLE); } - // Fill in service request with information if necessary + /** + * @brief Function to perform some user-defined operation on tick + * Fill in service request with information if necessary + */ virtual void on_tick() { } - // Check the future and decide the status of Behaviortree - virtual BT::NodeStatus check_future( - std::shared_future future_result) + /** + * @brief Check the future and decide the status of BT + * @return BT::NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise + */ + virtual BT::NodeStatus check_future() { - rclcpp::FutureReturnCode rc; - rc = rclcpp::spin_until_future_complete( - node_, - future_result, server_timeout_); - if (rc == rclcpp::FutureReturnCode::SUCCESS) { - return BT::NodeStatus::SUCCESS; - } else if (rc == rclcpp::FutureReturnCode::TIMEOUT) { - RCLCPP_WARN( - node_->get_logger(), - "Node timed out while executing service call to %s.", service_name_.c_str()); - on_wait_for_result(); + auto elapsed = (node_->now() - sent_time_).to_chrono(); + auto remaining = server_timeout_ - elapsed; + + if (remaining > std::chrono::milliseconds(0)) { + auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; + + rclcpp::FutureReturnCode rc; + rc = callback_group_executor_.spin_until_future_complete(future_result_, server_timeout_); + if (rc == rclcpp::FutureReturnCode::SUCCESS) { + request_sent_ = false; + return BT::NodeStatus::SUCCESS; + } + + if (rc == rclcpp::FutureReturnCode::TIMEOUT) { + on_wait_for_result(); + elapsed = (node_->now() - sent_time_).to_chrono(); + if (elapsed < server_timeout_) { + return BT::NodeStatus::RUNNING; + } + } } + + RCLCPP_WARN( + node_->get_logger(), + "Node timed out while executing service call to %s.", service_name_.c_str()); + request_sent_ = false; return BT::NodeStatus::FAILURE; } - // An opportunity to do something after - // a timeout waiting for a result that hasn't been received yet + /** + * @brief Function to perform some user-defined operation after a timeout waiting + * for a result that hasn't been received yet + */ virtual void on_wait_for_result() { } protected: + /** + * @brief Function to increment recovery count on blackboard if this node wraps a recovery + */ void increment_recovery_count() { int recovery_count = 0; @@ -137,10 +203,20 @@ class BtServiceNode : public BT::SyncActionNode // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; // The timeout value while to use in the tick loop while waiting for // a result from the server std::chrono::milliseconds server_timeout_; + + // The timeout value for BT loop execution + std::chrono::milliseconds bt_loop_duration_; + + // To track the server response when a new request is sent + std::shared_future future_result_; + bool request_sent_{false}; + rclcpp::Time sent_time_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp index e49dec78ab4..8584daa910f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp @@ -23,16 +23,32 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp + */ class BackUpAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::BackUpAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ BackUpAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp index 28d8b025841..0bb8ad0bb27 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp @@ -19,20 +19,109 @@ #include "nav2_behavior_tree/bt_service_node.hpp" #include "nav2_msgs/srv/clear_entire_costmap.hpp" +#include "nav2_msgs/srv/clear_costmap_around_robot.hpp" +#include "nav2_msgs/srv/clear_costmap_except_region.hpp" namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::ClearEntireCostmap + */ class ClearEntireCostmapService : public BtServiceNode { public: + /** + * @brief A constructor for nav2_behavior_tree::ClearEntireCostmapService + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ ClearEntireCostmapService( const std::string & service_node_name, const BT::NodeConfiguration & conf); + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ void on_tick() override; }; +/** + * @brief A nav2_behavior_tree::BtServiceNode class that + * wraps nav2_msgs::srv::ClearCostmapExceptRegion + */ +class ClearCostmapExceptRegionService + : public BtServiceNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ClearCostmapExceptRegionService + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ + ClearCostmapExceptRegionService( + const std::string & service_node_name, + const BT::NodeConfiguration & conf); + + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ + void on_tick() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort( + "reset_distance", 1, + "Distance from the robot above which obstacles are cleared") + }); + } +}; + +/** + * @brief A nav2_behavior_tree::BtServiceNode class that + * wraps nav2_msgs::srv::ClearCostmapAroundRobot + */ +class ClearCostmapAroundRobotService : public BtServiceNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ClearCostmapAroundRobotService + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ + ClearCostmapAroundRobotService( + const std::string & service_node_name, + const BT::NodeConfiguration & conf); + + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ + void on_tick() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort( + "reset_distance", 1, + "Distance from the robot under which obstacles are cleared") + }); + } +}; + } // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp new file mode 100644 index 00000000000..811ee005a27 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -0,0 +1,78 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_ + +#include +#include + +#include "nav2_msgs/action/compute_path_through_poses.hpp" +#include "nav_msgs/msg/path.h" +#include "nav2_behavior_tree/bt_action_node.hpp" + +namespace nav2_behavior_tree +{ + + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathThroughPoses + */ +class ComputePathThroughPosesAction + : public BtActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ComputePathThroughPosesAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + ComputePathThroughPosesAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Function to perform some user-defined operation on tick + */ + void on_tick() override; + + /** + * @brief Function to perform some user-defined operation upon successful completion of the action + */ + BT::NodeStatus on_success() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), + BT::InputPort>( + "goals", + "Destinations to plan through"), + BT::InputPort( + "start", "Start pose of the path if overriding current robot pose"), + BT::InputPort("planner_id", ""), + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index 989f4ffd8dc..2528a5ce20b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -24,24 +24,45 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathToPose + */ class ComputePathToPoseAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ ComputePathToPoseAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; + /** + * @brief Function to perform some user-defined operation upon successful completion of the action + */ BT::NodeStatus on_success() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( { BT::OutputPort("path", "Path created by ComputePathToPose node"), BT::InputPort("goal", "Destination to plan to"), + BT::InputPort( + "start", "Start pose of the path if overriding current robot pose"), BT::InputPort("planner_id", ""), }); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp new file mode 100644 index 00000000000..968750a05e9 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// 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_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_ + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "behaviortree_cpp_v3/action_node.h" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief The ControllerSelector behavior is used to switch the controller + * that will be used by the controller server. It subscribes to a topic "controller_selector" + * to get the decision about what controller must be used. It is usually used before of + * the FollowPath. The selected_controller output port is passed to controller_id + * input port of the FollowPath + */ +class ControllerSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ControllerSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + ControllerSelector( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "default_controller", + "the default controller to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "controller_selector", + "the input topic name to select the controller"), + + BT::OutputPort( + "selected_controller", + "Selected controller by subscription") + }; + } + +private: + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the controller_selector topic + * + * @param msg the message with the id of the controller_selector + */ + void callbackControllerSelect(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Subscription::SharedPtr controller_selector_sub_; + + std::string last_selected_controller_; + + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + std::string topic_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index 951c948c8e9..ef6ccf0d63b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -23,24 +23,45 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath + */ class FollowPathAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::FollowPathAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ FollowPathAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; + /** + * @brief Function to perform some user-defined operation after a timeout + * waiting for a result that hasn't been received yet + */ void on_wait_for_result() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( { BT::InputPort("path", "Path to follow"), BT::InputPort("controller_id", ""), + BT::InputPort("goal_checker_id", ""), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp new file mode 100644 index 00000000000..11f0d0f423d --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp @@ -0,0 +1,97 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// 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_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_ + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "behaviortree_cpp_v3/action_node.h" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief The GoalCheckerSelector behavior is used to switch the goal checker + * of the controller server. It subscribes to a topic "goal_checker_selector" + * to get the decision about what goal_checker must be used. It is usually used before of + * the FollowPath. The selected_goal_checker output port is passed to goal_checker_id + * input port of the FollowPath + */ +class GoalCheckerSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::GoalCheckerSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + GoalCheckerSelector( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "default_goal_checker", + "the default goal_checker to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "goal_checker_selector", + "the input topic name to select the goal_checker"), + + BT::OutputPort( + "selected_goal_checker", + "Selected goal_checker by subscription") + }; + } + +private: + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the goal_checker_selector topic + * + * @param msg the message with the id of the goal_checker_selector + */ + void callbackGoalCheckerSelect(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Subscription::SharedPtr goal_checker_selector_sub_; + + std::string last_selected_goal_checker_; + + rclcpp::Node::SharedPtr node_; + + std::string topic_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_ 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 new file mode 100644 index 00000000000..209a11b6bab --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -0,0 +1,65 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_ + +#include + +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "nav2_msgs/action/navigate_through_poses.hpp" +#include "nav2_behavior_tree/bt_action_node.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateThroughPoses + */ +class NavigateThroughPosesAction : public BtActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::NavigateThroughPosesAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ + NavigateThroughPosesAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Function to perform some user-defined operation on tick + */ + void on_tick() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort("goals", "Destinations to plan through"), + }); + } +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp index 383dd3d14ec..aa5f7a1e82a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp @@ -25,17 +25,32 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateToPose + */ class NavigateToPoseAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::NavigateToPoseAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ NavigateToPoseAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp new file mode 100644 index 00000000000..3aa9e6573e1 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp @@ -0,0 +1,100 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// 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_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_ + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "behaviortree_cpp_v3/action_node.h" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief The PlannerSelector behavior is used to switch the planner + * that will be used by the planner server. It subscribes to a topic "planner_selector" + * to get the decision about what planner must be used. It is usually used before of + * the ComputePathToPoseAction. The selected_planner output port is passed to planner_id + * input port of the ComputePathToPoseAction + */ +class PlannerSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::PlannerSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + PlannerSelector( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "default_planner", + "the default planner to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "planner_selector", + "the input topic name to select the planner"), + + BT::OutputPort( + "selected_planner", + "Selected planner by subscription") + }; + } + +private: + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the planner_selector topic + * + * @param msg the message with the id of the planner_selector + */ + void callbackPlannerSelect(const std_msgs::msg::String::SharedPtr msg); + + + rclcpp::Subscription::SharedPtr planner_selector_sub_; + + std::string last_selected_planner_; + + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + std::string topic_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp index c241687261c..8d90327f201 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp @@ -23,9 +23,17 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::Empty + */ class ReinitializeGlobalLocalizationService : public BtServiceNode { public: + /** + * @brief A constructor for nav2_behavior_tree::ReinitializeGlobalLocalizationService + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ ReinitializeGlobalLocalizationService( const std::string & service_node_name, const BT::NodeConfiguration & conf); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp new file mode 100644 index 00000000000..05c01ed58fd --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -0,0 +1,63 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "behaviortree_cpp_v3/action_node.h" + +namespace nav2_behavior_tree +{ + +class RemovePassedGoals : public BT::ActionNodeBase +{ +public: + typedef std::vector Goals; + + RemovePassedGoals( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("input_goals", "Original goals to remove viapoints from"), + BT::OutputPort("output_goals", "Goals with passed viapoints removed"), + BT::InputPort("radius", 0.5, "radius to goal for it to be considered for removal"), + BT::InputPort("global_frame", std::string("map"), "Global frame"), + BT::InputPort("robot_base_frame", std::string("base_link"), "Robot base frame"), + }; + } + +private: + void halt() override {} + BT::NodeStatus tick() override; + + double viapoint_achieved_radius_; + std::string robot_base_frame_, global_frame_; + double transform_tolerance_; + std::shared_ptr tf_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index 4120cad9c52..b3982d26316 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -23,16 +23,32 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Spin + */ class SpinAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::SpinAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ SpinAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp index f76b7df4d59..2604bcfea13 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp @@ -26,14 +26,25 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ActionNodeBase to shorten path by some distance + */ class TruncatePath : public BT::ActionNodeBase { public: + /** + * @brief A nav2_behavior_tree::TruncatePath constructor + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ TruncatePath( const std::string & xml_tag_name, const BT::NodeConfiguration & conf); - + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -44,7 +55,15 @@ class TruncatePath : public BT::ActionNodeBase } private: + /** + * @brief The other (optional) override required by a BT action. + */ void halt() override {} + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; double distance_; 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 a6e753da2ec..f452d24d320 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 @@ -23,17 +23,32 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait + */ class WaitAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::WaitAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ WaitAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index 7bd9b8627a7..6373a5600c1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -28,17 +28,34 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS every time the robot + * travels a specified distance and FAILURE otherwise + */ class DistanceTraveledCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::DistanceTraveledCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ DistanceTraveledCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); DistanceTraveledCondition() = delete; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index fc69d53d33c..a4d41437d84 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -25,23 +25,50 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS when a specified goal + * is reached and FAILURE otherwise + */ class GoalReachedCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::GoalReachedCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ GoalReachedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); GoalReachedCondition() = delete; + /** + * @brief A destructor for nav2_behavior_tree::GoalReachedCondition + */ ~GoalReachedCondition() override; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ void initialize(); + /** + * @brief Checks if the current robot pose lies within a given distance from the goal + * @return bool true when goal is reached, false otherwise + */ bool isGoalReached(); + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -52,6 +79,9 @@ class GoalReachedCondition : public BT::ConditionNode } protected: + /** + * @brief Cleanup function + */ void cleanup() {} diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index 45a6704ef90..1e8c9712c16 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -16,6 +16,7 @@ #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_ #include +#include #include "behaviortree_cpp_v3/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -23,17 +24,34 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS when goal is + * updated on the blackboard and FAILURE otherwise + */ class GoalUpdatedCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::GoalUpdatedCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ GoalUpdatedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); GoalUpdatedCondition() = delete; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return {}; @@ -41,6 +59,7 @@ class GoalUpdatedCondition : public BT::ConditionNode private: geometry_msgs::msg::PoseStamped goal_; + std::vector goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp index 9bed7e6ac4d..2bb7d995b80 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp @@ -19,6 +19,10 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS if initial pose + * has been received and FAILURE otherwise + */ BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 0bc30cd5635..5c4753cf9a1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -27,17 +27,34 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that listens to a battery topic and + * returns SUCCESS when battery is low and FAILURE otherwise + */ class IsBatteryLowCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::IsBatteryLowCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ IsBatteryLowCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); IsBatteryLowCondition() = delete; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -50,15 +67,20 @@ class IsBatteryLowCondition : public BT::ConditionNode } private: + /** + * @brief Callback function for battery topic + * @param msg Shared pointer to sensor_msgs::msg::BatteryState message + */ void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg); rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; rclcpp::Subscription::SharedPtr battery_sub_; std::string battery_topic_; double min_battery_; bool is_voltage_; bool is_battery_low_; - std::mutex mutex_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp index e46f03cccc5..565f261100f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp @@ -26,28 +26,69 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS + * if robot is stuck somewhere and FAILURE otherwise + */ class IsStuckCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::IsStuckCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ IsStuckCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); IsStuckCondition() = delete; + /** + * @brief A destructor for nav2_behavior_tree::IsStuckCondition + */ ~IsStuckCondition() override; + /** + * @brief Callback function for odom topic + * @param msg Shared pointer to nav_msgs::msg::Odometry::SharedPtr message + */ void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg); + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + + /** + * @brief Function to log status when robot is stuck/free + */ void logStuck(const std::string & msg) const; + + /** + * @brief Function to approximate acceleration from the odom history + */ void updateStates(); + + /** + * @brief Detect if robot bumped into something by checking for abnormal deceleration + * @return bool true if robot is stuck, false otherwise + */ bool isStuck(); + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() {return {};} private: // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + std::thread callback_group_executor_thread; std::atomic is_stuck_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp index ec40b9db434..9f8c47afab0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp @@ -24,18 +24,34 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS every time a specified + * time period passes and FAILURE otherwise + */ class TimeExpiredCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::TimeExpiredCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ TimeExpiredCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); TimeExpiredCondition() = delete; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 1a624a4cd2e..64572e21b7a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -26,19 +26,39 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS if there is a valid transform + * between two specified frames and FAILURE otherwise + */ class TransformAvailableCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::TransformAvailableCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ TransformAvailableCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); TransformAvailableCondition() = delete; + /** + * @brief A destructor for nav2_behavior_tree::TransformAvailableCondition + */ ~TransformAvailableCondition(); + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp index 666d1191640..0384381d2a9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp @@ -55,13 +55,37 @@ namespace nav2_behavior_tree class PipelineSequence : public BT::ControlNode { public: + /** + * @brief A constructor for nav2_behavior_tree::PipelineSequence + * @param name Name for the XML tag for this node + */ explicit PipelineSequence(const std::string & name); + + /** + * @brief A constructor for nav2_behavior_tree::PipelineSequence + * @param name Name for the XML tag for this node + * @param config BT node configuration + */ PipelineSequence(const std::string & name, const BT::NodeConfiguration & config); + + /** + * @brief The other (optional) override required by a BT action to reset node state + */ void halt() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() {return {};} protected: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + std::size_t last_child_ticked_ = 0; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp index 331a19d8074..89c0cfa300c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp @@ -35,13 +35,24 @@ namespace nav2_behavior_tree class RecoveryNode : public BT::ControlNode { public: + /** + * @brief A constructor for nav2_behavior_tree::RecoveryNode + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ RecoveryNode( const std::string & name, const BT::NodeConfiguration & conf); + /** + * @brief A destructor for nav2_behavior_tree::RecoveryNode + */ ~RecoveryNode() override = default; - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -54,7 +65,15 @@ class RecoveryNode : public BT::ControlNode unsigned int number_of_retries_; unsigned int retry_count_; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + + /** + * @brief The other (optional) override required by a BT action to reset node state + */ void halt() override; }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp index 8ebc2ab26cf..8c374ce6af0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp @@ -54,10 +54,34 @@ namespace nav2_behavior_tree class RoundRobinNode : public BT::ControlNode { public: + /** + * @brief A constructor for nav2_behavior_tree::RoundRobinNode + * @param name Name for the XML tag for this node + */ explicit RoundRobinNode(const std::string & name); + + /** + * @brief A constructor for nav2_behavior_tree::RoundRobinNode + * @param name Name for the XML tag for this node + * @param config BT node configuration + */ RoundRobinNode(const std::string & name, const BT::NodeConfiguration & config); + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + + /** + * @brief The other (optional) override required by a BT action to reset node state + */ void halt() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() {return {};} private: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp index 299833f5550..2d571597eab 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp @@ -27,14 +27,26 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::DecoratorNode that ticks its child every time the robot + * travels a specified distance + */ class DistanceController : public BT::DecoratorNode { public: + /** + * @brief A constructor for nav2_behavior_tree::DistanceController + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ DistanceController( const std::string & name, const BT::NodeConfiguration & conf); - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -45,6 +57,10 @@ class DistanceController : public BT::DecoratorNode } private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; rclcpp::Node::SharedPtr node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 1aba0ccd890..49dfbc1a0c5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -28,14 +28,26 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::DecoratorNode that subscribes to a goal topic and updates + * the current goal on the blackboard + */ class GoalUpdater : public BT::DecoratorNode { public: + /** + * @brief A constructor for nav2_behavior_tree::GoalUpdater + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ GoalUpdater( const std::string & xml_tag_name, const BT::NodeConfiguration & conf); - + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -47,13 +59,25 @@ class GoalUpdater : public BT::DecoratorNode } private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Callback function for goal update topic + * @param msg Shared pointer to geometry_msgs::msg::PoseStamped message + */ void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg); rclcpp::Subscription::SharedPtr goal_sub_; geometry_msgs::msg::PoseStamped last_goal_received_; + + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp index 01cb2687669..201f4868cfd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp @@ -23,14 +23,25 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::DecoratorNode that ticks its child at a specified rate + */ class RateController : public BT::DecoratorNode { public: + /** + * @brief A constructor for nav2_behavior_tree::RateController + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ RateController( const std::string & name, const BT::NodeConfiguration & conf); - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -39,6 +50,10 @@ class RateController : public BT::DecoratorNode } private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; std::chrono::time_point start_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp new file mode 100644 index 00000000000..c16ef63efa9 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp @@ -0,0 +1,63 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_ + +#include +#include + +#include "behaviortree_cpp_v3/decorator_node.h" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::DecoratorNode that triggers its child only once and returns FAILURE + * for every succeeding tick + */ +class SingleTrigger : public BT::DecoratorNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::SingleTrigger + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ + SingleTrigger( + const std::string & name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return {}; + } + +private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + bool first_time_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 5af04291170..15322ef85b5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include "nav_msgs/msg/odometry.hpp" @@ -28,14 +29,27 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::DecoratorNode that ticks its child every at a rate proportional to + * the speed of the robot. If the robot travels faster, this node will tick its child at a + * higher frequency and reduce the tick frequency if the robot slows down + */ class SpeedController : public BT::DecoratorNode { public: + /** + * @brief A constructor for nav2_behavior_tree::SpeedController + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ SpeedController( const std::string & name, const BT::NodeConfiguration & conf); - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -48,9 +62,16 @@ class SpeedController : public BT::DecoratorNode } private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; - // Scale the rate based speed + /** + * @brief Scale the rate based speed + * @return double Rate scaled by speed limits and clamped + */ inline double getScaledRate(const double & speed) { return std::max( @@ -59,7 +80,9 @@ class SpeedController : public BT::DecoratorNode max_rate_), min_rate_); } - // Update period based on current smoothed speed and reset timer + /** + * @brief Update period based on current smoothed speed and reset timer + */ inline void updatePeriod() { auto velocity = odom_smoother_->getTwist(); @@ -93,6 +116,7 @@ class SpeedController : public BT::DecoratorNode // current goal geometry_msgs::msg::PoseStamped goal_; + std::vector goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp new file mode 100644 index 00000000000..d71a57bc046 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp @@ -0,0 +1,107 @@ +// Copyright (c) 2019 Intel Corporation +// +// 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_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_ +#define NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_ + +#include +#include +#include + +#include "behaviortree_cpp_v3/loggers/abstract_logger.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_msgs/msg/behavior_tree_log.hpp" +#include "nav2_msgs/msg/behavior_tree_status_change.h" +#include "tf2_ros/buffer_interface.h" + +namespace nav2_behavior_tree +{ + +/** + * @brief A class to publish BT logs on BT status change + */ +class RosTopicLogger : public BT::StatusChangeLogger +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::RosTopicLogger + * @param ros_node Weak pointer to parent rclcpp::Node + * @param tree BT to monitor + */ + RosTopicLogger(const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree) + : StatusChangeLogger(tree.rootNode()) + { + auto node = ros_node.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + log_pub_ = node->create_publisher( + "behavior_tree_log", + rclcpp::QoS(10)); + } + + /** + * @brief Callback function which is called each time BT changes status + * @param timestamp Timestamp of BT status change + * @param node Node that changed status + * @param prev_status Previous status of the node + * @param status Current status of the node + */ + void callback( + BT::Duration timestamp, + const BT::TreeNode & node, + BT::NodeStatus prev_status, + BT::NodeStatus status) override + { + nav2_msgs::msg::BehaviorTreeStatusChange event; + + // BT timestamps are a duration since the epoch. Need to convert to a time_point + // before converting to a msg. + event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp)); + event.node_name = node.name(); + event.previous_status = toStr(prev_status, false); + event.current_status = toStr(status, false); + event_log_.push_back(std::move(event)); + + RCLCPP_DEBUG( + logger_, "[%.3f]: %25s %s -> %s", + std::chrono::duration(timestamp).count(), + node.name().c_str(), + toStr(prev_status, true).c_str(), + toStr(status, true).c_str() ); + } + + /** + * @brief Clear log buffer if any + */ + void flush() override + { + if (!event_log_.empty()) { + auto log_msg = std::make_unique(); + log_msg->timestamp = clock_->now(); + log_msg->event_log = event_log_; + log_pub_->publish(std::move(log_msg)); + event_log_.clear(); + } + } + +protected: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("bt_navigator")}; + rclcpp::Publisher::SharedPtr log_pub_; + std::vector event_log_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 9a36ccdc6e0..0deec06c0dc 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -18,20 +18,64 @@ Destination to plan to + Start pose of the path if overriding current robot pose Path created by ComputePathToPose node + + Destinations to plan through + Start pose of the path if overriding current robot pose + Path created by ComputePathToPose node + + + + + Input goals to remove if passed + Radius tolerance on a goal to consider it passed + Set of goals after removing any passed + + Path to follow + Goal checker Goal - + + Goals + + + + + + + Distance before goal to truncate + Path to truncate + Truncated path to utilize + + + + Name of the topic to receive planner selection commands + Default planner of the planner selector + Name of the selected planner received from the topic subcription + + + + Name of the topic to receive controller selection commands + Default controller of the controller selector + Name of the selected controller received from the topic subcription + + + + Name of the topic to receive goal checker selection commands + Default goal checker of the controller selector + Name of the selected goal checker received from the topic subcription + Spin distance @@ -55,6 +99,24 @@ + + Min battery % or voltage before triggering + Topic for battery info + Bool if check based on voltage or total % + + + + Distance to check if passed + reference frame to check in + Robot frame to check relative to global_frame + + + + Time to check if expired + + + + @@ -73,6 +135,14 @@ Distance + + + + + Original goal in + Output goal set by subscription + + Minimum rate Maximum rate diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index e3600947cc8..472de10433a 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.4.3 + 1.0.0 TODO Michael Jeronimo Carlos Orduno @@ -50,6 +50,7 @@ ament_lint_common ament_lint_auto + ament_cmake_gtest ament_cmake diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index aa269bdbcc3..08d2b00632b 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -32,10 +32,40 @@ void ClearEntireCostmapService::on_tick() increment_recovery_count(); } +ClearCostmapExceptRegionService::ClearCostmapExceptRegionService( + const std::string & service_node_name, + const BT::NodeConfiguration & conf) +: BtServiceNode(service_node_name, conf) +{ +} + +void ClearCostmapExceptRegionService::on_tick() +{ + getInput("reset_distance", request_->reset_distance); + increment_recovery_count(); +} + +ClearCostmapAroundRobotService::ClearCostmapAroundRobotService( + const std::string & service_node_name, + const BT::NodeConfiguration & conf) +: BtServiceNode(service_node_name, conf) +{ +} + +void ClearCostmapAroundRobotService::on_tick() +{ + getInput("reset_distance", request_->reset_distance); + increment_recovery_count(); +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("ClearEntireCostmap"); + factory.registerNodeType( + "ClearCostmapExceptRegion"); + factory.registerNodeType( + "ClearCostmapAroundRobot"); } 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 new file mode 100644 index 00000000000..f699b9fc0bc --- /dev/null +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -0,0 +1,61 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp" + +namespace nav2_behavior_tree +{ + +ComputePathThroughPosesAction::ComputePathThroughPosesAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ +} + +void ComputePathThroughPosesAction::on_tick() +{ + getInput("goals", goal_.goals); + getInput("planner_id", goal_.planner_id); + if (getInput("start", goal_.start)) { + goal_.use_start = true; + } +} + +BT::NodeStatus ComputePathThroughPosesAction::on_success() +{ + setOutput("path", result_.result->path); + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "compute_path_through_poses", config); + }; + + factory.registerBuilder( + "ComputePathThroughPoses", builder); +} 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 efbabbdbea9..83b29dfe3a3 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 @@ -30,8 +30,11 @@ ComputePathToPoseAction::ComputePathToPoseAction( void ComputePathToPoseAction::on_tick() { - getInput("goal", goal_.pose); + getInput("goal", goal_.goal); getInput("planner_id", goal_.planner_id); + if (getInput("start", goal_.start)) { + goal_.use_start = true; + } } BT::NodeStatus ComputePathToPoseAction::on_success() diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp new file mode 100644 index 00000000000..7d77adbee39 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +ControllerSelector::ControllerSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + getInput("topic_name", topic_name_); + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + controller_selector_sub_ = node_->create_subscription( + topic_name_, + qos, + std::bind(&ControllerSelector::callbackControllerSelect, this, _1), + sub_option); +} + +BT::NodeStatus ControllerSelector::tick() +{ + callback_group_executor_.spin_some(); + + // This behavior always use the last selected controller received from the topic input. + // When no input is specified it uses the default controller. + // If the default controller is not specified then we work in "required controller mode": + // In this mode, the behavior returns failure if the controller selection is not received from + // the topic input. + if (last_selected_controller_.empty()) { + std::string default_controller; + getInput("default_controller", default_controller); + if (default_controller.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_controller_ = default_controller; + } + } + + setOutput("selected_controller", last_selected_controller_); + + return BT::NodeStatus::SUCCESS; +} + +void +ControllerSelector::callbackControllerSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_controller_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ControllerSelector"); +} diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index bc53211c96d..d3353a8981d 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -32,6 +32,7 @@ void FollowPathAction::on_tick() { getInput("path", goal_.path); getInput("controller_id", goal_.controller_id); + getInput("goal_checker_id", goal_.goal_checker_id); } void FollowPathAction::on_wait_for_result() @@ -46,6 +47,22 @@ void FollowPathAction::on_wait_for_result() goal_.path = new_path; goal_updated_ = true; } + + std::string new_controller_id; + getInput("controller_id", new_controller_id); + + if (goal_.controller_id != new_controller_id) { + goal_.controller_id = new_controller_id; + goal_updated_ = true; + } + + std::string new_goal_checker_id; + getInput("goal_checker_id", new_goal_checker_id); + + if (goal_.goal_checker_id != new_goal_checker_id) { + goal_.goal_checker_id = new_goal_checker_id; + goal_updated_ = true; + } } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp new file mode 100644 index 00000000000..1a2e9c703b0 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp @@ -0,0 +1,82 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +GoalCheckerSelector::GoalCheckerSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + node_ = config().blackboard->get("node"); + + getInput("topic_name", topic_name_); + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + goal_checker_selector_sub_ = node_->create_subscription( + topic_name_, qos, std::bind(&GoalCheckerSelector::callbackGoalCheckerSelect, this, _1)); +} + +BT::NodeStatus GoalCheckerSelector::tick() +{ + rclcpp::spin_some(node_); + + // This behavior always use the last selected goal checker received from the topic input. + // When no input is specified it uses the default goal checker. + // If the default goal checker is not specified then we work in "required goal checker mode": + // In this mode, the behavior returns failure if the goal checker selection is not received from + // the topic input. + if (last_selected_goal_checker_.empty()) { + std::string default_goal_checker; + getInput("default_goal_checker", default_goal_checker); + if (default_goal_checker.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_goal_checker_ = default_goal_checker; + } + } + + setOutput("selected_goal_checker", last_selected_goal_checker_); + + return BT::NodeStatus::SUCCESS; +} + +void +GoalCheckerSelector::callbackGoalCheckerSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_goal_checker_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GoalCheckerSelector"); +} diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp new file mode 100644 index 00000000000..657b191491d --- /dev/null +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -0,0 +1,55 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp" + +namespace nav2_behavior_tree +{ + +NavigateThroughPosesAction::NavigateThroughPosesAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ +} + +void NavigateThroughPosesAction::on_tick() +{ + if (!getInput("goals", goal_.poses)) { + RCLCPP_ERROR( + node_->get_logger(), + "NavigateThroughPosesAction: goal not provided"); + return; + } +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "navigate_through_poses", config); + }; + + factory.registerBuilder( + "NavigateThroughPoses", builder); +} diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp new file mode 100644 index 00000000000..ccb85184127 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +PlannerSelector::PlannerSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + getInput("topic_name", topic_name_); + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + planner_selector_sub_ = node_->create_subscription( + topic_name_, + qos, + std::bind(&PlannerSelector::callbackPlannerSelect, this, _1), + sub_option); +} + +BT::NodeStatus PlannerSelector::tick() +{ + callback_group_executor_.spin_some(); + + // This behavior always use the last selected planner received from the topic input. + // When no input is specified it uses the default planner. + // If the default planner is not specified then we work in "required planner mode": + // In this mode, the behavior returns failure if the planner selection is not received from + // the topic input. + if (last_selected_planner_.empty()) { + std::string default_planner; + getInput("default_planner", default_planner); + if (default_planner.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_planner_ = default_planner; + } + } + + setOutput("selected_planner", last_selected_planner_); + + return BT::NodeStatus::SUCCESS; +} + +void +PlannerSelector::callbackPlannerSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_planner_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("PlannerSelector"); +} diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp new file mode 100644 index 00000000000..99568f933f9 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -0,0 +1,86 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp" + +namespace nav2_behavior_tree +{ + +RemovePassedGoals::RemovePassedGoals( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf), + viapoint_achieved_radius_(0.5) +{ + getInput("radius", viapoint_achieved_radius_); + + getInput("global_frame", global_frame_); + getInput("robot_base_frame", robot_base_frame_); + tf_ = config().blackboard->get>("tf_buffer"); + auto node = config().blackboard->get("node"); + node->get_parameter("transform_tolerance", transform_tolerance_); +} + +inline BT::NodeStatus RemovePassedGoals::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + Goals goal_poses; + getInput("input_goals", goal_poses); + + if (goal_poses.empty()) { + setOutput("output_goals", goal_poses); + return BT::NodeStatus::SUCCESS; + } + + using namespace nav2_util::geometry_utils; // NOLINT + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + return BT::NodeStatus::FAILURE; + } + + double dist_to_goal; + while (goal_poses.size() > 1) { + dist_to_goal = euclidean_distance(goal_poses[0].pose, current_pose.pose); + + if (dist_to_goal > viapoint_achieved_radius_) { + break; + } + + goal_poses.erase(goal_poses.begin()); + } + + setOutput("output_goals", goal_poses); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("RemovePassedGoals"); +} diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index a09b9f81df9..ddbb5402dc5 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include - +#include #include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp" namespace nav2_behavior_tree @@ -29,13 +29,19 @@ GoalUpdatedCondition::GoalUpdatedCondition( BT::NodeStatus GoalUpdatedCondition::tick() { if (status() == BT::NodeStatus::IDLE) { - goal_ = config().blackboard->get("goal"); + config().blackboard->get>("goals", goals_); + config().blackboard->get("goal", goal_); return BT::NodeStatus::FAILURE; } - auto current_goal = config().blackboard->get("goal"); - if (goal_ != current_goal) { + std::vector current_goals; + config().blackboard->get>("goals", current_goals); + geometry_msgs::msg::PoseStamped current_goal; + config().blackboard->get("goal", current_goal); + + if (goal_ != current_goal || goals_ != current_goals) { goal_ = current_goal; + goals_ = current_goals; return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index 4aad6d16769..b8630261e6e 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -33,15 +33,23 @@ IsBatteryLowCondition::IsBatteryLowCondition( getInput("battery_topic", battery_topic_); getInput("is_voltage", is_voltage_); node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; battery_sub_ = node_->create_subscription( battery_topic_, rclcpp::SystemDefaultsQoS(), - std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1)); + std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), + sub_option); } BT::NodeStatus IsBatteryLowCondition::tick() { - std::lock_guard lock(mutex_); + callback_group_executor_.spin_some(); if (is_battery_low_) { return BT::NodeStatus::SUCCESS; } @@ -50,7 +58,6 @@ BT::NodeStatus IsBatteryLowCondition::tick() void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg) { - std::lock_guard lock(mutex_); if (is_voltage_) { is_battery_low_ = msg->voltage <= min_battery_; } else { diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp index 185f331dc91..211254dafe9 100644 --- a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp @@ -32,11 +32,19 @@ IsStuckCondition::IsStuckCondition( brake_accel_limit_(-10.0) { node_ = config().blackboard->get("node"); - + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + callback_group_executor_thread = std::thread([this]() {callback_group_executor_.spin();}); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; odom_sub_ = node_->create_subscription( "odom", rclcpp::SystemDefaultsQoS(), - std::bind(&IsStuckCondition::onOdomReceived, this, std::placeholders::_1)); + std::bind(&IsStuckCondition::onOdomReceived, this, std::placeholders::_1), + sub_option); RCLCPP_DEBUG(node_->get_logger(), "Initialized an IsStuckCondition BT node"); @@ -46,6 +54,8 @@ IsStuckCondition::IsStuckCondition( IsStuckCondition::~IsStuckCondition() { RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStuckCondition BT node"); + callback_group_executor_.cancel(); + callback_group_executor_thread.join(); } void IsStuckCondition::onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg) @@ -85,7 +95,7 @@ void IsStuckCondition::logStuck(const std::string & msg) const return; } - RCLCPP_INFO(node_->get_logger(), msg); + RCLCPP_INFO(node_->get_logger(), msg.c_str()); prev_msg = msg; } diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 5279613498b..febaa7de648 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -33,13 +33,22 @@ GoalUpdater::GoalUpdater( const BT::NodeConfiguration & conf) : BT::DecoratorNode(name, conf) { - auto node = config().blackboard->get("node"); + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); std::string goal_updater_topic; - node->get_parameter_or("goal_updater_topic", goal_updater_topic, "goal_update"); - - goal_sub_ = node->create_subscription( - goal_updater_topic, 10, std::bind(&GoalUpdater::callback_updated_goal, this, _1)); + node_->get_parameter_or("goal_updater_topic", goal_updater_topic, "goal_update"); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + goal_sub_ = node_->create_subscription( + goal_updater_topic, + 10, + std::bind(&GoalUpdater::callback_updated_goal, this, _1), + sub_option); } inline BT::NodeStatus GoalUpdater::tick() @@ -48,6 +57,8 @@ inline BT::NodeStatus GoalUpdater::tick() getInput("input_goal", goal); + callback_group_executor_.spin_some(); + if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) { goal = last_goal_received_; } diff --git a/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp b/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp new file mode 100644 index 00000000000..84f0fadfbbc --- /dev/null +++ b/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp @@ -0,0 +1,69 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp" + +namespace nav2_behavior_tree +{ + +SingleTrigger::SingleTrigger( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::DecoratorNode(name, conf), + first_time_(true) +{ +} + +BT::NodeStatus SingleTrigger::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + first_time_ = true; + } + + setStatus(BT::NodeStatus::RUNNING); + + if (first_time_) { + const BT::NodeStatus child_state = child_node_->executeTick(); + + switch (child_state) { + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + + case BT::NodeStatus::SUCCESS: + first_time_ = false; + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::FAILURE: + first_time_ = false; + return BT::NodeStatus::FAILURE; + + default: + first_time_ = false; + return BT::NodeStatus::FAILURE; + } + } + + return BT::NodeStatus::FAILURE; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("SingleTrigger"); +} diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index b93b88d7b62..26d96de77a7 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -15,7 +15,7 @@ #include #include - +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp" @@ -43,7 +43,7 @@ SpeedController::SpeedController( if (min_rate_ <= 0.0 || max_rate_ <= 0.0) { std::string err_msg = "SpeedController node cannot have rate <= 0.0"; - RCLCPP_FATAL(node_->get_logger(), err_msg); + RCLCPP_FATAL(node_->get_logger(), err_msg.c_str()); throw BT::BehaviorTreeException(err_msg); } @@ -60,13 +60,16 @@ SpeedController::SpeedController( inline BT::NodeStatus SpeedController::tick() { auto current_goal = config().blackboard->get("goal"); + auto current_goals = + config().blackboard->get>("goals"); - if (goal_ != current_goal) { + if (goal_ != current_goal || goals_ != current_goals) { // Reset state and set period to max since we have a new goal period_ = 1.0 / max_rate_; start_ = node_->now(); first_tick_ = true; goal_ = current_goal; + goals_ = current_goals; } setStatus(BT::NodeStatus::RUNNING); diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 93841615d9e..e689cae927a 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -44,17 +44,24 @@ BehaviorTreeEngine::run( BT::NodeStatus result = BT::NodeStatus::RUNNING; // Loop until something happens with ROS or the node completes - while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - if (cancelRequested()) { - tree->rootNode()->halt(); - return BtStatus::CANCELED; - } + try { + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { + if (cancelRequested()) { + tree->rootNode()->halt(); + return BtStatus::CANCELED; + } - result = tree->tickRoot(); + result = tree->tickRoot(); - onLoop(); + onLoop(); - loopRate.sleep(); + loopRate.sleep(); + } + } catch (const std::exception & ex) { + RCLCPP_ERROR( + rclcpp::get_logger("BehaviorTreeEngine"), + "Behavior tree threw exception: %s. Exiting with failure.", ex.what()); + return BtStatus::FAILED; } return (result == BT::NodeStatus::SUCCESS) ? BtStatus::SUCCEEDED : BtStatus::FAILED; @@ -92,7 +99,9 @@ BehaviorTreeEngine::addGrootMonitoring( void BehaviorTreeEngine::resetGrootMonitor() { - groot_monitor_.reset(); + if (groot_monitor_) { + groot_monitor_.reset(); + } } // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index ec004fea2d3..1f69b5fce3d 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -1,3 +1,6 @@ +ament_add_gtest(test_bt_action_node test_bt_action_node.cpp) +ament_target_dependencies(test_bt_action_node ${dependencies}) + ament_add_gtest(test_action_spin_action test_spin_action.cpp) target_link_libraries(test_action_spin_action nav2_spin_action_bt_node) ament_target_dependencies(test_action_spin_action ${dependencies}) @@ -22,6 +25,10 @@ ament_add_gtest(test_action_compute_path_to_pose_action test_compute_path_to_pos target_link_libraries(test_action_compute_path_to_pose_action nav2_compute_path_to_pose_action_bt_node) ament_target_dependencies(test_action_compute_path_to_pose_action ${dependencies}) +ament_add_gtest(test_action_compute_path_through_poses_action test_compute_path_through_poses_action.cpp) +target_link_libraries(test_action_compute_path_through_poses_action nav2_compute_path_through_poses_action_bt_node) +ament_target_dependencies(test_action_compute_path_through_poses_action ${dependencies}) + ament_add_gtest(test_action_follow_path_action test_follow_path_action.cpp) target_link_libraries(test_action_follow_path_action nav2_follow_path_action_bt_node) ament_target_dependencies(test_action_follow_path_action ${dependencies}) @@ -30,6 +37,26 @@ ament_add_gtest(test_action_navigate_to_pose_action test_navigate_to_pose_action target_link_libraries(test_action_navigate_to_pose_action nav2_navigate_to_pose_action_bt_node) ament_target_dependencies(test_action_navigate_to_pose_action ${dependencies}) +ament_add_gtest(test_action_navigate_through_poses_action test_navigate_through_poses_action.cpp) +target_link_libraries(test_action_navigate_through_poses_action nav2_navigate_through_poses_action_bt_node) +ament_target_dependencies(test_action_navigate_through_poses_action ${dependencies}) + ament_add_gtest(test_truncate_path_action test_truncate_path_action.cpp) target_link_libraries(test_truncate_path_action nav2_truncate_path_action_bt_node) ament_target_dependencies(test_truncate_path_action ${dependencies}) + +ament_add_gtest(test_remove_passed_goals_action test_remove_passed_goals_action.cpp) +target_link_libraries(test_remove_passed_goals_action nav2_remove_passed_goals_action_bt_node) +ament_target_dependencies(test_remove_passed_goals_action ${dependencies}) + +ament_add_gtest(test_planner_selector_node test_planner_selector_node.cpp) +target_link_libraries(test_planner_selector_node nav2_planner_selector_bt_node) +ament_target_dependencies(test_planner_selector_node ${dependencies}) + +ament_add_gtest(test_controller_selector_node test_controller_selector_node.cpp) +target_link_libraries(test_controller_selector_node nav2_controller_selector_bt_node) +ament_target_dependencies(test_controller_selector_node ${dependencies}) + +ament_add_gtest(test_goal_checker_selector_node test_goal_checker_selector_node.cpp) +target_link_libraries(test_goal_checker_selector_node nav2_goal_checker_selector_bt_node) +ament_target_dependencies(test_goal_checker_selector_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index 6a847a7bc54..db970202128 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -32,9 +32,19 @@ class BackUpActionServer : public TestActionServer protected: void execute( - const typename std::shared_ptr>) + const typename std::shared_ptr> + goal_handle) override - {} + { + nav2_msgs::action::BackUp::Result::SharedPtr result = + std::make_shared(); + bool return_success = getReturnSuccess(); + if (return_success) { + goal_handle->succeed(result); + } else { + goal_handle->abort(result); + } + } }; class BackUpActionTestFixture : public ::testing::Test @@ -54,6 +64,9 @@ class BackUpActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -154,6 +167,34 @@ TEST_F(BackUpActionTestFixture, test_tick) EXPECT_EQ(goal->speed, 0.26f); } +TEST_F(BackUpActionTestFixture, test_failure) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setReturnSuccess(false); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + + auto goal = action_server_->getCurrentGoal(); + EXPECT_EQ(goal->target.x, 2.0); + EXPECT_EQ(goal->speed, 0.26f); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp new file mode 100644 index 00000000000..d95cbd4a848 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -0,0 +1,364 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/bt_action_node.hpp" + +#include "test_msgs/action/fibonacci.hpp" + +using namespace std::chrono_literals; // NOLINT +using namespace std::placeholders; // NOLINT + +class FibonacciActionServer : public rclcpp::Node +{ +public: + FibonacciActionServer() + : rclcpp::Node("fibonacci_node", rclcpp::NodeOptions()), + sleep_duration_(0ms) + { + this->action_server_ = rclcpp_action::create_server( + this->get_node_base_interface(), + this->get_node_clock_interface(), + this->get_node_logging_interface(), + this->get_node_waitables_interface(), + "fibonacci", + std::bind(&FibonacciActionServer::handle_goal, this, _1, _2), + std::bind(&FibonacciActionServer::handle_cancel, this, _1), + std::bind(&FibonacciActionServer::handle_accepted, this, _1)); + } + + void setHandleGoalSleepDuration(std::chrono::milliseconds sleep_duration) + { + sleep_duration_ = sleep_duration; + } + +protected: + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr) + { + if (sleep_duration_ > 0ms) { + std::this_thread::sleep_for(sleep_duration_); + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr>) + { + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted( + const std::shared_ptr> handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + if (handle) { + const auto goal = handle->get_goal(); + auto result = std::make_shared(); + + if (goal->order < 0) { + handle->abort(result); + return; + } + + auto & sequence = result->sequence; + sequence.push_back(0); + sequence.push_back(1); + + for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { + sequence.push_back(sequence[i] + sequence[i - 1]); + } + + handle->succeed(result); + } + } + +protected: + rclcpp_action::Server::SharedPtr action_server_; + std::chrono::milliseconds sleep_duration_; +}; + +class FibonacciAction : public nav2_behavior_tree::BtActionNode +{ +public: + FibonacciAction( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf) + : nav2_behavior_tree::BtActionNode(xml_tag_name, "fibonacci", conf) + {} + + void on_tick() override + { + getInput("order", goal_.order); + } + + BT::NodeStatus on_success() override + { + config().blackboard->set>("sequence", result_.result->sequence); + return BT::NodeStatus::SUCCESS; + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({BT::InputPort("order", "Fibonacci order")}); + } +}; + +class BTActionNodeTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("bt_action_node_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + config_->blackboard->set("server_timeout", 20ms); + config_->blackboard->set("bt_loop_duration", 10ms); + config_->blackboard->set("initial_pose_received", false); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique(name, config); + }; + + factory_->registerBuilder("Fibonacci", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void SetUp() override + { + // initialize action server and spin on new thread + action_server_ = std::make_shared(); + server_thread_ = std::make_shared( + []() { + while (rclcpp::ok() && BTActionNodeTestFixture::action_server_ != nullptr) { + rclcpp::spin_some(BTActionNodeTestFixture::action_server_); + std::this_thread::sleep_for(100ns); + } + }); + } + + void TearDown() override + { + action_server_.reset(); + tree_.reset(); + server_thread_->join(); + server_thread_.reset(); + } + + static std::shared_ptr action_server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; + static std::shared_ptr server_thread_; +}; + +rclcpp::Node::SharedPtr BTActionNodeTestFixture::node_ = nullptr; +std::shared_ptr BTActionNodeTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * BTActionNodeTestFixture::config_ = nullptr; +std::shared_ptr BTActionNodeTestFixture::factory_ = nullptr; +std::shared_ptr BTActionNodeTestFixture::tree_ = nullptr; +std::shared_ptr BTActionNodeTestFixture::server_thread_ = nullptr; + +TEST_F(BTActionNodeTestFixture, test_server_timeout_success) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + // the server timeout is larger than the goal handling duration + config_->blackboard->set("server_timeout", 20ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // setting a small action server goal handling duration + action_server_->setHandleGoalSleepDuration(2ms); + + // to keep track of the number of ticks it took to reach a terminal result + int ticks = 0; + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + // BT loop execution rate + rclcpp::WallRate loopRate(10ms); + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // get calculated fibonacci sequence from blackboard + auto sequence = config_->blackboard->get>("sequence"); + + // expected fibonacci sequence for order 5 + std::vector expected = {0, 1, 1, 2, 3, 5}; + + // since the server timeout was larger than the action server goal handling duration + // the BT should have succeeded + EXPECT_EQ(result, BT::NodeStatus::SUCCESS); + + // checking the output fibonacci sequence + EXPECT_EQ(sequence.size(), expected.size()); + for (size_t i = 0; i < expected.size(); ++i) { + EXPECT_EQ(sequence[i], expected[i]); + } + + // start a new execution cycle with the previous BT to ensure previous state doesn't leak into + // the new cycle + + // halt BT for a new execution cycle + tree_->haltTree(); + + // setting a large action server goal handling duration + action_server_->setHandleGoalSleepDuration(100ms); + + // reset state variables + ticks = 0; + result = BT::NodeStatus::RUNNING; + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // since the server timeout was smaller than the action server goal handling duration + // the BT should have failed + EXPECT_EQ(result, BT::NodeStatus::FAILURE); + + // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should be 2 + EXPECT_EQ(ticks, 2); +} + +TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + // setting a server timeout smaller than the time the action server will take to accept the goal + // to simulate a server timeout scenario + config_->blackboard->set("server_timeout", 90ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // the action server will take 100ms before accepting the goal + action_server_->setHandleGoalSleepDuration(100ms); + + // to keep track of the number of ticks it took to reach a terminal result + int ticks = 0; + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + // BT loop execution rate + rclcpp::WallRate loopRate(10ms); + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // since the server timeout was smaller than the action server goal handling duration + // the BT should have failed + EXPECT_EQ(result, BT::NodeStatus::FAILURE); + + // since the server timeout is 90ms and bt loop duration is 10ms, number of ticks should be 9 + EXPECT_EQ(ticks, 9); + + // start a new execution cycle with the previous BT to ensure previous state doesn't leak into + // the new cycle + + // halt BT for a new execution cycle + tree_->haltTree(); + + // setting a small action server goal handling duration + action_server_->setHandleGoalSleepDuration(25ms); + + // reset state variables + ticks = 0; + result = BT::NodeStatus::RUNNING; + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // since the server timeout was smaller than the action server goal handling duration + // the BT should have failed + EXPECT_EQ(result, BT::NodeStatus::SUCCESS); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index d58b6fdd85d..f10177ecffb 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -49,6 +49,9 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -106,6 +109,190 @@ TEST_F(ClearEntireCostmapServiceTestFixture, test_tick) EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); } +class ClearCostmapExceptRegionService : public TestService +{ +public: + ClearCostmapExceptRegionService() + : TestService("clear_costmap_except_region") + {} +}; + +class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("clear_costmap_except_region_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); + + factory_->registerNodeType( + "ClearCostmapExceptRegion"); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + server_.reset(); + factory_.reset(); + } + + void SetUp() override + { + config_->blackboard->set("number_recoveries", 0); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr +ClearCostmapExceptRegionServiceTestFixture::node_ = nullptr; +std::shared_ptr +ClearCostmapExceptRegionServiceTestFixture::server_ = nullptr; +BT::NodeConfiguration +* ClearCostmapExceptRegionServiceTestFixture::config_ = nullptr; +std::shared_ptr +ClearCostmapExceptRegionServiceTestFixture::factory_ = nullptr; +std::shared_ptr +ClearCostmapExceptRegionServiceTestFixture::tree_ = nullptr; + +TEST_F(ClearCostmapExceptRegionServiceTestFixture, test_tick) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); +} +//****************************************** +class ClearCostmapAroundRobotService : public TestService +{ +public: + ClearCostmapAroundRobotService() + : TestService("clear_costmap_around_robot") + {} +}; + +class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("clear_costmap_around_robot_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(10)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("number_recoveries", 0); + + factory_->registerNodeType( + "ClearCostmapAroundRobot"); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + server_.reset(); + factory_.reset(); + } + + void SetUp() override + { + config_->blackboard->set("number_recoveries", 0); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr +ClearCostmapAroundRobotServiceTestFixture::node_ = nullptr; +std::shared_ptr +ClearCostmapAroundRobotServiceTestFixture::server_ = nullptr; +BT::NodeConfiguration +* ClearCostmapAroundRobotServiceTestFixture::config_ = nullptr; +std::shared_ptr +ClearCostmapAroundRobotServiceTestFixture::factory_ = nullptr; +std::shared_ptr +ClearCostmapAroundRobotServiceTestFixture::tree_ = nullptr; + +TEST_F(ClearCostmapAroundRobotServiceTestFixture, test_tick) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); @@ -119,11 +306,25 @@ int main(int argc, char ** argv) rclcpp::spin(ClearEntireCostmapServiceTestFixture::server_); }); + ClearCostmapExceptRegionServiceTestFixture::server_ = + std::make_shared(); + std::thread server_thread_except_region([]() { + rclcpp::spin(ClearCostmapExceptRegionServiceTestFixture::server_); + }); + + ClearCostmapAroundRobotServiceTestFixture::server_ = + std::make_shared(); + std::thread server_thread_around_robot([]() { + rclcpp::spin(ClearCostmapAroundRobotServiceTestFixture::server_); + }); + int all_successful = RUN_ALL_TESTS(); // shutdown ROS rclcpp::shutdown(); server_thread.join(); + server_thread_except_region.join(); + server_thread_around_robot.join(); return all_successful; } diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp new file mode 100644 index 00000000000..122c7646212 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -0,0 +1,272 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp" + +class ComputePathThroughPosesActionServer + : public TestActionServer +{ +public: + ComputePathThroughPosesActionServer() + : TestActionServer("compute_path_through_poses") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> goal_handle) + override + { + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + result->path.poses.resize(2); + result->path.poses[1].pose.position.x = goal->goals[0].pose.position.x; + if (goal->use_start) { + result->path.poses[0].pose.position.x = goal->start.pose.position.x; + } else { + result->path.poses[0].pose.position.x = 0.0; + } + goal_handle->succeed(result); + } +}; + +class ComputePathThroughPosesActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("compute_path_through_poses_action_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set("initial_pose_received", false); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "compute_path_through_poses", config); + }; + + factory_->registerBuilder( + "ComputePathThroughPoses", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ComputePathThroughPosesActionTestFixture::node_ = nullptr; +std::shared_ptr +ComputePathThroughPosesActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * ComputePathThroughPosesActionTestFixture::config_ = nullptr; +std::shared_ptr ComputePathThroughPosesActionTestFixture::factory_ = + nullptr; +std::shared_ptr ComputePathThroughPosesActionTestFixture::tree_ = nullptr; + +TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + std::vector goals; + goals.resize(1); + goals[0].pose.position.x = 1.0; + config_->blackboard->set("goals", goals); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, 1.0); + EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); + + // check if returned path is correct + nav_msgs::msg::Path path; + config_->blackboard->get("path", path); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 0.0); + EXPECT_EQ(path.poses[1].pose.position.x, 1.0); + + // halt node so another goal can be sent + tree_->rootNode()->halt(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // set new goal + goals[0].pose.position.x = -2.5; + config_->blackboard->set("goals", goals); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); + + config_->blackboard->get("path", path); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 0.0); + EXPECT_EQ(path.poses[1].pose.position.x, -2.5); +} + +TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new start and set it on blackboard + geometry_msgs::msg::PoseStamped start; + start.header.stamp = node_->now(); + start.pose.position.x = 2.0; + config_->blackboard->set("start", start); + + // create new goal and set it on blackboard + std::vector goals; + goals.resize(1); + goals[0].pose.position.x = 1.0; + config_->blackboard->set("goals", goals); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0); + EXPECT_TRUE(action_server_->getCurrentGoal()->use_start); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); + + // check if returned path is correct + nav_msgs::msg::Path path; + config_->blackboard->get("path", path); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 2.0); + EXPECT_EQ(path.poses[1].pose.position.x, 1.0); + + // halt node so another goal can be sent + tree_->rootNode()->halt(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // set new goal and new start + goals[0].pose.position.x = -2.5; + start.pose.position.x = -1.5; + config_->blackboard->set("goals", goals); + config_->blackboard->set("start", start); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); + + config_->blackboard->get("path", path); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, -1.5); + EXPECT_EQ(path.poses[1].pose.position.x, -2.5); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + ComputePathThroughPosesActionTestFixture::action_server_ = + std::make_shared(); + + std::thread server_thread([]() { + rclcpp::spin(ComputePathThroughPosesActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index d5d1cd6621a..5e35a1981de 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -41,8 +41,13 @@ class ComputePathToPoseActionServer : public TestActionServerget_goal(); auto result = std::make_shared(); - result->path.poses.resize(1); - result->path.poses[0].pose.position.x = goal->pose.pose.position.x; + result->path.poses.resize(2); + result->path.poses[1].pose.position.x = goal->goal.pose.position.x; + if (goal->use_start) { + result->path.poses[0].pose.position.x = goal->start.pose.position.x; + } else { + result->path.poses[0].pose.position.x = 0.0; + } goal_handle->succeed(result); } }; @@ -65,6 +70,9 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); @@ -136,14 +144,16 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); - EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0); + EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct nav_msgs::msg::Path path; config_->blackboard->get("path", path); - EXPECT_EQ(path.poses.size(), 1u); - EXPECT_EQ(path.poses[0].pose.position.x, 1.0); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 0.0); + EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent tree_->rootNode()->halt(); @@ -158,11 +168,80 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) } EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, -2.5); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); + + config_->blackboard->get("path", path); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 0.0); + EXPECT_EQ(path.poses[1].pose.position.x, -2.5); +} + +TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new start and set it on blackboard + geometry_msgs::msg::PoseStamped start; + start.header.stamp = node_->now(); + start.pose.position.x = 2.0; + config_->blackboard->set("start", start); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0); + EXPECT_TRUE(action_server_->getCurrentGoal()->use_start); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); + + // check if returned path is correct + nav_msgs::msg::Path path; + config_->blackboard->get("path", path); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 2.0); + EXPECT_EQ(path.poses[1].pose.position.x, 1.0); + + // halt node so another goal can be sent + tree_->rootNode()->halt(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // set new goal and new start + goal.pose.position.x = -2.5; + start.pose.position.x = -1.5; + config_->blackboard->set("goal", goal); + config_->blackboard->set("start", start); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); config_->blackboard->get("path", path); - EXPECT_EQ(path.poses.size(), 1u); - EXPECT_EQ(path.poses[0].pose.position.x, -2.5); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, -1.5); + EXPECT_EQ(path.poses[1].pose.position.x, -2.5); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp new file mode 100644 index 00000000000..64ce7138887 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "../../test_action_server.hpp" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class ControllerSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("controller_selector_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, config); + }; + + factory_->registerBuilder( + "ControllerSelector", + builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ControllerSelectorTestFixture::node_ = nullptr; + +BT::NodeConfiguration * ControllerSelectorTestFixture::config_ = nullptr; +std::shared_ptr ControllerSelectorTestFixture::factory_ = nullptr; +std::shared_ptr ControllerSelectorTestFixture::tree_ = nullptr; + +TEST_F(ControllerSelectorTestFixture, test_custom_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_controller_result; + config_->blackboard->get("selected_controller", selected_controller_result); + + EXPECT_EQ(selected_controller_result, "DWB"); + + std_msgs::msg::String selected_controller_cmd; + + selected_controller_cmd.data = "DWC"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto controller_selector_pub = + node_->create_publisher("controller_selector_custom_topic_name", qos); + + // publish a few updates of the selected_controller + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + controller_selector_pub->publish(selected_controller_cmd); + + rclcpp::spin_some(node_); + } + + // check controller updated + config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_EQ("DWC", selected_controller_result); +} + +TEST_F(ControllerSelectorTestFixture, test_default_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_controller_result; + config_->blackboard->get("selected_controller", selected_controller_result); + + EXPECT_EQ(selected_controller_result, "GridBased"); + + std_msgs::msg::String selected_controller_cmd; + + selected_controller_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto controller_selector_pub = + node_->create_publisher("controller_selector", qos); + + // publish a few updates of the selected_controller + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + controller_selector_pub->publish(selected_controller_cmd); + + rclcpp::spin_some(node_); + } + + // check controller updated + config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_EQ("RRT", selected_controller_result); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 9c3c67fe4e7..1325070e000 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -63,6 +63,9 @@ class FollowPathActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp new file mode 100644 index 00000000000..cc0b574ff98 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "../../test_action_server.hpp" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class GoalCheckerSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("goal_checker_selector_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, config); + }; + + factory_->registerBuilder( + "GoalCheckerSelector", + builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr GoalCheckerSelectorTestFixture::node_ = nullptr; + +BT::NodeConfiguration * GoalCheckerSelectorTestFixture::config_ = nullptr; +std::shared_ptr GoalCheckerSelectorTestFixture::factory_ = nullptr; +std::shared_ptr GoalCheckerSelectorTestFixture::tree_ = nullptr; + +TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_goal_checker_result; + config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + + EXPECT_EQ(selected_goal_checker_result, "SimpleGoalCheck"); + + std_msgs::msg::String selected_goal_checker_cmd; + + selected_goal_checker_cmd.data = "AngularGoalChecker"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto goal_checker_selector_pub = + node_->create_publisher("goal_checker_selector_custom_topic_name", qos); + + // publish a few updates of the selected_goal_checker + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + goal_checker_selector_pub->publish(selected_goal_checker_cmd); + + rclcpp::spin_some(node_); + } + + // check goal_checker updated + config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_EQ("AngularGoalChecker", selected_goal_checker_result); +} + +TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_goal_checker_result; + config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + + EXPECT_EQ(selected_goal_checker_result, "GridBased"); + + std_msgs::msg::String selected_goal_checker_cmd; + + selected_goal_checker_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto goal_checker_selector_pub = + node_->create_publisher("goal_checker_selector", qos); + + // publish a few updates of the selected_goal_checker + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + goal_checker_selector_pub->publish(selected_goal_checker_cmd); + + rclcpp::spin_some(node_); + } + + // check goal_checker updated + config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_EQ("RRT", selected_goal_checker_result); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp new file mode 100644 index 00000000000..c62e15798e2 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -0,0 +1,174 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/quaternion.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp" + +class NavigateThroughPosesActionServer + : public TestActionServer +{ +public: + NavigateThroughPosesActionServer() + : TestActionServer("navigate_through_poses") + {} + +protected: + void execute( + const typename std::shared_ptr< + rclcpp_action::ServerGoalHandle> goal_handle) + override + { + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + goal_handle->succeed(result); + } +}; + +class NavigateThroughPosesActionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("navigate_through_poses_action_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", + std::chrono::milliseconds(10)); + config_->blackboard->set("initial_pose_received", false); + std::vector poses; + config_->blackboard->set>( + "goals", poses); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "navigate_through_poses", config); + }; + + factory_->registerBuilder( + "NavigateThroughPoses", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + action_server_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + + static std::shared_ptr action_server_; + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr NavigateThroughPosesActionTestFixture::node_ = nullptr; +std::shared_ptr +NavigateThroughPosesActionTestFixture::action_server_ = nullptr; +BT::NodeConfiguration * NavigateThroughPosesActionTestFixture::config_ = nullptr; +std::shared_ptr NavigateThroughPosesActionTestFixture::factory_ = nullptr; +std::shared_ptr NavigateThroughPosesActionTestFixture::tree_ = nullptr; + +TEST_F(NavigateThroughPosesActionTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + std::vector poses; + poses.resize(1); + poses[0].pose.position.x = -2.5; + poses[0].pose.orientation.x = 1.0; + config_->blackboard->set>("goals", poses); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->poses, poses); + + // halt node so another goal can be sent + tree_->rootNode()->halt(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + // initialize action server and spin on new thread + NavigateThroughPosesActionTestFixture::action_server_ = + std::make_shared(); + + std::thread server_thread([]() { + rclcpp::spin(NavigateThroughPosesActionTestFixture::action_server_); + }); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + server_thread.join(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index 7ea3e858f8d..19b96d6f682 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -64,6 +64,9 @@ class NavigateToPoseActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp new file mode 100644 index 00000000000..baf856de64e --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -0,0 +1,185 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "../../test_action_server.hpp" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class PlannerSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("planner_selector_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, config); + }; + + factory_->registerBuilder("PlannerSelector", builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr PlannerSelectorTestFixture::node_ = nullptr; + +BT::NodeConfiguration * PlannerSelectorTestFixture::config_ = nullptr; +std::shared_ptr PlannerSelectorTestFixture::factory_ = nullptr; +std::shared_ptr PlannerSelectorTestFixture::tree_ = nullptr; + +TEST_F(PlannerSelectorTestFixture, test_custom_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_planner_result; + config_->blackboard->get("selected_planner", selected_planner_result); + + EXPECT_EQ(selected_planner_result, "GridBased"); + + std_msgs::msg::String selected_planner_cmd; + + selected_planner_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto planner_selector_pub = + node_->create_publisher("planner_selector_custom_topic_name", qos); + + // publish a few updates of the selected_planner + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + planner_selector_pub->publish(selected_planner_cmd); + + rclcpp::spin_some(node_); + } + + // check planner updated + config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_EQ("RRT", selected_planner_result); +} + +TEST_F(PlannerSelectorTestFixture, test_default_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_planner_result; + config_->blackboard->get("selected_planner", selected_planner_result); + + EXPECT_EQ(selected_planner_result, "GridBased"); + + std_msgs::msg::String selected_planner_cmd; + + selected_planner_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto planner_selector_pub = + node_->create_publisher("planner_selector", qos); + + // publish a few updates of the selected_planner + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + planner_selector_pub->publish(selected_planner_cmd); + + rclcpp::spin_some(node_); + } + + // check planner updated + config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_EQ("RRT", selected_planner_result); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp index 0ce10e229f4..1dbf1a7e6d1 100644 --- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -49,6 +49,9 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp new file mode 100644 index 00000000000..4dc971a2caa --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -0,0 +1,160 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp" +#include "../../test_behavior_tree_fixture.hpp" + +class RemovePassedGoalsTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("passed_goals_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + transform_handler_ = std::make_shared(node_); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + config_->blackboard->set>( + "tf_buffer", + transform_handler_->getBuffer()); + + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, config); + }; + + factory_->registerBuilder( + "RemovePassedGoals", builder); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete config_; + config_ = nullptr; + transform_handler_.reset(); + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; + static std::shared_ptr transform_handler_; +}; + +rclcpp::Node::SharedPtr RemovePassedGoalsTestFixture::node_ = nullptr; + +BT::NodeConfiguration * RemovePassedGoalsTestFixture::config_ = nullptr; +std::shared_ptr RemovePassedGoalsTestFixture::factory_ = nullptr; +std::shared_ptr RemovePassedGoalsTestFixture::tree_ = nullptr; +std::shared_ptr +RemovePassedGoalsTestFixture::transform_handler_ = nullptr; + +TEST_F(RemovePassedGoalsTestFixture, test_tick) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 0.25; + pose.position.y = 0.0; + + transform_handler_->activate(); + transform_handler_->waitForTransform(); + transform_handler_->updateRobotPose(pose); + + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + std::vector poses; + poses.resize(4); + poses[0].pose.position.x = 0.0; + poses[0].pose.position.y = 0.0; + + poses[1].pose.position.x = 0.5; + poses[1].pose.position.y = 0.0; + + poses[2].pose.position.x = 1.0; + poses[2].pose.position.y = 0.0; + + poses[3].pose.position.x = 2.0; + poses[3].pose.position.y = 0.0; + + config_->blackboard->set("goals", poses); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check that it removed the point in range + std::vector output_poses; + config_->blackboard->get("goals", output_poses); + + EXPECT_EQ(output_poses.size(), 2u); + EXPECT_EQ(output_poses[0], poses[2]); + EXPECT_EQ(output_poses[1], poses[3]); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index f3d0b3d913d..332d5149417 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -32,9 +32,19 @@ class SpinActionServer : public TestActionServer protected: void execute( - const typename std::shared_ptr>) + const typename std::shared_ptr> + goal_handle) override - {} + { + nav2_msgs::action::Spin::Result::SharedPtr result = + std::make_shared(); + bool return_success = getReturnSuccess(); + if (return_success) { + goal_handle->succeed(result); + } else { + goal_handle->abort(result); + } + } }; class SpinActionTestFixture : public ::testing::Test @@ -54,6 +64,9 @@ class SpinActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -149,6 +162,33 @@ TEST_F(SpinActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f); } +TEST_F(SpinActionTestFixture, test_failure) +{ + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + action_server_->setReturnSuccess(false); + + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS && + tree_->rootNode()->status() != BT::NodeStatus::FAILURE) + { + tree_->rootNode()->executeTick(); + } + + std::cout << tree_->rootNode()->status(); + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); + EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); + EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index 193395f9401..05cd388d7a6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -55,6 +55,9 @@ class WaitActionTestFixture : public ::testing::Test node_); config_->blackboard->set( "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp index 37dfc32809b..a24dcbf530d 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp @@ -44,6 +44,9 @@ class TransformAvailableConditionTestFixture : public ::testing::Test transform_handler_->getBuffer()); config_->blackboard->set( "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); } diff --git a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt index ad53c5d1d1a..330ff3b2518 100644 --- a/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt @@ -13,3 +13,7 @@ ament_target_dependencies(test_decorator_rate_controller ${dependencies}) ament_add_gtest(test_goal_updater_node test_goal_updater_node.cpp) target_link_libraries(test_goal_updater_node nav2_goal_updater_node_bt_node) ament_target_dependencies(test_goal_updater_node ${dependencies}) + +ament_add_gtest(test_single_trigger_node test_single_trigger_node.cpp) +target_link_libraries(test_single_trigger_node nav2_single_trigger_bt_node) +ament_target_dependencies(test_single_trigger_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp new file mode 100644 index 00000000000..5b341290575 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp @@ -0,0 +1,94 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "../../test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class SingleTriggerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + bt_node_ = std::make_shared( + "single_trigger", *config_); + dummy_node_ = std::make_shared(); + bt_node_->setChild(dummy_node_.get()); + } + + void TearDown() + { + dummy_node_.reset(); + bt_node_.reset(); + } + +protected: + static std::shared_ptr bt_node_; + static std::shared_ptr dummy_node_; +}; + +std::shared_ptr +SingleTriggerTestFixture::bt_node_ = nullptr; +std::shared_ptr +SingleTriggerTestFixture::dummy_node_ = nullptr; + +TEST_F(SingleTriggerTestFixture, test_behavior) +{ + // starting in idle + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); + + // tick once, should work + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + // tick again with dummy node success, should fail + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // tick again with dummy node idle, should still fail + dummy_node_->changeStatus(BT::NodeStatus::IDLE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // halt BT for a new execution run, should work when dummy node is running + // and once when dummy node returns success and then fail + bt_node_->halt(); + dummy_node_->changeStatus(BT::NodeStatus::RUNNING); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index ddfef77c388..7e9d50c9c36 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -37,6 +37,10 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi geometry_msgs::msg::PoseStamped goal; goal.header.stamp = node_->now(); config_->blackboard->set("goal", goal); + + std::vector fake_poses; + config_->blackboard->set>("goals", fake_poses); // NOLINT + bt_node_ = std::make_shared("speed_controller", *config_); dummy_node_ = std::make_shared(); bt_node_->setChild(dummy_node_.get()); diff --git a/nav2_behavior_tree/test/test_action_server.hpp b/nav2_behavior_tree/test/test_action_server.hpp index 2c7891edb6b..5afbb25335b 100644 --- a/nav2_behavior_tree/test/test_action_server.hpp +++ b/nav2_behavior_tree/test/test_action_server.hpp @@ -48,6 +48,16 @@ class TestActionServer : public rclcpp::Node return current_goal_; } + void setReturnSuccess(bool return_success) + { + return_success_ = return_success; + } + + bool getReturnSuccess(void) + { + return return_success_; + } + protected: virtual rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID &, @@ -77,6 +87,7 @@ class TestActionServer : public rclcpp::Node private: typename rclcpp_action::Server::SharedPtr action_server_; std::shared_ptr current_goal_; + bool return_success_ = true; }; #endif // TEST_ACTION_SERVER_HPP_ diff --git a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp index 20c9ace7bf9..2a377f9caad 100644 --- a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp +++ b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp @@ -51,6 +51,9 @@ class BehaviorTreeTestFixture : public ::testing::Test transform_handler_->getBuffer()); config_->blackboard->set( "server_timeout", + std::chrono::milliseconds(20)); + config_->blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); config_->blackboard->set("initial_pose_received", false); diff --git a/nav2_bringup/bringup/README.md b/nav2_bringup/bringup/README.md index a4a1b9a6f66..da9e64eb411 100644 --- a/nav2_bringup/bringup/README.md +++ b/nav2_bringup/bringup/README.md @@ -1,20 +1,20 @@ # nav2_bringup -The `nav2_bringup` package is an example bringup system for Navigation2 applications. +The `nav2_bringup` package is an example bringup system for Nav2 applications. ### Pre-requisites: * [Install ROS 2](https://index.ros.org/doc/ros2/Installation/Dashing/) -* Install Navigation2 +* Install Nav2 ```sudo apt install ros--navigation2``` -* Install Navigation2 Bringup +* Install Nav2 Bringup ```sudo apt install ros--nav2-bringup``` * Install your robot specific package (ex:[Turtlebot 3](http://emanual.robotis.com/docs/en/platform/turtlebot3/ros2/)) -## Launch Navigation2 in *Simulation* with Gazebo +## Launch Nav2 in *Simulation* with Gazebo ### Pre-requisites: * [Install Gazebo](http://gazebosim.org/tutorials?tut=install_ubuntu&cat=install) @@ -43,7 +43,7 @@ export TURTLEBOT3_MODEL=waffle ros2 launch turtlebot3_bringup turtlebot3_state_publisher.launch.py use_sim_time:=True ``` -### Terminal 3: Launch Navigation2 +### Terminal 3: Launch Nav2 ```bash source /opt/ros/dashing/setup.bash @@ -51,7 +51,7 @@ ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True autostart:=True \ map:= ``` -### Terminal 4: Run RViz with Navigation2 config file +### Terminal 4: Run RViz with Nav2 config file ```bash source /opt/ros/dashing/setup.bash @@ -62,7 +62,7 @@ In RViz: * You should see the map * Localize the robot using “2D Pose Estimate” button. * Make sure all transforms from odom are present. (odom->base_link->base_scan) -* Send the robot a goal using “Navigation2 Goal” button. +* Send the robot a goal using "Nav2 Goal” button. Note: this uses a ROS2 Action to send the goal, and a pop-up window will appear on your screen with a 'cancel' button if you wish to cancel To view the robot model in RViz: @@ -70,7 +70,7 @@ To view the robot model in RViz: ### Advanced: single-terminal launch -A convenience file is provided to launch Gazebo, RVIZ and Navigation2 using a single command: +A convenience file is provided to launch Gazebo, RVIZ and Nav2 using a single command: ```bash ros2 launch nav2_bringup tb3_simulation_launch.py @@ -102,7 +102,7 @@ ros2 launch nav2_bringup multi_tb3_simulation_launch.py ``` -## Launch Navigation2 on a *Robot* +## Launch Nav2 on a *Robot* ### Pre-requisites: * Run SLAM with Navigation 2 or tele-op to drive the robot and generate a map of an area for testing first. The directions below assume this has already been done or there is already a map of the area. @@ -111,12 +111,12 @@ ros2 launch nav2_bringup multi_tb3_simulation_launch.py - [Navigation 2 with SLAM](https://github.com/ros-planning/navigation2/blob/main/doc/use_cases/navigation_with_slam.md) -* _Please note that currently, nav2_bringup works if you provide a map file. However, providing a map is not required to use Navigation2. Navigation2 can be configured to use the costmaps to navigate in an area without using a map file_ +* _Please note that currently, nav2_bringup works if you provide a map file. However, providing a map is not required to use Nav2. Nav2 can be configured to use the costmaps to navigate in an area without using a map file_ * Publish all the transforms from your robot from base_link to base_scan -### Terminal 1 : Launch Navigation2 using your map.yaml +### Terminal 1 : Launch Nav2 using your map.yaml ```bash source /opt/ros/dashing/setup.bash diff --git a/nav2_bringup/bringup/launch/bringup_launch.py b/nav2_bringup/bringup/launch/bringup_launch.py index 4e200fcde25..8d357ff5ad3 100644 --- a/nav2_bringup/bringup/launch/bringup_launch.py +++ b/nav2_bringup/bringup/launch/bringup_launch.py @@ -37,7 +37,6 @@ def generate_launch_description(): map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') - default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') autostart = LaunchConfiguration('autostart') stdout_linebuf_envvar = SetEnvironmentVariable( @@ -72,13 +71,6 @@ def generate_launch_description(): 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_bt_xml_cmd = DeclareLaunchArgument( - 'default_bt_xml_filename', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), - description='Full path to the behavior tree xml file to use') - declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') @@ -114,7 +106,6 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'autostart': autostart, 'params_file': params_file, - 'default_bt_xml_filename': default_bt_xml_filename, 'use_lifecycle_mgr': 'false', 'map_subscribe_transient_local': 'true'}.items()), ]) @@ -133,7 +124,6 @@ def generate_launch_description(): ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) - ld.add_action(declare_bt_xml_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py b/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py index 4612223f189..dd13fae8bbd 100644 --- a/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py +++ b/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py @@ -49,7 +49,6 @@ def generate_launch_description(): # On this example all robots are launched with the same settings map_yaml_file = LaunchConfiguration('map') - default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') autostart = LaunchConfiguration('autostart') rviz_config_file = LaunchConfiguration('rviz_config') use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') @@ -82,13 +81,6 @@ def generate_launch_description(): default_value=os.path.join(bringup_dir, 'params', 'nav2_multirobot_params_2.yaml'), description='Full path to the ROS2 parameters file to use for robot2 launched nodes') - declare_bt_xml_cmd = DeclareLaunchArgument( - 'default_bt_xml_filename', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), - description='Full path to the behavior tree xml file to use') - declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='false', description='Automatically startup the stacks') @@ -152,7 +144,6 @@ def generate_launch_description(): 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, - 'default_bt_xml_filename': default_bt_xml_filename, 'autostart': autostart, 'use_rviz': 'False', 'use_simulator': 'False', @@ -168,9 +159,6 @@ def generate_launch_description(): LogInfo( condition=IfCondition(log_settings), msg=[robot['name'], ' params yaml: ', params_file]), - LogInfo( - condition=IfCondition(log_settings), - msg=[robot['name'], ' behavior tree xml: ', default_bt_xml_filename]), LogInfo( condition=IfCondition(log_settings), msg=[robot['name'], ' rviz config file: ', rviz_config_file]), @@ -193,7 +181,6 @@ def generate_launch_description(): ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_robot1_params_file_cmd) ld.add_action(declare_robot2_params_file_cmd) - ld.add_action(declare_bt_xml_cmd) ld.add_action(declare_use_rviz_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_rviz_config_file_cmd) diff --git a/nav2_bringup/bringup/launch/navigation_launch.py b/nav2_bringup/bringup/launch/navigation_launch.py index c421d709731..0e544135657 100644 --- a/nav2_bringup/bringup/launch/navigation_launch.py +++ b/nav2_bringup/bringup/launch/navigation_launch.py @@ -31,8 +31,6 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') autostart = LaunchConfiguration('autostart') params_file = LaunchConfiguration('params_file') - default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') - map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') lifecycle_nodes = ['controller_server', 'planner_server', @@ -52,9 +50,7 @@ def generate_launch_description(): # Create our own temporary YAML files that include substitutions param_substitutions = { 'use_sim_time': use_sim_time, - 'default_bt_xml_filename': default_bt_xml_filename, - 'autostart': autostart, - 'map_subscribe_transient_local': map_subscribe_transient_local} + 'autostart': autostart} configured_params = RewrittenYaml( source_file=params_file, @@ -83,17 +79,6 @@ def generate_launch_description(): default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), description='Full path to the ROS2 parameters file to use'), - DeclareLaunchArgument( - 'default_bt_xml_filename', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), - description='Full path to the behavior tree xml file to use'), - - DeclareLaunchArgument( - 'map_subscribe_transient_local', default_value='false', - description='Whether to set the map subscriber QoS to transient local'), - Node( package='nav2_controller', executable='controller_server', diff --git a/nav2_bringup/bringup/launch/rviz_launch.py b/nav2_bringup/bringup/launch/rviz_launch.py index 3126b113485..9f123b81eb6 100644 --- a/nav2_bringup/bringup/launch/rviz_launch.py +++ b/nav2_bringup/bringup/launch/rviz_launch.py @@ -57,7 +57,6 @@ def generate_launch_description(): condition=UnlessCondition(use_namespace), package='rviz2', executable='rviz2', - name='rviz2', arguments=['-d', rviz_config_file], output='screen') @@ -69,7 +68,6 @@ def generate_launch_description(): condition=IfCondition(use_namespace), package='rviz2', executable='rviz2', - name='rviz2', namespace=namespace, arguments=['-d', namespaced_rviz_config_file], output='screen', diff --git a/nav2_bringup/bringup/launch/slam_launch.py b/nav2_bringup/bringup/launch/slam_launch.py index a32f68094be..f9fd723e4e0 100644 --- a/nav2_bringup/bringup/launch/slam_launch.py +++ b/nav2_bringup/bringup/launch/slam_launch.py @@ -18,10 +18,11 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from nav2_common.launch import RewrittenYaml +from nav2_common.launch import HasNodeParams, RewrittenYaml def generate_launch_description(): @@ -62,33 +63,47 @@ def generate_launch_description(): declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', - default_value='true', + default_value='True', description='Use simulation (Gazebo) clock if true') declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', + 'autostart', default_value='True', description='Automatically startup the nav2 stack') # Nodes launching commands - start_slam_toolbox_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(slam_launch_file), - launch_arguments={'use_sim_time': use_sim_time}.items()) start_map_saver_server_cmd = Node( package='nav2_map_server', - node_executable='map_saver_server', + executable='map_saver_server', output='screen', parameters=[configured_params]) start_lifecycle_manager_cmd = Node( package='nav2_lifecycle_manager', - node_executable='lifecycle_manager', - node_name='lifecycle_manager_slam', + executable='lifecycle_manager', + name='lifecycle_manager_slam', output='screen', parameters=[{'use_sim_time': use_sim_time}, {'autostart': autostart}, {'node_names': lifecycle_nodes}]) + # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' + # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load + # the default file + has_slam_toolbox_params = HasNodeParams(source_file=params_file, + node_name='slam_toolbox') + + start_slam_toolbox_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={'use_sim_time': use_sim_time}.items(), + condition=UnlessCondition(has_slam_toolbox_params)) + + start_slam_toolbox_cmd_with_params = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={'use_sim_time': use_sim_time, + 'slam_params_file': params_file}.items(), + condition=IfCondition(has_slam_toolbox_params)) + ld = LaunchDescription() # Declare the launch options @@ -97,11 +112,12 @@ def generate_launch_description(): ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_autostart_cmd) - # Running SLAM Toolbox - ld.add_action(start_slam_toolbox_cmd) - # Running Map Saver Server ld.add_action(start_map_saver_server_cmd) ld.add_action(start_lifecycle_manager_cmd) + # Running SLAM Toolbox (Only one of them will be run) + ld.add_action(start_slam_toolbox_cmd) + ld.add_action(start_slam_toolbox_cmd_with_params) + return ld diff --git a/nav2_bringup/bringup/launch/tb3_simulation_launch.py b/nav2_bringup/bringup/launch/tb3_simulation_launch.py index ce843a8adbe..5fad6db3f8a 100644 --- a/nav2_bringup/bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/bringup/launch/tb3_simulation_launch.py @@ -38,7 +38,6 @@ def generate_launch_description(): map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') - default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') autostart = LaunchConfiguration('autostart') # Launch configuration variables specific to simulation @@ -76,7 +75,8 @@ def generate_launch_description(): declare_map_yaml_cmd = DeclareLaunchArgument( 'map', - default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + default_value=os.path.join( + bringup_dir, 'maps', 'turtlebot3_world.yaml'), description='Full path to map file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( @@ -89,20 +89,14 @@ def generate_launch_description(): 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_bt_xml_cmd = DeclareLaunchArgument( - 'default_bt_xml_filename', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), - description='Full path to the behavior tree xml file to use') - declare_autostart_cmd = DeclareLaunchArgument( 'autostart', default_value='true', description='Automatically startup the nav2 stack') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', - default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), + default_value=os.path.join( + bringup_dir, 'rviz', 'nav2_default_view.rviz'), description='Full path to the RVIZ config file to use') declare_use_simulator_cmd = DeclareLaunchArgument( @@ -130,7 +124,7 @@ def generate_launch_description(): # TODO(orduno) Switch back once ROS argument passing has been fixed upstream # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/91 # default_value=os.path.join(get_package_share_directory('turtlebot3_gazebo'), - # 'worlds/turtlebot3_worlds/waffle.model'), + # worlds/turtlebot3_worlds/waffle.model') default_value=os.path.join(bringup_dir, 'worlds', 'waffle.model'), description='Full path to world model file to load') @@ -141,7 +135,8 @@ def generate_launch_description(): cwd=[launch_dir], output='screen') start_gazebo_client_cmd = ExecuteProcess( - condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])), + condition=IfCondition(PythonExpression( + [use_simulator, ' and not ', headless])), cmd=['gzclient'], cwd=[launch_dir], output='screen') @@ -159,21 +154,22 @@ def generate_launch_description(): arguments=[urdf]) rviz_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'rviz_launch.py')), condition=IfCondition(use_rviz), launch_arguments={'namespace': '', 'use_namespace': 'False', 'rviz_config': rviz_config_file}.items()) bringup_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={'namespace': namespace, 'use_namespace': use_namespace, 'slam': slam, 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'params_file': params_file, - 'default_bt_xml_filename': default_bt_xml_filename, 'autostart': autostart}.items()) # Create the launch description and populate @@ -186,7 +182,6 @@ def generate_launch_description(): ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) - ld.add_action(declare_bt_xml_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_rviz_config_file_cmd) diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index aa4fa05bb7d..3b8e82e5c92 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,8 +2,8 @@ nav2_bringup - 0.4.3 - Bringup scripts and configurations for the navigation2 stack + 1.0.0 + Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski Carlos Orduno diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index c5ca3429cdc..2c0d414abd9 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -53,9 +53,15 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom - default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node - nav2_follow_path_action_bt_node - nav2_back_up_action_bt_node - nav2_spin_action_bt_node @@ -73,9 +79,18 @@ bt_navigator: - nav2_goal_updater_node_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -170,6 +185,10 @@ local_costmap: max_obstacle_height: 2.0 clearing: True marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: map_subscribe_transient_local: True always_send_full_costmap: True @@ -193,6 +212,10 @@ global_costmap: max_obstacle_height: 2.0 clearing: True marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: map_subscribe_transient_local: True always_send_full_costmap: True @@ -229,7 +252,7 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "back_up", "wait"] + recovery_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 4f3146b1fbb..9f671b246a3 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -53,9 +53,15 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom - default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node - nav2_follow_path_action_bt_node - nav2_back_up_action_bt_node - nav2_spin_action_bt_node @@ -73,9 +79,18 @@ bt_navigator: - nav2_goal_updater_node_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -170,6 +185,10 @@ local_costmap: max_obstacle_height: 2.0 clearing: True marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: map_subscribe_transient_local: True always_send_full_costmap: True @@ -193,6 +212,10 @@ global_costmap: max_obstacle_height: 2.0 clearing: True marking: True + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: map_subscribe_transient_local: True always_send_full_costmap: True @@ -229,7 +252,7 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "back_up", "wait"] + recovery_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index f10edb77292..97508a0173a 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -53,12 +53,18 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 enable_groot_monitoring: True groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 - default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node - nav2_follow_path_action_bt_node - nav2_back_up_action_bt_node - nav2_spin_action_bt_node @@ -80,6 +86,14 @@ bt_navigator: - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -92,8 +106,9 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 progress_checker_plugin: "progress_checker" - goal_checker_plugin: "goal_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["FollowPath"] # Progress checker parameters @@ -102,11 +117,16 @@ controller_server: required_movement_radius: 0.5 movement_time_allowance: 10.0 # Goal checker parameters - goal_checker: + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 - stateful: True # DWB parameters FollowPath: plugin: "dwb_core::DWBLocalPlanner" @@ -149,7 +169,7 @@ controller_server: RotateToGoal.scale: 32.0 RotateToGoal.slowing_factor: 5.0 RotateToGoal.lookahead_time: -1.0 - + controller_server_rclcpp_node: ros__parameters: use_sim_time: True @@ -188,6 +208,10 @@ local_costmap: clearing: True marking: True data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: map_subscribe_transient_local: True always_send_full_costmap: True @@ -220,6 +244,10 @@ global_costmap: clearing: True marking: True data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz index c07e3e0cba6..904e51cb623 100644 --- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/bringup/rviz/nav2_default_view.rviz @@ -214,24 +214,19 @@ Visualization Manager: Use Timestamp: false Value: true - Alpha: 1 - Arrow Length: 0.019999999552965164 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray + Class: nav2_rviz_plugins/ParticleCloud Color: 0; 180; 0 Enabled: true - Head Length: 0.07000000029802322 - Head Radius: 0.029999999329447746 + Max Arrow Length: 0.3 + Min Arrow Length: 0.02 Name: Amcl Particle Swarm - Shaft Length: 0.23000000417232513 - Shaft Radius: 0.009999999776482582 Shape: Arrow (Flat) Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Best Effort - Value: /particlecloud + Value: /particle_cloud Value: true - Class: rviz_common/Group Displays: @@ -298,7 +293,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz_default_plugins/PointCloud + Class: rviz_default_plugins/PointCloud2 Color: 125; 125; 125 Color Transformer: FlatColor Decay Time: 0 @@ -412,7 +407,7 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensity - Class: rviz_default_plugins/PointCloud + Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 diff --git a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz index 2a024f75b94..4897dda6e4f 100644 --- a/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz +++ b/nav2_bringup/bringup/rviz/nav2_namespaced_view.rviz @@ -164,24 +164,19 @@ Visualization Manager: Use Timestamp: false Value: true - Alpha: 1 - Arrow Length: 0.019999999552965164 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray + Class: nav2_rviz_plugins/ParticleCloud Color: 0; 180; 0 Enabled: true - Head Length: 0.07000000029802322 - Head Radius: 0.029999999329447746 + Max Arrow Length: 0.3 + Min Arrow Length: 0.02 Name: Amcl Particle Swarm - Shaft Length: 0.23000000417232513 - Shaft Radius: 0.009999999776482582 Shape: Arrow (Flat) Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Best Effort - Value: /particlecloud + Value: /particle_cloud Value: true - Class: rviz_common/Group Displays: diff --git a/nav2_bringup/bringup/worlds/waffle.model b/nav2_bringup/bringup/worlds/waffle.model index 74af1c7670e..f8a9fa09ad3 100644 --- a/nav2_bringup/bringup/worlds/waffle.model +++ b/nav2_bringup/bringup/worlds/waffle.model @@ -137,6 +137,7 @@ + false ~/out:=imu @@ -266,7 +267,7 @@ 0.0 0.144 0.023 0 0 0 - model://turtlebot3_waffle/meshes/left_tire.dae + model://turtlebot3_waffle/meshes/tire.dae 0.001 0.001 0.001 @@ -324,7 +325,7 @@ 0.0 -0.144 0.023 0 0 0 - model://turtlebot3_waffle/meshes/right_tire.dae + model://turtlebot3_waffle/meshes/tire.dae 0.001 0.001 0.001 diff --git a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py index 05cc3c6dbc2..9f4daaf196c 100644 --- a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py +++ b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py @@ -25,7 +25,7 @@ def main(): # Get input arguments from user - parser = argparse.ArgumentParser(description='Spawn Robot into Gazebo with navigation2') + parser = argparse.ArgumentParser(description='Spawn Robot into Gazebo with Nav2') parser.add_argument('-n', '--robot_name', type=str, default='robot', help='Name of the robot to spawn') parser.add_argument('-ns', '--robot_namespace', type=str, default='robot', @@ -84,9 +84,10 @@ def main(): # broadcasting a transform between`odom` and `base_footprint` break - ros_params = plugin.find('ros') - ros_tf_remap = ET.SubElement(ros_params, 'remapping') - ros_tf_remap.text = '/tf:=/' + args.robot_namespace + '/tf' + if args.robot_namespace: + ros_params = plugin.find('ros') + ros_tf_remap = ET.SubElement(ros_params, 'remapping') + ros_tf_remap.text = '/tf:=/' + args.robot_namespace + '/tf' # Set data for request request = SpawnEntity.Request() @@ -99,7 +100,7 @@ def main(): node.get_logger().info('Sending service request to `/spawn_entity`') future = client.call_async(request) - rclpy.spin_until_future_complete(node, future, args.timeout) + rclpy.spin_until_future_complete(node, future, timeout_sec=args.timeout) if future.result() is not None: print('response: %r' % future.result()) else: diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index 58ca2100273..6f2d88b0c50 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,8 +2,8 @@ nav2_gazebo_spawner - 0.4.3 - Package for spawning a robot model into Gazebo for navigation2 + 1.0.0 + Package for spawning a robot model into Gazebo for Nav2 lkumarbe lkumarbe Apache-2.0 diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index da362c91068..df14b1e15a0 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(nav2_msgs REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) find_package(std_srvs REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav2_core REQUIRED) find_package(tf2_ros REQUIRED) nav2_package() @@ -30,11 +31,6 @@ add_executable(${executable_name} set(library_name ${executable_name}_core) -add_library(${library_name} SHARED - src/bt_navigator.cpp - src/ros_topic_logger.cpp -) - set(dependencies rclcpp rclcpp_action @@ -47,9 +43,16 @@ set(dependencies behaviortree_cpp_v3 std_srvs nav2_util + nav2_core tf2_ros ) +add_library(${library_name} SHARED + src/bt_navigator.cpp + src/navigators/navigate_to_pose.cpp + src/navigators/navigate_through_poses.cpp +) + ament_target_dependencies(${executable_name} ${dependencies} ) @@ -82,5 +85,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) - +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index f0864df97ad..112555570d8 100644 --- a/nav2_bt_navigator/README.md +++ b/nav2_bt_navigator/README.md @@ -1,6 +1,8 @@ # BT Navigator -The BT Navigator (Behavior Tree Navigator) module implements the [NavigateToPose task interface](../nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp). It is a [Behavior Tree](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/docs/BT_basics.md)-based implementation of navigation that is intended to allow for flexibility in the navigation task and provide a way to easily specify complex robot behaviors, including [recovery](#recovery). +The BT Navigator (Behavior Tree Navigator) module implements the [NavigateToPose task interface](../nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp). It is a [Behavior Tree](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/docs/BT_basics.md)-based implementation of navigation that is intended to allow for flexibility in the navigation task and provide a way to easily specify complex robot behaviors. + +NOTE: This readme may be outdated, please reference navigation.ros.org for the most current information. ## Overview @@ -20,13 +22,13 @@ Using the XML filename as a parameter makes it easy to change or extend the logi ## Behavior Tree nodes -A Behavior Tree consists of control flow nodes, such as fallback, sequence, parallel, and decorator, as well as two execution nodes: condition and action nodes. Execution nodes are the leaf nodes of the tree. When a leaf node is ticked, the node does some work and it returns either SUCCESS, FAILURE or RUNNING. The current Navigation2 software implements a few custom nodes, including Conditions and Actions. The user can also define and register additional node types that can then be used in BTs and the corresponding XML descriptions. +A Behavior Tree consists of control flow nodes, such as fallback, sequence, parallel, and decorator, as well as two execution nodes: condition and action nodes. Execution nodes are the leaf nodes of the tree. When a leaf node is ticked, the node does some work and it returns either SUCCESS, FAILURE or RUNNING. The current Nav2 software implements a few custom nodes, including Conditions and Actions. The user can also define and register additional node types that can then be used in BTs and the corresponding XML descriptions. ## Navigation Behavior Trees The BT Navigator package has four sample XML-based descriptions of BTs. These trees are [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml), [navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml), [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml) and -[followpoint.xml](behavior_trees/followpoint.xml). +[follow_point.xml](behavior_trees/follow_point.xml). The user may use any of these sample trees or develop a more complex tree which could better suit the user's needs. ### Navigate with Replanning (time-based) @@ -59,7 +61,6 @@ returns FAILURE, all nodes are halted and this node returns FAILURE.


- #### Decorator Nodes * RateController: A custom control flow node, which throttles down the tick rate. This custom node has only one child and its tick rate is defined with a pre-defined frequency that the user can set. This node returns RUNNING when it is not ticking its child. Currently, in the navigation, the `RateController` is used to tick the `ComputePathToPose` and `GoalReached` node at 1 Hz. @@ -69,7 +70,6 @@ returns FAILURE, all nodes are halted and this node returns FAILURE. * GoalUpdater: A custom control node, which updates the goal pose. It subscribes to a topic in which it can receive an updated goal pose to use instead of the one commanded in action. It is useful for dynamic object following tasks. - #### Condition Nodes * GoalReached: Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, which in that case the `ComputePathToPose` action node will not get ticked. @@ -90,7 +90,16 @@ The navigate with replanning BT first ticks the `RateController` node which spec ### Navigate with replanning and simple recovery actions -With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are: `clearEntirelyCostmapServiceRequest` for both global and local costmaps, `spin` and `wait`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below. +With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are: `clearEntirelyCostmapServiceRequest` for both global and local costmaps, `spin`, `wait`, and `backup`. + +This behavior tree implements multi-scope recovery where both the global planner and the controller have their own recovery actions on failure in addition to the main recovery subtree that gets triggered if the planner or controller continues to fail. The planner/controller specific recovery subtrees contain only a simple costmap clearing action in this BT. The main recovery subtree triggers recovery actions in a round robin fashion where the navigation subtree is retried after trying one recovery action. If the navigation subtree fails even after a retry, the `RoundRobin` node triggers the next recovery action in the sequence and the navigation subtree is retried. This cycle continues until: +- The navigation subtree succeeds +- All recovery actions fail +- Specified number of retries is exceeded + +All recovery actions are preemptable and are halted when a new navigation goal arrives. + +A graphical version of this simple recoverable Behavior Tree is depicted in the figure below.

@@ -114,17 +123,6 @@ This way, the recovery actions can be interrupted if a new goal is sent to the b #### Multi-Scope Recoveries Scope-based failure handling: Utilizing Behavior Trees with a recovery node allows one to handle failures at multiple scopes. With this capability, any action in a large system can be constructed with specific recovery actions suitable for that action. Thus, failures in these actions can be handled locally within the scope. With such design, a system can be recovered at multiple levels based on the nature of the failure. Higher level recovery actions could be recovery actions such as re-initializing the system, re-calibrating the robot, bringing the system to a good known state, etc. -### Navigate with replanning and simple Multi-Scope Recoveries -In the navigation stack, multi-scope recovery actions are implemented for each module. Currently, the recovery actions for the Global planner are: Clear Entire Global Costmap and Wait. The recovery actions for the Local planner are: Clear Entire Local Costmap and Spin; the recovery actions for the system level is just Wait. The figure below highlights a simple multi-scope recovery handling for the navigation task. With this tree, if the Global Planner fails, the ClearEntireCostmap which is the first recovery action for this module will be ticked, then the ComputePathToPose will be ticked again. If the ComputePathToPose fails again, the Wait which is the second recovery action for this module will be ticked. After trying the second recovery action, the ComputePathToPose will be ticked again. These actions can be repeated n times until ComputePathToPose succeeds. If the ComputePathToPose fails and the Global Planner cannot be recovered locally, the higher-level recovery actions will be ticked. In this simple example, our higher-level recovery action is just a longer wait. The same strategy is applied to the Local Planner. If the Local Planner fails, the tree will first tick the ClearEntireCostmap and then if it fails again the tree will tick the Spin. - -
- -

- -

- -This tree currently is not our default tree in the stack. The xml file is located here: [navigate_w_replanning_and_round_robin_recovery.xml](behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml). - ### Navigate following a dynamic point to a certain distance This tree is an example of how behavior trees can be used to make the robot do more than navigate the current position to the desired pose. In this case, the robot will follow a point that changes dynamically during the execution of the action. The robot will stop its advance and will be oriented towards the target position when it reaches a distance to the target established in the tree. The UpdateGoal decorator node will be used to update the target position. The TruncatePath node will modify the generated path by removing the end part of this path, to maintain a distance from the target, and changes the orientation of the last position. This tree never returns that the action has finished successfully, but must be canceled when you want to stop following the target position. @@ -132,12 +130,6 @@ This tree is an example of how behavior trees can be used to make the robot do m This tree currently is not our default tree in the stack. The xml file is located here: [follow_point.xml](behavior_trees/follow_point.xml). -
- -## Open Issues - -* **Schema definition and XML document validation** - Currently, there is no dynamic validation of incoming XML. The Behavior-Tree.CPP library is using tinyxml2, which doesn't have a validator. Instead, we can create a schema for the Mission Planning-level XML and use build-time validation of the XML input to ensure that it is well-formed and valid. - ## Legend Legend for the behavior tree diagrams: diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml new file mode 100644 index 00000000000..6bb780bd57c --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml new file mode 100644 index 00000000000..e211b96539b --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml deleted file mode 100644 index 9b1311a3914..00000000000 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml deleted file mode 100644 index 746ca53d8dd..00000000000 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index 8bc4b7f05ae..049e78794b8 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -1,5 +1,5 @@ diff --git a/nav2_bt_navigator/doc/AutoLocalization_w_recovery_parallel.png b/nav2_bt_navigator/doc/AutoLocalization_w_recovery_parallel.png deleted file mode 100644 index ef50b05451c..00000000000 Binary files a/nav2_bt_navigator/doc/AutoLocalization_w_recovery_parallel.png and /dev/null differ diff --git a/nav2_bt_navigator/doc/auto_localization.png b/nav2_bt_navigator/doc/auto_localization.png deleted file mode 100644 index 7bcb6ab7b58..00000000000 Binary files a/nav2_bt_navigator/doc/auto_localization.png and /dev/null differ diff --git a/nav2_bt_navigator/doc/parallel_w_recovery.png b/nav2_bt_navigator/doc/parallel_w_recovery.png index ac1022a5991..e67f3f77a53 100644 Binary files a/nav2_bt_navigator/doc/parallel_w_recovery.png and b/nav2_bt_navigator/doc/parallel_w_recovery.png differ diff --git a/nav2_bt_navigator/doc/parallel_w_round_robin_recovery.png b/nav2_bt_navigator/doc/parallel_w_round_robin_recovery.png deleted file mode 100644 index c0f67c7d5e5..00000000000 Binary files a/nav2_bt_navigator/doc/parallel_w_round_robin_recovery.png and /dev/null differ diff --git a/nav2_bt_navigator/doc/proposed_recovery.png b/nav2_bt_navigator/doc/proposed_recovery.png deleted file mode 100644 index 775a71bc25d..00000000000 Binary files a/nav2_bt_navigator/doc/proposed_recovery.png and /dev/null differ diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 9023f829af8..e7a61164d6e 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -19,19 +19,17 @@ #include #include -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_behavior_tree/behavior_tree_engine.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "nav2_msgs/action/navigate_to_pose.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/create_timer_ros.h" -#include "tf2_ros/buffer.h" +#include "nav2_bt_navigator/navigators/navigate_to_pose.hpp" +#include "nav2_bt_navigator/navigators/navigate_through_poses.hpp" namespace nav2_bt_navigator { + /** * @class nav2_bt_navigator::BtNavigator * @brief An action server that uses behavior tree for navigating a robot to its @@ -84,68 +82,23 @@ class BtNavigator : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - using Action = nav2_msgs::action::NavigateToPose; - - using ActionServer = nav2_util::SimpleActionServer; - - // Our action server implements the NavigateToPose action - std::unique_ptr action_server_; - - /** - * @brief Action server callbacks - */ - void navigateToPose(); - - /** - * @brief Goal pose initialization on the blackboard - */ - void initializeGoalPose(); - - /** - * @brief A subscription and callback to handle the topic-based goal published - * from rviz - */ - void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose); - rclcpp::Subscription::SharedPtr goal_sub_; + // To handle all the BT related execution + std::unique_ptr> pose_navigator_; + std::unique_ptr> + poses_navigator_; + nav2_bt_navigator::NavigatorMuxer plugin_muxer_; - /** - * @brief Replace current BT with another one - * @param bt_xml_filename The file containing the new BT - * @return true if the resulting BT correspond to the one in bt_xml_filename. false - * if something went wrong, and previous BT is mantained - */ - bool loadBehaviorTree(const std::string & bt_id); - - BT::Tree tree_; - - // The blackboard shared by all of the nodes in the tree - BT::Blackboard::Ptr blackboard_; - - // The XML fiñe that cointains the Behavior Tree to create - std::string current_bt_xml_filename_; - std::string default_bt_xml_filename_; - - // The wrapper class for the BT functionality - std::unique_ptr bt_; - - // Libraries to pull plugins (BT Nodes) from - std::vector plugin_lib_names_; - - // A client that we'll use to send a command message to our own task server - rclcpp_action::Client::SharedPtr self_client_; - - // A regular, non-spinning ROS node that we can use for calls to the action client - rclcpp::Node::SharedPtr client_node_; - - // Spinning transform that can be used by the BT nodes - std::shared_ptr tf_; - std::shared_ptr tf_listener_; + // Odometry smoother object + std::unique_ptr odom_smoother_; // Metrics for feedback - rclcpp::Time start_time_; std::string robot_frame_; std::string global_frame_; double transform_tolerance_; + + // Spinning transform that can be used by the BT nodes + std::shared_ptr tf_; + std::shared_ptr tf_listener_; }; } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp new file mode 100644 index 00000000000..68abf0830fc --- /dev/null +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp @@ -0,0 +1,313 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_ +#define NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_ + +#include +#include +#include +#include + +#include "tf2_ros/buffer.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "pluginlib/class_loader.hpp" +#include "nav2_behavior_tree/bt_action_server.hpp" + +namespace nav2_bt_navigator +{ + +/** + * @struct FeedbackUtils + * @brief Navigator feedback utilities required to get transforms and reference frames. + */ +struct FeedbackUtils +{ + std::string robot_frame; + std::string global_frame; + double transform_tolerance; + std::shared_ptr tf; +}; + +/** + * @class NavigatorMuxer + * @brief A class to control the state of the BT navigator by allowing only a single + * plugin to be processed at a time. + */ +class NavigatorMuxer +{ +public: + /** + * @brief A Navigator Muxer constructor + */ + NavigatorMuxer() + : current_navigator_(std::string("")) {} + + /** + * @brief Get the navigator muxer state + * @return bool If a navigator is in progress + */ + bool isNavigating() + { + std::scoped_lock l(mutex_); + return !current_navigator_.empty(); + } + + /** + * @brief Start navigating with a given navigator + * @param string Name of the navigator to start + */ + void startNavigating(const std::string & navigator_name) + { + std::scoped_lock l(mutex_); + if (!current_navigator_.empty()) { + RCLCPP_ERROR( + rclcpp::get_logger("NavigatorMutex"), + "Major error! Navigation requested while another navigation" + " task is in progress! This likely occurred from an incorrect" + "implementation of a navigator plugin."); + } + current_navigator_ = navigator_name; + } + + /** + * @brief Stop navigating with a given navigator + * @param string Name of the navigator ending task + */ + void stopNavigating(const std::string & navigator_name) + { + std::scoped_lock l(mutex_); + if (current_navigator_ != navigator_name) { + RCLCPP_ERROR( + rclcpp::get_logger("NavigatorMutex"), + "Major error! Navigation stopped while another navigation" + " task is in progress! This likely occurred from an incorrect" + "implementation of a navigator plugin."); + } else { + current_navigator_ = std::string(""); + } + } + +protected: + std::string current_navigator_; + std::mutex mutex_; +}; + +/** + * @class Navigator + * @brief Navigator interface that acts as a base class for all BT-based Navigator action's plugins + */ +template +class Navigator +{ +public: + using Ptr = std::shared_ptr>; + + /** + * @brief A Navigator constructor + */ + Navigator() + { + plugin_muxer_ = nullptr; + } + + /** + * @brief Virtual destructor + */ + virtual ~Navigator() = default; + + /** + * @brief Configuration to setup the navigator's backend BT and actions + * @param parent_node The ROS parent node to utilize + * @param plugin_lib_names a vector of plugin shared libraries to load + * @param feedback_utils Some utilities useful for navigators to have + * @param plugin_muxer The muxing object to ensure only one navigator + * can be active at a time + * @return bool If successful + */ + bool on_configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node, + const std::vector & plugin_lib_names, + const FeedbackUtils & feedback_utils, + nav2_bt_navigator::NavigatorMuxer * plugin_muxer) + { + auto node = parent_node.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); + feedback_utils_ = feedback_utils; + plugin_muxer_ = plugin_muxer; + + // get the default behavior tree for this navigator + std::string default_bt_xml_filename = getDefaultBTFilepath(parent_node); + + // Create the Behavior Tree Action Server for this navigator + bt_action_server_ = std::make_unique>( + node, + getName(), + plugin_lib_names, + default_bt_xml_filename, + std::bind(&Navigator::onGoalReceived, this, std::placeholders::_1), + std::bind(&Navigator::onLoop, this), + std::bind(&Navigator::onPreempt, this, std::placeholders::_1), + std::bind(&Navigator::onCompletion, this, std::placeholders::_1)); + + bool ok = true; + if (!bt_action_server_->on_configure()) { + ok = false; + } + + BT::Blackboard::Ptr blackboard = bt_action_server_->getBlackboard(); + blackboard->set>("tf_buffer", feedback_utils.tf); // NOLINT + blackboard->set("initial_pose_received", false); // NOLINT + blackboard->set("number_recoveries", 0); // NOLINT + + return configure(parent_node) && ok; + } + + /** + * @brief Actiation of the navigator's backend BT and actions + * @return bool If successful + */ + bool on_activate() + { + bool ok = true; + + if (!bt_action_server_->on_activate()) { + ok = false; + } + + return activate() && ok; + } + + /** + * @brief Dectiation of the navigator's backend BT and actions + * @return bool If successful + */ + bool on_deactivate() + { + bool ok = true; + if (!bt_action_server_->on_deactivate()) { + ok = false; + } + + return deactivate() && ok; + } + + /** + * @brief Cleanup a navigator + * @return bool If successful + */ + bool on_cleanup() + { + bool ok = true; + if (!bt_action_server_->on_cleanup()) { + ok = false; + } + + bt_action_server_.reset(); + + return cleanup() && ok; + } + + /** + * @brief Get the action name of this navigator to expose + * @return string Name of action to expose + */ + virtual std::string getName() = 0; + + virtual std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) = 0; + +protected: + /** + * @brief An intermediate goal reception function to mux navigators. + */ + bool onGoalReceived(typename ActionT::Goal::ConstSharedPtr goal) + { + if (plugin_muxer_->isNavigating()) { + RCLCPP_ERROR( + logger_, + "Requested navigation from %s while another navigator is processing," + " rejecting request.", getName().c_str()); + return false; + } + + plugin_muxer_->startNavigating(getName()); + + return goalReceived(goal); + } + + /** + * @brief An intermediate compution function to mux navigators + */ + void onCompletion(typename ActionT::Result::SharedPtr result) + { + plugin_muxer_->stopNavigating(getName()); + goalCompleted(result); + } + + /** + * @brief A callback to be called when a new goal is received by the BT action server + * Can be used to check if goal is valid and put values on + * the blackboard which depend on the received goal + */ + virtual bool goalReceived(typename ActionT::Goal::ConstSharedPtr goal) = 0; + + /** + * @brief A callback that defines execution that happens on one iteration through the BT + * Can be used to publish action feedback + */ + virtual void onLoop() = 0; + + /** + * @brief A callback that is called when a preempt is requested + */ + virtual void onPreempt(typename ActionT::Goal::ConstSharedPtr goal) = 0; + + /** + * @brief A callback that is called when a the action is completed, can fill in + * action result message or indicate that this action is done. + */ + virtual void goalCompleted(typename ActionT::Result::SharedPtr result) = 0; + + /** + * @param Method to configure resources. + */ + virtual bool configure(rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/) {return true;} + + /** + * @brief Method to cleanup resources. + */ + virtual bool cleanup() {return true;} + + /** + * @brief Method to active and any threads involved in execution. + */ + virtual bool activate() {return true;} + + /** + * @brief Method to deactive and any threads involved in execution. + */ + virtual bool deactivate() {return true;} + + std::unique_ptr> bt_action_server_; + rclcpp::Logger logger_{rclcpp::get_logger("Navigator")}; + rclcpp::Clock::SharedPtr clock_; + FeedbackUtils feedback_utils_; + NavigatorMuxer * plugin_muxer_; +}; + +} // namespace nav2_bt_navigator + +#endif // NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_ diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp new file mode 100644 index 00000000000..d86736bad1e --- /dev/null +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -0,0 +1,114 @@ +// Copyright (c) 2021 Samsung Research +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_ +#define NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_ + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_bt_navigator/navigator.hpp" +#include "nav2_msgs/action/navigate_through_poses.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/odometry_utils.hpp" + +namespace nav2_bt_navigator +{ + +/** + * @class NavigateToPoseNavigator + * @brief A navigator for navigating to a a bunch of intermediary poses + */ +class NavigateThroughPosesNavigator + : public nav2_bt_navigator::Navigator +{ +public: + using ActionT = nav2_msgs::action::NavigateThroughPoses; + typedef std::vector Goals; + + /** + * @brief A constructor for NavigateThroughPosesNavigator + */ + NavigateThroughPosesNavigator() + : Navigator() {} + + /** + * @brief A configure state transition to configure navigator's state + * @param node Weakptr to the lifecycle node + */ + bool configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; + + /** + * @brief Get action name for this navigator + * @return string Name of action server + */ + std::string getName() override {return std::string("navigate_through_poses");} + + /** + * @brief Get navigator's default BT + * @param node WeakPtr to the lifecycle node + * @return string Filepath to default XML + */ + std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; + +protected: + /** + * @brief A callback to be called when a new goal is received by the BT action server + * Can be used to check if goal is valid and put values on + * the blackboard which depend on the received goal + * @param goal Action template's goal message + * @return bool if goal was received successfully to be processed + */ + bool goalReceived(ActionT::Goal::ConstSharedPtr goal) override; + + /** + * @brief A callback that defines execution that happens on one iteration through the BT + * Can be used to publish action feedback + */ + void onLoop() override; + + /** + * @brief A callback that is called when a preempt is requested + */ + void onPreempt(ActionT::Goal::ConstSharedPtr goal) override; + + /** + * @brief A callback that is called when a the action is completed, can fill in + * action result message or indicate that this action is done. + * @param result Action template result message to populate + */ + void goalCompleted(typename ActionT::Result::SharedPtr result) override; + + /** + * @brief Goal pose initialization on the blackboard + */ + void initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal); + + rclcpp::Time start_time_; + std::string goals_blackboard_id_; + std::string path_blackboard_id_; + + // Odometry smoother object + std::unique_ptr odom_smoother_; +}; + +} // namespace nav2_bt_navigator + +#endif // NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_ diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp new file mode 100644 index 00000000000..9773ef65c3d --- /dev/null +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -0,0 +1,130 @@ +// Copyright (c) 2021 Samsung Research +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_ +#define NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_ + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_bt_navigator/navigator.hpp" +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/odometry_utils.hpp" + +namespace nav2_bt_navigator +{ + +/** + * @class NavigateToPoseNavigator + * @brief A navigator for navigating to a specified pose + */ +class NavigateToPoseNavigator + : public nav2_bt_navigator::Navigator +{ +public: + using ActionT = nav2_msgs::action::NavigateToPose; + + /** + * @brief A constructor for NavigateToPoseNavigator + */ + NavigateToPoseNavigator() + : Navigator() {} + + /** + * @brief A configure state transition to configure navigator's state + * @param node Weakptr to the lifecycle node + */ + bool configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; + + /** + * @brief A cleanup state transition to remove memory allocated + */ + bool cleanup() override; + + /** + * @brief A subscription and callback to handle the topic-based goal published + * from rviz + * @param pose Pose received via atopic + */ + void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose); + + /** + * @brief Get action name for this navigator + * @return string Name of action server + */ + std::string getName() {return std::string("navigate_to_pose");} + + /** + * @brief Get navigator's default BT + * @param node WeakPtr to the lifecycle node + * @return string Filepath to default XML + */ + std::string getDefaultBTFilepath(rclcpp_lifecycle::LifecycleNode::WeakPtr node) override; + +protected: + /** + * @brief A callback to be called when a new goal is received by the BT action server + * Can be used to check if goal is valid and put values on + * the blackboard which depend on the received goal + * @param goal Action template's goal message + * @return bool if goal was received successfully to be processed + */ + bool goalReceived(ActionT::Goal::ConstSharedPtr goal) override; + + /** + * @brief A callback that defines execution that happens on one iteration through the BT + * Can be used to publish action feedback + */ + void onLoop() override; + + /** + * @brief A callback that is called when a preempt is requested + */ + void onPreempt(ActionT::Goal::ConstSharedPtr goal) override; + + /** + * @brief A callback that is called when a the action is completed, can fill in + * action result message or indicate that this action is done. + * @param result Action template result message to populate + */ + void goalCompleted(typename ActionT::Result::SharedPtr result) override; + + /** + * @brief Goal pose initialization on the blackboard + * @param goal Action template's goal message to process + */ + void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal); + + rclcpp::Time start_time_; + + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp_action::Client::SharedPtr self_client_; + + std::string goal_blackboard_id_; + std::string path_blackboard_id_; + + // Odometry smoother object + std::unique_ptr odom_smoother_; +}; + +} // namespace nav2_bt_navigator + +#endif // NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_ diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp deleted file mode 100644 index 27fe61eebce..00000000000 --- a/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2019 Intel Corporation -// -// 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_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_ -#define NAV2_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_ - -#include -#include "behaviortree_cpp_v3/loggers/abstract_logger.h" -#include "rclcpp/rclcpp.hpp" -#include "nav2_msgs/msg/behavior_tree_log.hpp" -#include "nav2_msgs/msg/behavior_tree_status_change.h" - -namespace nav2_bt_navigator -{ - -class RosTopicLogger : public BT::StatusChangeLogger -{ -public: - RosTopicLogger(const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree); - - void callback( - BT::Duration timestamp, - const BT::TreeNode & node, - BT::NodeStatus prev_status, - BT::NodeStatus status) override; - - void flush() override; - -protected: - rclcpp::Clock::SharedPtr clock_; - rclcpp::Logger logger_{rclcpp::get_logger("bt_navigator")}; - rclcpp::Publisher::SharedPtr log_pub_; - std::vector event_log_; -}; - -} // namespace nav2_bt_navigator - -#endif // NAV2_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_ diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 443dfc2f230..7cfb160c01a 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.3 + 1.0.0 TODO Michael Jeronimo Apache-2.0 @@ -22,6 +22,7 @@ geometry_msgs std_srvs nav2_util + nav2_core behaviortree_cpp_v3 rclcpp @@ -33,6 +34,7 @@ std_msgs nav2_util geometry_msgs + nav2_core ament_lint_common ament_lint_auto diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 9d224b0d5a5..086e09189b4 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -14,30 +14,28 @@ #include "nav2_bt_navigator/bt_navigator.hpp" -#include #include #include #include -#include #include -#include +#include +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_behavior_tree/bt_conversions.hpp" -#include "nav2_bt_navigator/ros_topic_logger.hpp" namespace nav2_bt_navigator { BtNavigator::BtNavigator() -: nav2_util::LifecycleNode("bt_navigator", "", false), - start_time_(0) +: nav2_util::LifecycleNode("bt_navigator", "", false) { RCLCPP_INFO(get_logger(), "Creating"); const std::vector plugin_libs = { "nav2_compute_path_to_pose_action_bt_node", + "nav2_compute_path_through_poses_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", @@ -58,19 +56,22 @@ BtNavigator::BtNavigator() "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node", "nav2_time_expired_condition_bt_node", - "nav2_distance_traveled_condition_bt_node" + "nav2_distance_traveled_condition_bt_node", + "nav2_single_trigger_bt_node", + "nav2_is_battery_low_condition_bt_node", + "nav2_navigate_through_poses_action_bt_node", + "nav2_navigate_to_pose_action_bt_node", + "nav2_remove_passed_goals_action_bt_node" + "nav2_planner_selector_bt_node", + "nav2_controller_selector_bt_node", + "nav2_goal_checker_selector_bt_node" }; - // Declare this node's parameters - declare_parameter("default_bt_xml_filename"); declare_parameter("plugin_lib_names", plugin_libs); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter("global_frame", std::string("map")); declare_parameter("robot_base_frame", std::string("base_link")); declare_parameter("odom_topic", std::string("odom")); - declare_parameter("enable_groot_monitoring", true); - declare_parameter("groot_zmq_publisher_port", 1666); - declare_parameter("groot_zmq_server_port", 1667); } BtNavigator::~BtNavigator() @@ -82,17 +83,6 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - // use suffix '_rclcpp_node' to keep parameter file consistency #1773 - auto options = rclcpp::NodeOptions().arguments( - {"--ros-args", - "-r", std::string("__node:=") + get_name() + "_rclcpp_node", - "--"}); - // Support for handling the topic-based goal pose from rviz - client_node_ = std::make_shared("_", options); - - self_client_ = rclcpp_action::create_client( - client_node_, "navigate_to_pose"); - tf_ = std::make_shared(get_clock()); auto timer_interface = std::make_shared( get_node_base_interface(), get_node_timers_interface()); @@ -100,83 +90,38 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_->setUsingDedicatedThread(true); tf_listener_ = std::make_shared(*tf_, this, false); - goal_sub_ = create_subscription( - "goal_pose", - rclcpp::SystemDefaultsQoS(), - std::bind(&BtNavigator::onGoalPoseReceived, this, std::placeholders::_1)); - - action_server_ = std::make_unique( - get_node_base_interface(), - get_node_clock_interface(), - get_node_logging_interface(), - get_node_waitables_interface(), - "navigate_to_pose", std::bind(&BtNavigator::navigateToPose, this)); - - // Get the libraries to pull plugins from - plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array(); global_frame_ = get_parameter("global_frame").as_string(); robot_frame_ = get_parameter("robot_base_frame").as_string(); transform_tolerance_ = get_parameter("transform_tolerance").as_double(); - // Create the class that registers our custom nodes and executes the BT - bt_ = std::make_unique(plugin_lib_names_); + // Libraries to pull plugins (BT Nodes) from + auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array(); - // Create the blackboard that will be shared by all of the nodes in the tree - blackboard_ = BT::Blackboard::create(); + pose_navigator_ = std::make_unique(); + poses_navigator_ = std::make_unique(); - // Put items on the blackboard - blackboard_->set("node", client_node_); // NOLINT - blackboard_->set>("tf_buffer", tf_); // NOLINT - blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT - blackboard_->set("initial_pose_received", false); // NOLINT - blackboard_->set("number_recoveries", 0); // NOLINT + nav2_bt_navigator::FeedbackUtils feedback_utils; + feedback_utils.tf = tf_; + feedback_utils.global_frame = global_frame_; + feedback_utils.robot_frame = robot_frame_; + feedback_utils.transform_tolerance = transform_tolerance_; - // Get the BT filename to use from the node parameter - get_parameter("default_bt_xml_filename", default_bt_xml_filename_); - - return nav2_util::CallbackReturn::SUCCESS; -} - -bool -BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename) -{ - // Use previous BT if it is the existing one - if (current_bt_xml_filename_ == bt_xml_filename) { - RCLCPP_DEBUG(get_logger(), "BT will not be reloaded as the given xml is already loaded"); - return true; + if (!pose_navigator_->on_configure( + shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_)) + { + return nav2_util::CallbackReturn::FAILURE; } - // if a new tree is created, than the ZMQ Publisher must be destroyed - bt_->resetGrootMonitor(); - - // Read the input BT XML from the specified file into a string - std::ifstream xml_file(bt_xml_filename); - - if (!xml_file.good()) { - RCLCPP_ERROR(get_logger(), "Couldn't open input XML file: %s", bt_xml_filename.c_str()); - return false; + if (!poses_navigator_->on_configure( + shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_)) + { + return nav2_util::CallbackReturn::FAILURE; } - auto xml_string = std::string( - std::istreambuf_iterator(xml_file), - std::istreambuf_iterator()); - - // Create the Behavior Tree from the XML input - tree_ = bt_->createTreeFromText(xml_string, blackboard_); - current_bt_xml_filename_ = bt_xml_filename; - - // get parameter for monitoring with Groot via ZMQ Publisher - if (get_parameter("enable_groot_monitoring").as_bool()) { - uint16_t zmq_publisher_port = get_parameter("groot_zmq_publisher_port").as_int(); - uint16_t zmq_server_port = get_parameter("groot_zmq_server_port").as_int(); - // optionally add max_msg_per_second = 25 (default) here - try { - bt_->addGrootMonitoring(&tree_, zmq_publisher_port, zmq_server_port); - } catch (const std::logic_error & e) { - RCLCPP_ERROR(get_logger(), "ZMQ already enabled, Error: %s", e.what()); - } - } - return true; + // Odometry smoother object for getting current speed + odom_smoother_ = std::make_unique(shared_from_this(), 0.3); + + return nav2_util::CallbackReturn::SUCCESS; } nav2_util::CallbackReturn @@ -184,13 +129,10 @@ BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - if (!loadBehaviorTree(default_bt_xml_filename_)) { - RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str()); + if (!poses_navigator_->on_activate() || !pose_navigator_->on_activate()) { return nav2_util::CallbackReturn::FAILURE; } - action_server_->activate(); - // create bond connection createBond(); @@ -202,7 +144,9 @@ BtNavigator::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); - action_server_->deactivate(); + if (!poses_navigator_->on_deactivate() || !pose_navigator_->on_deactivate()) { + return nav2_util::CallbackReturn::FAILURE; + } // destroy bond connection destroyBond(); @@ -215,23 +159,16 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - // TODO(orduno) Fix the race condition between the worker thread ticking the tree - // and the main thread resetting the resources, see #1344 - goal_sub_.reset(); - client_node_.reset(); - self_client_.reset(); - // Reset the listener before the buffer tf_listener_.reset(); tf_.reset(); - action_server_.reset(); - plugin_lib_names_.clear(); - current_bt_xml_filename_.clear(); - blackboard_.reset(); - bt_->haltAllActions(tree_.rootNode()); - bt_->resetGrootMonitor(); - bt_.reset(); + if (!poses_navigator_->on_cleanup() || !pose_navigator_->on_cleanup()) { + return nav2_util::CallbackReturn::FAILURE; + } + + poses_navigator_.reset(); + pose_navigator_.reset(); RCLCPP_INFO(get_logger(), "Completed Cleaning up"); return nav2_util::CallbackReturn::SUCCESS; @@ -244,114 +181,4 @@ BtNavigator::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -void -BtNavigator::navigateToPose() -{ - initializeGoalPose(); - - auto is_canceling = [this]() { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Canceling."); - return true; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Canceling."); - return true; - } - - return action_server_->is_cancel_requested(); - }; - - std::string bt_xml_filename = action_server_->get_current_goal()->behavior_tree; - - // Empty id in request is default for backward compatibility - bt_xml_filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; - - if (!loadBehaviorTree(bt_xml_filename)) { - RCLCPP_ERROR( - get_logger(), "BT file not found: %s. Navigation canceled.", - bt_xml_filename.c_str()); - action_server_->terminate_current(); - return; - } - - RosTopicLogger topic_logger(client_node_, tree_); - std::shared_ptr feedback_msg = std::make_shared(); - - auto on_loop = [&]() { - if (action_server_->is_preempt_requested()) { - RCLCPP_INFO(get_logger(), "Received goal preemption request"); - action_server_->accept_pending_goal(); - initializeGoalPose(); - } - topic_logger.flush(); - - // action server feedback (pose, duration of task, - // number of recoveries, and distance remaining to goal) - nav2_util::getCurrentPose( - feedback_msg->current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_); - - geometry_msgs::msg::PoseStamped goal_pose; - blackboard_->get("goal", goal_pose); - - feedback_msg->distance_remaining = nav2_util::geometry_utils::euclidean_distance( - feedback_msg->current_pose.pose, goal_pose.pose); - - int recovery_count = 0; - blackboard_->get("number_recoveries", recovery_count); - feedback_msg->number_of_recoveries = recovery_count; - feedback_msg->navigation_time = now() - start_time_; - action_server_->publish_feedback(feedback_msg); - }; - - // Execute the BT that was previously created in the configure step - nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); - // Make sure that the Bt is not in a running state from a previous execution - // note: if all the ControlNodes are implemented correctly, this is not needed. - bt_->haltAllActions(tree_.rootNode()); - - switch (rc) { - case nav2_behavior_tree::BtStatus::SUCCEEDED: - RCLCPP_INFO(get_logger(), "Navigation succeeded"); - action_server_->succeeded_current(); - break; - - case nav2_behavior_tree::BtStatus::FAILED: - RCLCPP_ERROR(get_logger(), "Navigation failed"); - action_server_->terminate_current(); - break; - - case nav2_behavior_tree::BtStatus::CANCELED: - RCLCPP_INFO(get_logger(), "Navigation canceled"); - action_server_->terminate_all(); - break; - } -} - -void -BtNavigator::initializeGoalPose() -{ - auto goal = action_server_->get_current_goal(); - - RCLCPP_INFO( - get_logger(), "Begin navigating from current location to (%.2f, %.2f)", - goal->pose.pose.position.x, goal->pose.pose.position.y); - - // Reset state for new action feedback - start_time_ = now(); - blackboard_->set("number_recoveries", 0); // NOLINT - - // Update the goal pose on the blackboard - blackboard_->set("goal", goal->pose); -} - -void -BtNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose) -{ - nav2_msgs::action::NavigateToPose::Goal goal; - goal.pose = *pose; - self_client_->async_send_goal(goal); -} - } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp new file mode 100644 index 00000000000..da724884ec2 --- /dev/null +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -0,0 +1,207 @@ +// Copyright (c) 2021 Samsung Research +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include "nav2_bt_navigator/navigators/navigate_through_poses.hpp" + +namespace nav2_bt_navigator +{ + +bool +NavigateThroughPosesNavigator::configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) +{ + start_time_ = rclcpp::Time(0); + auto node = parent_node.lock(); + node->declare_parameter("goals_blackboard_id", std::string("goals")); + goals_blackboard_id_ = node->get_parameter("goals_blackboard_id").as_string(); + if (!node->has_parameter("path_blackboard_id")) { + node->declare_parameter("path_blackboard_id", std::string("path")); + } + path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); + + // Odometry smoother object for getting current speed + odom_smoother_ = std::make_unique(node, 0.3); + + return true; +} + +std::string +NavigateThroughPosesNavigator::getDefaultBTFilepath( + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) +{ + std::string default_bt_xml_filename; + auto node = parent_node.lock(); + if (!node->has_parameter("default_nav_through_poses_bt_xml")) { + std::string pkg_share_dir = + ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + std::string tree_file = pkg_share_dir + + "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml"; + node->declare_parameter("default_nav_through_poses_bt_xml", tree_file); + } + node->get_parameter("default_nav_through_poses_bt_xml", default_bt_xml_filename); + + return default_bt_xml_filename; +} + +bool +NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) +{ + auto bt_xml_filename = goal->behavior_tree; + + if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { + RCLCPP_ERROR( + logger_, "BT file not found: %s. Navigation canceled.", + bt_xml_filename.c_str()); + return false; + } + + initializeGoalPoses(goal); + + return true; +} + +void +NavigateThroughPosesNavigator::goalCompleted(typename ActionT::Result::SharedPtr /*result*/) +{ +} + +void +NavigateThroughPosesNavigator::onLoop() +{ + using namespace nav2_util::geometry_utils; // NOLINT + + // action server feedback (pose, duration of task, + // number of recoveries, and distance remaining to goal, etc) + auto feedback_msg = std::make_shared(); + + auto blackboard = bt_action_server_->getBlackboard(); + + Goals goal_poses; + blackboard->get(goals_blackboard_id_, goal_poses); + + if (goal_poses.size() == 0) { + bt_action_server_->publishFeedback(feedback_msg); + return; + } + + geometry_msgs::msg::PoseStamped current_pose; + nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance); + + try { + // Get current path points + nav_msgs::msg::Path current_path; + blackboard->get(path_blackboard_id_, current_path); + + // Find the closest pose to current pose on global path + auto find_closest_pose_idx = + [¤t_pose, ¤t_path]() { + size_t closest_pose_idx = 0; + double curr_min_dist = std::numeric_limits::max(); + for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { + double curr_dist = nav2_util::geometry_utils::euclidean_distance( + current_pose, current_path.poses[curr_idx]); + if (curr_dist < curr_min_dist) { + curr_min_dist = curr_dist; + closest_pose_idx = curr_idx; + } + } + return closest_pose_idx; + }; + + // Calculate distance on the path + double distance_remaining = + nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx()); + + // Default value for time remaining + rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0); + + // Get current speed + geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist(); + double current_linear_speed = std::hypot(current_odom.linear.x, current_odom.linear.y); + + // Calculate estimated time taken to goal if speed is higher than 1cm/s + // and at least 10cm to go + if ((std::abs(current_linear_speed) > 0.01) && (distance_remaining > 0.1)) { + estimated_time_remaining = + rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed)); + } + + feedback_msg->distance_remaining = distance_remaining; + feedback_msg->estimated_time_remaining = estimated_time_remaining; + } catch (...) { + // Ignore + } + + int recovery_count = 0; + blackboard->get("number_recoveries", recovery_count); + feedback_msg->number_of_recoveries = recovery_count; + feedback_msg->current_pose = current_pose; + feedback_msg->navigation_time = clock_->now() - start_time_; + feedback_msg->number_of_poses_remaining = goal_poses.size(); + + bt_action_server_->publishFeedback(feedback_msg); +} + +void +NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) +{ + RCLCPP_INFO(logger_, "Received goal preemption request"); + + if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() || + (goal->behavior_tree.empty() && + bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename())) + { + // if pending goal requests the same BT as the current goal, accept the pending goal + // if pending goal has an empty behavior_tree field, it requests the default BT file + // accept the pending goal if the current goal is running the default BT file + initializeGoalPoses(bt_action_server_->acceptPendingGoal()); + } else { + RCLCPP_WARN( + logger_, + "Preemption request was rejected since the requested BT XML file is not the same " + "as the one that the current goal is executing. Preemption with a new BT is invalid " + "since it would require cancellation of the previous goal instead of true preemption." + "\nCancel the current goal and send a new action request if you want to use a " + "different BT XML file. For now, continuing to track the last goal until completion."); + bt_action_server_->terminatePendingGoal(); + } +} + +void +NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal) +{ + if (goal->poses.size() > 0) { + RCLCPP_INFO( + logger_, "Begin navigating from current location through %li poses to (%.2f, %.2f)", + goal->poses.size(), goal->poses.back().pose.position.x, goal->poses.back().pose.position.y); + } + + // Reset state for new action feedback + start_time_ = clock_->now(); + auto blackboard = bt_action_server_->getBlackboard(); + blackboard->set("number_recoveries", 0); // NOLINT + + // Update the goal pose on the blackboard + blackboard->set(goals_blackboard_id_, goal->poses); +} + +} // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp new file mode 100644 index 00000000000..250aa45a144 --- /dev/null +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -0,0 +1,216 @@ +// Copyright (c) 2021 Samsung Research +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include "nav2_bt_navigator/navigators/navigate_to_pose.hpp" + +namespace nav2_bt_navigator +{ + +bool +NavigateToPoseNavigator::configure( + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) +{ + start_time_ = rclcpp::Time(0); + auto node = parent_node.lock(); + node->declare_parameter("goal_blackboard_id", std::string("goal")); + goal_blackboard_id_ = node->get_parameter("goal_blackboard_id").as_string(); + if (!node->has_parameter("path_blackboard_id")) { + node->declare_parameter("path_blackboard_id", std::string("path")); + } + path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string(); + + // Odometry smoother object for getting current speed + odom_smoother_ = std::make_unique(node, 0.3); + + self_client_ = rclcpp_action::create_client(node, getName()); + + goal_sub_ = node->create_subscription( + "goal_pose", + rclcpp::SystemDefaultsQoS(), + std::bind(&NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1)); + return true; +} + +std::string +NavigateToPoseNavigator::getDefaultBTFilepath( + rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node) +{ + std::string default_bt_xml_filename; + auto node = parent_node.lock(); + if (!node->has_parameter("default_nav_to_pose_bt_xml")) { + std::string pkg_share_dir = + ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + std::string tree_file = pkg_share_dir + + "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"; + node->declare_parameter("default_nav_to_pose_bt_xml", tree_file); + } + node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename); + + return default_bt_xml_filename; +} + +bool +NavigateToPoseNavigator::cleanup() +{ + goal_sub_.reset(); + self_client_.reset(); + return true; +} + +bool +NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal) +{ + auto bt_xml_filename = goal->behavior_tree; + + if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) { + RCLCPP_ERROR( + logger_, "BT file not found: %s. Navigation canceled.", + bt_xml_filename.c_str()); + return false; + } + + initializeGoalPose(goal); + + return true; +} + +void +NavigateToPoseNavigator::goalCompleted(typename ActionT::Result::SharedPtr /*result*/) +{ +} + +void +NavigateToPoseNavigator::onLoop() +{ + // action server feedback (pose, duration of task, + // number of recoveries, and distance remaining to goal) + auto feedback_msg = std::make_shared(); + + geometry_msgs::msg::PoseStamped current_pose; + nav2_util::getCurrentPose( + current_pose, *feedback_utils_.tf, + feedback_utils_.global_frame, feedback_utils_.robot_frame, + feedback_utils_.transform_tolerance); + + auto blackboard = bt_action_server_->getBlackboard(); + + try { + // Get current path points + nav_msgs::msg::Path current_path; + blackboard->get(path_blackboard_id_, current_path); + + // Find the closest pose to current pose on global path + auto find_closest_pose_idx = + [¤t_pose, ¤t_path]() { + size_t closest_pose_idx = 0; + double curr_min_dist = std::numeric_limits::max(); + for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { + double curr_dist = nav2_util::geometry_utils::euclidean_distance( + current_pose, current_path.poses[curr_idx]); + if (curr_dist < curr_min_dist) { + curr_min_dist = curr_dist; + closest_pose_idx = curr_idx; + } + } + return closest_pose_idx; + }; + + // Calculate distance on the path + double distance_remaining = + nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx()); + + // Default value for time remaining + rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0); + + // Get current speed + geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist(); + double current_linear_speed = std::hypot(current_odom.linear.x, current_odom.linear.y); + + // Calculate estimated time taken to goal if speed is higher than 1cm/s + // and at least 10cm to go + if ((std::abs(current_linear_speed) > 0.01) && (distance_remaining > 0.1)) { + estimated_time_remaining = + rclcpp::Duration::from_seconds(distance_remaining / std::abs(current_linear_speed)); + } + + feedback_msg->distance_remaining = distance_remaining; + feedback_msg->estimated_time_remaining = estimated_time_remaining; + } catch (...) { + // Ignore + } + + int recovery_count = 0; + blackboard->get("number_recoveries", recovery_count); + feedback_msg->number_of_recoveries = recovery_count; + feedback_msg->current_pose = current_pose; + feedback_msg->navigation_time = clock_->now() - start_time_; + + bt_action_server_->publishFeedback(feedback_msg); +} + +void +NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) +{ + RCLCPP_INFO(logger_, "Received goal preemption request"); + + if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() || + (goal->behavior_tree.empty() && + bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename())) + { + // if pending goal requests the same BT as the current goal, accept the pending goal + // if pending goal has an empty behavior_tree field, it requests the default BT file + // accept the pending goal if the current goal is running the default BT file + initializeGoalPose(bt_action_server_->acceptPendingGoal()); + } else { + RCLCPP_WARN( + logger_, + "Preemption request was rejected since the requested BT XML file is not the same " + "as the one that the current goal is executing. Preemption with a new BT is invalid " + "since it would require cancellation of the previous goal instead of true preemption." + "\nCancel the current goal and send a new action request if you want to use a " + "different BT XML file. For now, continuing to track the last goal until completion."); + bt_action_server_->terminatePendingGoal(); + } +} + +void +NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal) +{ + RCLCPP_INFO( + logger_, "Begin navigating from current location to (%.2f, %.2f)", + goal->pose.pose.position.x, goal->pose.pose.position.y); + + // Reset state for new action feedback + start_time_ = clock_->now(); + auto blackboard = bt_action_server_->getBlackboard(); + blackboard->set("number_recoveries", 0); // NOLINT + + // Update the goal pose on the blackboard + blackboard->set(goal_blackboard_id_, goal->pose); +} + +void +NavigateToPoseNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose) +{ + ActionT::Goal goal; + goal.pose = *pose; + self_client_->async_send_goal(goal); +} + +} // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp deleted file mode 100644 index e85bae5fa81..00000000000 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) 2019 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include "nav2_bt_navigator/ros_topic_logger.hpp" -#include "tf2_ros/buffer_interface.h" - -namespace nav2_bt_navigator -{ - -RosTopicLogger::RosTopicLogger( - const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree) -: StatusChangeLogger(tree.rootNode()) -{ - auto node = ros_node.lock(); - clock_ = node->get_clock(); - logger_ = node->get_logger(); - log_pub_ = node->create_publisher( - "behavior_tree_log", - rclcpp::QoS(10)); -} - -void RosTopicLogger::callback( - BT::Duration timestamp, - const BT::TreeNode & node, - BT::NodeStatus prev_status, - BT::NodeStatus status) -{ - nav2_msgs::msg::BehaviorTreeStatusChange event; - - // BT timestamps are a duration since the epoch. Need to convert to a time_point - // before converting to a msg. - event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp)); - event.node_name = node.name(); - event.previous_status = toStr(prev_status, false); - event.current_status = toStr(status, false); - event_log_.push_back(std::move(event)); - - RCLCPP_DEBUG( - logger_, "[%.3f]: %25s %s -> %s", - std::chrono::duration(timestamp).count(), - node.name().c_str(), - toStr(prev_status, true).c_str(), - toStr(status, true).c_str() ); -} - -void RosTopicLogger::flush() -{ - if (!event_log_.empty()) { - auto log_msg = std::make_unique(); - log_msg->timestamp = clock_->now(); - log_msg->event_log = event_log_; - log_pub_->publish(std::move(log_msg)); - event_log_.clear(); - } -} - -} // namespace nav2_bt_navigator diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake index ee8555f799f..b00d30c17b2 100644 --- a/nav2_common/cmake/nav2_package.cmake +++ b/nav2_common/cmake/nav2_package.cmake @@ -29,7 +29,7 @@ macro(nav2_package) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py index 1f0638f81a0..c25fadb220c 100644 --- a/nav2_common/nav2_common/launch/__init__.py +++ b/nav2_common/nav2_common/launch/__init__.py @@ -12,5 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. +from .has_node_params import HasNodeParams from .rewritten_yaml import RewrittenYaml from .replace_string import ReplaceString diff --git a/nav2_common/nav2_common/launch/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py new file mode 100644 index 00000000000..92f96790b5b --- /dev/null +++ b/nav2_common/nav2_common/launch/has_node_params.py @@ -0,0 +1,60 @@ +# Copyright (c) 2021 PAL Robotics S.L. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from typing import List +from typing import Text + +import yaml +import launch + +import sys # delete this + +class HasNodeParams(launch.Substitution): + """ + Substitution that checks if a param file contains parameters for a node + + Used in launch system + """ + + def __init__(self, + source_file: launch.SomeSubstitutionsType, + node_name: Text) -> None: + super().__init__() + """ + Construct the substitution + + :param: source_file the parameter YAML file + :param: node_name the name of the node to check + """ + + from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__node_name = node_name + + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' + + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions(context, self.name) + data = yaml.safe_load(open(yaml_filename, 'r')) + + if self.__node_name in data.keys(): + return "True" + return "False" diff --git a/nav2_common/nav2_common/launch/replace_string.py b/nav2_common/nav2_common/launch/replace_string.py index d2bbada1c3e..4b5c82e757a 100644 --- a/nav2_common/nav2_common/launch/replace_string.py +++ b/nav2_common/nav2_common/launch/replace_string.py @@ -51,7 +51,7 @@ def perform(self, context: launch.LaunchContext) -> Text: try: input_file = open(launch.utilities.perform_substitutions(context, self.name), 'r') self.replace(input_file, output_file, replacements) - except Exception as err: + except Exception as err: # noqa: B902 print('ReplaceString substitution error: ', err) finally: input_file.close() diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 462f204e8df..4e08efbf9d0 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -106,11 +106,32 @@ def resolve_rewrites(self, context): return resolved_params, resolved_keys def substitute_params(self, yaml, param_rewrites): + # substitute leaf-only parameters for key in self.getYamlLeafKeys(yaml): if key.key() in param_rewrites: raw_value = param_rewrites[key.key()] key.setValue(self.convert(raw_value)) + # substitute total path parameters + yaml_paths = self.pathify(yaml) + for path in yaml_paths: + if path in param_rewrites: + # this is an absolute path (ex. 'key.keyA.keyB.val') + rewrite_val = self.convert(param_rewrites[path]) + yaml_keys = path.split('.') + yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val) + + + def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): + for key in yaml_key_list: + if key == yaml_key_list[-1]: + yaml[key] = rewrite_val + break + key = yaml_key_list.pop(0) + yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) + + return yaml + def substitute_keys(self, yaml, key_rewrites): if len(key_rewrites) != 0: for key, val in yaml.items(): @@ -129,6 +150,24 @@ def getYamlLeafKeys(self, yamlData): except AttributeError: return + def pathify(self, d, p=None, paths=None, joinchar='.'): + if p is None: + paths = {} + self.pathify(d, "", paths, joinchar=joinchar) + return paths + pn = p + if p != "": + pn += '.' + if isinstance(d, dict): + for k in d: + v = d[k] + self.pathify(v, pn + k, paths, joinchar=joinchar) + elif isinstance(d, list): + for idx, e in enumerate(d): + self.pathify(e, pn + str(idx), paths, joinchar=joinchar) + else: + paths[p] = d + def convert(self, text_value): if self.__convert_types: # try converting to int or float diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 07e75e63088..c2e32109889 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.3 + 1.0.0 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index bc6420b5b53..8114c53c7cd 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -48,6 +48,7 @@ class ControllerServer : public nav2_util::LifecycleNode { public: using ControllerMap = std::unordered_map; + using GoalCheckerMap = std::unordered_map; /** * @brief Constructor for nav2_controller::ControllerServer @@ -130,6 +131,15 @@ class ControllerServer : public nav2_util::LifecycleNode */ bool findControllerId(const std::string & c_name, std::string & name); + /** + * @brief Find the valid goal checker ID name for the specified parameter + * + * @param c_name The goal checker name + * @param name Reference to the name to use for goal checking if any valid available + * @return bool Whether it found a valid goal checker to use + */ + bool findGoalCheckerId(const std::string & c_name, std::string & name); + /** * @brief Assigns path to controller * @param path Path received from action server @@ -209,11 +219,12 @@ class ControllerServer : public nav2_util::LifecycleNode // Goal Checker Plugin pluginlib::ClassLoader goal_checker_loader_; - nav2_core::GoalChecker::Ptr goal_checker_; - std::string default_goal_checker_id_; - std::string default_goal_checker_type_; - std::string goal_checker_id_; - std::string goal_checker_type_; + GoalCheckerMap goal_checkers_; + std::vector default_goal_checker_ids_; + std::vector default_goal_checker_types_; + std::vector goal_checker_ids_; + std::vector goal_checker_types_; + std::string goal_checker_ids_concat_, current_goal_checker_; // Controller Plugins pluginlib::ClassLoader lp_loader_; @@ -229,9 +240,17 @@ class ControllerServer : public nav2_util::LifecycleNode double min_y_velocity_threshold_; double min_theta_velocity_threshold_; + double failure_tolerance_; + // Whether we've published the single controller warning yet geometry_msgs::msg::Pose end_pose_; + // Last time the controller generated a valid command + rclcpp::Time last_valid_cmd_time_; + + // Current path container + nav_msgs::msg::Path current_path_; + private: /** * @brief Callback for speed limiting messages diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index f95b5f2f921..705ee2a0af7 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -64,12 +64,25 @@ class SimpleGoalChecker : public nav2_core::GoalChecker bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist & velocity) override; + bool getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & vel_tolerance) override; protected: double xy_goal_tolerance_, yaw_goal_tolerance_; bool stateful_, check_xy_; // Cached squared xy_goal_tolerance_ double xy_goal_tolerance_sq_; + // Subscription for parameter change + rclcpp::AsyncParametersClient::SharedPtr parameters_client_; + rclcpp::Subscription::SharedPtr parameter_event_sub_; + std::string plugin_name_; + + /** + * @brief Callback executed when a paramter change is detected + * @param event ParameterEvent message + */ + void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); }; } // namespace nav2_controller diff --git a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp index f4a2c6d25a0..1b8706a5adb 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp @@ -61,6 +61,16 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker rclcpp::Time baseline_time_; bool baseline_pose_set_{false}; + // Subscription for parameter change + rclcpp::AsyncParametersClient::SharedPtr parameters_client_; + rclcpp::Subscription::SharedPtr parameter_event_sub_; + std::string plugin_name_; + + /** + * @brief Callback executed when a paramter change is detected + * @param event ParameterEvent message + */ + void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); }; } // namespace nav2_controller diff --git a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index 160ac60f091..29c05f983df 100644 --- a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -60,9 +60,22 @@ class StoppedGoalChecker : public SimpleGoalChecker bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist & velocity) override; + bool getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & vel_tolerance) override; protected: double rot_stopped_velocity_, trans_stopped_velocity_; + // Subscription for parameter change + rclcpp::AsyncParametersClient::SharedPtr parameters_client_; + rclcpp::Subscription::SharedPtr parameter_event_sub_; + std::string plugin_name_; + + /** + * @brief Callback executed when a paramter change is detected + * @param event ParameterEvent message + */ + void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); }; } // namespace nav2_controller diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 0dd318d8dc4..7bcfb7df892 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.4.3 + 1.0.0 Controller action interface Carl Delsey Apache-2.0 @@ -22,6 +22,8 @@ ament_lint_common ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest ament_cmake diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index 983032ee660..e03ce8b421c 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -34,15 +34,20 @@ #include #include +#include #include "nav2_controller/plugins/simple_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "angles/angles.h" #include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #include "tf2/utils.h" #pragma GCC diagnostic pop +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + namespace nav2_controller { @@ -59,6 +64,7 @@ void SimpleGoalChecker::initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) { + plugin_name_ = plugin_name; auto node = parent.lock(); nav2_util::declare_parameter_if_not_declared( @@ -76,6 +82,16 @@ void SimpleGoalChecker::initialize( node->get_parameter(plugin_name + ".stateful", stateful_); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; + + // Setup callback for changes to parameters. + parameters_client_ = std::make_shared( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + parameter_event_sub_ = parameters_client_->on_parameter_event( + std::bind(&SimpleGoalChecker::on_parameter_event_callback, this, _1)); } void SimpleGoalChecker::reset() @@ -105,6 +121,53 @@ bool SimpleGoalChecker::isGoalReached( return fabs(dyaw) < yaw_goal_tolerance_; } +bool SimpleGoalChecker::getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & vel_tolerance) +{ + double invalid_field = std::numeric_limits::lowest(); + + pose_tolerance.position.x = xy_goal_tolerance_; + pose_tolerance.position.y = xy_goal_tolerance_; + pose_tolerance.position.z = invalid_field; + pose_tolerance.orientation = + nav2_util::geometry_utils::orientationAroundZAxis(yaw_goal_tolerance_); + + vel_tolerance.linear.x = invalid_field; + vel_tolerance.linear.y = invalid_field; + vel_tolerance.linear.z = invalid_field; + + vel_tolerance.angular.x = invalid_field; + vel_tolerance.angular.y = invalid_field; + vel_tolerance.angular.z = invalid_field; + + return true; +} + +void +SimpleGoalChecker::on_parameter_event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + for (auto & changed_parameter : event->changed_parameters) { + const auto & type = changed_parameter.value.type; + const auto & name = changed_parameter.name; + const auto & value = changed_parameter.value; + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".xy_goal_tolerance") { + xy_goal_tolerance_ = value.double_value; + xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; + } else if (name == plugin_name_ + ".yaw_goal_tolerance") { + yaw_goal_tolerance_ = value.double_value; + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".stateful") { + stateful_ = value.bool_value; + } + } + } +} + } // namespace nav2_controller PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 542c8226efa..b7186076bc0 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -15,6 +15,7 @@ #include "nav2_controller/plugins/simple_progress_checker.hpp" #include #include +#include #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -22,6 +23,9 @@ #include "nav2_util/node_utils.hpp" #include "pluginlib/class_list_macros.hpp" +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + namespace nav2_controller { static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); @@ -30,6 +34,7 @@ void SimpleProgressChecker::initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) { + plugin_name_ = plugin_name; auto node = parent.lock(); clock_ = node->get_clock(); @@ -43,6 +48,16 @@ void SimpleProgressChecker::initialize( double time_allowance_param = 0.0; node->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); + + // Setup callback for changes to parameters. + parameters_client_ = std::make_shared( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + parameter_event_sub_ = parameters_client_->on_parameter_event( + std::bind(&SimpleProgressChecker::on_parameter_event_callback, this, _1)); } bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose) @@ -86,6 +101,25 @@ static double pose_distance( return std::hypot(dx, dy); } +void +SimpleProgressChecker::on_parameter_event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + for (auto & changed_parameter : event->changed_parameters) { + const auto & type = changed_parameter.value.type; + const auto & name = changed_parameter.name; + const auto & value = changed_parameter.value; + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".required_movement_radius") { + radius_ = value.double_value; + } else if (name == plugin_name_ + ".movement_time_allowance") { + time_allowance_ = rclcpp::Duration::from_seconds(value.double_value); + } + } + } +} + } // namespace nav2_controller PLUGINLIB_EXPORT_CLASS(nav2_controller::SimpleProgressChecker, nav2_core::ProgressChecker) diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index d6d409f1900..07d2228f384 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include "nav2_controller/plugins/stopped_goal_checker.hpp" #include "pluginlib/class_list_macros.hpp" #include "nav2_util/node_utils.hpp" @@ -42,6 +43,9 @@ using std::hypot; using std::fabs; +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + namespace nav2_controller { @@ -54,6 +58,7 @@ void StoppedGoalChecker::initialize( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) { + plugin_name_ = plugin_name; SimpleGoalChecker::initialize(parent, plugin_name); auto node = parent.lock(); @@ -67,6 +72,16 @@ void StoppedGoalChecker::initialize( node->get_parameter(plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_); node->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_); + + // Setup callback for changes to parameters. + parameters_client_ = std::make_shared( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + parameter_event_sub_ = parameters_client_->on_parameter_event( + std::bind(&StoppedGoalChecker::on_parameter_event_callback, this, _1)); } bool StoppedGoalChecker::isGoalReached( @@ -82,6 +97,46 @@ bool StoppedGoalChecker::isGoalReached( hypot(velocity.linear.x, velocity.linear.y) <= trans_stopped_velocity_; } +bool StoppedGoalChecker::getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & vel_tolerance) +{ + double invalid_field = std::numeric_limits::lowest(); + + // populate the poses + bool rtn = SimpleGoalChecker::getTolerances(pose_tolerance, vel_tolerance); + + // override the velocities + vel_tolerance.linear.x = trans_stopped_velocity_; + vel_tolerance.linear.y = trans_stopped_velocity_; + vel_tolerance.linear.z = invalid_field; + + vel_tolerance.angular.x = invalid_field; + vel_tolerance.angular.y = invalid_field; + vel_tolerance.angular.z = rot_stopped_velocity_; + + return true && rtn; +} + +void +StoppedGoalChecker::on_parameter_event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + for (auto & changed_parameter : event->changed_parameters) { + const auto & type = changed_parameter.value.type; + const auto & name = changed_parameter.name; + const auto & value = changed_parameter.value; + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".rot_stopped_velocity") { + rot_stopped_velocity_ = value.double_value; + } else if (name == plugin_name_ + ".trans_stopped_velocity") { + trans_stopped_velocity_ = value.double_value; + } + } + } +} + } // namespace nav2_controller PLUGINLIB_EXPORT_CLASS(nav2_controller::StoppedGoalChecker, nav2_core::GoalChecker) diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index bdf8ec4b83c..1c13117d5be 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" @@ -36,8 +37,8 @@ ControllerServer::ControllerServer() default_progress_checker_id_{"progress_checker"}, default_progress_checker_type_{"nav2_controller::SimpleProgressChecker"}, goal_checker_loader_("nav2_core", "nav2_core::GoalChecker"), - default_goal_checker_id_{"goal_checker"}, - default_goal_checker_type_{"nav2_controller::SimpleGoalChecker"}, + default_goal_checker_ids_{"goal_checker"}, + default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"}, lp_loader_("nav2_core", "nav2_core::Controller"), default_ids_{"FollowPath"}, default_types_{"dwb_core::DWBLocalPlanner"} @@ -47,7 +48,7 @@ ControllerServer::ControllerServer() declare_parameter("controller_frequency", 20.0); declare_parameter("progress_checker_plugin", default_progress_checker_id_); - declare_parameter("goal_checker_plugin", default_goal_checker_id_); + declare_parameter("goal_checker_plugins", default_goal_checker_ids_); declare_parameter("controller_plugins", default_ids_); declare_parameter("min_x_velocity_threshold", rclcpp::ParameterValue(0.0001)); declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001)); @@ -55,6 +56,8 @@ ControllerServer::ControllerServer() declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit")); + declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0)); + // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( "local_costmap", std::string{get_namespace()}, "local_costmap"); @@ -66,7 +69,7 @@ ControllerServer::ControllerServer() ControllerServer::~ControllerServer() { progress_checker_.reset(); - goal_checker_.reset(); + goal_checkers_.clear(); controllers_.clear(); } @@ -83,11 +86,15 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) node, default_progress_checker_id_ + ".plugin", rclcpp::ParameterValue(default_progress_checker_type_)); } - get_parameter("goal_checker_plugin", goal_checker_id_); - if (goal_checker_id_ == default_goal_checker_id_) { - nav2_util::declare_parameter_if_not_declared( - node, default_goal_checker_id_ + ".plugin", - rclcpp::ParameterValue(default_goal_checker_type_)); + + RCLCPP_INFO(get_logger(), "getting goal checker plugins.."); + get_parameter("goal_checker_plugins", goal_checker_ids_); + if (goal_checker_ids_ == default_goal_checker_ids_) { + for (size_t i = 0; i < default_goal_checker_ids_.size(); ++i) { + nav2_util::declare_parameter_if_not_declared( + node, default_goal_checker_ids_[i] + ".plugin", + rclcpp::ParameterValue(default_goal_checker_types_[i])); + } } get_parameter("controller_plugins", controller_ids_); @@ -100,6 +107,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } controller_types_.resize(controller_ids_.size()); + goal_checker_types_.resize(goal_checker_ids_.size()); get_parameter("controller_frequency", controller_frequency_); get_parameter("min_x_velocity_threshold", min_x_velocity_threshold_); @@ -109,6 +117,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) std::string speed_limit_topic; get_parameter("speed_limit_topic", speed_limit_topic); + get_parameter("failure_tolerance", failure_tolerance_); costmap_ros_->on_configure(state); @@ -125,20 +134,33 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) "Failed to create progress_checker. Exception: %s", ex.what()); return nav2_util::CallbackReturn::FAILURE; } - try { - goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_); - goal_checker_ = goal_checker_loader_.createUniqueInstance(goal_checker_type_); - RCLCPP_INFO( - get_logger(), "Created goal_checker : %s of type %s", - goal_checker_id_.c_str(), goal_checker_type_.c_str()); - goal_checker_->initialize(node, goal_checker_id_); - } catch (const pluginlib::PluginlibException & ex) { - RCLCPP_FATAL( - get_logger(), - "Failed to create goal_checker. Exception: %s", ex.what()); - return nav2_util::CallbackReturn::FAILURE; + + for (size_t i = 0; i != goal_checker_ids_.size(); i++) { + try { + goal_checker_types_[i] = nav2_util::get_plugin_type_param(node, goal_checker_ids_[i]); + nav2_core::GoalChecker::Ptr goal_checker = + goal_checker_loader_.createUniqueInstance(goal_checker_types_[i]); + RCLCPP_INFO( + get_logger(), "Created goal checker : %s of type %s", + goal_checker_ids_[i].c_str(), goal_checker_types_[i].c_str()); + goal_checker->initialize(node, goal_checker_ids_[i]); + goal_checkers_.insert({goal_checker_ids_[i], goal_checker}); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + get_logger(), + "Failed to create goal checker. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; + } + } + + for (size_t i = 0; i != goal_checker_ids_.size(); i++) { + goal_checker_ids_concat_ += goal_checker_ids_[i] + std::string(" "); } + RCLCPP_INFO( + get_logger(), + "Controller Server has %s goal checkers available.", goal_checker_ids_concat_.c_str()); + for (size_t i = 0; i != controller_ids_.size(); i++) { try { controller_types_[i] = nav2_util::get_plugin_type_param(node, controller_ids_[i]); @@ -234,6 +256,8 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) it->second->cleanup(); } controllers_.clear(); + + goal_checkers_.clear(); costmap_ros_->on_cleanup(state); // Release any allocated resources @@ -242,7 +266,6 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) vel_publisher_.reset(); speed_limit_sub_.reset(); action_server_.reset(); - goal_checker_->reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -268,7 +291,7 @@ bool ControllerServer::findControllerId( } else { RCLCPP_ERROR( get_logger(), "FollowPath called with controller name %s, " - "which does not exist. Available controllers are %s.", + "which does not exist. Available controllers are: %s.", c_name.c_str(), controller_ids_concat_.c_str()); return false; } @@ -280,6 +303,32 @@ bool ControllerServer::findControllerId( return true; } +bool ControllerServer::findGoalCheckerId( + const std::string & c_name, + std::string & current_goal_checker) +{ + if (goal_checkers_.find(c_name) == goal_checkers_.end()) { + if (goal_checkers_.size() == 1 && c_name.empty()) { + RCLCPP_WARN_ONCE( + get_logger(), "No goal checker was specified in parameter 'current_goal_checker'." + " Server will use only plugin loaded %s. " + "This warning will appear once.", goal_checker_ids_concat_.c_str()); + current_goal_checker = goal_checkers_.begin()->first; + } else { + RCLCPP_ERROR( + get_logger(), "FollowPath called with goal_checker name %s in parameter" + " 'current_goal_checker', which does not exist. Available goal checkers are: %s.", + c_name.c_str(), goal_checker_ids_concat_.c_str()); + return false; + } + } else { + RCLCPP_DEBUG(get_logger(), "Selected goal checker: %s.", c_name.c_str()); + current_goal_checker = c_name; + } + + return true; +} + void ControllerServer::computeControl() { RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); @@ -294,9 +343,19 @@ void ControllerServer::computeControl() return; } + std::string gc_name = action_server_->get_current_goal()->goal_checker_id; + std::string current_goal_checker; + if (findGoalCheckerId(gc_name, current_goal_checker)) { + current_goal_checker_ = current_goal_checker; + } else { + action_server_->terminate_current(); + return; + } + setPlannerPath(action_server_->get_current_goal()->path); progress_checker_->reset(); + last_valid_cmd_time_ = now(); rclcpp::WallRate loop_rate(controller_frequency_); while (rclcpp::ok()) { if (action_server_ == nullptr || !action_server_->is_server_active()) { @@ -363,12 +422,13 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) nav_2d_utils::transformPose( costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(), end_pose, end_pose, tolerance); - goal_checker_->reset(); + goal_checkers_[current_goal_checker_]->reset(); RCLCPP_DEBUG( get_logger(), "Path end point is (%.2f, %.2f)", end_pose.pose.position.x, end_pose.pose.position.y); end_pose_ = end_pose.pose; + current_path_ = path; } void ControllerServer::computeAndPublishVelocity() @@ -385,14 +445,58 @@ void ControllerServer::computeAndPublishVelocity() nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); - auto cmd_vel_2d = - controllers_[current_controller_]->computeVelocityCommands( - pose, - nav_2d_utils::twist2Dto3D(twist)); + geometry_msgs::msg::TwistStamped cmd_vel_2d; + + try { + cmd_vel_2d = + controllers_[current_controller_]->computeVelocityCommands( + pose, + nav_2d_utils::twist2Dto3D(twist), + goal_checkers_[current_goal_checker_].get()); + last_valid_cmd_time_ = now(); + } catch (nav2_core::PlannerException & e) { + if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { + RCLCPP_WARN(this->get_logger(), e.what()); + cmd_vel_2d.twist.angular.x = 0; + cmd_vel_2d.twist.angular.y = 0; + cmd_vel_2d.twist.angular.z = 0; + cmd_vel_2d.twist.linear.x = 0; + cmd_vel_2d.twist.linear.y = 0; + cmd_vel_2d.twist.linear.z = 0; + cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); + cmd_vel_2d.header.stamp = now(); + if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ && + failure_tolerance_ != -1.0) + { + throw nav2_core::PlannerException("Controller patience exceeded"); + } + } else { + throw nav2_core::PlannerException(e.what()); + } + } std::shared_ptr feedback = std::make_shared(); feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y); - feedback->distance_to_goal = nav2_util::geometry_utils::euclidean_distance(end_pose_, pose.pose); + + // Find the closest pose to current pose on global path + nav_msgs::msg::Path & current_path = current_path_; + auto find_closest_pose_idx = + [&pose, ¤t_path]() { + size_t closest_pose_idx = 0; + double curr_min_dist = std::numeric_limits::max(); + for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) { + double curr_dist = nav2_util::geometry_utils::euclidean_distance( + pose, current_path.poses[curr_idx]); + if (curr_dist < curr_min_dist) { + curr_min_dist = curr_dist; + closest_pose_idx = curr_idx; + } + } + return closest_pose_idx; + }; + + feedback->distance_to_goal = + nav2_util::geometry_utils::calculate_path_length(current_path_, find_closest_pose_idx()); action_server_->publish_feedback(feedback); RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds()); @@ -450,7 +554,7 @@ bool ControllerServer::isGoalReached() nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist); - return goal_checker_->isGoalReached(pose.pose, end_pose_, velocity); + return goal_checkers_[current_goal_checker_]->isGoalReached(pose.pose, end_pose_, velocity); } bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose) @@ -467,11 +571,7 @@ void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::Shar { ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { - if (!msg->percentage) { - RCLCPP_ERROR(get_logger(), "Speed limit in absolute values is not implemented yet"); - return; - } - it->second->setSpeedLimit(msg->speed_limit); + it->second->setSpeedLimit(msg->speed_limit, msg->percentage); } } diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 9253d3f96d6..d1fb1053ea3 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -47,6 +47,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/path.hpp" +#include "nav2_core/goal_checker.hpp" namespace nav2_core @@ -107,17 +108,22 @@ class Controller * * @param pose Current robot pose * @param velocity Current robot velocity + * @param goal_checker Pointer to the current goal checker the task is utilizing * @return The best command for the robot to drive */ virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) = 0; + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * goal_checker) = 0; /** * @brief Limits the maximum linear speed of the robot. - * @param speed_limit expressed in percentage from maximum robot speed. + * @param speed_limit expressed in absolute value (in m/s) + * or in percentage from maximum robot speed. + * @param percentage Setting speed limit in percentage if true + * or in absolute values in false case. */ - virtual void setSpeedLimit(const double & speed_limit) = 0; + virtual void setSpeedLimit(const double & speed_limit, const bool & percentage) = 0; }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/nav2_core/include/nav2_core/goal_checker.hpp index 2cb331688b0..fc1d34f010c 100644 --- a/nav2_core/include/nav2_core/goal_checker.hpp +++ b/nav2_core/include/nav2_core/goal_checker.hpp @@ -70,6 +70,7 @@ class GoalChecker const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) = 0; virtual void reset() = 0; + /** * @brief Check whether the goal should be considered reached * @param query_pose The pose to check @@ -80,6 +81,20 @@ class GoalChecker virtual bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, const geometry_msgs::msg::Twist & velocity) = 0; + + /** + * @brief Get the maximum possible tolerances used for goal checking in the major types. + * Any field without a valid entry is replaced with std::numeric_limits::lowest() + * to indicate that it is not measured. For tolerance across multiple entries + * (e.x. XY tolerances), both fields will contain this value since it is the maximum tolerance + * that each independent field could be assuming the other has no error (e.x. X and Y). + * @param pose_tolerance The tolerance used for checking in Pose fields + * @param vel_tolerance The tolerance used for checking velocity fields + * @return True if the tolerances are valid to use + */ + virtual bool getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & vel_tolerance) = 0; }; } // namespace nav2_core diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 91241953ae4..dbd14fd7ff6 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,8 +2,8 @@ nav2_core - 0.4.3 - A set of headers for plugins core to the navigation2 stack + 1.0.0 + A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey Apache-2.0 diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md index dc937f0435f..238eecce2d0 100644 --- a/nav2_costmap_2d/README.md +++ b/nav2_costmap_2d/README.md @@ -93,7 +93,7 @@ In order to add multiple sources to the global costmap, follow the same procedur ### Overview -Costmap Filters - is a costmap layer-based instrument which provides an ability to apply to map spatial-dependent raster features named as filter-masks. These features are used in plugin algorithms when filling costmaps in order to allow robots to change their trajectory, behavior or speed when a robot enters/leaves an area marked in a filter masks. Examples of costmap filters include keep-out/safety zones where robots will never enter, speed restriction areas, preferred lanes for robots moving in industries and warehouses. More information about design, architecture of the feature and how it works could be found on navigation2 web-site: https://navigation.ros.org. +Costmap Filters - is a costmap layer-based instrument which provides an ability to apply to map spatial-dependent raster features named as filter-masks. These features are used in plugin algorithms when filling costmaps in order to allow robots to change their trajectory, behavior or speed when a robot enters/leaves an area marked in a filter masks. Examples of costmap filters include keep-out/safety zones where robots will never enter, speed restriction areas, preferred lanes for robots moving in industries and warehouses. More information about design, architecture of the feature and how it works could be found on Nav2 website: https://navigation.ros.org. ## Future Plans - Conceptually, the costmap_2d model acts as a world model of what is known from the map, sensor, robot pose, etc. We'd like diff --git a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg index 0337e4123ad..a94e98fbbef 100755 --- a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg +++ b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg @@ -15,6 +15,8 @@ combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum) -#gen.add("max_obstacle_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50) -#gen.add("raytrace_range", double_t, 0, "The default range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) +# gen.add("obstacle_max_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50) +# gen.add("obstacle_min_range", double_t, 0, "The default minimum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 0.0, 0, 50) +# gen.add("raytrace_max_range", double_t, 0, "The default maximum range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) +# gen.add("raytrace_min_range", double_t, 0, "The default minimum range in meters from which to raytrace out obstacles from the map using sensor data.", 0.0, 0, 50) exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "ObstaclePlugin")) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 589f11781a6..46d79bd9df5 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp @@ -31,17 +31,31 @@ namespace nav2_costmap_2d class Costmap2DROS; +/** + * @class ClearCostmapService + * @brief Exposes services to clear costmap objects in inclusive/exclusive regions or completely + */ class ClearCostmapService { public: + /** + * @brief A constructor + */ ClearCostmapService(const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap); + /** + * @brief A constructor + */ ClearCostmapService() = delete; - // Clears the region outside of a user-specified area reverting to the static map + /** + * @brief Clears the region outside of a user-specified area reverting to the static map + */ void clearRegion(double reset_distance, bool invert); - // Clears all layers + /** + * @brief Clears all layers + */ void clearEntirely(); private: @@ -57,27 +71,42 @@ class ClearCostmapService // Server for clearing the costmap rclcpp::Service::SharedPtr clear_except_service_; + /** + * @brief Callback to clear costmap except in a given region + */ void clearExceptRegionCallback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response); rclcpp::Service::SharedPtr clear_around_service_; + /** + * @brief Callback to clear costmap in a given region + */ void clearAroundRobotCallback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response); rclcpp::Service::SharedPtr clear_entire_service_; + /** + * @brief Callback to clear costmap + */ void clearEntireCallback( const std::shared_ptr request_header, const std::shared_ptr request, const std::shared_ptr response); + /** + * @brief Function used to clear a given costmap layer + */ void clearLayerRegion( std::shared_ptr & costmap, double pose_x, double pose_y, double reset_distance, bool invert); + /** + * @brief Get the robot's position in the costmap using the master costmap + */ bool getPosition(double & x, double & y) const; }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index a5df5fd1918..ad56711c3fb 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -114,6 +114,22 @@ class Costmap2D double win_size_x, double win_size_y); + /** + * @brief Copies the (x0,y0)..(xn,yn) window from source costmap into a current costmap + @param source Source costmap where the window will be copied from + @param sx0 Lower x-boundary of the source window to copy, in cells + @param sy0 Lower y-boundary of the source window to copy, in cells + @param sxn Upper x-boundary of the source window to copy, in cells + @param syn Upper y-boundary of the source window to copy, in cells + @param dx0 Lower x-boundary of the destination window to copy, in cells + @param dx0 Lower y-boundary of the destination window to copy, in cells + @returns true if copy was succeeded or false in negative case + */ + bool copyWindow( + const Costmap2D & source, + unsigned int sx0, unsigned int sy0, unsigned int sxn, unsigned int syn, + unsigned int dx0, unsigned int dy0); + /** * @brief Default constructor */ @@ -258,11 +274,19 @@ class Costmap2D */ double getResolution() const; + /** + * @brief Set the default background value of the costmap + * @param c default value + */ void setDefaultValue(unsigned char c) { default_value_ = c; } + /** + * @brief Get the default background value of the costmap + * @return default value + */ unsigned char getDefaultValue() { return default_value_; @@ -309,12 +333,21 @@ class Costmap2D */ bool saveMap(std::string file_name); + /** + * @brief Resize the costmap + */ void resizeMap( unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y); + /** + * @brief Reset the costmap in bounds + */ void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn); + /** + * @brief Reset the costmap in bounds to a value + */ void resetMapToValue( unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn, unsigned char value); @@ -390,13 +423,15 @@ class Costmap2D * @param y0 The starting y coordinate * @param x1 The ending x coordinate * @param y1 The ending y coordinate - * @param max_length The maximum desired length of the segment... allows you to not go all the way to the endpoint + * @param max_length The maximum desired length of the segment... + * allows you to not go all the way to the endpoint + * @param min_length The minimum desired length of the segment */ template inline void raytraceLine( ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, - unsigned int max_length = UINT_MAX) + unsigned int max_length = UINT_MAX, unsigned int min_length = 0) { int dx = x1 - x0; int dy = y1 - y0; @@ -407,32 +442,45 @@ class Costmap2D int offset_dx = sign(dx); int offset_dy = sign(dy) * size_x_; - unsigned int offset = y0 * size_x_ + x0; - // we need to chose how much to scale our dominant dimension, // based on the maximum length of the line double dist = std::hypot(dx, dy); - double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); + if (dist < min_length) { + return; + } + + // Adjust starting point and offset to start from min_length distance + unsigned int min_x0 = (unsigned int)(x0 + dx / dist * min_length); + unsigned int min_y0 = (unsigned int)(y0 + dy / dist * min_length); + unsigned int offset = min_y0 * size_x_ + min_x0; + double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist); + unsigned int length; // if x is dominant if (abs_dx >= abs_dy) { int error_y = abs_dx / 2; + + // Subtract minlength from length since initial point (x0, y0)has been adjusted by min Z + length = (unsigned int)(scale * abs_dx) - min_length; + bresenham2D( - at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, - (unsigned int)(scale * abs_dx)); + at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, length); return; } // otherwise y is dominant int error_x = abs_dy / 2; + + // Subtract minlength from total length since initial point (x0, y0) has been adjusted by min Z + length = (unsigned int)(scale * abs_dy) - min_length; bresenham2D( - at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, - (unsigned int)(scale * abs_dy)); + at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, length); } private: /** - * @brief A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step + * @brief A 2D implementation of Bresenham's raytracing algorithm... + * applies an action at each step */ template inline void bresenham2D( @@ -454,6 +502,9 @@ class Costmap2D at(offset); } + /** + * @brief get the sign of an int + */ inline int sign(int x) { return x > 0 ? 1.0 : -1.0; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index ee276442ca4..2074ad5ea60 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -52,7 +52,7 @@ #include "tf2/transform_datatypes.h" #include "nav2_util/lifecycle_node.hpp" #include "tf2/LinearMath/Quaternion.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace nav2_costmap_2d { @@ -78,19 +78,34 @@ class Costmap2DPublisher */ ~Costmap2DPublisher(); + /** + * @brief Configure node + */ void on_configure() {} + + /** + * @brief Activate node + */ void on_activate() { costmap_pub_->on_activate(); costmap_update_pub_->on_activate(); costmap_raw_pub_->on_activate(); } + + /** + * @brief deactivate node + */ void on_deactivate() { costmap_pub_->on_deactivate(); costmap_update_pub_->on_deactivate(); costmap_raw_pub_->on_deactivate(); } + + /** + * @brief Cleanup node + */ void on_cleanup() {} /** @brief Include the given bounds in the changed-rectangle. */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 1bc733460d8..babf691fda3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -90,12 +90,34 @@ class Costmap2DROS : public nav2_util::LifecycleNode const std::string & parent_namespace, const std::string & local_namespace); + /** + * @brief A destructor + */ ~Costmap2DROS(); + /** + * @brief Configure node + */ nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Activate node + */ nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Deactivate node + */ nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Cleanup node + */ nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + + /** + * @brief shutdown node + */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; /** @@ -120,6 +142,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ void resume(); + /** + * @brief Update the map with the layered costmap / plugins + */ void updateMap(); /** @@ -140,6 +165,16 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose); + /** + * @brief Transform the input_pose in the global frame of the costmap + * @param input_pose pose to be transformed + * @param transformed_pose pose transformed + * @return True if the pose was transformed successfully, false otherwise + */ + bool transformPoseToGlobalFrame( + const geometry_msgs::msg::PoseStamped & input_pose, + geometry_msgs::msg::PoseStamped & transformed_pose); + /** @brief Returns costmap name */ std::string getName() const { @@ -180,6 +215,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode return robot_base_frame_; } + /** + * @brief Get the layered costmap object used in the node + */ LayeredCostmap * getLayeredCostmap() { return layered_costmap_.get(); @@ -274,6 +312,10 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::unique_ptr layered_costmap_{nullptr}; std::string name_; std::string parent_namespace_; + + /** + * @brief Function on timer for costmap update + */ void mapUpdateLoop(double frequency); bool map_update_thread_shutdown_{false}; bool stop_updates_{false}; @@ -284,7 +326,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode rclcpp::Duration publish_cycle_{1, 0}; pluginlib::ClassLoader plugin_loader_{"nav2_costmap_2d", "nav2_costmap_2d::Layer"}; - // Parameters + /** + * @brief Get parameters for node + */ void getParameters(); bool always_send_full_costmap_{false}; std::string footprint_; @@ -300,6 +344,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector default_types_; std::vector plugin_names_; std::vector plugin_types_; + std::vector filter_names_; + std::vector filter_types_; double resolution_{0}; std::string robot_base_frame_; ///< The frame_id of the robot base double robot_radius_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index 9f265e41e86..ecfb9d71f8c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -54,7 +54,13 @@ namespace nav2_costmap_2d class CostmapFilter : public Layer { public: + /** + * @brief A constructor + */ CostmapFilter(); + /** + * @brief A destructor + */ ~CostmapFilter(); /** @@ -69,18 +75,53 @@ class CostmapFilter : public Layer return access_; } - /** Layer API **/ + /** + * @brief Initialization process of layer on startup + */ virtual void onInitialize() final; + + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ virtual void updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) final; + + /** + * @brief Update the costs in the master costmap in the window + * @param master_grid The master costmap grid to update + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ virtual void updateCosts( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) final; + /** + * @brief Activate the layer + */ virtual void activate() final; + /** + * @brief Deactivate the layer + */ virtual void deactivate() final; + /** + * @brief Reset the layer + */ virtual void reset() final; + + /** + * @brief If clearing operations should be processed on this layer or not + */ virtual bool isClearable() {return false;} /** CostmapFilter API **/ 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 01b40a7bfd7..1755d57eb77 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 @@ -50,25 +50,51 @@ namespace nav2_costmap_2d { +/** + * @class KeepoutFilter + * @brief Reads in a keepout mask and marks keepout regions in the map + * to prevent planning or control in restricted areas + */ class KeepoutFilter : public CostmapFilter { public: + /** + * @brief A constructor + */ KeepoutFilter(); + /** + * @brief Initialize the filter and subscribe to the info topic + */ void initializeFilter( const std::string & filter_info_topic); + /** + * @brief Process the keepout layer at the current pose / bounds / grid + */ void 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); + /** + * @brief Reset the costmap filter / topic / info + */ void resetFilter(); + /** + * @brief If this filter is active + */ bool isActive(); private: + /** + * @brief Callback for the filter information + */ void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg); + /** + * @brief Callback for the filter mask + */ void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); rclcpp::Subscription::SharedPtr filter_info_sub_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp index 3d9b2e87181..c6b86799aa8 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp @@ -48,22 +48,42 @@ namespace nav2_costmap_2d { - +/** + * @class SpeedFilter + * @brief Reads in a speed restriction mask and enables a robot to + * dynamically adjust speed based on pose in map to slow in dangerous + * areas. Done via absolute speed setting or percentage of maximum speed + */ class SpeedFilter : public CostmapFilter { public: + /** + * @brief A constructor + */ SpeedFilter(); + /** + * @brief Initialize the filter and subscribe to the info topic + */ void initializeFilter( const std::string & filter_info_topic); + /** + * @brief Process the keepout layer at the current pose / bounds / grid + */ void 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); + /** + * @brief Reset the costmap filter / topic / info + */ void resetFilter(); + /** + * @brief If this filter is active + */ bool isActive(); protected: @@ -98,7 +118,13 @@ class SpeedFilter : public CostmapFilter const unsigned int mx, const unsigned int my) const; private: + /** + * @brief Callback for the filter information + */ void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg); + /** + * @brief Callback for the filter mask + */ void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); rclcpp::Subscription::SharedPtr filter_info_sub_; @@ -112,6 +138,7 @@ class SpeedFilter : public CostmapFilter std::string global_frame_; // Frame of currnet layer (master_grid) double base_, multiplier_; + bool percentage_; double speed_limit_, speed_limit_prev_; }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp index 5df24b543b1..15285df5fe2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp @@ -45,21 +45,40 @@ namespace nav2_costmap_2d { +/** + * @class CostmapLayer + * @brief A costmap layer base class for costmap plugin layers. + * Rather than just a layer, this object also contains an internal + * costmap object to populate and maintain state. + */ class CostmapLayer : public Layer, public Costmap2D { public: + /** + * @brief CostmapLayer constructor + */ CostmapLayer() : has_extra_bounds_(false), extra_min_x_(1e6), extra_max_x_(-1e6), extra_min_y_(1e6), extra_max_y_(-1e6) {} + /** + * @brief If layer is discrete + */ bool isDiscretized() { return true; } + /** + * @brief Match the size of the master costmap + */ virtual void matchSize(); + /** + * @brief Clear an are in the costmap with the given dimension + * if invert, then clear everything except these dimensions + */ virtual void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert); /** diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp index 795bac53fc4..2c21d859b20 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp @@ -56,11 +56,13 @@ inline double sign0(double x) return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0); } +/** @brief Gets L2 norm distance */ inline double distance(double x0, double y0, double x1, double y1) { return hypot(x1 - x0, y1 - y0); } +/** @brief Gets point distance to a line */ double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1); #endif // NAV2_COSTMAP_2D__COSTMAP_MATH_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index 0873d8781ec..5c65b1161c7 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -25,24 +25,45 @@ namespace nav2_costmap_2d { - +/** + * @class CostmapSubscriber + * @brief Subscribes to the costmap via a ros topic + */ class CostmapSubscriber { public: + /** + * @brief A constructor + */ CostmapSubscriber( const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name); + /** + * @brief A constructor + */ CostmapSubscriber( const rclcpp::Node::WeakPtr & parent, const std::string & topic_name); + /** + * @brief A destructor + */ ~CostmapSubscriber() {} + /** + * @brief A Get the costmap from topic + */ std::shared_ptr getCostmap(); protected: + /** + * @brief Convert an occ grid message into a costmap object + */ void toCostmap2D(); + /** + * @brief Callback for the costmap topic + */ void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg); std::shared_ptr costmap_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index 9f54728e635..b436f98cbd7 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp @@ -37,10 +37,17 @@ namespace nav2_costmap_2d { - +/** + * @class CostmapTopicCollisionChecker + * @brief Using a costmap via a ros topic, this object is used to + * find if robot poses are in collision with the costmap environment + */ class CostmapTopicCollisionChecker { public: + /** + * @brief A constructor + */ CostmapTopicCollisionChecker( CostmapSubscriber & costmap_sub, FootprintSubscriber & footprint_sub, @@ -49,15 +56,28 @@ class CostmapTopicCollisionChecker std::string global_frame = "map", std::string robot_base_frame = "base_link", double transform_tolerance = 0.1); - + /** + * @brief A destructor + */ ~CostmapTopicCollisionChecker() = default; - // Returns the obstacle footprint score for a particular pose + /** + * @brief Returns the obstacle footprint score for a particular pose + */ double scorePose(const geometry_msgs::msg::Pose2D & pose); + /** + * @brief Returns if a pose is collision free + */ bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose); protected: + /** + * @brief Set a new footprint + */ void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint); + /** + * @brief Get a footprint at a set pose + */ Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose); // Name used for logging diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp index 0646555dadc..0db73867cb7 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp @@ -40,7 +40,11 @@ namespace nav2_costmap_2d { - +/** + * @class CollisionCheckerException + * @brief Exceptions thrown if collision checker determines a pose is in + * collision with the environment costmap + */ class CollisionCheckerException : public std::runtime_error { public: diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp index 39c51328b8d..c90bad5dddc 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp @@ -32,18 +32,53 @@ namespace nav2_costmap_2d { typedef std::vector Footprint; +/** + * @class FootprintCollisionChecker + * @brief Checker for collision with a footprint on a costmap + */ template class FootprintCollisionChecker { public: + /** + * @brief A constructor. + */ FootprintCollisionChecker(); + /** + * @brief A constructor. + */ explicit FootprintCollisionChecker(CostmapT costmap); + /** + * @brief Find the footprint cost in oriented footprint + */ double footprintCost(const Footprint footprint); + /** + * @brief Find the footprint cost a a post with an unoriented footprint + */ double footprintCostAtPose(double x, double y, double theta, const Footprint footprint); + /** + * @brief Get the cost for a line segment + */ double lineCost(int x0, int x1, int y0, int y1) const; + /** + * @brief Get the map coordinates from a world point + */ bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); + /** + * @brief Get the cost of a point + */ double pointCost(int x, int y) const; + /** + * @brief Set the current costmap object to use for collision detection + */ void setCostmap(CostmapT costmap); + /** + * @brief Get the current costmap object + */ + CostmapT getCostmap() + { + return costmap_; + } protected: CostmapT costmap_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp index b6eef0c043e..d757db53bdc 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp @@ -24,30 +24,49 @@ namespace nav2_costmap_2d { - +/** + * @class FootprintSubscriber + * @brief Subscriber to the footprint topic to get current robot footprint + * (if changing) for use in collision avoidance + */ class FootprintSubscriber { public: + /** + * @brief A constructor + */ FootprintSubscriber( const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name, const double & footprint_timeout); + /** + * @brief A constructor + */ FootprintSubscriber( const rclcpp::Node::WeakPtr & parent, const std::string & topic_name, const double & footprint_timeout); + /** + * @brief A destructor + */ ~FootprintSubscriber() {} - // Returns an oriented robot footprint at current time. + /** + * @brief Returns an oriented robot footprint at current time. + */ bool getFootprint( std::vector & footprint, rclcpp::Duration & valid_footprint_timeout); + /** + * @brief Returns an oriented robot footprint without timeout + */ bool getFootprint(std::vector & footprint); - // Returns an oriented robot footprint. - // The second parameter is the time the fooptrint was at this orientation. + /** + * @brief Returns an oriented robot footprint at stamped time. + */ bool getFootprint( std::vector & footprint, rclcpp::Time & stamp, rclcpp::Duration valid_footprint_timeout); @@ -55,6 +74,9 @@ class FootprintSubscriber protected: rclcpp::Clock::SharedPtr clock_; + /** + * @brief Callback to process new footprint updates. + */ void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg); std::string topic_name_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 225c0fbbc77..66f6163b3cf 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -73,26 +73,69 @@ class CellData unsigned int src_x_, src_y_; }; +/** + * @class InflationLayer + * @brief Layer to convolve costmap by robot's radius or footprint to prevent + * collisions and largely simply collision checking + */ class InflationLayer : public Layer { public: + /** + * @brief A constructor + */ InflationLayer(); + /** + * @brief A destructor + */ ~InflationLayer(); + /** + * @brief Initialization process of layer on startup + */ void onInitialize() override; + + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ void updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) override; + /** + * @brief Update the costs in the master costmap in the window + * @param master_grid The master costmap grid to update + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ void updateCosts( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) override; + /** + * @brief Match the size of the master costmap + */ void matchSize() override; + + /** + * @brief If clearing operations should be processed on this layer or not + */ virtual bool isClearable() {return false;} + /** + * @brief Reset this costmap + */ void reset() override { matchSize(); @@ -120,12 +163,19 @@ class InflationLayer : public Layer // Provide a typedef to ease future code maintenance typedef std::recursive_mutex mutex_t; + + /** + * @brief Get the mutex of the inflation inforamtion + */ mutex_t * getMutex() { return access_; } protected: + /** + * @brief Process updates on footprint changes to the inflation layer + */ void onFootprintChanged() override; private: @@ -163,15 +213,27 @@ class InflationLayer : public Layer return cached_costs_[dx * cache_length_ + dy]; } + /** + * @brief Compute cached dsitances + */ void computeCaches(); + /** + * @brief Compute cached dsitances + */ int generateIntegerDistances(); + /** + * @brief Compute cached dsitances + */ unsigned int cellDistance(double world_dist) { return layered_costmap_->getCostmap()->cellDistance(world_dist); } + /** + * @brief Enqueue new cells in cache distance update search + */ inline void enqueue( unsigned int index, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index 467bfae5bf6..5f8c152dc43 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -51,13 +51,25 @@ namespace nav2_costmap_2d { class LayeredCostmap; -class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface +/** + * @class Layer + * @brief Abstract class for layered costmap plugin implementations + */ +class Layer { public: + /** + * @brief A constructor + */ Layer(); + /** + * @brief A destructor + */ virtual ~Layer() {} - // TODO(mjeronimo): should the following functions changed to a lifecycle-style interface? + /** + * @brief Initialization process of layer on startup + */ void initialize( LayeredCostmap * parent, std::string name, @@ -65,9 +77,17 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface const nav2_util::LifecycleNode::WeakPtr & node, rclcpp::Node::SharedPtr client_node, rclcpp::Node::SharedPtr rclcpp_node); - virtual void deactivate() {} /** @brief Stop publishers. */ - virtual void activate() {} /** @brief Restart publishers if they've been stopped. */ + /** @brief Stop publishers. */ + virtual void deactivate() {} + /** @brief Restart publishers if they've been stopped. */ + virtual void activate() {} + /** + * @brief Reset this costmap + */ virtual void reset() = 0; + /** + * @brief If clearing operations should be processed on this layer or not + */ virtual bool isClearable() = 0; /** @@ -99,7 +119,7 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface * changes (via LayeredCostmap::setFootprint()). Override to be * notified of changes to the robot's footprint. */ virtual void onFootprintChanged() {} - + /** @brief Get the name of the costmap layer */ std::string getName() const { return name_; @@ -127,9 +147,13 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface void declareParameter( const std::string & param_name, const rclcpp::ParameterValue & value); + /** @brief Convenience functions for declaring ROS parameters */ void declareParameter( - const std::string & param_name); + const std::string & param_name, + const rclcpp::ParameterType & param_type); + /** @brief Convenience functions for declaring ROS parameters */ bool hasParameter(const std::string & param_name); + /** @brief Convenience functions for declaring ROS parameters */ std::string getFullName(const std::string & param_name); protected: diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp index 4964b03ef8a..a0602b6865a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp @@ -78,11 +78,17 @@ class LayeredCostmap return global_frame_; } + /** + * @brief Resize the map to a new size, resolution, or origin + */ void resizeMap( unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked = false); + /** + * @brief Get the size of the bounds for update + */ void getUpdatedBounds(double & minx, double & miny, double & maxx, double & maxy) { minx = minx_; @@ -91,38 +97,79 @@ class LayeredCostmap maxy = maxy_; } + /** + * @brief If the costmap is current, e.g. are all the layers processing recent data + * and not stale information for a good state. + */ bool isCurrent(); + /** + * @brief Get the costmap pointer to the master costmap + */ Costmap2D * getCostmap() { - return &costmap_; + return &combined_costmap_; } + /** + * @brief If this costmap is rolling or not + */ bool isRolling() { return rolling_window_; } + /** + * @brief If this costmap is tracking unknown space or not + */ bool isTrackingUnknown() { - return costmap_.getDefaultValue() == nav2_costmap_2d::NO_INFORMATION; + return combined_costmap_.getDefaultValue() == nav2_costmap_2d::NO_INFORMATION; } + /** + * @brief Get the vector of pointers to the costmap plugins + */ std::vector> * getPlugins() { return &plugins_; } + /** + * @brief Get the vector of pointers to the costmap filters + */ + std::vector> * getFilters() + { + return &filters_; + } + + /** + * @brief Add a new plugin to the plugins vector to process + */ void addPlugin(std::shared_ptr plugin) { plugins_.push_back(plugin); } + /** + * @brief Add a new costmap filter plugin to the filters vector to process + */ + void addFilter(std::shared_ptr filter) + { + filters_.push_back(filter); + } + + /** + * @brief Get if the size of the costmap is locked + */ bool isSizeLocked() { return size_locked_; } + /** + * @brief Get the bounds of the costmap + */ void getBounds(unsigned int * x0, unsigned int * xn, unsigned int * y0, unsigned int * yn) { *x0 = bx0_; @@ -131,6 +178,9 @@ class LayeredCostmap *yn = byn_; } + /** + * @brief if the costmap is initialized + */ bool isInitialized() { return initialized_; @@ -163,7 +213,12 @@ class LayeredCostmap bool isOutofBounds(double robot_x, double robot_y); private: - Costmap2D costmap_; + // primary_costmap_ is a bottom costmap used by plugins when costmap filters were enabled. + // combined_costmap_ is a final costmap where all results produced by plugins and filters (if any) + // to be merged. + // The separation is aimed to avoid interferences of work between plugins and filters. + // primay_costmap_ and combined_costmap_ have the same sizes, origins and default values. + Costmap2D primary_costmap_, combined_costmap_; std::string global_frame_; bool rolling_window_; /// < @brief Whether or not the costmap should roll with the robot @@ -173,6 +228,7 @@ class LayeredCostmap unsigned int bx0_, bxn_, by0_, byn_; std::vector> plugins_; + std::vector> filters_; bool initialized_; bool size_locked_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp index e3195020780..9e1356cb6cf 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp @@ -50,32 +50,52 @@ class Observation * @brief Creates an empty observation */ Observation() - : cloud_(new sensor_msgs::msg::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0) + : cloud_(new sensor_msgs::msg::PointCloud2()), obstacle_max_range_(0.0), obstacle_min_range_(0.0), + raytrace_max_range_(0.0), + raytrace_min_range_(0.0) { } - + /** + * @brief A destructor + */ virtual ~Observation() { delete cloud_; } /** - * @brief Explicitly define copy assignment operator for Observation as it has a user-declared destructor - */ - Observation & operator=(const Observation &) = default; + * @brief Copy assignment operator + * @param obs The observation to copy + */ + Observation & operator=(const Observation & obs) + { + origin_ = obs.origin_; + cloud_ = new sensor_msgs::msg::PointCloud2(*(obs.cloud_)); + obstacle_max_range_ = obs.obstacle_max_range_; + obstacle_min_range_ = obs.obstacle_min_range_; + raytrace_max_range_ = obs.raytrace_max_range_; + raytrace_min_range_ = obs.raytrace_min_range_; + + return *this; + } /** * @brief Creates an observation from an origin point and a point cloud * @param origin The origin point of the observation * @param cloud The point cloud of the observation - * @param obstacle_range The range out to which an observation should be able to insert obstacles - * @param raytrace_range The range out to which an observation should be able to clear via raytracing + * @param obstacle_max_range The range out to which an observation should be able to insert obstacles + * @param obstacle_min_range The range from which an observation should be able to insert obstacles + * @param raytrace_max_range The range out to which an observation should be able to clear via raytracing + * @param raytrace_min_range The range from which an observation should be able to clear via raytracing */ Observation( geometry_msgs::msg::Point & origin, const sensor_msgs::msg::PointCloud2 & cloud, - double obstacle_range, double raytrace_range) + double obstacle_max_range, double obstacle_min_range, double raytrace_max_range, + double raytrace_min_range) : origin_(origin), cloud_(new sensor_msgs::msg::PointCloud2(cloud)), - obstacle_range_(obstacle_range), raytrace_range_(raytrace_range) + obstacle_max_range_(obstacle_max_range), obstacle_min_range_(obstacle_min_range), + raytrace_max_range_(raytrace_max_range), raytrace_min_range_( + raytrace_min_range) { } @@ -85,24 +105,30 @@ class Observation */ Observation(const Observation & obs) : origin_(obs.origin_), cloud_(new sensor_msgs::msg::PointCloud2(*(obs.cloud_))), - obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_) + obstacle_max_range_(obs.obstacle_max_range_), obstacle_min_range_(obs.obstacle_min_range_), + raytrace_max_range_(obs.raytrace_max_range_), + raytrace_min_range_(obs.raytrace_min_range_) { } /** * @brief Creates an observation from a point cloud * @param cloud The point cloud of the observation - * @param obstacle_range The range out to which an observation should be able to insert obstacles + * @param obstacle_max_range The range out to which an observation should be able to insert obstacles + * @param obstacle_min_range The range from which an observation should be able to insert obstacles */ - Observation(const sensor_msgs::msg::PointCloud2 & cloud, double obstacle_range) - : cloud_(new sensor_msgs::msg::PointCloud2(cloud)), obstacle_range_(obstacle_range), - raytrace_range_(0.0) + Observation( + const sensor_msgs::msg::PointCloud2 & cloud, double obstacle_max_range, + double obstacle_min_range) + : cloud_(new sensor_msgs::msg::PointCloud2(cloud)), obstacle_max_range_(obstacle_max_range), + obstacle_min_range_(obstacle_min_range), + raytrace_max_range_(0.0), raytrace_min_range_(0.0) { } geometry_msgs::msg::Point origin_; sensor_msgs::msg::PointCloud2 * cloud_; - double obstacle_range_, raytrace_range_; + double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp index 715231668d4..2ff834a41e7 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp @@ -41,10 +41,10 @@ #include #include -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/time.hpp" #include "tf2_ros/buffer.h" -#include "tf2_sensor_msgs/tf2_sensor_msgs.h" +#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "nav2_costmap_2d/observation.hpp" #include "nav2_util/lifecycle_node.hpp" @@ -66,8 +66,10 @@ class ObservationBuffer * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit * @param min_obstacle_height The minimum height of a hitpoint to be considered legal * @param max_obstacle_height The minimum height of a hitpoint to be considered legal - * @param obstacle_range The range to which the sensor should be trusted for inserting obstacles - * @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space + * @param obstacle_max_range The range to which the sensor should be trusted for inserting obstacles + * @param obstacle_min_range The range from which the sensor should be trusted for inserting obstacles + * @param raytrace_max_range The range to which the sensor should be trusted for raytracing to clear out space + * @param raytrace_min_range The range from which the sensor should be trusted for raytracing to clear out space * @param tf2_buffer A reference to a tf2 Buffer * @param global_frame The frame to transform PointClouds into * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages @@ -78,8 +80,10 @@ class ObservationBuffer std::string topic_name, double observation_keep_time, double expected_update_rate, - double min_obstacle_height, double max_obstacle_height, double obstacle_range, - double raytrace_range, tf2_ros::Buffer & tf2_buffer, std::string global_frame, + double min_obstacle_height, double max_obstacle_height, double obstacle_max_range, + double obstacle_min_range, + double raytrace_max_range, double raytrace_min_range, tf2_ros::Buffer & tf2_buffer, + std::string global_frame, std::string sensor_frame, tf2::Duration tf_tolerance); @@ -146,7 +150,7 @@ class ObservationBuffer std::string topic_name_; double min_obstacle_height_, max_obstacle_height_; std::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely - double obstacle_range_, raytrace_range_; + double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_; tf2::Duration tf_tolerance_; }; } // 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 3e7413ac1de..deac55b2695 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -61,28 +61,74 @@ namespace nav2_costmap_2d { +/** + * @class ObstacleLayer + * @brief Takes in laser and pointcloud data to populate into 2D costmap + */ class ObstacleLayer : public CostmapLayer { public: + /** + * @brief A constructor + */ ObstacleLayer() { costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D. } + /** + * @brief A destructor + */ virtual ~ObstacleLayer(); + /** + * @brief Initialization process of layer on startup + */ virtual void onInitialize(); + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ virtual void updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y); + /** + * @brief Update the costs in the master costmap in the window + * @param master_grid The master costmap grid to update + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ virtual void updateCosts( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); - virtual void activate(); + /** + * @brief Deactivate the layer + */ virtual void deactivate(); + + /** + * @brief Activate the layer + */ + virtual void activate(); + + /** + * @brief Reset this costmap + */ virtual void reset(); + + /** + * @brief If clearing operations should be processed on this layer or not + */ virtual bool isClearable() {return true;} /** @@ -152,14 +198,20 @@ class ObstacleLayer : public CostmapLayer double * max_x, double * max_y); + /** + * @brief Process update costmap with raytracing the window bounds + */ void updateRaytraceBounds( - double ox, double oy, double wx, double wy, double range, + double ox, double oy, double wx, double wy, double max_range, double min_range, double * min_x, double * min_y, double * max_x, double * max_y); std::vector transformed_footprint_; bool footprint_clearing_enabled_; + /** + * @brief Clear costmap layer info below the robot's footprint + */ void updateFootprint( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp index 840ee20a0f1..71cd4654a96 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -47,12 +47,16 @@ #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "rclcpp/rclcpp.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "sensor_msgs/msg/range.hpp" namespace nav2_costmap_2d { +/** + * @class RangeSensorLayer + * @brief Takes in IR/Sonar/similar point measurement sensors and populates in costmap + */ class RangeSensorLayer : public CostmapLayer { public: @@ -63,45 +67,132 @@ class RangeSensorLayer : public CostmapLayer ALL }; + /** + * @brief A constructor + */ RangeSensorLayer(); + /** + * @brief Initialization process of layer on startup + */ virtual void onInitialize(); + + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ virtual void updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y); + + /** + * @brief Update the costs in the master costmap in the window + * @param master_grid The master costmap grid to update + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ virtual void updateCosts( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); + + /** + * @brief Reset this costmap + */ virtual void reset(); + + /** + * @brief Deactivate the layer + */ virtual void deactivate(); + + /** + * @brief Activate the layer + */ virtual void activate(); + + /** + * @brief If clearing operations should be processed on this layer or not + */ virtual bool isClearable() {return true;} + /** + * @brief Handle an incoming Range message to populate into costmap + */ void bufferIncomingRangeMsg(const sensor_msgs::msg::Range::SharedPtr range_message); private: + /** + * @brief Processes all sensors into the costmap buffered from callbacks + */ void updateCostmap(); + /** + * @brief Update the actual costmap with the values processed + */ void updateCostmap(sensor_msgs::msg::Range & range_message, bool clear_sensor_cone); + /** + * @brief Process general incoming range sensor data. If min=max ranges, + * fixed processor callback is used, else uses variable callback + */ void processRangeMsg(sensor_msgs::msg::Range & range_message); + /** + * @brief Process fixed range incoming range sensor data + */ void processFixedRangeMsg(sensor_msgs::msg::Range & range_message); + /** + * @brief Process variable range incoming range sensor data + */ void processVariableRangeMsg(sensor_msgs::msg::Range & range_message); + /** + * @brief Reset the angle min/max x, and min/max y values + */ void resetRange(); + /** + * @brief Get the gamma value for an angle, theta + */ inline double gamma(double theta); + /** + * @brief Get the delta value for an angle, phi + */ inline double delta(double phi); + /** + * @brief Apply the sensor model of the layer for range sensors + */ inline double sensor_model(double r, double phi, double theta); + /** + * @brief Get angles + */ inline void get_deltas(double angle, double * dx, double * dy); + + /** + * @brief Update the cost in a cell with information + */ inline void update_cell( double ox, double oy, double ot, double r, double nx, double ny, bool clear); + /** + * @brief Find probability value of a cost + */ inline double to_prob(unsigned char c) { return static_cast(c) / nav2_costmap_2d::LETHAL_OBSTACLE; } + + /** + * @brief Find cost value of a probability + */ inline unsigned char to_cost(double p) { return static_cast(p * nav2_costmap_2d::LETHAL_OBSTACLE); @@ -126,11 +217,17 @@ class RangeSensorLayer : public CostmapLayer std::vector::SharedPtr> range_subs_; double min_x_, min_y_, max_x_, max_y_; + /** + * @brief Find the area of 3 points of a triangle + */ float area(int x1, int y1, int x2, int y2, int x3, int y3) { return fabs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0); } + /** + * @brief Find the cross product of 3 vectors, A,B,C + */ int orient2d(int Ax, int Ay, int Bx, int By, int Cx, int Cy) { return (Bx - Ax) * (Cy - Ay) - (By - Ay) * (Cx - Ax); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index e2be08d010f..bf047d7f383 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -51,30 +51,86 @@ namespace nav2_costmap_2d { +/** + * @class StaticLayer + * @brief Takes in a map generated from SLAM to add costs to costmap + */ class StaticLayer : public CostmapLayer { public: + /** + * @brief Static Layer constructor + */ StaticLayer(); + /** + * @brief Static Layer destructor + */ virtual ~StaticLayer(); + /** + * @brief Initialization process of layer on startup + */ virtual void onInitialize(); + + /** + * @brief Activate this layer + */ virtual void activate(); + /** + * @brief Deactivate this layer + */ virtual void deactivate(); + + /** + * @brief Reset this costmap + */ virtual void reset(); + + /** + * @brief If clearing operations should be processed on this layer or not + */ virtual bool isClearable() {return false;} + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ virtual void updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y); + /** + * @brief Update the costs in the master costmap in the window + * @param master_grid The master costmap grid to update + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ virtual void updateCosts( nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); + /** + * @brief Match the size of the master costmap + */ virtual void matchSize(); private: + /** + * @brief Get parameters of layer + */ void getParameters(); + + /** + * @brief Process a new map coming from a topic + */ void processMap(const nav_msgs::msg::OccupancyGrid & new_map); /** @@ -84,8 +140,16 @@ class StaticLayer : public CostmapLayer * static map are overwritten. */ void incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map); + /** + * @brief Callback to update the costmap's map from the map_server (or SLAM) + * with an update in a particular area of the map + */ void incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update); + /** + * @brief Interpret the value in the static map given on the topic to + * convert into costs for the costmap to utilize + */ unsigned char interpretValue(unsigned char value); std::string global_frame_; ///< @brief The global frame for the costmap diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 22cf4269863..f2f6635c313 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -55,39 +55,90 @@ namespace nav2_costmap_2d { +/** + * @class VoxelLayer + * @brief Takes laser and pointcloud data to populate a 3D voxel representation of the environment + */ class VoxelLayer : public ObstacleLayer { public: + /** + * @brief Voxel Layer constructor + */ VoxelLayer() : voxel_grid_(0, 0, 0) { costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D } + /** + * @brief Voxel Layer destructor + */ virtual ~VoxelLayer(); + /** + * @brief Initialization process of layer on startup + */ virtual void onInitialize(); + + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ virtual void updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y); + /** + * @brief Update the layer's origin to a new pose, often when in a rolling costmap + */ void updateOrigin(double new_origin_x, double new_origin_y); + + /** + * @brief If layer is discretely populated + */ bool isDiscretized() { return true; } + + /** + * @brief Match the size of the master costmap + */ virtual void matchSize(); + + /** + * @brief Reset this costmap + */ virtual void reset(); + + /** + * @brief If clearing operations should be processed on this layer or not + */ virtual bool isClearable() {return true;} protected: + /** + * @brief Reset internal maps + */ virtual void resetMaps(); private: - void reconfigureCB(); + /** + * @brief Clear any non-lethal cost columns + */ void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info); + /** + * @brief Use raycasting between 2 points to clear freespace + */ virtual void raytraceFreespace( const nav2_costmap_2d::Observation & clearing_observation, double * min_x, double * min_y, @@ -99,8 +150,12 @@ class VoxelLayer : public ObstacleLayer nav2_voxel_grid::VoxelGrid voxel_grid_; double z_resolution_, origin_z_; int unknown_threshold_, mark_threshold_, size_z_; - rclcpp::Publisher::SharedPtr clearing_endpoints_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + clearing_endpoints_pub_; + /** + * @brief Covert world coordinates into map coordinates + */ inline bool worldToMap3DFloat( double wx, double wy, double wz, double & mx, double & my, double & mz) @@ -118,6 +173,9 @@ class VoxelLayer : public ObstacleLayer return false; } + /** + * @brief Covert world coordinates into map coordinates + */ inline bool worldToMap3D( double wx, double wy, double wz, unsigned int & mx, unsigned int & my, unsigned int & mz) @@ -137,6 +195,9 @@ class VoxelLayer : public ObstacleLayer return false; } + /** + * @brief Covert map coordinates into world coordinates + */ inline void mapToWorld3D( unsigned int mx, unsigned int my, unsigned int mz, double & wx, double & wy, @@ -148,11 +209,17 @@ class VoxelLayer : public ObstacleLayer wz = origin_z_ + (mz + 0.5) * z_resolution_; } + /** + * @brief Find L2 norm distance in 3D + */ inline double dist(double x0, double y0, double z0, double x1, double y1, double z1) { return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0)); } + /** + * @brief Get the height of the voxel sizes in meters + */ double getSizeInMetersZ() const { return (size_z_ - 1 + 0.5) * z_resolution_; diff --git a/nav2_costmap_2d/launch/example_params.yaml b/nav2_costmap_2d/launch/example_params.yaml index 783d348663d..72bf695fad3 100644 --- a/nav2_costmap_2d/launch/example_params.yaml +++ b/nav2_costmap_2d/launch/example_params.yaml @@ -26,9 +26,12 @@ mark_threshold: 0 #END VOXEL STUFF transform_tolerance: 0.3 -obstacle_range: 2.5 +obstacle_max_range: 2.5 +obstacle_min_range: 0.0 max_obstacle_height: 2.0 -raytrace_range: 3.0 +raytrace_max_range: 3.0 +raytrace_min_range: 0.0 + footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] #robot_radius: 0.46 footprint_padding: 0.01 diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index cbd4b2dbf76..8865f88b558 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.3 + 1.0.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/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index bc788f91947..cdec0a4b2ef 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -60,22 +60,22 @@ void CostmapFilter::onInitialize() throw std::runtime_error{"Failed to lock node"}; } - // Declare common for all costmap filters parameters - declareParameter("enabled", rclcpp::ParameterValue(true)); - declareParameter("filter_info_topic"); - declareParameter("transform_tolerance", rclcpp::ParameterValue(0.1)); - - // Get parameters - node->get_parameter(name_ + "." + "enabled", enabled_); try { + // Declare common for all costmap filters parameters + declareParameter("enabled", rclcpp::ParameterValue(true)); + declareParameter("filter_info_topic", rclcpp::PARAMETER_STRING); + declareParameter("transform_tolerance", rclcpp::ParameterValue(0.1)); + + // Get parameters + node->get_parameter(name_ + "." + "enabled", enabled_); filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string(); - } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { - RCLCPP_ERROR(logger_, "filter_info_topic parameter is not set"); + double transform_tolerance; + node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance); + transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + } catch (const std::exception & ex) { + RCLCPP_ERROR(logger_, "Parameter problem: %s", ex.what()); throw ex; } - double transform_tolerance; - node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance); - transform_tolerance_ = tf2::durationFromSec(transform_tolerance); } void CostmapFilter::activate() diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index d0f8a670182..6b4dbefdcd2 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -35,8 +35,11 @@ * Author: Alexey Merzlyakov *********************************************************************/ +#include +#include +#include #include "tf2/convert.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index 348687b300c..cc368b46833 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -38,8 +38,10 @@ #include "nav2_costmap_2d/costmap_filters/speed_filter.hpp" #include - -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include +#include +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "geometry_msgs/msg/point_stamped.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" @@ -90,6 +92,7 @@ void SpeedFilter::initializeFilter( // Reset speed conversion states base_ = BASE_DEFAULT; multiplier_ = MULTIPLIER_DEFAULT; + percentage_ = false; } void SpeedFilter::filterInfoCallback( @@ -120,15 +123,19 @@ void SpeedFilter::filterInfoCallback( multiplier_ = msg->multiplier; if (msg->type == SPEED_FILTER_PERCENT) { // Using speed limit in % of maximum speed + percentage_ = true; RCLCPP_INFO( logger_, "SpeedFilter: Using expressed in a percent from maximum speed" "speed_limit = %f + filter_mask_data * %f", base_, multiplier_); } else if (msg->type == SPEED_FILTER_ABSOLUTE) { - // Speed limit in absolute values is not implemented yet - RCLCPP_ERROR(logger_, "SpeedFilter: Speed limit in absolute values is not implemented yet"); - return; + // Using speed limit in m/s + percentage_ = false; + RCLCPP_INFO( + logger_, + "SpeedFilter: Using absolute speed_limit = %f + filter_mask_data * %f", + base_, multiplier_); } else { RCLCPP_ERROR(logger_, "SpeedFilter: Mode is not supported"); return; @@ -278,30 +285,30 @@ void SpeedFilter::process( } else { // Normal case: speed_mask_data in range of [1..100] speed_limit_ = speed_mask_data * multiplier_ + base_; - if (speed_limit_ < 0.0) { - RCLCPP_WARN( - logger_, - "SpeedFilter: Speed limit in filter_mask[%i, %i] is less than 0%, " - "which can not be true. Setting it to 0% value.", - mask_robot_i, mask_robot_j); - speed_limit_ = NO_SPEED_LIMIT; - } - if (speed_limit_ > 100.0) { - RCLCPP_WARN( - logger_, - "SpeedFilter: Speed limit in filter_mask[%i, %i] is higher than 100%, " - "which can not be true. Setting it to 100% value.", - mask_robot_i, mask_robot_j); - speed_limit_ = 100.0; + if (percentage_) { + if (speed_limit_ < 0.0 || speed_limit_ > 100.0) { + RCLCPP_WARN( + logger_, + "SpeedFilter: Speed limit in filter_mask[%i, %i] is %f%%, " + "out of bounds of [0, 100]. Setting it to no-limit value.", + mask_robot_i, mask_robot_j, speed_limit_); + speed_limit_ = NO_SPEED_LIMIT; + } + } else { + if (speed_limit_ < 0.0) { + RCLCPP_WARN( + logger_, + "SpeedFilter: Speed limit in filter_mask[%i, %i] is less than 0 m/s, " + "which can not be true. Setting it to no-limit value.", + mask_robot_i, mask_robot_j); + speed_limit_ = NO_SPEED_LIMIT; + } } } if (speed_limit_ != speed_limit_prev_) { if (speed_limit_ != NO_SPEED_LIMIT) { - RCLCPP_DEBUG( - logger_, - "SpeedFilter: Speed limit is set to %f%% of maximum speed", - speed_limit_); + RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to %f", speed_limit_); } else { RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to its default value"); } @@ -311,7 +318,7 @@ void SpeedFilter::process( std::make_unique(); msg->header.frame_id = global_frame_; msg->header.stamp = clock_->now(); - msg->percentage = true; + msg->percentage = percentage_; msg->speed_limit = speed_limit_; speed_limit_pub_->publish(std::move(msg)); diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index de60b3427f8..daff9ccd082 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -199,6 +199,10 @@ InflationLayer::updateCosts( // box min_i...max_j, by the amount cell_inflation_radius_. Cells // up to that distance outside the box can still influence the costs // stored in cells inside the box. + const int base_min_i = min_i; + const int base_min_j = min_j; + const int base_max_i = max_i; + const int base_max_j = max_j; min_i -= static_cast(cell_inflation_radius_); min_j -= static_cast(cell_inflation_radius_); max_i += static_cast(cell_inflation_radius_); @@ -251,12 +255,21 @@ InflationLayer::updateCosts( // assign the cost associated with the distance from an obstacle to the cell unsigned char cost = costLookup(mx, my, sx, sy); unsigned char old_cost = master_array[index]; - if (old_cost == NO_INFORMATION && - (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) + // In order to avoid artifacts appeared out of boundary areas + // when some layer is going after inflation_layer, + // we need to apply inflation_layer only to inside of given bounds + if (static_cast(mx) >= base_min_i && + static_cast(my) >= base_min_j && + static_cast(mx) < base_max_i && + static_cast(my) < base_max_j) { - master_array[index] = cost; - } else { - master_array[index] = std::max(old_cost, cost); + if (old_cost == NO_INFORMATION && + (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) + { + master_array[index] = cost; + } else { + master_array[index] = std::max(old_cost, cost); + } } // attempt to put the neighbors of the current cell onto the inflation list diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 1a227961049..11a79558ab5 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -132,8 +132,10 @@ void ObstacleLayer::onInitialize() declareParameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false)); declareParameter(source + "." + "marking", rclcpp::ParameterValue(true)); declareParameter(source + "." + "clearing", rclcpp::ParameterValue(false)); - declareParameter(source + "." + "obstacle_range", rclcpp::ParameterValue(2.5)); - declareParameter(source + "." + "raytrace_range", rclcpp::ParameterValue(3.0)); + declareParameter(source + "." + "obstacle_max_range", rclcpp::ParameterValue(2.5)); + declareParameter(source + "." + "obstacle_min_range", rclcpp::ParameterValue(0.0)); + declareParameter(source + "." + "raytrace_max_range", rclcpp::ParameterValue(3.0)); + declareParameter(source + "." + "raytrace_min_range", rclcpp::ParameterValue(0.0)); node->get_parameter(name_ + "." + source + "." + "topic", topic); node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame); @@ -159,12 +161,15 @@ void ObstacleLayer::onInitialize() } // get the obstacle range for the sensor - double obstacle_range; - node->get_parameter(name_ + "." + source + "." + "obstacle_range", obstacle_range); + double obstacle_max_range, obstacle_min_range; + node->get_parameter(name_ + "." + source + "." + "obstacle_max_range", obstacle_max_range); + node->get_parameter(name_ + "." + source + "." + "obstacle_min_range", obstacle_min_range); + + // get the raytrace ranges for the sensor + double raytrace_max_range, raytrace_min_range; + node->get_parameter(name_ + "." + source + "." + "raytrace_min_range", raytrace_min_range); + node->get_parameter(name_ + "." + source + "." + "raytrace_max_range", raytrace_max_range); - // get the raytrace range for the sensor - double raytrace_range; - node->get_parameter(name_ + "." + source + "." + "raytrace_range", raytrace_range); RCLCPP_DEBUG( logger_, @@ -179,7 +184,9 @@ void ObstacleLayer::onInitialize() new ObservationBuffer( node, topic, observation_keep_time, expected_update_rate, min_obstacle_height, - max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, + max_obstacle_height, obstacle_max_range, obstacle_min_range, raytrace_max_range, + raytrace_min_range, *tf_, + global_frame_, sensor_frame, tf2::durationFromSec(transform_tolerance)))); // check if we'll add this buffer to our marking observation buffers @@ -377,7 +384,8 @@ ObstacleLayer::updateBounds( const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_); - double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; + double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_; + double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_; sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); @@ -399,11 +407,17 @@ ObstacleLayer::updateBounds( (pz - obs.origin_.z) * (pz - obs.origin_.z); // if the point is far enough away... we won't consider it - if (sq_dist >= sq_obstacle_range) { + if (sq_dist >= sq_obstacle_max_range) { RCLCPP_DEBUG(logger_, "The point is too far away"); continue; } + // if the point is too close, do not conisder it + if (sq_dist < sq_obstacle_min_range) { + RCLCPP_DEBUG(logger_, "The point is too close"); + continue; + } + // now we need to compute the map coordinates for the observation unsigned int mx, my; if (!worldToMap(px, py, mx, my)) { @@ -600,13 +614,15 @@ ObstacleLayer::raytraceFreespace( continue; } - unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); + unsigned int cell_raytrace_max_range = cellDistance(clearing_observation.raytrace_max_range_); + unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_); MarkCell marker(costmap_, FREE_SPACE); // and finally... we can execute our trace to clear obstacles along that line - raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); + raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_max_range, cell_raytrace_min_range); updateRaytraceBounds( - ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, + ox, oy, wx, wy, clearing_observation.raytrace_max_range_, + clearing_observation.raytrace_min_range_, min_x, min_y, max_x, max_y); } } @@ -639,12 +655,15 @@ ObstacleLayer::deactivate() void ObstacleLayer::updateRaytraceBounds( - double ox, double oy, double wx, double wy, double range, + double ox, double oy, double wx, double wy, double max_range, double min_range, double * min_x, double * min_y, double * max_x, double * max_y) { double dx = wx - ox, dy = wy - oy; double full_distance = hypot(dx, dy); - double scale = std::min(1.0, range / full_distance); + if (full_distance < min_range) { + return; + } + double scale = std::min(1.0, max_range / full_distance); double ex = ox + dx * scale, ey = oy + dy * scale; touch(ex, ey, min_x, min_y, max_x, max_y); } diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 37a5cef248a..75956d64621 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -44,7 +44,7 @@ #include "pluginlib/class_list_macros.hpp" #include "tf2/convert.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 4dc669035bd..b8e5261bc14 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -96,8 +96,9 @@ void VoxelLayer::onInitialize() voxel_pub_->on_activate(); } - clearing_endpoints_pub_ = node->create_publisher( + clearing_endpoints_pub_ = node->create_publisher( "clearing_endpoints", custom_qos); + clearing_endpoints_pub_->on_activate(); unknown_threshold_ += (VOXEL_BITS - size_z_); matchSize(); @@ -168,7 +169,8 @@ void VoxelLayer::updateBounds( const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_); - double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; + double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_; + double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_; sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); @@ -186,7 +188,12 @@ void VoxelLayer::updateBounds( (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z); // if the point is far enough away... we won't consider it - if (sq_dist >= sq_obstacle_range) { + if (sq_dist >= sq_obstacle_max_range) { + continue; + } + + // If the point is too close, do not consider it + if (sq_dist < sq_obstacle_min_range) { continue; } @@ -296,11 +303,9 @@ void VoxelLayer::raytraceFreespace( double * max_x, double * max_y) { - auto clearing_endpoints_ = std::make_unique(); + auto clearing_endpoints_ = std::make_unique(); - size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * - clearing_observation.cloud_->width; - if (clearing_observation_cloud_size == 0) { + if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) { return; } @@ -328,10 +333,23 @@ void VoxelLayer::raytraceFreespace( } if (publish_clearing_points) { - clearing_endpoints_->points.clear(); - clearing_endpoints_->points.reserve(clearing_observation_cloud_size); + clearing_endpoints_->data.clear(); + clearing_endpoints_->width = clearing_observation.cloud_->width; + clearing_endpoints_->height = clearing_observation.cloud_->height; + clearing_endpoints_->is_dense = true; + clearing_endpoints_->is_bigendian = false; } + sensor_msgs::PointCloud2Modifier modifier(*clearing_endpoints_); + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + + sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_x(*clearing_endpoints_, "x"); + sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_y(*clearing_endpoints_, "y"); + sensor_msgs::PointCloud2Iterator clearing_endpoints_iter_z(*clearing_endpoints_, "z"); + // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later double map_end_x = origin_x_ + getSizeInMetersX(); double map_end_y = origin_y_ + getSizeInMetersY(); @@ -390,26 +408,31 @@ void VoxelLayer::raytraceFreespace( double point_x, point_y, point_z; if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)) { - unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); + unsigned int cell_raytrace_max_range = cellDistance(clearing_observation.raytrace_max_range_); + unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_); + // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z); voxel_grid_.clearVoxelLineInMap( sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_, unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, - cell_raytrace_range); + cell_raytrace_max_range, cell_raytrace_min_range); updateRaytraceBounds( - ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, + ox, oy, wpx, wpy, clearing_observation.raytrace_max_range_, + clearing_observation.raytrace_min_range_, min_x, min_y, max_x, max_y); if (publish_clearing_points) { - geometry_msgs::msg::Point32 point; - point.x = wpx; - point.y = wpy; - point.z = wpz; - clearing_endpoints_->points.push_back(point); + *clearing_endpoints_iter_x = wpx; + *clearing_endpoints_iter_y = wpy; + *clearing_endpoints_iter_z = wpz; + + ++clearing_endpoints_iter_x; + ++clearing_endpoints_iter_y; + ++clearing_endpoints_iter_z; } } } diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 61f89c1f085..922478bb0dd 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -68,7 +68,7 @@ void ClearCostmapService::clearExceptRegionCallback( { RCLCPP_INFO( logger_, - "Received request to clear except a region the " + costmap_.getName()); + ("Received request to clear except a region the " + costmap_.getName()).c_str()); clearRegion(request->reset_distance, true); } @@ -88,7 +88,7 @@ void ClearCostmapService::clearEntireCallback( { RCLCPP_INFO( logger_, - "Received request to clear entirely the " + costmap_.getName()); + ("Received request to clear entirely the " + costmap_.getName()).c_str()); clearEntirely(); } @@ -112,6 +112,9 @@ void ClearCostmapService::clearRegion(const double reset_distance, bool invert) clearLayerRegion(costmap_layer, x, y, reset_distance, invert); } } + + // AlexeyMerzlyakov: No need to clear layer region for costmap filters + // as they are always supposed to be not clearable. } void ClearCostmapService::clearLayerRegion( diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 372e0b1f7a5..e72807da59b 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -184,6 +184,29 @@ bool Costmap2D::copyCostmapWindow( return true; } +bool Costmap2D::copyWindow( + const Costmap2D & source, + unsigned int sx0, unsigned int sy0, unsigned int sxn, unsigned int syn, + unsigned int dx0, unsigned int dy0) +{ + const unsigned int sz_x = sxn - sx0; + const unsigned int sz_y = syn - sy0; + + if (sxn > source.getSizeInCellsX() || syn > source.getSizeInCellsY()) { + return false; + } + + if (dx0 + sz_x > size_x_ || dy0 + sz_y > size_y_) { + return false; + } + + copyMapRegion( + source.costmap_, sx0, sy0, source.size_x_, + costmap_, dx0, dy0, size_x_, + sz_x, sz_y); + return true; +} + Costmap2D & Costmap2D::operator=(const Costmap2D & map) { // check for self assignement @@ -267,13 +290,12 @@ bool Costmap2D::worldToMap(double wx, double wy, unsigned int & mx, unsigned int return false; } - mx = static_cast((wx - origin_x_) / resolution_); - my = static_cast((wy - origin_y_) / resolution_); + mx = static_cast((wx - origin_x_) / resolution_); + my = static_cast((wy - origin_y_) / resolution_); if (mx < size_x_ && my < size_y_) { return true; } - return false; } @@ -468,7 +490,7 @@ void Costmap2D::convexFillCells( MapLocation pt; // loop though cells in the column - for (unsigned int y = min_pt.y; y < max_pt.y; ++y) { + for (unsigned int y = min_pt.y; y <= max_pt.y; ++y) { pt.x = x; pt.y = y; polygon_cells.push_back(pt); diff --git a/nav2_costmap_2d/src/costmap_2d_cloud.cpp b/nav2_costmap_2d/src/costmap_2d_cloud.cpp index 52157b3b7a5..8c40709be6d 100644 --- a/nav2_costmap_2d/src/costmap_2d_cloud.cpp +++ b/nav2_costmap_2d/src/costmap_2d_cloud.cpp @@ -31,8 +31,8 @@ #include #include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/point_cloud.hpp" -#include "sensor_msgs/msg/channel_float32.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" #include "nav2_voxel_grid/voxel_grid.hpp" #include "nav2_msgs/msg/voxel_grid.hpp" #include "nav2_util/execution_timer.hpp" @@ -70,8 +70,63 @@ V_Cell g_unknown; rclcpp::Node::SharedPtr g_node; -rclcpp::Publisher::SharedPtr pub_marked; -rclcpp::Publisher::SharedPtr pub_unknown; +rclcpp::Publisher::SharedPtr pub_marked; +rclcpp::Publisher::SharedPtr pub_unknown; + +/** + * @brief An helper function to fill pointcloud2 of both the marked and unknown points from voxel_grid + * @param cloud PointCloud2 Ptr which needs to be filled + * @param num_channels Represents the total number of points that are going to be filled + * @param header Carries the header information that needs to be assigned to PointCloud2 header + * @param g_cells contains the x, y, z values that needs to be added to the PointCloud2 + */ +void pointCloud2Helper( + std::unique_ptr & cloud, + uint32_t num_channels, + std_msgs::msg::Header header, + V_Cell & g_cells) +{ + cloud->header = header; + cloud->width = num_channels; + cloud->height = 1; + cloud->is_dense = true; + cloud->is_bigendian = false; + sensor_msgs::PointCloud2Modifier modifier(*cloud); + + modifier.setPointCloud2Fields( + 6, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "r", 1, sensor_msgs::msg::PointField::UINT8, + "g", 1, sensor_msgs::msg::PointField::UINT8, + "b", 1, sensor_msgs::msg::PointField::UINT8); + + sensor_msgs::PointCloud2Iterator iter_x(*cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*cloud, "z"); + + sensor_msgs::PointCloud2Iterator iter_r(*cloud, "r"); + sensor_msgs::PointCloud2Iterator iter_g(*cloud, "g"); + sensor_msgs::PointCloud2Iterator iter_b(*cloud, "b"); + + for (uint32_t i = 0; i < num_channels; ++i) { + Cell & c = g_cells[i]; + // assigning value to the point cloud2's iterator + *iter_x = c.x; + *iter_y = c.y; + *iter_z = c.z; + *iter_r = g_colors_r[c.status] * 255.0; + *iter_g = g_colors_g[c.status] * 255.0; + *iter_b = g_colors_b[c.status] * 255.0; + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_r; + ++iter_g; + ++iter_b; + } +} void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid) { @@ -133,65 +188,19 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid) } } - { - auto cloud = std::make_unique(); - cloud->points.resize(num_marked); - cloud->channels.resize(1); - cloud->channels[0].values.resize(num_marked); - cloud->channels[0].name = "rgb"; - cloud->header.frame_id = frame_id; - cloud->header.stamp = stamp; - - sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0]; - for (uint32_t i = 0; i < num_marked; ++i) { - geometry_msgs::msg::Point32 & p = cloud->points[i]; - float & cval = chan.values[i]; - Cell & c = g_marked[i]; - - p.x = c.x; - p.y = c.y; - p.z = c.z; - - uint32_t r = g_colors_r[c.status] * 255.0; - uint32_t g = g_colors_g[c.status] * 255.0; - uint32_t b = g_colors_b[c.status] * 255.0; - // uint32_t a = g_colors_a[c.status] * 255.0; - - uint32_t col = (r << 16) | (g << 8) | b; - memcpy(&cval, &col, sizeof col); - } + std_msgs::msg::Header pcl_header; + pcl_header.frame_id = frame_id; + pcl_header.stamp = stamp; + { + auto cloud = std::make_unique(); + pointCloud2Helper(cloud, num_marked, pcl_header, g_marked); pub_marked->publish(std::move(cloud)); } { - auto cloud = std::make_unique(); - cloud->points.resize(num_unknown); - cloud->channels.resize(1); - cloud->channels[0].values.resize(num_unknown); - cloud->channels[0].name = "rgb"; - cloud->header.frame_id = frame_id; - cloud->header.stamp = stamp; - - sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0]; - for (uint32_t i = 0; i < num_unknown; ++i) { - geometry_msgs::msg::Point32 & p = cloud->points[i]; - float & cval = chan.values[i]; - Cell & c = g_unknown[i]; - - p.x = c.x; - p.y = c.y; - p.z = c.z; - - uint32_t r = g_colors_r[c.status] * 255.0; - uint32_t g = g_colors_g[c.status] * 255.0; - uint32_t b = g_colors_b[c.status] * 255.0; - // uint32_t a = g_colors_a[c.status] * 255.0; - - uint32_t col = (r << 16) | (g << 8) | b; - memcpy(&cval, &col, sizeof col); - } - + auto cloud = std::make_unique(); + pointCloud2Helper(cloud, num_unknown, pcl_header, g_unknown); pub_unknown->publish(std::move(cloud)); } @@ -208,9 +217,9 @@ int main(int argc, char ** argv) RCLCPP_DEBUG(g_node->get_logger(), "Starting up costmap_2d_cloud"); - pub_marked = g_node->create_publisher( + pub_marked = g_node->create_publisher( "voxel_marked_cloud", 1); - pub_unknown = g_node->create_publisher( + pub_unknown = g_node->create_publisher( "voxel_unknown_cloud", 1); auto sub = g_node->create_subscription( "voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback); diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 70de2770f9a..71eb8349b5f 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -47,7 +47,7 @@ #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_util/execution_timer.hpp" #include "nav2_util/node_utils.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/create_timer_ros.h" #include "nav2_util/robot_utils.hpp" @@ -101,6 +101,7 @@ Costmap2DROS::Costmap2DROS( declare_parameter("origin_x", rclcpp::ParameterValue(0.0)); declare_parameter("origin_y", rclcpp::ParameterValue(0.0)); declare_parameter("plugins", rclcpp::ParameterValue(default_plugins_)); + declare_parameter("filters", rclcpp::ParameterValue(std::vector())); declare_parameter("publish_frequency", rclcpp::ParameterValue(1.0)); declare_parameter("resolution", rclcpp::ParameterValue(0.1)); declare_parameter("robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); @@ -157,6 +158,19 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str()); } + // and costmap filters as well + for (unsigned int i = 0; i < filter_names_.size(); ++i) { + RCLCPP_INFO(get_logger(), "Using costmap filter \"%s\"", filter_names_[i].c_str()); + + std::shared_ptr filter = plugin_loader_.createSharedInstance(filter_types_[i]); + layered_costmap_->addFilter(filter); + + filter->initialize( + layered_costmap_.get(), filter_names_[i], tf_buffer_.get(), + shared_from_this(), client_node_, rclcpp_node_); + + RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str()); + } // Create the publishers and subscribers footprint_sub_ = create_subscription( @@ -300,6 +314,7 @@ Costmap2DROS::getParameters() get_parameter("update_frequency", map_update_frequency_); get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); + get_parameter("filters", filter_names_); auto node = shared_from_this(); @@ -310,11 +325,15 @@ Costmap2DROS::getParameters() } } plugin_types_.resize(plugin_names_.size()); + filter_types_.resize(filter_names_.size()); // 1. All plugins must have 'plugin' param defined in their namespace to define the plugin type for (size_t i = 0; i < plugin_names_.size(); ++i) { plugin_types_[i] = nav2_util::get_plugin_type_param(node, plugin_names_[i]); } + for (size_t i = 0; i < filter_names_.size(); ++i) { + filter_types_[i] = nav2_util::get_plugin_type_param(node, filter_names_[i]); + } // 2. The map publish frequency cannot be 0 (to avoid a divde-by-zero) if (map_publish_frequency_ > 0) { @@ -455,6 +474,7 @@ Costmap2DROS::start() { RCLCPP_INFO(get_logger(), "start"); std::vector> * plugins = layered_costmap_->getPlugins(); + std::vector> * filters = layered_costmap_->getFilters(); // check if we're stopped or just paused if (stopped_) { @@ -465,6 +485,12 @@ Costmap2DROS::start() { (*plugin)->activate(); } + for (std::vector>::iterator filter = filters->begin(); + filter != filters->end(); + ++filter) + { + (*filter)->activate(); + } stopped_ = false; } stop_updates_ = false; @@ -482,12 +508,18 @@ Costmap2DROS::stop() { stop_updates_ = true; std::vector> * plugins = layered_costmap_->getPlugins(); + std::vector> * filters = layered_costmap_->getFilters(); // unsubscribe from topics for (std::vector>::iterator plugin = plugins->begin(); plugin != plugins->end(); ++plugin) { (*plugin)->deactivate(); } + for (std::vector>::iterator filter = filters->begin(); + filter != filters->end(); ++filter) + { + (*filter)->deactivate(); + } initialized_ = false; stopped_ = true; } @@ -519,11 +551,17 @@ Costmap2DROS::resetLayers() // Reset each of the plugins std::vector> * plugins = layered_costmap_->getPlugins(); + std::vector> * filters = layered_costmap_->getFilters(); for (std::vector>::iterator plugin = plugins->begin(); plugin != plugins->end(); ++plugin) { (*plugin)->reset(); } + for (std::vector>::iterator filter = filters->begin(); + filter != filters->end(); ++filter) + { + (*filter)->reset(); + } } bool @@ -534,4 +572,19 @@ Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) global_frame_, robot_base_frame_, transform_tolerance_); } +bool +Costmap2DROS::transformPoseToGlobalFrame( + const geometry_msgs::msg::PoseStamped & input_pose, + geometry_msgs::msg::PoseStamped & transformed_pose) +{ + if (input_pose.header.frame_id == global_frame_) { + transformed_pose = input_pose; + return true; + } else { + return nav2_util::transformPoseInTargetFrame( + input_pose, transformed_pose, *tf_buffer_, + global_frame_, transform_tolerance_); + } +} + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp index 20d60b3442a..09876fb7c7f 100644 --- a/nav2_costmap_2d/src/footprint_collision_checker.cpp +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -51,12 +51,17 @@ double FootprintCollisionChecker::footprintCost(const Footprint footpr unsigned int x0, x1, y0, y1; double footprint_cost = 0.0; + // get the cell coord of the first point + if (!worldToMap(footprint[0].x, footprint[0].y, x0, y0)) { + return static_cast(LETHAL_OBSTACLE); + } + + // cache the start to eliminate a worldToMap call + unsigned int xstart = x0; + unsigned int ystart = y0; + // we need to rasterize each line in the footprint for (unsigned int i = 0; i < footprint.size() - 1; ++i) { - // get the cell coord of the first point - if (!worldToMap(footprint[i].x, footprint[i].y, x0, y0)) { - return static_cast(LETHAL_OBSTACLE); - } // get the cell coord of the second point if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) { @@ -64,23 +69,22 @@ double FootprintCollisionChecker::footprintCost(const Footprint footpr } footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - } - // we also need to connect the first point in the footprint to the last point - // get the cell coord of the last point - if (!worldToMap(footprint.back().x, footprint.back().y, x0, y0)) { - return static_cast(LETHAL_OBSTACLE); - } + // the second point is next iteration's first point + x0 = x1; + y0 = y1; - // get the cell coord of the first point - if (!worldToMap(footprint.front().x, footprint.front().y, x1, y1)) { - return static_cast(LETHAL_OBSTACLE); + // if in collision, no need to continue + if (footprint_cost == static_cast(INSCRIBED_INFLATED_OBSTACLE) || + footprint_cost == static_cast(LETHAL_OBSTACLE)) + { + return footprint_cost; + } } - footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - - // if all line costs are legal... then we can return that the footprint is legal - return footprint_cost; + // we also need to connect the first point in the footprint to the last point + // the last iteration's x1, y1 are the last footprint point's coordinates + return std::max(lineCost(xstart, x1, ystart, y1), footprint_cost); } template @@ -95,6 +99,13 @@ double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int if (line_cost < point_cost) { line_cost = point_cost; } + + // if in collision, no need to continue + if (line_cost == static_cast(INSCRIBED_INFLATED_OBSTACLE) || + line_cost == static_cast(LETHAL_OBSTACLE)) + { + return line_cost; + } } return line_cost; diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index 0eacd5c3e63..e76f7de7bf8 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -91,7 +91,8 @@ Layer::declareParameter( void Layer::declareParameter( - const std::string & param_name) + const std::string & param_name, + const rclcpp::ParameterType & param_type) { auto node = node_.lock(); if (!node) { @@ -99,7 +100,7 @@ Layer::declareParameter( } local_params_.insert(param_name); nav2_util::declare_parameter_if_not_declared( - node, getFullName(param_name)); + node, getFullName(param_name), param_type); } bool diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index a03b79c0dcd..e31ebe1414c 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -53,7 +53,7 @@ namespace nav2_costmap_2d { LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) -: costmap_(), +: primary_costmap_(), combined_costmap_(), global_frame_(global_frame), rolling_window_(rolling_window), current_(false), @@ -71,9 +71,11 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo inscribed_radius_(0.1) { if (track_unknown) { - costmap_.setDefaultValue(255); + primary_costmap_.setDefaultValue(255); + combined_costmap_.setDefaultValue(255); } else { - costmap_.setDefaultValue(0); + primary_costmap_.setDefaultValue(0); + combined_costmap_.setDefaultValue(0); } } @@ -82,6 +84,9 @@ LayeredCostmap::~LayeredCostmap() while (plugins_.size() > 0) { plugins_.pop_back(); } + while (filters_.size() > 0) { + filters_.pop_back(); + } } void LayeredCostmap::resizeMap( @@ -90,34 +95,41 @@ void LayeredCostmap::resizeMap( double origin_y, bool size_locked) { - std::unique_lock lock(*(costmap_.getMutex())); + std::unique_lock lock(*(combined_costmap_.getMutex())); size_locked_ = size_locked; - costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); + primary_costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); + combined_costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y); for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { (*plugin)->matchSize(); } + for (vector>::iterator filter = filters_.begin(); + filter != filters_.end(); ++filter) + { + (*filter)->matchSize(); + } } bool LayeredCostmap::isOutofBounds(double robot_x, double robot_y) { unsigned int mx, my; - return !costmap_.worldToMap(robot_x, robot_y, mx, my); + return !combined_costmap_.worldToMap(robot_x, robot_y, mx, my); } void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) { // Lock for the remainder of this function, some plugins (e.g. VoxelLayer) // implement thread unsafe updateBounds() functions. - std::unique_lock lock(*(costmap_.getMutex())); + std::unique_lock lock(*(combined_costmap_.getMutex())); // if we're using a rolling buffer costmap... // we need to update the origin using the robot's position if (rolling_window_) { - double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2; - double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2; - costmap_.updateOrigin(new_origin_x, new_origin_y); + double new_origin_x = robot_x - combined_costmap_.getSizeInMetersX() / 2; + double new_origin_y = robot_y - combined_costmap_.getSizeInMetersY() / 2; + primary_costmap_.updateOrigin(new_origin_x, new_origin_y); + combined_costmap_.updateOrigin(new_origin_x, new_origin_y); } if (isOutofBounds(robot_x, robot_y)) { @@ -126,7 +138,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) "Robot is out of bounds of the costmap!"); } - if (plugins_.size() == 0) { + if (plugins_.size() == 0 && filters_.size() == 0) { return; } @@ -151,15 +163,33 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) (*plugin)->getName().c_str()); } } + for (vector>::iterator filter = filters_.begin(); + filter != filters_.end(); ++filter) + { + double prev_minx = minx_; + double prev_miny = miny_; + double prev_maxx = maxx_; + double prev_maxy = maxy_; + (*filter)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); + if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { + RCLCPP_WARN( + rclcpp::get_logger( + "nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " + "is now [tl: (%f, %f), br: (%f, %f)]. The offending filter is %s", + prev_minx, prev_miny, prev_maxx, prev_maxy, + minx_, miny_, maxx_, maxy_, + (*filter)->getName().c_str()); + } + } int x0, xn, y0, yn; - costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); - costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn); + combined_costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); + combined_costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn); x0 = std::max(0, x0); - xn = std::min(static_cast(costmap_.getSizeInCellsX()), xn + 1); + xn = std::min(static_cast(combined_costmap_.getSizeInCellsX()), xn + 1); y0 = std::max(0, y0); - yn = std::min(static_cast(costmap_.getSizeInCellsY()), yn + 1); + yn = std::min(static_cast(combined_costmap_.getSizeInCellsY()), yn + 1); RCLCPP_DEBUG( rclcpp::get_logger( @@ -169,11 +199,41 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) return; } - costmap_.resetMap(x0, y0, xn, yn); - for (vector>::iterator plugin = plugins_.begin(); - plugin != plugins_.end(); ++plugin) - { - (*plugin)->updateCosts(costmap_, x0, y0, xn, yn); + if (filters_.size() == 0) { + // If there are no filters enabled just update costmap sequentially by each plugin + combined_costmap_.resetMap(x0, y0, xn, yn); + for (vector>::iterator plugin = plugins_.begin(); + plugin != plugins_.end(); ++plugin) + { + (*plugin)->updateCosts(combined_costmap_, x0, y0, xn, yn); + } + } else { + // Costmap Filters enabled + // 1. Update costmap by plugins + primary_costmap_.resetMap(x0, y0, xn, yn); + for (vector>::iterator plugin = plugins_.begin(); + plugin != plugins_.end(); ++plugin) + { + (*plugin)->updateCosts(primary_costmap_, x0, y0, xn, yn); + } + + // 2. Copy processed costmap window to a final costmap. + // primary_costmap_ remain to be untouched for further usage by plugins. + if (!combined_costmap_.copyWindow(primary_costmap_, x0, y0, xn, yn, x0, y0)) { + RCLCPP_ERROR( + rclcpp::get_logger("nav2_costmap_2d"), + "Can not copy costmap (%i,%i)..(%i,%i) window", + x0, y0, xn, yn); + throw std::runtime_error{"Can not copy costmap"}; + } + + // 3. Apply filters over the plugins in order to make filters' work + // not being considered by plugins on next updateMap() calls + for (vector>::iterator filter = filters_.begin(); + filter != filters_.end(); ++filter) + { + (*filter)->updateCosts(combined_costmap_, x0, y0, xn, yn); + } } bx0_ = x0; @@ -192,6 +252,11 @@ bool LayeredCostmap::isCurrent() { current_ = current_ && (*plugin)->isCurrent(); } + for (vector>::iterator filter = filters_.begin(); + filter != filters_.end(); ++filter) + { + current_ = current_ && (*filter)->isCurrent(); + } return current_; } @@ -208,6 +273,12 @@ void LayeredCostmap::setFootprint(const std::vector & { (*plugin)->onFootprintChanged(); } + for (vector>::iterator filter = filters_.begin(); + filter != filters_.end(); + ++filter) + { + (*filter)->onFootprintChanged(); + } } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index 85708043b16..8a06823df38 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -53,9 +53,12 @@ ObservationBuffer::ObservationBuffer( std::string topic_name, double observation_keep_time, double expected_update_rate, - double min_obstacle_height, double max_obstacle_height, double obstacle_range, - double raytrace_range, tf2_ros::Buffer & tf2_buffer, std::string global_frame, - std::string sensor_frame, tf2::Duration tf_tolerance) + double min_obstacle_height, double max_obstacle_height, double obstacle_max_range, + double obstacle_min_range, + double raytrace_max_range, double raytrace_min_range, tf2_ros::Buffer & tf2_buffer, + std::string global_frame, + std::string sensor_frame, + tf2::Duration tf_tolerance) : tf2_buffer_(tf2_buffer), observation_keep_time_(rclcpp::Duration::from_seconds(observation_keep_time)), expected_update_rate_(rclcpp::Duration::from_seconds(expected_update_rate)), @@ -63,7 +66,9 @@ ObservationBuffer::ObservationBuffer( sensor_frame_(sensor_frame), topic_name_(topic_name), min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height), - obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance) + obstacle_max_range_(obstacle_max_range), obstacle_min_range_(obstacle_min_range), + raytrace_max_range_(raytrace_max_range), raytrace_min_range_( + raytrace_min_range), tf_tolerance_(tf_tolerance) { auto node = parent.lock(); clock_ = node->get_clock(); @@ -100,8 +105,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) // make sure to pass on the raytrace/obstacle range // of the observation buffer to the observations - observation_list_.front().raytrace_range_ = raytrace_range_; - observation_list_.front().obstacle_range_ = obstacle_range_; + observation_list_.front().raytrace_max_range_ = raytrace_max_range_; + observation_list_.front().raytrace_min_range_ = raytrace_min_range_; + observation_list_.front().obstacle_max_range_ = obstacle_max_range_; + observation_list_.front().obstacle_min_range_ = obstacle_min_range_; sensor_msgs::msg::PointCloud2 global_frame_cloud; diff --git a/nav2_costmap_2d/test/costmap_params.yaml b/nav2_costmap_2d/test/costmap_params.yaml index b402413b2e7..d3d9403b86e 100644 --- a/nav2_costmap_2d/test/costmap_params.yaml +++ b/nav2_costmap_2d/test/costmap_params.yaml @@ -15,9 +15,11 @@ mark_threshold: 0 #END VOXEL STUFF transform_tolerance: 0.3 -obstacle_range: 2.5 +obstacle_max_range: 2.5 +obstacle_min_range: 0.0 max_obstacle_height: 2.0 -raytrace_range: 3.0 +raytrace_max_range: 3.0 +raytrace_min_range: 0.0 footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] #robot_radius: 0.46 footprint_padding: 0.01 diff --git a/nav2_costmap_2d/test/integration/footprint_tests.cpp b/nav2_costmap_2d/test/integration/footprint_tests.cpp index 8f668e9b5c8..0cab6fb818a 100644 --- a/nav2_costmap_2d/test/integration/footprint_tests.cpp +++ b/nav2_costmap_2d/test/integration/footprint_tests.cpp @@ -40,7 +40,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/transform_listener.h" #include "nav2_costmap_2d/footprint.hpp" diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 82c7b9d3c41..e3619c070dd 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -328,7 +328,7 @@ TEST_F(TestNode, testInflationAroundUnkown) layers.updateMap(0, 0, 0); layers.getCostmap()->setCost(4, 4, nav2_costmap_2d::NO_INFORMATION); - ilayer->updateCosts(*layers.getCostmap(), 0, 0, 8, 8); + ilayer->updateCosts(*layers.getCostmap(), 0, 0, 10, 10); validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius); } diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index a635cd8e6a8..643a3007fe1 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -369,3 +369,41 @@ TEST_F(TestNode, testRepeatedResets) { plugin->reset(); })); } + + +/** + * Test for ray tracing free space + */ +TEST_F(TestNode, testRaytracing) { + tf2_ros::Buffer tf(node_->get_clock()); + + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + std::shared_ptr slayer = nullptr; + addStaticLayer(layers, tf, node_, slayer); + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2); + + // This actually puts the LETHAL (254) point in the costmap at (0,0) + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose + // printMap(*(layers.getCostmap())); + + int lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE); + + ASSERT_EQ(lethal_count, 1); + + addObservation(olayer, 1.0, 1.0, MAX_Z / 2, 0, 0, MAX_Z / 2, true, true, 100.0, 5.0, 100.0, 5.0); + + // This actually puts the LETHAL (254) point in the costmap at (0,0) + layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose + // printMap(*(layers.getCostmap())); + + // New observation should not be recorded as min_range is higher than obstacle range + lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE); + + ASSERT_EQ(lethal_count, 1); + +} diff --git a/nav2_costmap_2d/test/module_tests.cpp b/nav2_costmap_2d/test/module_tests.cpp index 8df74ac724f..989d81fe721 100644 --- a/nav2_costmap_2d/test/module_tests.cpp +++ b/nav2_costmap_2d/test/module_tests.cpp @@ -70,8 +70,10 @@ const double RESOLUTION(1); const double WINDOW_LENGTH(10); const unsigned char THRESHOLD(100); const double MAX_Z(1.0); -const double RAYTRACE_RANGE(20.0); -const double OBSTACLE_RANGE(20.0); +const double RAYTRACE_MAX_RANGE(20.0); +const double RAYTRACE_MIN_RANGE(3.0); +const double OBSTACLE_MAX_RANGE(20.0); +const double OBSTACLE_MIN_RANGE(0.0); const double ROBOT_RADIUS(1.0); bool find(const std::vector & l, unsigned int n) @@ -99,7 +101,8 @@ TEST(costmap, testResetForStaticMap) { // Allocate the cost map, with a inflation to 3 cells all around nav2_costmap_2d::Costmap2D map(10, 10, RESOLUTION, 0.0, 0.0, 3, 3, 3, - OBSTACLE_RANGE, MAX_Z, RAYTRACE_RANGE, 25, staticMap, THRESHOLD); + OBSTACLE_MAX_RANGE, OBSTACLE_MIN_RANGE, MAX_Z, RAYTRACE_MAX_RANGE, RAYTRACE_MIN_RANGE, 25, + staticMap, THRESHOLD); // Populate the cost map with a wall around the perimeter. Free space should clear out the room. pcl::PointCloud cloud; @@ -138,7 +141,9 @@ TEST(costmap, testResetForStaticMap) { p.x = wx; p.y = wy; p.z = MAX_Z; - nav2_costmap_2d::Observation obs(p, cloud, OBSTACLE_RANGE, RAYTRACE_RANGE); + nav2_costmap_2d::Observation obs(p, cloud, OBSTACLE_MAX_RANGE, OBSTACLE_MIN_RANGE, + RAYTRACE_MAX_RANGE, + RAYTRACE_MIN_RANGE); std::vector obsBuf; obsBuf.push_back(obs); diff --git a/nav2_costmap_2d/test/testing_helper.hpp b/nav2_costmap_2d/test/testing_helper.hpp index 06cdbe625f3..957b858452a 100644 --- a/nav2_costmap_2d/test/testing_helper.hpp +++ b/nav2_costmap_2d/test/testing_helper.hpp @@ -100,7 +100,11 @@ void addRangeLayer( void addObservation( std::shared_ptr olayer, double x, double y, double z = 0.0, - double ox = 0.0, double oy = 0.0, double oz = MAX_Z, bool marking = true, bool clearing = true) + double ox = 0.0, double oy = 0.0, double oz = MAX_Z, bool marking = true, bool clearing = true, + double raytrace_max_range = 100.0, + double raytrace_min_range = 0.0, + double obstacle_max_range = 100.0, + double obstacle_min_range = 0.0) { sensor_msgs::msg::PointCloud2 cloud; sensor_msgs::PointCloud2Modifier modifier(cloud); @@ -118,8 +122,8 @@ void addObservation( p.y = oy; p.z = oz; - // obstacle range = raytrace range = 100.0 - nav2_costmap_2d::Observation obs(p, cloud, 100.0, 100.0); + nav2_costmap_2d::Observation obs(p, cloud, obstacle_max_range, obstacle_min_range, + raytrace_max_range, raytrace_min_range); olayer->addStaticObservation(obs, marking, clearing); } diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 18b213d0576..7a1ec6a9098 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -29,3 +29,8 @@ target_link_libraries(speed_filter_test nav2_costmap_2d_core filters ) + +ament_add_gtest(copy_window_test copy_window_test.cpp) +target_link_libraries(copy_window_test + nav2_costmap_2d_core +) diff --git a/nav2_costmap_2d/test/unit/copy_window_test.cpp b/nav2_costmap_2d/test/unit/copy_window_test.cpp new file mode 100644 index 00000000000..0f4f9ebe0eb --- /dev/null +++ b/nav2_costmap_2d/test/unit/copy_window_test.cpp @@ -0,0 +1,52 @@ +// Copyright (c) 2021 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(CopyWindow, copyValidWindow) +{ + nav2_costmap_2d::Costmap2D src(10, 10, 0.1, 0.0, 0.0); + nav2_costmap_2d::Costmap2D dst(5, 5, 0.2, 100.0, 100.0); + // Adding 2 marked cells to source costmap + src.setCost(2, 2, 100); + src.setCost(5, 5, 200); + + ASSERT_TRUE(dst.copyWindow(src, 2, 2, 6, 6, 0, 0)); + // Check that both marked cells were copied to destination costmap + ASSERT_TRUE(dst.getCost(0, 0) == 100); + ASSERT_TRUE(dst.getCost(3, 3) == 200); +} + +TEST(CopyWindow, copyInvalidWindow) +{ + nav2_costmap_2d::Costmap2D src(10, 10, 0.1, 0.0, 0.0); + nav2_costmap_2d::Costmap2D dst(5, 5, 0.2, 100.0, 100.0); + + // Case1: incorrect source bounds + ASSERT_FALSE(dst.copyWindow(src, 9, 9, 11, 11, 0, 0)); + // Case2: incorrect destination bounds + ASSERT_FALSE(dst.copyWindow(src, 0, 0, 1, 1, 5, 5)); + ASSERT_FALSE(dst.copyWindow(src, 0, 0, 6, 6, 0, 0)); +} diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp index ae4649e4840..4edb6af29cd 100644 --- a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp +++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp @@ -36,7 +36,7 @@ class LayerWrapper : public nav2_costmap_2d::Layer bool isClearable() {return false;} }; -TEST(DeclareParameter, useValidParameter2Args) +TEST(DeclareParameter, useValidParameter) { LayerWrapper layer; nav2_util::LifecycleNode::SharedPtr node = @@ -55,7 +55,7 @@ TEST(DeclareParameter, useValidParameter2Args) } } -TEST(DeclareParameter, useValidParameter1Arg) +TEST(DeclareParameter, useInvalidParameter) { LayerWrapper layer; nav2_util::LifecycleNode::SharedPtr node = @@ -65,31 +65,11 @@ TEST(DeclareParameter, useValidParameter1Arg) layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); - layer.declareParameter("test2"); + layer.declareParameter("test2", rclcpp::PARAMETER_STRING); try { - node->set_parameter(rclcpp::Parameter("test_layer.test2", "test_val2")); std::string val = node->get_parameter("test_layer.test2").as_string(); - EXPECT_EQ(val, "test_val2"); - } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { - FAIL() << "test_layer.test2 parameter is not set"; - } -} - -TEST(DeclareParameter, useInvalidParameter1Arg) -{ - LayerWrapper layer; - nav2_util::LifecycleNode::SharedPtr node = - std::make_shared("test_node"); - tf2_ros::Buffer tf(node->get_clock()); - nav2_costmap_2d::LayeredCostmap layers("frame", false, false); - - layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); - - layer.declareParameter("test3"); - try { - std::string val = node->get_parameter("test_layer.test3").as_string(); - FAIL() << "Incorrectly handling test_layer.test3 parameters which was not set"; - } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + FAIL() << "Incorrectly handling test_layer.test2 parameter which was not set"; + } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { SUCCEED(); } } diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index 43648d90cb7..a92bd5e156b 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -223,7 +223,7 @@ class TestNode : public ::testing::Test uint8_t type, double base, double multiplier, double tr_x, double tr_y); void testOutOfMask(uint8_t type, double base, double multiplier); - void testIncorrectPercentage(uint8_t type, double base, double multiplier); + void testIncorrectLimits(uint8_t type, double base, double multiplier); void reset(); @@ -420,16 +420,20 @@ void TestNode::verifySpeedLimit( // expected_limit is being calculated by using float32 base and multiplier double expected_limit = cost * multiplier + base; if (type == nav2_costmap_2d::SPEED_FILTER_PERCENT) { - if (expected_limit < 0.0) { + if (expected_limit < 0.0 || expected_limit > 100.0) { expected_limit = nav2_costmap_2d::NO_SPEED_LIMIT; } - if (expected_limit > 100.0) { - expected_limit = 100.0; - } EXPECT_TRUE(speed_limit->percentage); EXPECT_TRUE(speed_limit->speed_limit >= 0.0); EXPECT_TRUE(speed_limit->speed_limit <= 100.0); EXPECT_NEAR(speed_limit->speed_limit, expected_limit, EPSILON); + } else if (type == nav2_costmap_2d::SPEED_FILTER_ABSOLUTE) { + if (expected_limit < 0.0) { + expected_limit = nav2_costmap_2d::NO_SPEED_LIMIT; + } + EXPECT_FALSE(speed_limit->percentage); + EXPECT_TRUE(speed_limit->speed_limit >= 0.0); + EXPECT_NEAR(speed_limit->speed_limit, expected_limit, EPSILON); } else { FAIL() << "The type of costmap filter is unknown"; } @@ -581,7 +585,7 @@ void TestNode::testOutOfMask(uint8_t type, double base, double multiplier) ASSERT_TRUE(speed_limit == old_speed_limit); } -void TestNode::testIncorrectPercentage(uint8_t type, double base, double multiplier) +void TestNode::testIncorrectLimits(uint8_t type, double base, double multiplier) { const int min_i = 0; const int min_j = 0; @@ -593,14 +597,14 @@ void TestNode::testIncorrectPercentage(uint8_t type, double base, double multipl std::vector> points; - // Some middle point corresponding to correct percentage + // Some middle point corresponding to correct speed limit value points.push_back(std::make_tuple(width_ / 2 - 1, height_ / 2 - 1)); - // (0, 1) point corresponding to incorrect percentage: data = 1, percentage < 0% + // (0, 1) point corresponding to incorrect limit value: data = 1, value < 0 points.push_back(std::make_tuple(0, 1)); - // Some middle point corresponding to correct percentage + // Some middle point corresponding to correct speed limit value points.push_back(std::make_tuple(width_ / 2 - 1, height_ / 2 - 1)); - // (width_ - 1, height_ - 1) point corresponding to incorrect percentage: - // data = 100, percentage > 100% + // (width_ - 1, height_ - 1) point corresponding to incorrect limit value: + // data = 100, value > 100 points.push_back(std::make_tuple(width_ - 1, height_ - 1)); for (auto it = points.begin(); it != points.end(); ++it) { @@ -650,7 +654,7 @@ TEST_F(TestNode, testIncorrectPercentSpeedLimit) EXPECT_TRUE(createSpeedFilter("map")); // Test SpeedFilter - testIncorrectPercentage(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0); + testIncorrectLimits(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0); // Clean-up speed_filter_->resetFilter(); @@ -661,8 +665,26 @@ TEST_F(TestNode, testAbsoluteSpeedLimit) { // Initilize test system createMaps("map"); - publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -1.23, 4.5); - EXPECT_FALSE(createSpeedFilter("map")); + publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5); + EXPECT_TRUE(createSpeedFilter("map")); + + // Test SpeedFilter + testFullMask(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5, NO_TRANSLATION, NO_TRANSLATION); + + // Clean-up + speed_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testIncorrectAbsoluteSpeedLimit) +{ + // Initilize test system + createMaps("map"); + publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -50.0, 2.0); + EXPECT_TRUE(createSpeedFilter("map")); + + // Test SpeedFilter + testIncorrectLimits(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -50.0, 2.0); // Clean-up speed_filter_->resetFilter(); @@ -688,7 +710,7 @@ TEST_F(TestNode, testInfoRePublish) { // Initilize test system createMaps("map"); - publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 1.2, 3.4); + publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5); EXPECT_TRUE(createSpeedFilter("map")); // Re-publish filter info (with incorrect base and multiplier) @@ -708,7 +730,7 @@ TEST_F(TestNode, testMaskRePublish) { // Initilize test system createMaps("map"); - publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.1, 0.2); + publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5); EXPECT_TRUE(createSpeedFilter("map")); // Re-publish filter mask and test that everything is working after @@ -716,7 +738,7 @@ TEST_F(TestNode, testMaskRePublish) // Test SpeedFilter testSimpleMask( - nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.1, 0.2, NO_TRANSLATION, NO_TRANSLATION); + nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5, NO_TRANSLATION, NO_TRANSLATION); // Clean-up speed_filter_->resetFilter(); @@ -727,7 +749,7 @@ TEST_F(TestNode, testIncorrectFilterType) { // Initilize test system createMaps("map"); - publishMaps(INCORRECT_TYPE, -1.23, 4.5); + publishMaps(INCORRECT_TYPE, 1.23, 4.5); EXPECT_FALSE(createSpeedFilter("map")); // Clean-up diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 5f075bfca20..92081c71267 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.3 + 1.0.0 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 3b58a37882d..c95de462381 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -93,11 +93,13 @@ class DWBLocalPlanner : public nav2_core::Controller * * @param pose Current robot pose * @param velocity Current robot velocity + * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands * @return The best command for the robot to drive */ geometry_msgs::msg::TwistStamped computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) override; + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * /*goal_checker*/) override; /** * @brief Score a given command. Can be used for testing. @@ -133,12 +135,15 @@ class DWBLocalPlanner : public nav2_core::Controller /** * @brief Limits the maximum linear speed of the robot. - * @param speed_limit expressed in percentage from maximum robot speed. + * @param speed_limit expressed in absolute value (in m/s) + * or in percentage from maximum robot speed. + * @param percentage Setting speed limit in percentage if true + * or in absolute values in false case. */ - void setSpeedLimit(const double & speed_limit) override + void setSpeedLimit(const double & speed_limit, const bool & percentage) override { if (traj_generator_) { - traj_generator_->setSpeedLimit(speed_limit); + traj_generator_->setSpeedLimit(speed_limit, percentage); } } @@ -171,6 +176,11 @@ class DWBLocalPlanner : public nav2_core::Controller * 3) If prune_plan_ is true, it will remove all points that we've already passed from both the transformed plan * and the saved global_plan_. Technically, it iterates to a pose on the path that is within prune_distance_ * of the robot and erases all poses before that. + * + * Additionally, shorten_transformed_plan_ determines whether we will pass the full plan all + * the way to the nav goal on to the critics or just a subset of the plan near the robot. + * True means pass just a subset. This gives DWB less discretion to decide how it gets to the + * nav goal. Instead it is encouraged to try to get on to the path generated by the global planner. */ virtual nav_2d_msgs::msg::Path2D transformGlobalPlan( const nav_2d_msgs::msg::Pose2DStamped & pose); @@ -179,6 +189,7 @@ class DWBLocalPlanner : public nav2_core::Controller double prune_distance_; bool debug_trajectory_details_; rclcpp::Duration transform_tolerance_{0, 0}; + bool shorten_transformed_plan_; /** * @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic" diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp index aba5354c652..bd2b210e033 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp @@ -124,7 +124,7 @@ class DWBPublisher std::shared_ptr> transformed_pub_; std::shared_ptr> local_pub_; std::shared_ptr> marker_pub_; - std::shared_ptr> cost_grid_pc_pub_; + std::shared_ptr> cost_grid_pc_pub_; rclcpp_lifecycle::LifecycleNode::WeakPtr node_; rclcpp::Clock::SharedPtr clock_; diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp index 2724bfd8a10..17899a29947 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -166,7 +167,7 @@ class TrajectoryCritic * * @param pc PointCloud to add channels to */ - virtual void addCriticVisualization(sensor_msgs::msg::PointCloud &) {} + virtual void addCriticVisualization(std::vector>> &) {} std::string getName() { diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp index 7965cd4b812..d918d61ddd2 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp @@ -126,9 +126,12 @@ class TrajectoryGenerator /** * @brief Limits the maximum linear speed of the robot. - * @param speed_limit expressed in percentage from maximum robot speed. + * @param speed_limit expressed in absolute value (in m/s) + * or in percentage from maximum robot speed. + * @param percentage Setting speed limit in percentage if true + * or in absolute values in false case. */ - virtual void setSpeedLimit(const double & speed_limit) = 0; + virtual void setSpeedLimit(const double & speed_limit, const bool & percentage) = 0; }; } // namespace dwb_core diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 76beb12cc3e..08e005440fd 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.3 + 1.0.0 TODO Carl Delsey BSD-3-Clause @@ -41,6 +41,7 @@ ament_lint_common ament_lint_auto + ament_cmake_gtest ament_cmake diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index c214922ed62..ab7baed0462 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -76,8 +76,12 @@ void DWBLocalPlanner::configure( costmap_ros_ = costmap_ros; tf_ = tf; dwb_plugin_name_ = name; - declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".critics"); - declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".default_critic_namespaces"); + declare_parameter_if_not_declared( + node, dwb_plugin_name_ + ".critics", + rclcpp::PARAMETER_STRING_ARRAY); + declare_parameter_if_not_declared( + node, dwb_plugin_name_ + ".default_critic_namespaces", + rclcpp::ParameterValue(std::vector())); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".prune_plan", rclcpp::ParameterValue(true)); @@ -93,6 +97,9 @@ void DWBLocalPlanner::configure( declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, dwb_plugin_name_ + ".shorten_transformed_plan", + rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", rclcpp::ParameterValue(true)); @@ -111,6 +118,7 @@ void DWBLocalPlanner::configure( node->get_parameter( dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", short_circuit_trajectory_evaluation_); + node->get_parameter(dwb_plugin_name_ + ".shorten_transformed_plan", shorten_transformed_plan_); pub_ = std::make_unique(node, dwb_plugin_name_); pub_->on_configure(); @@ -227,7 +235,8 @@ DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) geometry_msgs::msg::TwistStamped DWBLocalPlanner::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * /*goal_checker*/) { std::shared_ptr results = nullptr; if (pub_->shouldRecordEvaluation()) { @@ -474,18 +483,11 @@ DWBLocalPlanner::transformGlobalPlan( sq_transform_start_threshold = sq_dist_threshold; } - // Determines whether we will pass the full plan all the way to the nav goal on - // to the critics or just a subset of the plan near the robot. True means pass - // just a subset. This gives DWB less discretion to decide how it gets to the - // nav goal. Instead it is encouraged to try to get on to the path generated - // by the global planner. - bool shorten_transformed_plan = true; - // Set the maximum distance we'll include points after the part of the part of // the plan near the robot (the end of the plan). This determines the amount // of the plan passed on to the critics double sq_transform_end_threshold; - if (shorten_transformed_plan) { + if (shorten_transformed_plan_) { sq_transform_end_threshold = std::min(sq_dist_threshold, sq_prune_dist); } else { sq_transform_end_threshold = sq_dist_threshold; diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index 70adb22d819..7b860fef65a 100644 --- a/nav2_dwb_controller/dwb_core/src/publisher.cpp +++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp @@ -40,6 +40,7 @@ #include #include +#include "sensor_msgs/point_cloud2_iterator.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/node_utils.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" @@ -105,7 +106,7 @@ DWBPublisher::on_configure() transformed_pub_ = node->create_publisher("transformed_global_plan", 1); local_pub_ = node->create_publisher("local_plan", 1); marker_pub_ = node->create_publisher("marker", 1); - cost_grid_pc_pub_ = node->create_publisher("cost_cloud", 1); + cost_grid_pc_pub_ = node->create_publisher("cost_cloud", 1); double marker_lifetime = 0.0; node->get_parameter(plugin_name_ + ".marker_lifetime", marker_lifetime); @@ -256,7 +257,7 @@ DWBPublisher::publishCostGrid( if (!publish_cost_grid_pc_) {return;} - auto cost_grid_pc = std::make_unique(); + auto cost_grid_pc = std::make_unique(); cost_grid_pc->header.frame_id = costmap_ros->getGlobalFrameID(); cost_grid_pc->header.stamp = clock_->now(); @@ -264,38 +265,75 @@ DWBPublisher::publishCostGrid( double x_coord, y_coord; unsigned int size_x = costmap->getSizeInCellsX(); unsigned int size_y = costmap->getSizeInCellsY(); - cost_grid_pc->points.resize(size_x * size_y); - unsigned int i = 0; - for (unsigned int cy = 0; cy < size_y; cy++) { - for (unsigned int cx = 0; cx < size_x; cx++) { - costmap->mapToWorld(cx, cy, x_coord, y_coord); - cost_grid_pc->points[i].x = x_coord; - cost_grid_pc->points[i].y = y_coord; - i++; - } - } - sensor_msgs::msg::ChannelFloat32 totals; - totals.name = "total_cost"; - totals.values.resize(size_x * size_y, 0.0); + std::vector>> cost_channels; + std::vector total_cost(size_x * size_y, 0.0); for (TrajectoryCritic::Ptr critic : critics) { - unsigned int channel_index = cost_grid_pc->channels.size(); - critic->addCriticVisualization(*cost_grid_pc); - if (channel_index == cost_grid_pc->channels.size()) { + unsigned int channel_index = cost_channels.size(); + critic->addCriticVisualization(cost_channels); + if (channel_index == cost_channels.size()) { // No channels were added, so skip to next critic continue; } double scale = critic->getScale(); - for (i = 0; i < size_x * size_y; i++) { - totals.values[i] += cost_grid_pc->channels[channel_index].values[i] * scale; + for (unsigned int i = 0; i < size_x * size_y; i++) { + total_cost[i] += cost_channels[channel_index].second[i] * scale; + } + } + + cost_channels.push_back(std::make_pair("total_cost", total_cost)); + + cost_grid_pc->width = size_x * size_y; + cost_grid_pc->height = 1; + cost_grid_pc->fields.resize(3 + cost_channels.size()); // x,y,z, + cost channels + cost_grid_pc->is_dense = true; + cost_grid_pc->is_bigendian = false; + + int offset = 0; + for (size_t i = 0; i < cost_grid_pc->fields.size(); ++i, offset += 4) { + cost_grid_pc->fields[i].offset = offset; + cost_grid_pc->fields[i].count = 1; + cost_grid_pc->fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32; + if (i >= 3) { + cost_grid_pc->fields[i].name = cost_channels[i - 3].first; + } + } + + cost_grid_pc->fields[0].name = "x"; + cost_grid_pc->fields[1].name = "y"; + cost_grid_pc->fields[2].name = "z"; + + cost_grid_pc->point_step = offset; + cost_grid_pc->row_step = cost_grid_pc->point_step * cost_grid_pc->width; + cost_grid_pc->data.resize(cost_grid_pc->row_step * cost_grid_pc->height); + + std::vector> cost_grid_pc_iter; + + for (size_t i = 0; i < cost_grid_pc->fields.size(); ++i) { + sensor_msgs::PointCloud2Iterator iter(*cost_grid_pc, cost_grid_pc->fields[i].name); + cost_grid_pc_iter.push_back(iter); + } + + unsigned int j = 0; + for (unsigned int cy = 0; cy < size_y; cy++) { + for (unsigned int cx = 0; cx < size_x; cx++) { + costmap->mapToWorld(cx, cy, x_coord, y_coord); + *cost_grid_pc_iter[0] = x_coord; + *cost_grid_pc_iter[1] = y_coord; + *cost_grid_pc_iter[2] = 0.0; // z value + + for (size_t i = 3; i < cost_grid_pc_iter.size(); ++i) { + *cost_grid_pc_iter[i] = cost_channels[i - 3].second[j]; + ++cost_grid_pc_iter[i]; + } + ++cost_grid_pc_iter[0]; + ++cost_grid_pc_iter[1]; + ++cost_grid_pc_iter[2]; + j++; } } - cost_grid_pc->channels.push_back(totals); - // TODO(crdelsey): convert pc to pc2 - // sensor_msgs::msg::PointCloud2 cost_grid_pc2; - // convertPointCloudToPointCloud2(cost_grid_pc, cost_grid_pc2); cost_grid_pc_pub_->publish(std::move(cost_grid_pc)); } diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp index 5f33445995c..9bd4675ccb8 100644 --- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/base_obstacle.hpp @@ -35,6 +35,10 @@ #ifndef DWB_CRITICS__BASE_OBSTACLE_HPP_ #define DWB_CRITICS__BASE_OBSTACLE_HPP_ +#include +#include +#include + #include "dwb_core/trajectory_critic.hpp" namespace dwb_critics @@ -55,7 +59,8 @@ class BaseObstacleCritic : public dwb_core::TrajectoryCritic public: void onInit() override; double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override; - void addCriticVisualization(sensor_msgs::msg::PointCloud & pc) override; + void addCriticVisualization( + std::vector>> & cost_channels) override; /** * @brief Return the obstacle score for a particular pose diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp index 4d77e5019c0..611aad46445 100644 --- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/map_grid.hpp @@ -37,6 +37,9 @@ #include #include +#include +#include + #include "dwb_core/trajectory_critic.hpp" #include "costmap_queue/costmap_queue.hpp" @@ -61,7 +64,8 @@ class MapGridCritic : public dwb_core::TrajectoryCritic // Standard TrajectoryCritic Interface void onInit() override; double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override; - void addCriticVisualization(sensor_msgs::msg::PointCloud & pc) override; + void addCriticVisualization( + std::vector>> & cost_channels) override; double getScale() const override {return costmap_->getResolution() * 0.5 * scale_;} // Helper Functions diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index a1b190038a1..1d6a1668dc4 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.3 + 1.0.0 The dwb_critics package David V. Lu!! BSD-3-Clause @@ -23,6 +23,7 @@ ament_lint_common ament_lint_auto + ament_cmake_gtest ament_cmake diff --git a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp index e409b86ce4e..2dfa3d415a7 100644 --- a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp +++ b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp @@ -31,6 +31,9 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include +#include #include "dwb_critics/base_obstacle.hpp" #include "dwb_core/exceptions.hpp" @@ -92,22 +95,23 @@ bool BaseObstacleCritic::isValidCost(const unsigned char cost) cost != nav2_costmap_2d::NO_INFORMATION; } -void BaseObstacleCritic::addCriticVisualization(sensor_msgs::msg::PointCloud & pc) +void BaseObstacleCritic::addCriticVisualization( + std::vector>> & cost_channels) { - sensor_msgs::msg::ChannelFloat32 grid_scores; - grid_scores.name = name_; + std::pair> grid_scores; + grid_scores.first = name_; unsigned int size_x = costmap_->getSizeInCellsX(); unsigned int size_y = costmap_->getSizeInCellsY(); - grid_scores.values.resize(size_x * size_y); + grid_scores.second.resize(size_x * size_y); unsigned int i = 0; for (unsigned int cy = 0; cy < size_y; cy++) { for (unsigned int cx = 0; cx < size_x; cx++) { - grid_scores.values[i] = costmap_->getCost(cx, cy); + grid_scores.second[i] = costmap_->getCost(cx, cy); i++; } } - pc.channels.push_back(grid_scores); + cost_channels.push_back(grid_scores); } } // namespace dwb_critics diff --git a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp index c70f7125e4c..030e77cd637 100644 --- a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp +++ b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp @@ -35,6 +35,8 @@ #include "dwb_critics/map_grid.hpp" #include #include +#include +#include #include #include #include "dwb_core/exceptions.hpp" @@ -164,23 +166,24 @@ double MapGridCritic::scorePose(const geometry_msgs::msg::Pose2D & pose) return getScore(cell_x, cell_y); } -void MapGridCritic::addCriticVisualization(sensor_msgs::msg::PointCloud & pc) +void MapGridCritic::addCriticVisualization( + std::vector>> & cost_channels) { - sensor_msgs::msg::ChannelFloat32 grid_scores; - grid_scores.name = name_; + std::pair> grid_scores; + grid_scores.first = name_; nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); unsigned int size_x = costmap->getSizeInCellsX(); unsigned int size_y = costmap->getSizeInCellsY(); - grid_scores.values.resize(size_x * size_y); + grid_scores.second.resize(size_x * size_y); unsigned int i = 0; for (unsigned int cy = 0; cy < size_y; cy++) { for (unsigned int cx = 0; cx < size_x; cx++) { - grid_scores.values[i] = getScore(cx, cy); + grid_scores.second[i] = getScore(cx, cy); i++; } } - pc.channels.push_back(grid_scores); + cost_channels.push_back(grid_scores); } } // namespace dwb_critics diff --git a/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp b/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp index 7cf8de890bf..15f5dd70a6e 100644 --- a/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp +++ b/nav2_dwb_controller/dwb_critics/test/base_obstacle_test.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" @@ -141,8 +142,8 @@ TEST(BaseObstacle, CriticVisualization) costmap_ros->getCostmap()->setCost(10, 49, 24); costmap_ros->getCostmap()->setCost(45, 2, 12); - sensor_msgs::msg::PointCloud pointcloud; - critic->addCriticVisualization(pointcloud); + std::vector>> cost_channels; + critic->addCriticVisualization(cost_channels); unsigned int size_x = costmap_ros->getCostmap()->getSizeInCellsX(); unsigned int size_y = costmap_ros->getCostmap()->getSizeInCellsY(); @@ -150,7 +151,7 @@ TEST(BaseObstacle, CriticVisualization) // The values in the pointcloud should be equal to the values in the costmap for (unsigned int y = 0; y < size_y; y++) { for (unsigned int x = 0; x < size_x; x++) { - float pointValue = pointcloud.channels[0].values[y * size_y + x]; + float pointValue = cost_channels[0].second[y * size_y + x]; ASSERT_EQ(static_cast(pointValue), costmap_ros->getCostmap()->getCost(x, y)); } } diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 7b21d0881ef..5b146ed08fc 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.3 + 1.0.0 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index f578d102533..9f0e378ee91 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -113,7 +113,7 @@ class KinematicsHandler inline KinematicParameters getKinematics() {return *kinematics_.load();} - void setSpeedLimit(const double & speed_limit); + void setSpeedLimit(const double & speed_limit, const bool & percentage); using Ptr = std::shared_ptr; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp index 87b293683fa..87ba77baf02 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp @@ -70,12 +70,15 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator /** * @brief Limits the maximum linear speed of the robot. - * @param speed_limit expressed in percentage from maximum robot speed. + * @param speed_limit expressed in absolute value (in m/s) + * or in percentage from maximum robot speed. + * @param percentage Setting speed limit in percentage if true + * or in absolute values in false case. */ - void setSpeedLimit(const double & speed_limit) override + void setSpeedLimit(const double & speed_limit, const bool & percentage) override { if (kinematics_handler_) { - kinematics_handler_->setSpeedLimit(speed_limit); + kinematics_handler_->setSpeedLimit(speed_limit, percentage); } } diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 754402d6f9b..e9535c72709 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.3 + 1.0.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 51a843318cd..d5a0670e499 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -129,7 +129,8 @@ void KinematicsHandler::initialize( update_kinematics(kinematics); } -void KinematicsHandler::setSpeedLimit(const double & speed_limit) +void KinematicsHandler::setSpeedLimit( + const double & speed_limit, const bool & percentage) { KinematicParameters kinematics(*kinematics_.load()); @@ -140,11 +141,26 @@ void KinematicsHandler::setSpeedLimit(const double & speed_limit) kinematics.max_vel_y_ = kinematics.base_max_vel_y_; kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_; } else { - // Speed limit is expressed in % from maximum speed of robot - kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_ * speed_limit / 100.0; - kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * speed_limit / 100.0; - kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * speed_limit / 100.0; - kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * speed_limit / 100.0; + if (percentage) { + // Speed limit is expressed in % from maximum speed of robot + kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_ * speed_limit / 100.0; + kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * speed_limit / 100.0; + kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * speed_limit / 100.0; + kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * speed_limit / 100.0; + } else { + // Speed limit is expressed in absolute value + if (speed_limit < kinematics.base_max_speed_xy_) { + kinematics.max_speed_xy_ = speed_limit; + // Handling components and angular velocity changes: + // Max velocities are being changed in the same proportion + // as absolute linear speed changed in order to preserve + // robot moving trajectories to be the same after speed change. + const double ratio = speed_limit / kinematics.base_max_speed_xy_; + kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * ratio; + kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * ratio; + kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * ratio; + } + } } // Do not forget to update max_speed_xy_sq_ as well diff --git a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp index cc56b11f6c5..7591563370c 100644 --- a/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp @@ -51,10 +51,20 @@ void LimitedAccelGenerator::initialize( plugin_name_ = plugin_name; StandardTrajectoryGenerator::initialize(nh, plugin_name_); - nav2_util::declare_parameter_if_not_declared(nh, plugin_name + ".sim_period"); - - if (nh->get_parameter(plugin_name + ".sim_period", acceleration_time_)) { - } else { + try { + nav2_util::declare_parameter_if_not_declared( + nh, plugin_name + ".sim_period", rclcpp::PARAMETER_DOUBLE); + if (!nh->get_parameter(plugin_name + ".sim_period", acceleration_time_)) { + // This actually should never appear, since declare_parameter_if_not_declared() + // completed w/o exceptions guarantee that static parameter will be initialized + // with some value. However for reliability we should also process the case + // when get_parameter() will return a failure for some other reasons. + throw std::runtime_error("Failed to get 'sim_period' value"); + } + } catch (std::exception &) { + RCLCPP_WARN( + rclcpp::get_logger("LimitedAccelGenerator"), + "'sim_period' parameter is not set for %s", plugin_name.c_str()); double controller_frequency = nav_2d_utils::searchAndGetParam( nh, "controller_frequency", 20.0); if (controller_frequency > 0) { diff --git a/nav2_dwb_controller/dwb_plugins/test/speed_limit_test.cpp b/nav2_dwb_controller/dwb_plugins/test/speed_limit_test.cpp index f6f07ded491..ca102dae46a 100644 --- a/nav2_dwb_controller/dwb_plugins/test/speed_limit_test.cpp +++ b/nav2_dwb_controller/dwb_plugins/test/speed_limit_test.cpp @@ -103,7 +103,7 @@ TEST_F(TestNode, TestPercentLimit) EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON); // Set speed limit 30% from maximum robot speed - kh.setSpeedLimit(30); + kh.setSpeedLimit(30, true); // Update KinematicParameters values from KinematicsHandler kp = kh.getKinematics(); @@ -113,7 +113,39 @@ TEST_F(TestNode, TestPercentLimit) EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR * 0.3, EPSILON); // Restore maximum speed to its default - kh.setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT); + kh.setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, true); + + // Update KinematicParameters values from KinematicsHandler + kp = kh.getKinematics(); + EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X, EPSILON); + EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y, EPSILON); + EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA, EPSILON); + EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON); +} + +TEST_F(TestNode, TestAbsoluteLimit) +{ + dwb_plugins::KinematicsHandler kh; + kh.initialize(node_, NODE_NAME); + + dwb_plugins::KinematicParameters kp = kh.getKinematics(); + EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X, EPSILON); + EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y, EPSILON); + EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA, EPSILON); + EXPECT_NEAR(kp.getMaxSpeedXY(), MAX_VEL_LINEAR, EPSILON); + + // Set speed limit 35.0 m/s + kh.setSpeedLimit(35.0, false); + + // Update KinematicParameters values from KinematicsHandler + kp = kh.getKinematics(); + EXPECT_NEAR(kp.getMaxX(), MAX_VEL_X * 35.0 / MAX_VEL_LINEAR, EPSILON); + EXPECT_NEAR(kp.getMaxY(), MAX_VEL_Y * 35.0 / MAX_VEL_LINEAR, EPSILON); + EXPECT_NEAR(kp.getMaxTheta(), MAX_VEL_THETA * 35.0 / MAX_VEL_LINEAR, EPSILON); + EXPECT_NEAR(kp.getMaxSpeedXY(), 35.0, EPSILON); + + // Restore maximum speed to its default + kh.setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, false); // Update KinematicParameters values from KinematicsHandler kp = kh.getKinematics(); diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index d63553fc44b..52d09579a53 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 - 0.4.3 + 1.0.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 4b87fa5093d..1cb957c74a6 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 - 0.4.3 + 1.0.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/include/nav_2d_utils/tf_help.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp index 3b4687bd108..442eb36797e 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/tf_help.hpp @@ -41,7 +41,7 @@ #include "nav_2d_utils/conversions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_2d_msgs/msg/pose2_d_stamped.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace nav_2d_utils { diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 1e30a029ef3..e93a6f01f7a 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 - 0.4.3 + 1.0.0 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp index 78056fa1dea..85cae2cb699 100644 --- a/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp +++ b/nav2_dwb_controller/nav_2d_utils/src/conversions.cpp @@ -33,6 +33,11 @@ */ #include "nav_2d_utils/conversions.hpp" +#include +#include +#include +#include +#include #include #include #pragma GCC diagnostic push diff --git a/nav2_lifecycle_manager/README.md b/nav2_lifecycle_manager/README.md index d2112266b52..83b4a61c1b5 100644 --- a/nav2_lifecycle_manager/README.md +++ b/nav2_lifecycle_manager/README.md @@ -1,9 +1,9 @@ ### Background on lifecycle enabled nodes -Using ROS2’s managed/lifecycle nodes feature allows the system startup to ensure that all required nodes have been instantiated correctly before they begin their execution. Using lifecycle nodes also allows nodes to be restarted or replaced on-line. More details about managed nodes can be found on [ROS2 Design website](https://design.ros2.org/articles/node_lifecycle.html). Several nodes in the navigation2 stack, such as map_server, planner_server, and controller_server, are lifecycle enabled. These nodes provide the required overrides of the lifecycle functions: ```on_configure()```, ```on_activate()```, ```on_deactivate()```, ```on_cleanup()```, ```on_shutdown()```, and ```on_error()```. +Using ROS2’s managed/lifecycle nodes feature allows the system startup to ensure that all required nodes have been instantiated correctly before they begin their execution. Using lifecycle nodes also allows nodes to be restarted or replaced on-line. More details about managed nodes can be found on [ROS2 Design website](https://design.ros2.org/articles/node_lifecycle.html). Several nodes in Nav2, such as map_server, planner_server, and controller_server, are lifecycle enabled. These nodes provide the required overrides of the lifecycle functions: ```on_configure()```, ```on_activate()```, ```on_deactivate()```, ```on_cleanup()```, ```on_shutdown()```, and ```on_error()```. ### nav2_lifecycle_manager -Navigation2’s lifecycle manager is used to change the states of the lifecycle nodes in order to achieve a controlled _startup_, _shutdown_, _reset_, _pause_, or _resume_ of the navigation stack. The lifecycle manager presents a ```lifecycle_manager/manage_nodes``` service, from which clients can invoke the startup, shutdown, reset, pause, or resume functions. Based on this service request, the lifecycle manager calls the necessary lifecycle services in the lifecycle managed nodes. Currently, the RVIZ panel uses this ```lifecycle_manager/manage_nodes``` service when user presses the buttons on the RVIZ panel (e.g.,startup, reset, shutdown, etc.). +Nav2's lifecycle manager is used to change the states of the lifecycle nodes in order to achieve a controlled _startup_, _shutdown_, _reset_, _pause_, or _resume_ of the navigation stack. The lifecycle manager presents a ```lifecycle_manager/manage_nodes``` service, from which clients can invoke the startup, shutdown, reset, pause, or resume functions. Based on this service request, the lifecycle manager calls the necessary lifecycle services in the lifecycle managed nodes. Currently, the RVIZ panel uses this ```lifecycle_manager/manage_nodes``` service when user presses the buttons on the RVIZ panel (e.g.,startup, reset, shutdown, etc.). In order to start the navigation stack and be able to navigate, the necessary nodes must be configured and activated. Thus, for example when _startup_ is requested from the lifecycle manager's manage_nodes service, the lifecycle managers calls _configure()_ and _activate()_ on the lifecycle enabled nodes in the node list. diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index dc58026f2dc..c40ddfa5d02 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -36,7 +36,7 @@ using nav2_msgs::srv::ManageLifecycleNodes; /** * @class nav2_lifecycle_manager::LifecycleManager * @brief Implements service interface to transition the lifecycle nodes of - * Navigation2 stack. It receives transition request and then uses lifecycle + * Nav2 stack. It receives transition request and then uses lifecycle * interface to change lifecycle node's state. */ class LifecycleManager : public rclcpp::Node @@ -52,8 +52,7 @@ class LifecycleManager : public rclcpp::Node ~LifecycleManager(); protected: - // The ROS node to use when calling lifecycle services - rclcpp::Node::SharedPtr service_client_node_; + // The ROS node to create bond rclcpp::Node::SharedPtr bond_client_node_; std::unique_ptr bond_node_thread_; @@ -169,6 +168,7 @@ class LifecycleManager : public rclcpp::Node void message(const std::string & msg); // Timer thread to look at bond connections + rclcpp::TimerBase::SharedPtr init_timer_; rclcpp::TimerBase::SharedPtr bond_timer_; std::chrono::milliseconds bond_timeout_; diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index bb597fbd424..00e38ee82cd 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -26,6 +26,7 @@ #include "std_srvs/srv/empty.hpp" #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp" #include "std_srvs/srv/trigger.hpp" +#include "nav2_util/service_client.hpp" namespace nav2_lifecycle_manager { @@ -44,8 +45,21 @@ class LifecycleManagerClient public: /** * @brief A constructor for LifeCycleMangerClient + * @param name Managed node name + * @param ns Service node namespace */ - explicit LifecycleManagerClient(const std::string & name); + explicit LifecycleManagerClient( + const std::string & name, + const std::string & ns = ""); + + /** + * @brief A constructor for LifeCycleMangerClient + * @param name Managed node name + * @param parent_node Node that execute the service calls + */ + explicit LifecycleManagerClient( + const std::string & name, + std::shared_ptr parent_node); // Client-side interface to the Nav2 lifecycle manager /** @@ -110,8 +124,8 @@ class LifecycleManagerClient // The node to use for the service call rclcpp::Node::SharedPtr node_; - rclcpp::Client::SharedPtr manager_client_; - rclcpp::Client::SharedPtr is_active_client_; + std::shared_ptr> manager_client_; + std::shared_ptr> is_active_client_; std::string manage_service_name_; std::string active_service_name_; }; diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 4e8295a4cb4..e349e3af490 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.3 + 1.0.0 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 @@ -34,6 +34,8 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest + ament_cmake_pytest ament_cmake diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index dcf9aaa19ce..9ca4dc1ebba 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -38,7 +38,7 @@ LifecycleManager::LifecycleManager() // The list of names is parameterized, allowing this module to be used with a different set // of nodes - declare_parameter("node_names"); + declare_parameter("node_names", rclcpp::PARAMETER_STRING_ARRAY); declare_parameter("autostart", rclcpp::ParameterValue(false)); declare_parameter("bond_timeout", 4.0); @@ -57,11 +57,8 @@ LifecycleManager::LifecycleManager() get_name() + std::string("/is_active"), std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3)); - auto service_options = rclcpp::NodeOptions().arguments( - {"--ros-args", "-r", std::string("__node:=") + get_name() + "_service_client", "--"}); auto bond_options = rclcpp::NodeOptions().arguments( {"--ros-args", "-r", std::string("__node:=") + get_name() + "_bond_client", "--"}); - service_client_node_ = std::make_shared("_", service_options); bond_client_node_ = std::make_shared("_", bond_options); bond_node_thread_ = std::make_unique(bond_client_node_); @@ -79,11 +76,15 @@ LifecycleManager::LifecycleManager() transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] = std::string("Shutting down "); - createLifecycleServiceClients(); - - if (autostart_) { - startup(); - } + init_timer_ = this->create_wall_timer( + std::chrono::nanoseconds(10), + [this]() -> void { + init_timer_->cancel(); + createLifecycleServiceClients(); + if (autostart_) { + startup(); + } + }); } LifecycleManager::~LifecycleManager() @@ -131,7 +132,7 @@ LifecycleManager::createLifecycleServiceClients() message("Creating and initializing lifecycle service clients"); for (auto & node_name : node_names_) { node_map_[node_name] = - std::make_shared(node_name, service_client_node_); + std::make_shared(node_name, shared_from_this()); } } diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index c7931539f7b..2c71afb6068 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -19,24 +19,45 @@ #include #include -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_util/geometry_utils.hpp" namespace nav2_lifecycle_manager { using nav2_util::geometry_utils::orientationAroundZAxis; -LifecycleManagerClient::LifecycleManagerClient(const std::string & name) +LifecycleManagerClient::LifecycleManagerClient( + const std::string & name, + const std::string & ns) { manage_service_name_ = name + std::string("/manage_nodes"); active_service_name_ = name + std::string("/is_active"); // Create the node to use for all of the service clients - node_ = std::make_shared(name + "_service_client"); + node_ = std::make_shared(name + "_service_client", ns); // Create the service clients - manager_client_ = node_->create_client(manage_service_name_); - is_active_client_ = node_->create_client(active_service_name_); + manager_client_ = std::make_shared>( + manage_service_name_, node_); + is_active_client_ = std::make_shared>( + active_service_name_, node_); +} + +LifecycleManagerClient::LifecycleManagerClient( + const std::string & name, + std::shared_ptr parent_node) +{ + manage_service_name_ = name + std::string("/manage_nodes"); + active_service_name_ = name + std::string("/is_active"); + + // Use parent node for service call and logging + node_ = parent_node; + + // Create the service clients + manager_client_ = std::make_shared>( + manage_service_name_, node_); + is_active_client_ = std::make_shared>( + active_service_name_, node_); } bool @@ -73,27 +94,27 @@ SystemStatus LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) { auto request = std::make_shared(); + auto response = std::make_shared(); - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "Waiting for the %s service...", active_service_name_.c_str()); - if (!is_active_client_->wait_for_service(timeout)) { + if (!is_active_client_->wait_for_service(std::chrono::seconds(1))) { return SystemStatus::TIMEOUT; } - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "Sending %s request", active_service_name_.c_str()); - auto future_result = is_active_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { + try { + response = is_active_client_->invoke(request, timeout); + } catch (std::runtime_error &) { return SystemStatus::TIMEOUT; } - if (future_result.get()->success) { + if (response->success) { return SystemStatus::ACTIVE; } else { return SystemStatus::INACTIVE; @@ -106,30 +127,27 @@ LifecycleManagerClient::callService(uint8_t command, const std::chrono::nanoseco auto request = std::make_shared(); request->command = command; - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "Waiting for the %s service...", manage_service_name_.c_str()); - while (!manager_client_->wait_for_service(std::chrono::seconds(1))) { + while (!manager_client_->wait_for_service(timeout)) { if (!rclcpp::ok()) { RCLCPP_ERROR(node_->get_logger(), "Client interrupted while waiting for service to appear"); return false; } - RCLCPP_INFO(node_->get_logger(), "Waiting for service to appear..."); + RCLCPP_DEBUG(node_->get_logger(), "Waiting for service to appear..."); } - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "Sending %s request", manage_service_name_.c_str()); - auto future_result = manager_client_->async_send_request(request); - - if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { + try { + auto future_result = manager_client_->invoke(request, timeout); + return future_result->success; + } catch (std::runtime_error &) { return false; } - - return future_result.get()->success; } } // namespace nav2_lifecycle_manager diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index 8a81a2ddc8f..a5f87748e1c 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -1,6 +1,6 @@ # Map Server -The `Map Server` provides maps to the rest of the Navigation2 system using both topic and +The `Map Server` provides maps to the rest of the Nav2 system using both topic and service interfaces. ## Changes from ROS1 Navigation Map Server @@ -62,7 +62,7 @@ occupied_thresh: 0.65 free_thresh: 0.196 ``` -The Navigation2 software retains the map YAML file format from Nav1, but uses the ROS2 parameter +The Nav2 software retains the map YAML file format from Nav1, but uses the ROS2 parameter mechanism to get the name of the YAML file to use. This effectively introduces a level of indirection to get the map yaml filename. For example, for a node named 'map_server', the parameter file would look like this: diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index ce2a04b735d..d89039889ce 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.3 + 1.0.0 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/map_saver/main_cli.cpp b/nav2_map_server/src/map_saver/main_cli.cpp index 89c95e057ea..09cf039f441 100644 --- a/nav2_map_server/src/map_saver/main_cli.cpp +++ b/nav2_map_server/src/map_saver/main_cli.cpp @@ -108,10 +108,10 @@ ARGUMENTS_STATUS parse_arguments( save_parameters.map_file_name = *it; break; case COMMAND_FREE_THRESH: - save_parameters.free_thresh = atoi(it->c_str()); + save_parameters.free_thresh = atof(it->c_str()); break; case COMMAND_OCCUPIED_THRESH: - save_parameters.occupied_thresh = atoi(it->c_str()); + save_parameters.occupied_thresh = atof(it->c_str()); break; case COMMAND_IMAGE_FORMAT: save_parameters.image_format = *it; diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index a0b140463f2..fecd3f61967 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -68,7 +68,7 @@ MapServer::MapServer() RCLCPP_INFO(get_logger(), "Creating"); // Declare the node parameters - declare_parameter("yaml_filename"); + declare_parameter("yaml_filename", rclcpp::PARAMETER_STRING); declare_parameter("topic_name", "map"); declare_parameter("frame_id", "map"); } diff --git a/nav2_map_server/test/component/test_map_saver_publisher.cpp b/nav2_map_server/test/component/test_map_saver_publisher.cpp index 70f450523e9..9e69672cde7 100644 --- a/nav2_map_server/test/component/test_map_saver_publisher.cpp +++ b/nav2_map_server/test/component/test_map_saver_publisher.cpp @@ -35,7 +35,7 @@ class TestPublisher : public rclcpp::Node nav_msgs::msg::OccupancyGrid msg; LOAD_MAP_STATUS status = loadMapFromYaml(pub_map_file, msg); if (status != LOAD_MAP_SUCCESS) { - RCLCPP_ERROR(get_logger(), "Can not load %s map file", pub_map_file); + RCLCPP_ERROR(get_logger(), "Can not load %s map file", pub_map_file.c_str()); return; } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 0891844443b..80986c1a515 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -31,8 +31,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SaveMap.srv" "action/BackUp.action" "action/ComputePathToPose.action" + "action/ComputePathThroughPoses.action" "action/FollowPath.action" "action/NavigateToPose.action" + "action/NavigateThroughPoses.action" "action/Wait.action" "action/Spin.action" "action/DummyRecovery.action" diff --git a/nav2_msgs/README.md b/nav2_msgs/README.md index 0ca22a23401..b71a9c899f9 100644 --- a/nav2_msgs/README.md +++ b/nav2_msgs/README.md @@ -1,5 +1,5 @@ # nav2_msgs -The `nav2_msgs` package is a set of messages, services, and actions for the `navigation2` stack. The `navigation2` stack still makes use of `nav_msgs` from ROS (1) Navigation. +The `nav2_msgs` package is a set of messages, services, and actions for the `Nav2` system. `Nav2` still makes use of `nav_msgs` from ROS (1) Navigation. See the ROS 1 to ROS 2 [Migration Guide](https://index.ros.org/doc/ros2/Migration-Guide/#messages-and-services) for details about use of the new message and service types. diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action new file mode 100644 index 00000000000..abdf66867ff --- /dev/null +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -0,0 +1,11 @@ +#goal definition +geometry_msgs/PoseStamped[] goals +geometry_msgs/PoseStamped start +string planner_id +bool use_start # If true, use current robot pose as path start, if false, use start above instead +--- +#result definition +nav_msgs/Path path +builtin_interfaces/Duration planning_time +--- +#feedback diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index add60a824dc..090c28ccaf4 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,6 +1,8 @@ #goal definition -geometry_msgs/PoseStamped pose +geometry_msgs/PoseStamped goal +geometry_msgs/PoseStamped start string planner_id +bool use_start # If true, use current robot pose as path start, if false, use start above instead --- #result definition nav_msgs/Path path diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 8b9384492cd..0e935962844 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -1,6 +1,7 @@ #goal definition nav_msgs/Path path string controller_id +string goal_checker_id --- #result definition std_msgs/Empty result diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action new file mode 100644 index 00000000000..2a0d29e5a75 --- /dev/null +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -0,0 +1,13 @@ +#goal definition +geometry_msgs/PoseStamped[] poses +string behavior_tree +--- +#result definition +std_msgs/Empty result +--- +geometry_msgs/PoseStamped current_pose +builtin_interfaces/Duration navigation_time +builtin_interfaces/Duration estimated_time_remaining +int16 number_of_recoveries +float32 distance_remaining +int16 number_of_poses_remaining diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 792bee20ad1..a7b778f4d6a 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -7,5 +7,6 @@ std_msgs/Empty result --- geometry_msgs/PoseStamped current_pose builtin_interfaces/Duration navigation_time +builtin_interfaces/Duration estimated_time_remaining int16 number_of_recoveries float32 distance_remaining diff --git a/nav2_msgs/msg/CostmapFilterInfo.msg b/nav2_msgs/msg/CostmapFilterInfo.msg index c020d3d3a59..fb9d9af4ca7 100644 --- a/nav2_msgs/msg/CostmapFilterInfo.msg +++ b/nav2_msgs/msg/CostmapFilterInfo.msg @@ -2,7 +2,7 @@ std_msgs/Header header # Type of plugin used (keepout filter, speed limit in m/s, speed limit in percent, etc...) # 0: keepout/lanes filter # 1: speed limit filter in % of maximum speed -# 2: speed limit filter in absolute values +# 2: speed limit filter in absolute values (m/s) uint8 type # Name of filter mask topic string filter_mask_topic diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 00265980c7d..c9e90e6fbaf 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,8 +2,8 @@ nav2_msgs - 0.4.3 - Messages and service files for the navigation2 stack + 1.0.0 + Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski Carlos Orduno diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index c6a4fa9699d..a4ce84404a8 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -219,6 +219,10 @@ class NavFn */ void updateCellAstar(int n); + /** + * @brief Set up navigation potential arrays for new propagation + * @param keepit whether or not use COST_NEUTRAL + */ void setupNavFn(bool keepit = false); /** @@ -253,7 +257,13 @@ class NavFn */ int calcPath(int n, int * st = NULL); + /** + * @brief Calculate gradient at a cell + * @param n Cell number + * @return float norm + */ float gradCell(int n); /**< calculates gradient at cell , returns norm */ + float pathStep; /**< step size for following gradient */ /** display callback */ diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index 5f2a8613259..98180cdd17a 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -37,52 +37,100 @@ namespace nav2_navfn_planner class NavfnPlanner : public nav2_core::GlobalPlanner { public: + /** + * @brief constructor + */ NavfnPlanner(); + + /** + * @brief destructor + */ ~NavfnPlanner(); - // plugin configure + /** + * @brief Configuring plugin + * @param parent Lifecycle node pointer + * @param name Name of plugin map + * @param tf Shared ptr of TF2 buffer + * @param costmap_ros Costmap2DROS object + */ void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, std::shared_ptr tf, std::shared_ptr costmap_ros) override; - // plugin cleanup + /** + * @brief Cleanup lifecycle node + */ void cleanup() override; - // plugin activate + /** + * @brief Activate lifecycle node + */ void activate() override; - // plugin deactivate + /** + * @brief Deactivate lifecycle node + */ void deactivate() override; - // plugin create path + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @return nav_msgs::Path of the generated path + */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; protected: - // Compute a plan given start and goal poses, provided in global world frame. + /** + * @brief Compute a plan given start and goal poses, provided in global world frame. + * @param start Start pose + * @param goal Goal pose + * @param tolerance Relaxation constraint in x and y + * @param plan Path to be computed + * @return true if can find the path + */ bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, nav_msgs::msg::Path & plan); - // Compute the navigation function given a seed point in the world to start from + /** + * @brief Compute the navigation function given a seed point in the world to start from + * @param world_point Point in world coordinate frame + * @return true if can compute + */ bool computePotential(const geometry_msgs::msg::Point & world_point); - // Compute a plan to a goal from a potential - must call computePotential first + /** + * @brief Compute a plan to a goal from a potential - must call computePotential first + * @param goal Goal pose + * @param plan Path to be computed + * @return true if can compute a plan path + */ bool getPlanFromPotential( const geometry_msgs::msg::Pose & goal, nav_msgs::msg::Path & plan); - // Remove artifacts at the end of the path - originated from planning on a discretized world + /** + * @brief Remove artifacts at the end of the path - originated from planning on a discretized world + * @param goal Goal pose + * @param plan Computed path + */ void smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, nav_msgs::msg::Path & plan); - // Compute the potential, or navigation cost, at a given point in the world - // - must call computePotential first + /** + * @brief Compute the potential, or navigation cost, at a given point in the world + * must call computePotential first + * @param world_point Point in world coordinate frame + * @return double point potential (navigation cost) + */ double getPointPotential(const geometry_msgs::msg::Point & world_point); // Check for a valid potential value at a given point in the world @@ -91,7 +139,12 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // bool validPointPotential(const geometry_msgs::msg::Point & world_point); // bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance); - // Compute the squared distance between two points + /** + * @brief Compute the squared distance between two points + * @param p1 Point 1 + * @param p2 Point 2 + * @return double squared distance between two points + */ inline double squared_distance( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2) @@ -101,16 +154,36 @@ class NavfnPlanner : public nav2_core::GlobalPlanner return dx * dx + dy * dy; } - // Transform a point from world to map frame + /** + * @brief Transform a point from world to map frame + * @param wx double of world X coordinate + * @param wy double of world Y coordinate + * @param mx int of map X coordinate + * @param my int of map Y coordinate + * @return true if can transform + */ bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); - // Transform a point from map to world frame + /** + * @brief Transform a point from map to world frame + * @param mx double of map X coordinate + * @param my double of map Y coordinate + * @param wx double of world X coordinate + * @param wy double of world Y coordinate + */ void mapToWorld(double mx, double my, double & wx, double & wy); - // Set the corresponding cell cost to be free space + /** + * @brief Set the corresponding cell cost to be free space + * @param mx int of map X coordinate + * @param my int of map Y coordinate + */ void clearRobotCell(unsigned int mx, unsigned int my); - // Determine if a new planner object should be made + /** + * @brief Determine if a new planner object should be made + * @return true if planner object is out of date + */ bool isPlannerOutOfDate(); // Planner based on ROS1 NavFn algorithm @@ -140,6 +213,16 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // Whether to use the astar planner or default dijkstras bool use_astar_; + + // Subscription for parameter change + rclcpp::AsyncParametersClient::SharedPtr parameters_client_; + rclcpp::Subscription::SharedPtr parameter_event_sub_; + + /** + * @brief Callback executed when a paramter change is detected + * @param event ParameterEvent message + */ + void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); }; } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 0776fa8bb04..6ef9dcce8f8 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.3 + 1.0.0 TODO Steve Macenski Carlos Orduno diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index f741bf799c0..4cdaab3b73b 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -40,6 +40,8 @@ using namespace std::chrono_literals; using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; namespace nav2_navfn_planner { @@ -88,6 +90,16 @@ NavfnPlanner::configure( planner_ = std::make_unique( costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + + // Setup callback for changes to parameters. + parameters_client_ = std::make_shared( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + parameter_event_sub_ = parameters_client_->on_parameter_event( + std::bind(&NavfnPlanner::on_parameter_event_callback, this, _1)); } void @@ -459,6 +471,29 @@ NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my) costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE); } +void +NavfnPlanner::on_parameter_event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + for (auto & changed_parameter : event->changed_parameters) { + const auto & type = changed_parameter.value.type; + const auto & name = changed_parameter.name; + const auto & value = changed_parameter.value; + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + ".tolerance") { + tolerance_ = value.double_value; + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == name_ + ".use_astar") { + use_astar_ = value.bool_value; + } else if (name == name_ + ".allow_unknown") { + allow_unknown_ = value.bool_value; + } + } + } +} + } // namespace nav2_navfn_planner #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 175b69165c6..079c958091f 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -26,6 +26,7 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/compute_path_to_pose.hpp" +#include "nav2_msgs/action/compute_path_through_poses.hpp" #include "nav2_msgs/msg/costmap.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/simple_action_server.hpp" @@ -101,17 +102,102 @@ class PlannerServer : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; - using ActionT = nav2_msgs::action::ComputePathToPose; - using ActionServer = nav2_util::SimpleActionServer; + using ActionToPose = nav2_msgs::action::ComputePathToPose; + using ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses; + using ActionServerToPose = nav2_util::SimpleActionServer; + using ActionServerThroughPoses = nav2_util::SimpleActionServer; + + /** + * @brief Check if an action server is valid / active + * @param action_server Action server to test + * @return SUCCESS or FAILURE + */ + template + bool isServerInactive(std::unique_ptr> & action_server); + + /** + * @brief Check if an action server has a cancellation request pending + * @param action_server Action server to test + * @return SUCCESS or FAILURE + */ + template + bool isCancelRequested(std::unique_ptr> & action_server); + + /** + * @brief Wait for costmap to be valid with updated sensor data or repopulate after a + * clearing recovery. Blocks until true without timeout. + */ + void waitForCostmap(); + + /** + * @brief Check if an action server has a preemption request and replaces the goal + * with the new preemption goal. + * @param action_server Action server to get updated goal if required + * @param goal Goal to overwrite + */ + template + void getPreemptedGoalIfRequested( + std::unique_ptr> & action_server, + typename std::shared_ptr goal); + + /** + * @brief Get the starting pose from costmap or message, if valid + * @param action_server Action server to terminate if required + * @param goal Goal to find start from + * @param start The starting pose to use + * @return bool If successful in finding a valid starting pose + */ + template + bool getStartPose( + std::unique_ptr> & action_server, + typename std::shared_ptr goal, + geometry_msgs::msg::PoseStamped & start); + + /** + * @brief Transform start and goal poses into the costmap + * global frame for path planning plugins to utilize + * @param action_server Action server to terminate if required + * @param start The starting pose to transform + * @param goal Goal pose to transform + * @return bool If successful in transforming poses + */ + template + bool transformPosesToGlobalFrame( + std::unique_ptr> & action_server, + geometry_msgs::msg::PoseStamped & curr_start, + geometry_msgs::msg::PoseStamped & curr_goal); + + /** + * @brief Validate that the path contains a meaningful path + * @param action_server Action server to terminate if required + * @param goal Goal Current goal + * @param path Current path + * @param planner_id The planner ID used to generate the path + * @return bool If path is valid + */ + template + bool validatePath( + std::unique_ptr> & action_server, + const geometry_msgs::msg::PoseStamped & curr_goal, + const nav_msgs::msg::Path & path, + const std::string & planner_id); // Our action server implements the ComputePathToPose action - std::unique_ptr action_server_; + std::unique_ptr action_server_pose_; + std::unique_ptr action_server_poses_; /** * @brief The action server callback which calls planner to get the path + * ComputePathToPose */ void computePlan(); + /** + * @brief The action server callback which calls planner to get the path + * ComputePathThroughPoses + */ + void computePlanThroughPoses(); + /** * @brief Publish a path for visualization purposes * @param path Reference to Global Path diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index ade94a94f43..e75e383729b 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.3 + 1.0.0 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index f7bd83ea153..87e565bff96 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -129,12 +130,17 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) // Initialize pubs & subs plan_publisher_ = create_publisher("plan", 1); - // Create the action server that we implement with our navigateToPose method - action_server_ = std::make_unique( + // Create the action servers for path planning to a pose and through poses + action_server_pose_ = std::make_unique( rclcpp_node_, "compute_path_to_pose", std::bind(&PlannerServer::computePlan, this)); + action_server_poses_ = std::make_unique( + rclcpp_node_, + "compute_path_through_poses", + std::bind(&PlannerServer::computePlanThroughPoses, this)); + return nav2_util::CallbackReturn::SUCCESS; } @@ -144,7 +150,8 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & state) RCLCPP_INFO(get_logger(), "Activating"); plan_publisher_->on_activate(); - action_server_->activate(); + action_server_pose_->activate(); + action_server_poses_->activate(); costmap_ros_->on_activate(state); PlannerMap::iterator it; @@ -163,7 +170,8 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Deactivating"); - action_server_->deactivate(); + action_server_pose_->deactivate(); + action_server_poses_->deactivate(); plan_publisher_->on_deactivate(); costmap_ros_->on_deactivate(state); @@ -183,7 +191,8 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Cleaning up"); - action_server_.reset(); + action_server_pose_.reset(); + action_server_poses_.reset(); plan_publisher_.reset(); tf_.reset(); costmap_ros_->on_cleanup(state); @@ -205,59 +214,232 @@ PlannerServer::on_shutdown(const rclcpp_lifecycle::State &) return nav2_util::CallbackReturn::SUCCESS; } +template +bool PlannerServer::isServerInactive( + std::unique_ptr> & action_server) +{ + if (action_server == nullptr || !action_server->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); + return true; + } + + return false; +} + +void PlannerServer::waitForCostmap() +{ + // Don't compute a plan until costmap is valid (after clear costmap) + rclcpp::Rate r(100); + while (!costmap_ros_->isCurrent()) { + r.sleep(); + } +} + +template +bool PlannerServer::isCancelRequested( + std::unique_ptr> & action_server) +{ + if (action_server->is_cancel_requested()) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); + action_server->terminate_all(); + return true; + } + + return false; +} + +template +void PlannerServer::getPreemptedGoalIfRequested( + std::unique_ptr> & action_server, + typename std::shared_ptr goal) +{ + if (action_server->is_preempt_requested()) { + goal = action_server->accept_pending_goal(); + } +} + +template +bool PlannerServer::getStartPose( + std::unique_ptr> & action_server, + typename std::shared_ptr goal, + geometry_msgs::msg::PoseStamped & start) +{ + if (goal->use_start) { + start = goal->start; + } else if (!costmap_ros_->getRobotPose(start)) { + action_server->terminate_current(); + return false; + } + + return true; +} + +template +bool PlannerServer::transformPosesToGlobalFrame( + std::unique_ptr> & action_server, + geometry_msgs::msg::PoseStamped & curr_start, + geometry_msgs::msg::PoseStamped & curr_goal) +{ + if (!costmap_ros_->transformPoseToGlobalFrame(curr_start, curr_start) || + !costmap_ros_->transformPoseToGlobalFrame(curr_goal, curr_goal)) + { + RCLCPP_WARN( + get_logger(), "Could not transform the start or goal pose in the costmap frame"); + action_server->terminate_current(); + return false; + } + + return true; +} + +template +bool PlannerServer::validatePath( + std::unique_ptr> & action_server, + const geometry_msgs::msg::PoseStamped & goal, + const nav_msgs::msg::Path & path, + const std::string & planner_id) +{ + if (path.poses.size() == 0) { + RCLCPP_WARN( + get_logger(), "Planning algorithm %s failed to generate a valid" + " path to (%.2f, %.2f)", planner_id.c_str(), + goal.pose.position.x, goal.pose.position.y); + action_server->terminate_current(); + return false; + } + + RCLCPP_DEBUG( + get_logger(), + "Found valid path of size %lu to (%.2f, %.2f)", + path.poses.size(), goal.pose.position.x, + goal.pose.position.y); + + return true; +} + void -PlannerServer::computePlan() +PlannerServer::computePlanThroughPoses() { auto start_time = steady_clock_.now(); // Initialize the ComputePathToPose goal and result - auto goal = action_server_->get_current_goal(); - auto result = std::make_shared(); + auto goal = action_server_poses_->get_current_goal(); + auto result = std::make_shared(); + nav_msgs::msg::Path concat_path; try { - if (action_server_ == nullptr || !action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); + if (isServerInactive(action_server_poses_) || isCancelRequested(action_server_poses_)) { return; } - if (action_server_->is_cancel_requested()) { - RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); - action_server_->terminate_all(); - return; - } + waitForCostmap(); + + getPreemptedGoalIfRequested(action_server_poses_, goal); - // Don't compute a plan until costmap is valid (after clear costmap) - rclcpp::Rate r(100); - while (!costmap_ros_->isCurrent()) { - r.sleep(); + if (goal->goals.size() == 0) { + RCLCPP_WARN( + get_logger(), + "Compute path through poses requested a plan with no viapoint poses, returning."); + action_server_poses_->terminate_current(); } + // Use start pose if provided otherwise use current robot pose geometry_msgs::msg::PoseStamped start; - if (!costmap_ros_->getRobotPose(start)) { - action_server_->terminate_current(); + if (!getStartPose(action_server_poses_, goal, start)) { return; } - if (action_server_->is_preempt_requested()) { - goal = action_server_->accept_pending_goal(); + // Get consecutive paths through these points + std::vector::iterator goal_iter; + geometry_msgs::msg::PoseStamped curr_start, curr_goal; + for (unsigned int i = 0; i != goal->goals.size(); i++) { + // Get starting point + if (i == 0) { + curr_start = start; + } else { + curr_start = goal->goals[i - 1]; + } + curr_goal = goal->goals[i]; + + // Transform them into the global frame + if (!transformPosesToGlobalFrame(action_server_poses_, curr_start, curr_goal)) { + return; + } + + // Get plan from start -> goal + nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + + // check path for validity + if (!validatePath(action_server_poses_, curr_goal, curr_path, goal->planner_id)) { + return; + } + + // Concatenate paths together + concat_path.poses.insert( + concat_path.poses.end(), curr_path.poses.begin(), curr_path.poses.end()); + concat_path.header = curr_path.header; } - result->path = getPlan(start, goal->pose, goal->planner_id); + // Publish the plan for visualization purposes + result->path = concat_path; + publishPlan(result->path); - if (result->path.poses.size() == 0) { + auto cycle_duration = steady_clock_.now() - start_time; + result->planning_time = cycle_duration; + + if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { RCLCPP_WARN( - get_logger(), "Planning algorithm %s failed to generate a valid" - " path to (%.2f, %.2f)", goal->planner_id.c_str(), - goal->pose.pose.position.x, goal->pose.pose.position.y); - action_server_->terminate_current(); - return; + get_logger(), + "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", + 1 / max_planner_duration_, 1 / cycle_duration.seconds()); } - RCLCPP_DEBUG( + action_server_poses_->succeeded_current(result); + } catch (std::exception & ex) { + RCLCPP_WARN( get_logger(), - "Found valid path of size %u to (%.2f, %.2f)", - result->path.poses.size(), goal->pose.pose.position.x, - goal->pose.pose.position.y); + "%s plugin failed to plan through %li points with final goal (%.2f, %.2f): \"%s\"", + goal->planner_id.c_str(), goal->goals.size(), goal->goals.back().pose.position.x, + goal->goals.back().pose.position.y, ex.what()); + action_server_poses_->terminate_current(); + } +} + +void +PlannerServer::computePlan() +{ + auto start_time = steady_clock_.now(); + + // Initialize the ComputePathToPose goal and result + auto goal = action_server_pose_->get_current_goal(); + auto result = std::make_shared(); + + try { + if (isServerInactive(action_server_pose_) || isCancelRequested(action_server_pose_)) { + return; + } + + waitForCostmap(); + + getPreemptedGoalIfRequested(action_server_pose_, goal); + + // Use start pose if provided otherwise use current robot pose + geometry_msgs::msg::PoseStamped start; + if (!getStartPose(action_server_pose_, goal, start)) { + return; + } + + // Transform them into the global frame + geometry_msgs::msg::PoseStamped goal_pose = goal->goal; + if (!transformPosesToGlobalFrame(action_server_pose_, start, goal_pose)) { + return; + } + + result->path = getPlan(start, goal_pose, goal->planner_id); + + if (!validatePath(action_server_pose_, goal_pose, result->path, goal->planner_id)) { + return; + } // Publish the plan for visualization purposes publishPlan(result->path); @@ -272,15 +454,13 @@ PlannerServer::computePlan() 1 / max_planner_duration_, 1 / cycle_duration.seconds()); } - action_server_->succeeded_current(result); + action_server_pose_->succeeded_current(result); } catch (std::exception & ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", - goal->planner_id.c_str(), goal->pose.pose.position.x, - goal->pose.pose.position.y, ex.what()); - // TODO(orduno): provide information about fail error to parent task, - // for example: couldn't get costmap update - action_server_->terminate_current(); + goal->planner_id.c_str(), goal->goal.pose.position.x, + goal->goal.pose.position.y, ex.what()); + action_server_pose_->terminate_current(); } } diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index edf7dbff0ff..43dba83b263 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -43,12 +43,19 @@ enum class Status : int8_t using namespace std::chrono_literals; //NOLINT +/** + * @class nav2_recoveries::Recovery + * @brief An action server recovery base class implementing the action server and basic factory. + */ template class Recovery : public nav2_core::Recovery { public: using ActionServer = nav2_util::SimpleActionServer; + /** + * @brief A Recovery constructor + */ Recovery() : action_server_(nullptr), cycle_frequency_(10.0), @@ -85,6 +92,7 @@ class Recovery : public nav2_core::Recovery { } + // configure the server on lifecycle setup void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & name, std::shared_ptr tf, @@ -116,6 +124,7 @@ class Recovery : public nav2_core::Recovery onConfigure(); } + // Cleanup server on lifecycle transition void cleanup() override { action_server_.reset(); @@ -123,6 +132,7 @@ class Recovery : public nav2_core::Recovery onCleanup(); } + // Activate server on lifecycle transition void activate() override { RCLCPP_INFO(logger_, "Activating %s", recovery_name_.c_str()); @@ -132,6 +142,7 @@ class Recovery : public nav2_core::Recovery enabled_ = true; } + // Deactivate server on lifecycle transition void deactivate() override { vel_pub_->on_deactivate(); @@ -160,6 +171,8 @@ class Recovery : public nav2_core::Recovery // Logger rclcpp::Logger logger_{rclcpp::get_logger("nav2_recoveries")}; + // Main execution callbacks for the action server implementation calling the recovery's + // onRun and cycle functions to execute a specific behavior void execute() { RCLCPP_INFO(logger_, "Attempting %s", recovery_name_.c_str()); @@ -244,6 +257,7 @@ class Recovery : public nav2_core::Recovery } } + // Stop the robot with a commanded velocity void stopRobot() { auto cmd_vel = std::make_unique(); diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index 190a150d9aa..d6350013a02 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -31,20 +31,49 @@ namespace recovery_server { +/** + * @class recovery_server::RecoveryServer + * @brief An server hosting a map of recovery plugins + */ class RecoveryServer : public nav2_util::LifecycleNode { public: + /** + * @brief A constructor for recovery_server::RecoveryServer + */ RecoveryServer(); ~RecoveryServer(); + /** + * @brief Loads recovery plugins from parameter file + * @return bool if successfully loaded the plugins + */ bool loadRecoveryPlugins(); protected: - // Implement the lifecycle interface + /** + * @brief Configure lifecycle server + */ nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Activate lifecycle server + */ nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Deactivate lifecycle server + */ nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Cleanup lifecycle server + */ nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + + /** + * @brief Shutdown lifecycle server + */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; std::shared_ptr tf_; diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 1c8f25d39d8..ca8e0b0af92 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.3 + 1.0.0 TODO Carlos Orduno Steve Macenski diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 91baa7ba823..9fcb592a6f0 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -108,7 +108,7 @@ Status BackUp::onCycleUpdate() if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) { stopRobot(); RCLCPP_WARN(logger_, "Collision Ahead - Exiting BackUp"); - return Status::SUCCEEDED; + return Status::FAILED; } vel_pub_->publish(std::move(cmd_vel)); @@ -126,11 +126,12 @@ bool BackUp::isCollisionFree( double sim_position_change; const double diff_dist = abs(command_x_) - distance; const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); + geometry_msgs::msg::Pose2D init_pose = pose2d; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / cycle_frequency_); - pose2d.x += sim_position_change * cos(pose2d.theta); - pose2d.y += sim_position_change * sin(pose2d.theta); + pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta); + pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta); cycle_count++; if (diff_dist - abs(sim_position_change) <= 0.) { diff --git a/nav2_recoveries/plugins/back_up.hpp b/nav2_recoveries/plugins/back_up.hpp index ac219e65a7d..f566f2e8688 100644 --- a/nav2_recoveries/plugins/back_up.hpp +++ b/nav2_recoveries/plugins/back_up.hpp @@ -25,22 +25,48 @@ namespace nav2_recoveries { using BackUpAction = nav2_msgs::action::BackUp; +/** + * @class nav2_recoveries::BackUp + * @brief An action server recovery for spinning in + */ class BackUp : public Recovery { public: + /** + * @brief A constructor for nav2_recoveries::BackUp + */ BackUp(); ~BackUp(); + /** + * @brief Initialization to run behavior + * @param command Goal to execute + * @return Status of recovery + */ Status onRun(const std::shared_ptr command) override; + /** + * @brief Loop function to run behavior + * @return Status of recovery + */ Status onCycleUpdate() override; protected: + /** + * @brief Check if pose is collision free + * @param distance Distance to check forward + * @param cmd_vel current commanded velocity + * @param pose2d Current pose + * @return is collision free or not + */ bool isCollisionFree( const double & distance, geometry_msgs::msg::Twist * cmd_vel, geometry_msgs::msg::Pose2D & pose2d); + /** + * @brief Configuration of recovery action + */ void onConfigure() override; double min_linear_vel_; diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 011d1c00960..4b11dcb59fa 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -26,7 +26,7 @@ #include "tf2/utils.h" #pragma GCC diagnostic pop #include "tf2/LinearMath/Quaternion.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_util/node_utils.hpp" using namespace std::chrono_literals; @@ -119,7 +119,7 @@ Status Spin::onCycleUpdate() action_server_->publish_feedback(feedback_); double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_); - if (remaining_yaw <= 0) { + if (remaining_yaw < 1e-6) { stopRobot(); return Status::SUCCEEDED; } @@ -138,7 +138,7 @@ Status Spin::onCycleUpdate() if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) { stopRobot(); RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin"); - return Status::SUCCEEDED; + return Status::FAILED; } vel_pub_->publish(std::move(cmd_vel)); @@ -155,10 +155,11 @@ bool Spin::isCollisionFree( int cycle_count = 0; double sim_position_change; const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); + geometry_msgs::msg::Pose2D init_pose = pose2d; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_); - pose2d.theta += sim_position_change; + pose2d.theta = init_pose.theta + sim_position_change; cycle_count++; if (abs(relative_yaw) - abs(sim_position_change) <= 0.) { diff --git a/nav2_recoveries/plugins/spin.hpp b/nav2_recoveries/plugins/spin.hpp index cf1d6b7d044..ff719c91552 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_recoveries/plugins/spin.hpp @@ -27,17 +27,45 @@ namespace nav2_recoveries { using SpinAction = nav2_msgs::action::Spin; +/** + * @class nav2_recoveries::Spin + * @brief An action server recovery for spinning in + */ class Spin : public Recovery { public: + /** + * @brief A constructor for nav2_recoveries::Spin + */ Spin(); ~Spin(); + /** + * @brief Initialization to run behavior + * @param command Goal to execute + * @return Status of recovery + */ Status onRun(const std::shared_ptr command) override; + + /** + * @brief Configuration of recovery action + */ void onConfigure() override; + + /** + * @brief Loop function to run behavior + * @return Status of recovery + */ Status onCycleUpdate() override; protected: + /** + * @brief Check if pose is collision free + * @param distance Distance to check forward + * @param cmd_vel current commanded velocity + * @param pose2d Current pose + * @return is collision free or not + */ bool isCollisionFree( const double & distance, geometry_msgs::msg::Twist * cmd_vel, diff --git a/nav2_recoveries/plugins/wait.hpp b/nav2_recoveries/plugins/wait.hpp index 34364ce1742..41ade5e93b3 100644 --- a/nav2_recoveries/plugins/wait.hpp +++ b/nav2_recoveries/plugins/wait.hpp @@ -26,14 +26,30 @@ namespace nav2_recoveries { using WaitAction = nav2_msgs::action::Wait; +/** + * @class nav2_recoveries::Wait + * @brief An action server recovery for waiting a fixed duration + */ class Wait : public Recovery { public: + /** + * @brief A constructor for nav2_recoveries::Wait + */ Wait(); ~Wait(); + /** + * @brief Initialization to run behavior + * @param command Goal to execute + * @return Status of recovery + */ Status onRun(const std::shared_ptr command) override; + /** + * @brief Loop function to run behavior + * @return Status of recovery + */ Status onCycleUpdate() override; protected: diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt new file mode 100644 index 00000000000..9c87714fad7 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_regulated_pure_pursuit_controller) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_util REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(tf2 REQUIRED) + +nav2_package() +set(CMAKE_CXX_STANDARD 17) + +include_directories( + include +) + +set(dependencies + rclcpp + geometry_msgs + nav2_costmap_2d + pluginlib + nav_msgs + nav2_util + nav2_core + tf2 +) + +set(library_name nav2_regulated_pure_pursuit_controller) + +add_library(${library_name} SHARED + src/regulated_pure_pursuit_controller.cpp) + +# prevent pluginlib from using boost +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +ament_target_dependencies(${library_name} + ${dependencies} +) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + add_subdirectory(test) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) + +pluginlib_export_plugin_description_file(nav2_core nav2_regulated_pure_pursuit_controller.xml) + +ament_package() + diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md new file mode 100644 index 00000000000..c3dbbba57f3 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -0,0 +1,146 @@ +# Nav2 Regulated Pure Pursuit Controller + +This is a controller (local trajectory planner) that implements a variant on the pure pursuit algorithm to track a path. This variant we call the Regulated Pure Pursuit Algorithm, due to its additional regulation terms on collision and linear speed. It also implements the basics behind the Adaptive Pure Pursuit algorithm to vary lookahead distances by current speed. It was developed by [Shrijit Singh](https://www.linkedin.com/in/shrijitsingh99/) and [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/) as part of the Nav2 working group. + +Code based on a simplified version of this controller is referenced in the [Writing a New Nav2 Controller](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html) tutorial. + +This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). + +It builds on top of the ordinary pure pursuit algorithm in a number of ways. It also implements all the common variants of the pure pursuit algorithm such as adaptive pure pursuit. This controller is suitable for use on all types of robots, including differential, legged, and ackermann steering vehicles. It may also be used on omni-directional platforms, but won't be able to fully leverage the lateral movements of the base (you may consider DWB instead). + +This controller has been measured to run at well over 1 kHz on a modern intel processor. + +

+ +

+ +## Pure Pursuit Basics + +The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance. + +In order to simply book-keeping, the global path is continuously pruned to the closest point to the robot (see the figure below) so that we only have to process useful path points. Then, the section of the path within the local costmap bounds is transformed to the robot frame and a lookahead point is determined using a predefined distance. + +Finally, the lookahead point will be given to the pure pursuit algorithm which finds the curvature of the path required to drive the robot to the lookahead point. This curvature is then applied to the velocity commands to allow the robot to drive. + +Note that a pure pursuit controller is that, it "purely" pursues the path without interest or concern about dynamic obstacles. Therefore, this controller should only be used when paired with a path planner that can generate a path the robot can follow. For a circular (or can be treated as circular) robot, this can really be any planner since you can leverage the particle / inflation relationship in planning. For a "large" robot for the environment or general non-circular robots, this must be something kinematically feasible, like the Smac Planner, such that the path is followable. + +![Lookahead algorithm](./doc/lookahead_algorithm.png) + +## Regulated Pure Pursuit Features + +We have created a new variation on the pure pursuit algorithm that we dubb the Regulated Pure Pursuit algorithm. We combine the features of the Adaptive Pure Pursuit algorithm with rules around linear velocity with a focus on consumer, industrial, and service robot's needs. We also implement several common-sense safety mechanisms like collision detection and ensuring that commands are kinematically feasible that are missing from all other variants of pure pursuit (for some remarkable reason). + +The Regulated Pure Pursuit controller implements active collision detection. We use a parameter to set the maximum allowable time before a potential collision on the current velocity command. Using the current linear and angular velocity, we project forward in time that duration and check for collisions. Intuitively, you may think that collision checking between the robot and the lookahead point seems logical. However, if you're maneuvering in tight spaces, it makes alot of sense to only search forward a given amount of time to give the system a little leeway to get itself out. In confined spaces especially, we want to make sure that we're collision checking a reasonable amount of space for the current action being taken (e.g. if moving at 0.1 m/s, it makes no sense to look 10 meters ahead to the carrot, or 100 seconds into the future). This helps look further at higher speeds / angular rotations and closer with fine, slow motions in constrained environments so it doesn't over report collisions from valid motions near obstacles. If you set the maximum allowable to a large number, it will collision check all the way, but not exceeding, the lookahead point. We visualize the collision checking arc on the `lookahead_arc` topic. + +The regulated pure pursuit algorithm also makes use of the common variations on the pure pursuit algorithm. We implement the adaptive pure pursuit's main contribution of having velocity-scaled lookahead point distances. This helps make the controller more stable over a larger range of potential linear velocities. There are parameters for setting the lookahead gain (or lookahead time) and thresholded values for minimum and maximum. + +We also implement kinematic speed limits on the linear velocities in operations and angular velocities during pure rotations. This makes sure that the output commands are smooth, safe, and kinematically feasible. This is especially important at the beginning and end of a path tracking task, where you are ramping up to speed and slowing down to the goal. + +The final minor improvement we make is slowing on approach to the goal. Knowing that the optimal lookahead distance is `X`, we can take the difference in `X` and the actual distance of the lookahead point found to find the lookahead point error. During operations, the variation in this error should be exceptionally small and won't be triggered. However, at the end of the path, there are no more points at a lookahead distance away from the robot, so it uses the last point on the path. So as the robot approaches a target, its error will grow and the robot's velocity will be reduced proportional to this error until a minimum threshold. This is also tracked by the kinematic speed limits to ensure drivability. + +The major improvements that this work implements is the regulations on the linear velocity based on some cost functions. They were selected to remove long-standing bad behavior within the pure pursuit algorithm. Normal Pure Pursuit has an issue with overshoot and poor handling in particularly high curvature (or extremely rapidly changing curvature) environments. It is commonly known that this will cause the robot to overshoot from the path and potentially collide with the environment. These cost functions in the Regulated Pure Pursuit algorithm were also chosen based on common requirements and needs of mobile robots uses in service, commercial, and industrial use-cases; scaling by curvature creates intuitive behavior of slowing the robot when making sharp turns and slowing when its near a potential collision so that small variations don't clip obstacles. This is also really useful when working in partially observable environments (like turning in and out of aisles / hallways often) so that you slow before a sharp turn into an unknown dynamic environment to be more conservative in case something is in the way immediately requiring a stop. + +The cost functions penalize the robot's speed based on its proximity to obstacles and the curvature of the path. This is helpful to slow the robot when moving close to things in narrow spaces and scaling down the linear velocity by curvature helps to stabilize the controller over a larger range of lookahead point distances. This also has the added benefit of removing the sensitive tuning of the lookahead point / range, as the robot will track paths far better. Tuning is still required, but it is substantially easier to get reasonable behavior with minor adjustments. + +An unintended tertiary benefit of scaling the linear velocities by curvature is that a robot will natively rotate to rough path heading when using holonomic planners that don't start aligned with the robot pose orientation. As the curvature will be very high, the linear velocity drops and the angular velocity takes over to rotate to heading. While not perfect, it does dramatically reduce the need to rotate to a close path heading before following and opens up a broader range of planning techniques. Pure Pursuit controllers otherwise would be completely unable to recover from this in even modestly confined spaces. + +Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. + +## Configuration + +| Parameter | Description | +|-----|----| +| `desired_linear_vel` | The desired maximum linear velocity to use. | +| `max_linear_accel` | Acceleration for linear velocity | +| `max_linear_decel` | Deceleration for linear velocity | +| `lookahead_dist` | The lookahead distance to use to find the lookahead point | +| `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | +| `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | +| `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | +| `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | +| `transform_tolerance` | The TF transform tolerance | +| `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | +| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | +| `use_approach_linear_velocity_scaling` | Whether to scale the linear velocity down on approach to the goal for a smooth stop | +| `max_allowed_time_to_collision` | The time to project a velocity command to check for collisions | +| `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | +| `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | +| `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | +| `cost_scaling_gain` | A multiplier gain, which should be <= 1.0, used to further scale the speed when an obstacle is within `cost_scaling_dist`. Lower value reduces speed more quickly. | +| `inflation_cost_scaling_factor` | The value of `cost_scaling_factor` set for the inflation layer in the local costmap. The value should be exactly the same for accurately computing distance from obstacles using the inflated cell values | +| `regulated_linear_scaling_min_radius` | The turning radius for which the regulation features are triggered. Remember, sharper turns have smaller radii | +| `regulated_linear_scaling_min_speed` | The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature. | +| `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | +| `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | +| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | + +Example fully-described XML with default parameter values: + +``` +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.5 + max_linear_accel: 2.5 + max_linear_decel: 2.5 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + use_approach_linear_velocity_scaling: true + max_allowed_time_to_collision: 1.0 + use_regulated_linear_velocity_scaling: true + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + cost_scaling_dist: 0.3 + cost_scaling_gain: 1.0 + inflation_cost_scaling_factor: 3.0 +``` + +## Topics + +| Topic | Type | Description | +|-----|----|----| +| `lookahead_point` | `geometry_msgs/PointStamped` | The current lookahead point on the path | +| `lookahead_arc` | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | + +Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed guage. + +## Notes to users + +By default, the `use_cost_regulated_linear_velocity_scaling` is set to `false` because the generic sandbox environment we have setup is the TB3 world. This is a highly constrained environment so it overly triggers to slow the robot as everywhere has high costs. This is recommended to be set to `true` when not working in constantly high-cost spaces. + +To tune to get Adaptive Pure Pursuit behaviors, set all boolean parameters to false except `use_velocity_scaled_lookahead_dist` and make sure to tune `lookahead_time`, `min_lookahead_dist` and `max_lookahead_dist`. + +To tune to get Pure Pursuit behaviors, set all boolean parameters to false and make sure to tune `lookahead_dist`. + +Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way. + +The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it. diff --git a/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png b/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png new file mode 100644 index 00000000000..8507ddd3e03 Binary files /dev/null and b/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png differ 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 new file mode 100644 index 00000000000..390f05d2ace --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -0,0 +1,264 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "nav2_core/controller.hpp" +#include "rclcpp/rclcpp.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + * @brief Regulated pure pursuit controller plugin + */ +class RegulatedPurePursuitController : public nav2_core::Controller +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + */ + RegulatedPurePursuitController() = default; + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + */ + ~RegulatedPurePursuitController() override = default; + + /** + * @brief Configure controller state machine + * @param parent WeakPtr to node + * @param name Name of plugin + * @param tf TF buffer + * @param costmap_ros Costmap2DROS object of environment + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, const std::shared_ptr & tf, + const std::shared_ptr & costmap_ros) override; + + /** + * @brief Cleanup controller state machine + */ + void cleanup() override; + + /** + * @brief Activate controller state machine + */ + void activate() override; + + /** + * @brief Deactivate controller state machine + */ + void deactivate() override; + + /** + * @brief Compute the best command given the current pose and velocity, with possible debug information + * + * Same as above computeVelocityCommands, but with debug results. + * If the results pointer is not null, additional information about the twists + * evaluated will be in results after the call. + * + * @param pose Current robot pose + * @param velocity Current robot velocity + * @param goal_checker Ptr to the goal checker for this task in case useful in computing commands + * @return Best command + */ + geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * /*goal_checker*/) override; + + /** + * @brief nav2_core setPlan - Sets the global plan + * @param path The global plan + */ + void setPlan(const nav_msgs::msg::Path & path) override; + + /** + * @brief Limits the maximum linear speed of the robot. + * @param speed_limit expressed in absolute value (in m/s) + * or in percentage from maximum robot speed. + * @param percentage Setting speed limit in percentage if true + * or in absolute values in false case. + */ + void setSpeedLimit(const double & speed_limit, const bool & percentage) override; + +protected: + /** + * @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses + * @param pose pose to transform + * @return Path in new frame + */ + nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Transform a pose to another frame. + * @param frame Frame ID to transform to + * @param in_pose Pose input to transform + * @param out_pose transformed output + * @return bool if successful + */ + bool transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const; + + /** + * @brief Get lookahead distance + * @param cmd the current speed to use to compute lookahead point + * @return lookahead distance + */ + double getLookAheadDistance(const geometry_msgs::msg::Twist &); + + /** + * @brief Creates a PointStamped message for visualization + * @param carrot_pose Input carrot point as a PoseStamped + * @return CarrotMsg a carrot point marker, PointStamped + */ + std::unique_ptr createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose); + + /** + * @brief Whether robot should rotate to rough path heading + * @param carrot_pose current lookahead point + * @param angle_to_path Angle of robot output relatie to carrot marker + * @return Whether should rotate to path heading + */ + bool shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path); + + /** + * @brief Whether robot should rotate to final goal orientation + * @param carrot_pose current lookahead point + * @return Whether should rotate to goal heading + */ + bool shouldRotateToGoalHeading(const geometry_msgs::msg::PoseStamped & carrot_pose); + + /** + * @brief Create a smooth and kinematically smoothed rotation command + * @param linear_vel linear velocity + * @param angular_vel angular velocity + * @param angle_to_path Angle of robot output relatie to carrot marker + * @param curr_speed the current robot speed + */ + void rotateToHeading( + double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed); + + /** + * @brief Whether collision is imminent + * @param robot_pose Pose of robot + * @param carrot_pose Pose of carrot + * @param linear_vel linear velocity to forward project + * @param angular_vel angular velocity to forward project + * @return Whether collision is imminent + */ + bool isCollisionImminent( + const geometry_msgs::msg::PoseStamped &, + const double &, const double &); + + /** + * @brief Whether point is in collision + * @param x Pose of pose x + * @param y Pose of pose y + * @return Whether in collision + */ + bool inCollision(const double & x, const double & y); + + /** + * @brief Cost at a point + * @param x Pose of pose x + * @param y Pose of pose y + * @return Cost of pose in costmap + */ + double costAtPose(const double & x, const double & y); + + /** + * @brief apply regulation constraints to the system + * @param linear_vel robot command linear velocity input + * @param dist_error error in the carrot distance and lookahead distance + * @param lookahead_dist optimal lookahead distance + * @param curvature curvature of path + * @param speed Speed of robot + * @param pose_cost cost at this pose + */ + void applyConstraints( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & speed, + const double & pose_cost, double & linear_vel); + + /** + * @brief Get lookahead point + * @param lookahead_dist Optimal lookahead distance + * @param path Current global path + * @return Lookahead point + */ + geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + + std::shared_ptr tf_; + std::string plugin_name_; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; + + double desired_linear_vel_, base_desired_linear_vel_; + double lookahead_dist_; + double rotate_to_heading_angular_vel_; + double max_lookahead_dist_; + double min_lookahead_dist_; + double lookahead_time_; + double max_linear_accel_; + double max_linear_decel_; + bool use_velocity_scaled_lookahead_dist_; + tf2::Duration transform_tolerance_; + bool use_approach_vel_scaling_; + double min_approach_linear_velocity_; + double control_duration_; + double max_allowed_time_to_collision_; + bool use_regulated_linear_velocity_scaling_; + bool use_cost_regulated_linear_velocity_scaling_; + double cost_scaling_dist_; + double cost_scaling_gain_; + double inflation_cost_scaling_factor_; + double regulated_linear_scaling_min_radius_; + double regulated_linear_scaling_min_speed_; + bool use_rotate_to_heading_; + double max_angular_accel_; + double rotate_to_heading_min_angle_; + double goal_dist_tol_; + + nav_msgs::msg::Path global_plan_; + std::shared_ptr> global_path_pub_; + std::shared_ptr> + carrot_pub_; + std::shared_ptr> carrot_arc_pub_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml new file mode 100644 index 00000000000..d695bcec7f8 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml @@ -0,0 +1,10 @@ + + + + + nav2_regulated_pure_pursuit_controller + + + + + diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml new file mode 100644 index 00000000000..317dd765fed --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -0,0 +1,31 @@ + + + + nav2_regulated_pure_pursuit_controller + 1.0.0 + Regulated Pure Pursuit Controller + Steve Macenski + Shrijit Singh + Apache-2.0 + + ament_cmake + + nav2_common + nav2_core + nav2_util + nav2_costmap_2d + rclcpp + geometry_msgs + nav2_msgs + pluginlib + tf2 + + ament_cmake_gtest + ament_lint_common + ament_lint_auto + + + ament_cmake + + + 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 new file mode 100644 index 00000000000..dbd0949dfb6 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -0,0 +1,596 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" +#include "nav2_core/exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" + +using std::hypot; +using std::min; +using std::max; +using std::abs; +using nav2_util::declare_parameter_if_not_declared; +using nav2_util::geometry_utils::euclidean_distance; +using namespace nav2_costmap_2d; // NOLINT + +namespace nav2_regulated_pure_pursuit_controller +{ + +void RegulatedPurePursuitController::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, const std::shared_ptr & tf, + const std::shared_ptr & costmap_ros) +{ + auto node = parent.lock(); + if (!node) { + throw std::runtime_error("Unable to lock node!"); + } + + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + tf_ = tf; + plugin_name_ = name; + logger_ = node->get_logger(); + + double transform_tolerance = 0.1; + double control_frequency = 20.0; + goal_dist_tol_ = 0.25; // reasonable default before first update + + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_approach_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_allowed_time_to_collision", 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( + node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + + node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); + base_desired_linear_vel_ = desired_linear_vel_; + node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); + node->get_parameter(plugin_name_ + ".max_linear_decel", max_linear_decel_); + node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); + node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); + node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); + node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_angular_vel", + rotate_to_heading_angular_vel_); + node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); + node->get_parameter( + plugin_name_ + ".use_velocity_scaled_lookahead_dist", + use_velocity_scaled_lookahead_dist_); + node->get_parameter( + plugin_name_ + ".min_approach_linear_velocity", + min_approach_linear_velocity_); + node->get_parameter( + plugin_name_ + ".use_approach_linear_velocity_scaling", + use_approach_vel_scaling_); + node->get_parameter( + plugin_name_ + ".max_allowed_time_to_collision", + max_allowed_time_to_collision_); + node->get_parameter( + plugin_name_ + ".use_regulated_linear_velocity_scaling", + use_regulated_linear_velocity_scaling_); + node->get_parameter( + plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + use_cost_regulated_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".cost_scaling_dist", cost_scaling_dist_); + node->get_parameter(plugin_name_ + ".cost_scaling_gain", cost_scaling_gain_); + node->get_parameter( + plugin_name_ + ".inflation_cost_scaling_factor", + inflation_cost_scaling_factor_); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_radius", + regulated_linear_scaling_min_radius_); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_speed", + regulated_linear_scaling_min_speed_); + node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_); + node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); + node->get_parameter("controller_frequency", control_frequency); + + transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + control_duration_ = 1.0 / control_frequency; + + if (inflation_cost_scaling_factor_ <= 0.0) { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Disabling cost regulated linear velocity scaling."); + use_cost_regulated_linear_velocity_scaling_ = false; + } + + global_path_pub_ = node->create_publisher("received_global_plan", 1); + carrot_pub_ = node->create_publisher("lookahead_point", 1); + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); +} + +void RegulatedPurePursuitController::cleanup() +{ + RCLCPP_INFO( + logger_, + "Cleaning up controller: %s of type" + " regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_.reset(); + carrot_pub_.reset(); + carrot_arc_pub_.reset(); +} + +void RegulatedPurePursuitController::activate() +{ + RCLCPP_INFO( + logger_, + "Activating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_->on_activate(); + carrot_pub_->on_activate(); + carrot_arc_pub_->on_activate(); +} + +void RegulatedPurePursuitController::deactivate() +{ + RCLCPP_INFO( + logger_, + "Deactivating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_->on_deactivate(); + carrot_pub_->on_deactivate(); + carrot_arc_pub_->on_deactivate(); +} + +std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + auto carrot_msg = std::make_unique(); + carrot_msg->header = carrot_pose.header; + carrot_msg->point.x = carrot_pose.pose.position.x; + carrot_msg->point.y = carrot_pose.pose.position.y; + carrot_msg->point.z = 0.01; // publish right over map to stand out + return carrot_msg; +} + +double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) +{ + // If using velocity-scaled look ahead distances, find and clamp the dist + // Else, use the static look ahead distance + double lookahead_dist = lookahead_dist_; + if (use_velocity_scaled_lookahead_dist_) { + lookahead_dist = speed.linear.x * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } + + return lookahead_dist; +} + +geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & speed, + nav2_core::GoalChecker * goal_checker) +{ + // Update for the current goal checker's state + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist vel_tolerance; + if (!goal_checker->getTolerances(pose_tolerance, vel_tolerance)) { + RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!"); + } else { + goal_dist_tol_ = pose_tolerance.position.x; + } + + // Transform path to robot base frame + auto transformed_plan = transformGlobalPlan(pose); + + // Find look ahead distance and point on path and publish + const double lookahead_dist = getLookAheadDistance(speed); + auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + carrot_pub_->publish(createCarrotMsg(carrot_pose)); + + double linear_vel, angular_vel; + + // Find distance^2 to look ahead point (carrot) in robot base frame + // This is the chord length of the circle + const double carrot_dist2 = + (carrot_pose.pose.position.x * carrot_pose.pose.position.x) + + (carrot_pose.pose.position.y * carrot_pose.pose.position.y); + + // Find curvature of circle (k = 1 / R) + double curvature = 0.0; + if (carrot_dist2 > 0.001) { + curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; + } + + linear_vel = desired_linear_vel_; + + // Make sure we're in compliance with basic constraints + double angle_to_heading; + if (shouldRotateToGoalHeading(carrot_pose)) { + double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); + rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); + } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { + rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); + } else { + applyConstraints( + fabs(lookahead_dist - sqrt(carrot_dist2)), + lookahead_dist, curvature, speed, + costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel); + + // Apply curvature to angular velocity after constraining linear velocity + angular_vel = linear_vel * curvature; + } + + // Collision checking on this velocity heading + if (isCollisionImminent(pose, linear_vel, angular_vel)) { + throw std::runtime_error("RegulatedPurePursuitController detected collision ahead!"); + } + + // populate and return message + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header = pose.header; + cmd_vel.twist.linear.x = linear_vel; + cmd_vel.twist.angular.z = angular_vel; + return cmd_vel; +} + +bool RegulatedPurePursuitController::shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) +{ + // Whether we should rotate robot to rough path heading + angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); + return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_; +} + +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 use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_; +} + +void RegulatedPurePursuitController::rotateToHeading( + double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) +{ + // Rotate in place using max angular velocity / acceleration possible + linear_vel = 0.0; + const double sign = angle_to_path > 0.0 ? 1.0 : -1.0; + angular_vel = sign * rotate_to_heading_angular_vel_; + + const double & dt = control_duration_; + const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt; + const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt; + angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); +} + +geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( + const double & lookahead_dist, + const nav_msgs::msg::Path & transformed_plan) +{ + // Find the first pose which is at a distance greater than the lookahead distance + auto goal_pose_it = std::find_if( + transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { + return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist; + }); + + // If the no pose is not far enough, take the last pose + if (goal_pose_it == transformed_plan.poses.end()) { + goal_pose_it = std::prev(transformed_plan.poses.end()); + } + + return *goal_pose_it; +} + +bool RegulatedPurePursuitController::isCollisionImminent( + const geometry_msgs::msg::PoseStamped & robot_pose, + const double & linear_vel, const double & angular_vel) +{ + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in + // odom frame and the carrot_pose is in robot base frame. + + // check current point is OK + if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y)) { + return true; + } + + // visualization messages + nav_msgs::msg::Path arc_pts_msg; + arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); + arc_pts_msg.header.stamp = robot_pose.header.stamp; + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = arc_pts_msg.header.frame_id; + pose_msg.header.stamp = arc_pts_msg.header.stamp; + + const double projection_time = costmap_->getResolution() / linear_vel; + + geometry_msgs::msg::Pose2D curr_pose; + curr_pose.x = robot_pose.pose.position.x; + curr_pose.y = robot_pose.pose.position.y; + curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); + + int i = 1; + while (true) { + // only forward simulate within time requested + if (i * projection_time > max_allowed_time_to_collision_) { + break; + } + + i++; + + // apply velocity at curr_pose over distance + curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); + curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); + curr_pose.theta += projection_time * angular_vel; + + // store it for visualization + pose_msg.pose.position.x = curr_pose.x; + pose_msg.pose.position.y = curr_pose.y; + pose_msg.pose.position.z = 0.01; + arc_pts_msg.poses.push_back(pose_msg); + + // check for collision at this point + if (inCollision(curr_pose.x, curr_pose.y)) { + carrot_arc_pub_->publish(arc_pts_msg); + return true; + } + } + + carrot_arc_pub_->publish(arc_pts_msg); + + return false; +} + +bool RegulatedPurePursuitController::inCollision(const double & x, const double & y) +{ + unsigned int mx, my; + costmap_->worldToMap(x, y, mx, my); + + unsigned char cost = costmap_->getCost(mx, my); + + if (costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) { + return cost >= INSCRIBED_INFLATED_OBSTACLE && cost != NO_INFORMATION; + } else { + return cost >= INSCRIBED_INFLATED_OBSTACLE; + } +} + +double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) +{ + unsigned int mx, my; + costmap_->worldToMap(x, y, mx, my); + + unsigned char cost = costmap_->getCost(mx, my); + return static_cast(cost); +} + +void RegulatedPurePursuitController::applyConstraints( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & pose_cost, double & linear_vel) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + double approach_vel = linear_vel; + + // limit the linear velocity by curvature + const double radius = fabs(1.0 / curvature); + const double & min_rad = regulated_linear_scaling_min_radius_; + if (use_regulated_linear_velocity_scaling_ && radius < min_rad) { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + } + + // limit the linear velocity by proximity to obstacles + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(NO_INFORMATION) && + pose_cost != static_cast(FREE_SPACE)) + { + const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * + std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; + + if (min_distance_to_obstacle < cost_scaling_dist_) { + cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; + } + } + + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + linear_vel = std::min(cost_vel, curvature_vel); + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop + if (use_approach_vel_scaling_ && dist_error > 2.0 * costmap_->getResolution()) { + double velocity_scaling = 1.0 - (dist_error / lookahead_dist); + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) { + approach_vel = min_approach_linear_velocity_; + } else { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + linear_vel = std::min(linear_vel, approach_vel); + } + + // Limit linear velocities to be valid and kinematically feasible, v = v0 + a * dt + double & dt = control_duration_; + const double max_feasible_linear_speed = curr_speed.linear.x + max_linear_accel_ * dt; + const double min_feasible_linear_speed = curr_speed.linear.x - max_linear_decel_ * dt; + linear_vel = std::clamp(linear_vel, min_feasible_linear_speed, max_feasible_linear_speed); + linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); +} + +void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; +} + +void RegulatedPurePursuitController::setSpeedLimit( + const double & speed_limit, + const bool & percentage) +{ + if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { + // Restore default value + desired_linear_vel_ = base_desired_linear_vel_; + } else { + if (percentage) { + // Speed limit is expressed in % from maximum speed of robot + desired_linear_vel_ = base_desired_linear_vel_ * speed_limit / 100.0; + } else { + // Speed limit is expressed in absolute value + desired_linear_vel_ = speed_limit; + } + } +} + +nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_.poses.empty()) { + throw nav2_core::PlannerException("Received plan with zero length"); + } + + // let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); + } + + // We'll discard points on the plan that are outside the local costmap + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + const double max_costmap_dim = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()); + const double max_transform_dist = max_costmap_dim * costmap->getResolution() / 2.0; + + // First find the closest pose on the path to the robot + auto transformation_begin = + nav2_util::geometry_utils::min_by( + global_plan_.poses.begin(), global_plan_.poses.end(), + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // Find points definitely outside of the costmap so we won't transform them. + auto transformation_end = std::find_if( + transformation_begin, end(global_plan_.poses), + [&](const auto & global_plan_pose) { + return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose); + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + global_path_pub_->publish(transformed_plan); + + if (transformed_plan.poses.empty()) { + throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +bool RegulatedPurePursuitController::transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); + out_pose.header.frame_id = frame; + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); + } + return false; +} + +} // namespace nav2_regulated_pure_pursuit_controller + +// Register this controller as a nav2_core plugin +PLUGINLIB_EXPORT_CLASS( + nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController, + nav2_core::Controller) diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt new file mode 100644 index 00000000000..aee6297fcbb --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -0,0 +1,10 @@ +# tests for regulated PP +ament_add_gtest(test_regulated_pp + test_regulated_pp.cpp +) +ament_target_dependencies(test_regulated_pp + ${dependencies} +) +target_link_libraries(test_regulated_pp + ${library_name} +) diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp new file mode 100644 index 00000000000..06a626a393c --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -0,0 +1,350 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController +{ +public: + BasicAPIRPP() + : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {} + + nav_msgs::msg::Path getPlan() {return global_plan_;} + + double getSpeed() {return desired_linear_vel_;} + + std::unique_ptr createCarrotMsgWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose) + { + return createCarrotMsg(carrot_pose); + } + + void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;} + void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;} + void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;} + void resetVelocityApproachScaling() {use_approach_vel_scaling_ = false;} + + double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist) + { + return getLookAheadDistance(twist); + } + + geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( + const double & dist, const nav_msgs::msg::Path & path) + { + return getLookAheadPoint(dist, path); + } + + bool shouldRotateToPathWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) + { + return shouldRotateToPath(carrot_pose, angle_to_path); + } + + bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose) + { + return shouldRotateToGoalHeading(carrot_pose); + } + + void rotateToHeadingWrapper( + double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) + { + return rotateToHeading(linear_vel, angular_vel, angle_to_path, curr_speed); + } + + void applyConstraintsWrapper( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & pose_cost, double & linear_vel) + { + return applyConstraints( + dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + linear_vel); + } +}; + +TEST(RegulatedPurePursuitTest, basicAPI) +{ + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + + // instantiate + auto ctrl = std::make_shared(); + ctrl->configure(node, name, tf, costmap); + ctrl->activate(); + ctrl->deactivate(); + ctrl->cleanup(); + + // setPlan and get plan + nav_msgs::msg::Path path; + path.poses.resize(2); + path.poses[0].header.frame_id = "fake_frame"; + ctrl->setPlan(path); + EXPECT_EQ(ctrl->getPlan().poses.size(), 2ul); + EXPECT_EQ(ctrl->getPlan().poses[0].header.frame_id, std::string("fake_frame")); + + // set speed limit + const double base_speed = ctrl->getSpeed(); + EXPECT_EQ(ctrl->getSpeed(), base_speed); + ctrl->setSpeedLimit(0.51, false); + EXPECT_EQ(ctrl->getSpeed(), 0.51); + ctrl->setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, false); + EXPECT_EQ(ctrl->getSpeed(), base_speed); + ctrl->setSpeedLimit(30, true); + EXPECT_EQ(ctrl->getSpeed(), base_speed * 0.3); + ctrl->setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, true); + EXPECT_EQ(ctrl->getSpeed(), base_speed); +} + +TEST(RegulatedPurePursuitTest, createCarrotMsg) +{ + auto ctrl = std::make_shared(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "Hi!"; + pose.pose.position.x = 1.0; + pose.pose.position.y = 12.0; + pose.pose.orientation.w = 0.5; + + auto rtn = ctrl->createCarrotMsgWrapper(pose); + EXPECT_EQ(rtn->header.frame_id, std::string("Hi!")); + EXPECT_EQ(rtn->point.x, 1.0); + EXPECT_EQ(rtn->point.y, 12.0); + EXPECT_EQ(rtn->point.z, 0.01); +} + +TEST(RegulatedPurePursuitTest, lookaheadAPI) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + ctrl->configure(node, name, tf, costmap); + + geometry_msgs::msg::Twist twist; + + // test getLookAheadDistance + double rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.6); // default lookahead_dist + + // shouldn't be a function of speed + twist.linear.x = 10.0; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.6); + + // now it should be a function of velocity, max out + ctrl->setVelocityScaledLookAhead(); + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.9); // 10 speed maxes out at max_lookahead_dist + + // check normal range + twist.linear.x = 0.35; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_NEAR(rtn, 0.525, 0.0001); // 1.5 * 0.35 + + // check minimum range + twist.linear.x = 0.0; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.3); + + // test getLookAheadPoint + double dist = 1.0; + nav_msgs::msg::Path path; + path.poses.resize(10); + for (uint i = 0; i != path.poses.size(); i++) { + path.poses[i].pose.position.x = static_cast(i); + } + + // test exact hits + auto pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 1.0); + + // test getting next closest point + dist = 3.8; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 4.0); + + // test end of path + dist = 100.0; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 9.0); +} + +TEST(RegulatedPurePursuitTest, rotateTests) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + ctrl->configure(node, name, tf, costmap); + + // shouldRotateToPath + geometry_msgs::msg::PoseStamped carrot; + double angle_to_path_rtn; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), false); + + carrot.pose.position.x = 0.5; + carrot.pose.position.y = 0.25; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), false); + + carrot.pose.position.x = 0.5; + carrot.pose.position.y = 1.0; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), true); + + // shouldRotateToGoalHeading + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.0; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.24; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.26; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false); + + // rotateToHeading + double lin_v = 10.0; + double ang_v = 0.5; + double angle_to_path = 0.4; + geometry_msgs::msg::Twist curr_speed; + curr_speed.angular.z = 1.75; + + // basic full speed at a speed + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_EQ(lin_v, 0.0); + EXPECT_EQ(ang_v, 1.8); + + // negative direction + angle_to_path = -0.4; + curr_speed.angular.z = -1.75; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_EQ(ang_v, -1.8); + + // kinematic clamping, no speed, some speed accelerating, some speed decelerating + angle_to_path = 0.4; + curr_speed.angular.z = 0.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 0.16, 0.01); + + curr_speed.angular.z = 1.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 1.16, 0.01); + + angle_to_path = -0.4; + 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); +} + +TEST(RegulatedPurePursuitTest, applyConstraints) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + ctrl->configure(node, name, tf, costmap); + + double dist_error = 0.0; + double lookahead_dist = 0.6; + double curvature = 0.5; + geometry_msgs::msg::Twist curr_speed; + double pose_cost = 0.0; + double linear_vel = 0.0; + + // since costmaps here are bogus, we can't access them + ctrl->resetVelocityApproachScaling(); + + // test curvature regulation (default) + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper( + dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + linear_vel); + EXPECT_EQ(linear_vel, 0.25); // min set speed + + linear_vel = 1.0; + curvature = 0.7407; + curr_speed.linear.x = 0.5; + ctrl->applyConstraintsWrapper( + dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + linear_vel); + EXPECT_NEAR(linear_vel, 0.5, 0.01); // lower by curvature + + linear_vel = 1.0; + curvature = 1000.0; + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper( + dist_error, lookahead_dist, curvature, curr_speed, pose_cost, + linear_vel); + EXPECT_NEAR(linear_vel, 0.25, 0.01); // min out by curvature + + + // now try with cost regulation (turn off velocity and only cost) + // ctrl->setCostRegulationScaling(); + // ctrl->resetVelocityRegulationScaling(); + // curvature = 0.0; + + // min changable cost + // pose_cost = 1; + // linear_vel = 0.5; + // curr_speed.linear.x = 0.5; + // ctrl->applyConstraintsWrapper( + // dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.498, 0.01); + + // max changing cost + // pose_cost = 127; + // curr_speed.linear.x = 0.255; + // ctrl->applyConstraintsWrapper( + // dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.255, 0.01); + + // over max cost thresh + // pose_cost = 200; + // curr_speed.linear.x = 0.25; + // ctrl->applyConstraintsWrapper( + // dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.25, 0.01); + + // test kinematic clamping + // pose_cost = 200; + // curr_speed.linear.x = 1.0; + // ctrl->applyConstraintsWrapper( + // dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.5, 0.01); +} diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index f344363296c..f34f9d29b5e 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -3,7 +3,7 @@ project(nav2_rviz_plugins) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -37,6 +37,8 @@ set(nav2_rviz_plugins_headers_to_moc include/nav2_rviz_plugins/goal_common include/nav2_rviz_plugins/goal_tool.hpp include/nav2_rviz_plugins/nav2_panel.hpp + include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp + include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp ) include_directories( @@ -48,6 +50,8 @@ set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED src/goal_tool.cpp src/nav2_panel.cpp + src/particle_cloud_display/flat_weighted_arrows_array.cpp + src/particle_cloud_display/particle_cloud_display.cpp ${nav2_rviz_plugins_headers_to_moc} ) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 772759aa47f..dd67b3110d8 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -24,12 +24,13 @@ #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" #include "nav2_rviz_plugins/ros_action_qevent.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rviz_common/panel.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include "nav2_util/geometry_utils.hpp" @@ -62,7 +63,8 @@ private Q_SLOTS: void onCancel(); void onPause(); void onResume(); - void onAccumulated(); + void onAccumulatedWp(); + void onAccumulatedNTP(); void onAccumulating(); void onNewGoal(double x, double y, double theta, QString frame); @@ -76,10 +78,13 @@ private Q_SLOTS: // Call to send NavigateToPose action request for goal poses void startWaypointFollowing(std::vector poses); void startNavigation(geometry_msgs::msg::PoseStamped); + void startNavThroughPoses(std::vector poses); using NavigationGoalHandle = rclcpp_action::ClientGoalHandle; using WaypointFollowerGoalHandle = rclcpp_action::ClientGoalHandle; + using NavThroughPosesGoalHandle = + rclcpp_action::ClientGoalHandle; // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; @@ -94,12 +99,26 @@ private Q_SLOTS: rclcpp_action::Client::SharedPtr navigation_action_client_; rclcpp_action::Client::SharedPtr waypoint_follower_action_client_; + rclcpp_action::Client::SharedPtr + nav_through_poses_action_client_; + + // Navigation action feedback subscribers + rclcpp::Subscription::SharedPtr + navigation_feedback_sub_; + rclcpp::Subscription::SharedPtr + nav_through_poses_feedback_sub_; + rclcpp::Subscription::SharedPtr + navigation_goal_status_sub_; + rclcpp::Subscription::SharedPtr + nav_through_poses_goal_status_sub_; // Goal-related state nav2_msgs::action::NavigateToPose::Goal navigation_goal_; nav2_msgs::action::FollowWaypoints::Goal waypoint_follower_goal_; + nav2_msgs::action::NavigateThroughPoses::Goal nav_through_poses_goal_; NavigationGoalHandle::SharedPtr navigation_goal_handle_; WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_; + NavThroughPosesGoalHandle::SharedPtr nav_through_poses_goal_handle_; // The client used to control the nav2 stack nav2_lifecycle_manager::LifecycleManagerClient client_nav_; @@ -111,6 +130,8 @@ private Q_SLOTS: QLabel * navigation_status_indicator_{nullptr}; QLabel * localization_status_indicator_{nullptr}; + QLabel * navigation_goal_status_indicator_{nullptr}; + QLabel * navigation_feedback_indicator_{nullptr}; QStateMachine state_machine_; InitialThread * initial_thread_; @@ -128,9 +149,10 @@ private Q_SLOTS: QState * running_{nullptr}; QState * canceled_{nullptr}; // The following states are added to allow to collect several poses to perform a waypoint-mode - // navigation + // navigation or navigate through poses mode. QState * accumulating_{nullptr}; - QState * accumulated_{nullptr}; + QState * accumulated_wp_{nullptr}; + QState * accumulated_nav_through_poses_{nullptr}; std::vector acummulated_poses_; @@ -142,6 +164,23 @@ private Q_SLOTS: void resetUniqueId(); + // create label string from goal status msg + static inline QString getGoalStatusLabel( + int8_t status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN); + + // create label string from feedback msg + static inline QString getNavToPoseFeedbackLabel( + nav2_msgs::action::NavigateToPose::Feedback msg = + nav2_msgs::action::NavigateToPose::Feedback()); + static inline QString getNavThroughPosesFeedbackLabel( + nav2_msgs::action::NavigateThroughPoses::Feedback = + nav2_msgs::action::NavigateThroughPoses::Feedback()); + template + static inline std::string toLabel(T & msg); + + // round off double to the specified precision and convert to string + static inline std::string toString(double val, int precision = 0); + // Waypoint navigation visual markers publisher rclcpp::Publisher::SharedPtr wp_navigation_markers_pub_; }; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp new file mode 100644 index 00000000000..7647aaf67fa --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp @@ -0,0 +1,94 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ +#define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp" + +namespace nav2_rviz_plugins +{ + +struct OgrePoseWithWeight; + +class FlatWeightedArrowsArray +{ +public: + explicit FlatWeightedArrowsArray(Ogre::SceneManager * scene_manager_); + ~FlatWeightedArrowsArray(); + + void createAndAttachManualObject(Ogre::SceneNode * scene_node); + void updateManualObject( + Ogre::ColourValue color, + float alpha, + float min_length, + float max_length, + const std::vector & poses); + void clear(); + +private: + void setManualObjectMaterial(); + void setManualObjectVertices( + const Ogre::ColourValue & color, + float min_length, + float max_length, + const std::vector & poses); + + Ogre::SceneManager * scene_manager_; + Ogre::ManualObject * manual_object_; + Ogre::MaterialPtr material_; +}; + +} // namespace nav2_rviz_plugins + +#endif // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_ diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp new file mode 100644 index 00000000000..f67a6e05382 --- /dev/null +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp @@ -0,0 +1,158 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_ +#define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_ + +#include +#include + +#include "nav2_msgs/msg/particle_cloud.hpp" + +#include "rviz_rendering/objects/shape.hpp" +#include "rviz_common/message_filter_display.hpp" + +namespace Ogre +{ +class ManualObject; +} // namespace Ogre + +namespace rviz_common +{ +namespace properties +{ +class EnumProperty; +class ColorProperty; +class FloatProperty; +} // namespace properties +} // namespace rviz_common + +namespace rviz_rendering +{ +class Arrow; +class Axes; +} // namespace rviz_rendering + +namespace nav2_rviz_plugins +{ +class FlatWeightedArrowsArray; +struct OgrePoseWithWeight +{ + Ogre::Vector3 position; + Ogre::Quaternion orientation; + float weight; +}; + +/** @brief Displays a nav2_msgs/ParticleCloud message as a bunch of line-drawn weighted arrows. */ +class ParticleCloudDisplay : public rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + // TODO(botteroa-si): Constructor for testing, remove once ros_nodes can be mocked and call + // initialize instead + ParticleCloudDisplay( + rviz_common::DisplayContext * display_context, + Ogre::SceneNode * scene_node); + ParticleCloudDisplay(); + ~ParticleCloudDisplay() override; + + void processMessage(nav2_msgs::msg::ParticleCloud::ConstSharedPtr msg) override; + void setShape(QString shape); // for testing + +protected: + void onInitialize() override; + void reset() override; + +private Q_SLOTS: + /// Update the interface and visible shapes based on the selected shape type. + void updateShapeChoice(); + + /// Update the arrow color. + void updateArrowColor(); + + /// Update arrow geometry + void updateGeometry(); + +private: + void initializeProperties(); + bool validateFloats(const nav2_msgs::msg::ParticleCloud & msg); + bool setTransform(std_msgs::msg::Header const & header); + void updateDisplay(); + void updateArrows2d(); + void updateArrows3d(); + void updateAxes(); + void updateArrow3dGeometry(); + void updateAxesGeometry(); + + std::unique_ptr makeAxes(); + std::unique_ptr makeArrow3d(); + + std::vector poses_; + std::unique_ptr arrows2d_; + std::vector> arrows3d_; + std::vector> axes_; + + Ogre::SceneNode * arrow_node_; + Ogre::SceneNode * axes_node_; + + rviz_common::properties::EnumProperty * shape_property_; + rviz_common::properties::ColorProperty * arrow_color_property_; + rviz_common::properties::FloatProperty * arrow_alpha_property_; + + rviz_common::properties::FloatProperty * arrow_min_length_property_; + rviz_common::properties::FloatProperty * arrow_max_length_property_; + + float min_length_; + float max_length_; + float length_scale_; + float head_radius_scale_; + float head_length_scale_; + float shaft_radius_scale_; +}; + +} // namespace nav2_rviz_plugins + +#endif // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_ diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 0a66d3764ae..e40a5c9058a 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.3 + 1.0.0 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_rviz_plugins/plugins_description.xml b/nav2_rviz_plugins/plugins_description.xml index 34d27f28d09..197a9a750d7 100644 --- a/nav2_rviz_plugins/plugins_description.xml +++ b/nav2_rviz_plugins/plugins_description.xml @@ -9,7 +9,13 @@ - The Navigation2 rviz panel. + The Nav2 rviz panel. + + + + The Particle Cloud rviz display. diff --git a/nav2_rviz_plugins/src/goal_tool.cpp b/nav2_rviz_plugins/src/goal_tool.cpp index 97d5e54b096..7fdfff779be 100644 --- a/nav2_rviz_plugins/src/goal_tool.cpp +++ b/nav2_rviz_plugins/src/goal_tool.cpp @@ -37,7 +37,7 @@ GoalTool::~GoalTool() void GoalTool::onInitialize() { PoseTool::onInitialize(); - setName("Navigation2 Goal"); + setName("Nav2 Goal"); setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/SetGoal.png")); } diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index e43992fbd4e..bdadb0ffaeb 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -21,9 +21,11 @@ #include #include #include +#include #include "nav2_rviz_plugins/goal_common.hpp" #include "rviz_common/display_context.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" using namespace std::chrono_literals; @@ -36,7 +38,7 @@ GoalPoseUpdater GoalUpdater; Nav2Panel::Nav2Panel(QWidget * parent) : Panel(parent), - server_timeout_(10), + server_timeout_(100), client_nav_("lifecycle_manager_navigation"), client_loc_("lifecycle_manager_localization") { @@ -47,6 +49,8 @@ Nav2Panel::Nav2Panel(QWidget * parent) navigation_mode_button_ = new QPushButton; navigation_status_indicator_ = new QLabel; localization_status_indicator_ = new QLabel; + navigation_goal_status_indicator_ = new QLabel; + navigation_feedback_indicator_ = new QLabel; // Create the state machine used to present the proper control button states in the UI @@ -55,9 +59,10 @@ Nav2Panel::Nav2Panel(QWidget * parent) const char * cancel_msg = "Cancel navigation"; const char * pause_msg = "Deactivate all nav2 lifecycle nodes"; const char * resume_msg = "Activate all nav2 lifecycle nodes"; - const char * single_goal_msg = "Change to waypoint mode navigation"; - const char * waypoint_goal_msg = "Start navigation"; - const char * cancel_waypoint_msg = "Cancel waypoint mode"; + const char * single_goal_msg = "Change to waypoint / nav through poses style navigation"; + const char * waypoint_goal_msg = "Start following waypoints"; + const char * nft_goal_msg = "Start navigating through poses"; + const char * cancel_waypoint_msg = "Cancel waypoint / viapoint accumulation mode"; const QString navigation_active("" "
Navigation:active
"); @@ -74,8 +79,12 @@ Nav2Panel::Nav2Panel(QWidget * parent) navigation_status_indicator_->setText(navigation_unknown); localization_status_indicator_->setText(localization_unknown); + navigation_goal_status_indicator_->setText(getGoalStatusLabel()); + navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel()); navigation_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); localization_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + navigation_goal_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + navigation_feedback_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); pre_initial_ = new QState(); pre_initial_->setObjectName("pre_initial"); @@ -85,7 +94,9 @@ Nav2Panel::Nav2Panel(QWidget * parent) pre_initial_->assignProperty(pause_resume_button_, "text", "Pause"); pre_initial_->assignProperty(pause_resume_button_, "enabled", false); - pre_initial_->assignProperty(navigation_mode_button_, "text", "Waypoint mode"); + pre_initial_->assignProperty( + navigation_mode_button_, "text", + "Waypoint / Nav Through Poses Mode"); pre_initial_->assignProperty(navigation_mode_button_, "enabled", false); initial_ = new QState(); @@ -97,7 +108,7 @@ Nav2Panel::Nav2Panel(QWidget * parent) initial_->assignProperty(pause_resume_button_, "text", "Pause"); initial_->assignProperty(pause_resume_button_, "enabled", false); - initial_->assignProperty(navigation_mode_button_, "text", "Waypoint mode"); + initial_->assignProperty(navigation_mode_button_, "text", "Waypoint / Nav Through Poses Mode"); initial_->assignProperty(navigation_mode_button_, "enabled", false); // State entered when navigate_to_pose action is not active @@ -111,26 +122,58 @@ Nav2Panel::Nav2Panel(QWidget * parent) idle_->assignProperty(pause_resume_button_, "enabled", true); idle_->assignProperty(pause_resume_button_, "toolTip", pause_msg); - idle_->assignProperty(navigation_mode_button_, "text", "Waypoint mode"); + idle_->assignProperty(navigation_mode_button_, "text", "Waypoint / Nav Through Poses Mode"); idle_->assignProperty(navigation_mode_button_, "enabled", true); idle_->assignProperty(navigation_mode_button_, "toolTip", single_goal_msg); // State entered when navigate_to_pose action is not active accumulating_ = new QState(); accumulating_->setObjectName("accumulating"); - accumulating_->assignProperty(start_reset_button_, "text", "Reset"); + accumulating_->assignProperty(start_reset_button_, "text", "Cancel Accumulation"); accumulating_->assignProperty(start_reset_button_, "toolTip", cancel_waypoint_msg); accumulating_->assignProperty(start_reset_button_, "enabled", true); - accumulating_->assignProperty(pause_resume_button_, "text", "Pause"); - accumulating_->assignProperty(pause_resume_button_, "enabled", false); - accumulating_->assignProperty(pause_resume_button_, "toolTip", pause_msg); + accumulating_->assignProperty(pause_resume_button_, "text", "Start Nav Through Poses"); + accumulating_->assignProperty(pause_resume_button_, "enabled", true); + accumulating_->assignProperty(pause_resume_button_, "toolTip", nft_goal_msg); - accumulating_->assignProperty(navigation_mode_button_, "text", "Start Navigation"); + accumulating_->assignProperty(navigation_mode_button_, "text", "Start Waypoint Following"); accumulating_->assignProperty(navigation_mode_button_, "enabled", true); accumulating_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg); - accumulated_ = new QState(); + accumulated_wp_ = new QState(); + accumulated_wp_->setObjectName("accumulated_wp"); + accumulated_wp_->assignProperty(start_reset_button_, "text", "Cancel"); + accumulated_wp_->assignProperty(start_reset_button_, "toolTip", cancel_msg); + accumulated_wp_->assignProperty(start_reset_button_, "enabled", true); + + accumulated_wp_->assignProperty(pause_resume_button_, "text", "Start Nav Through Poses"); + accumulated_wp_->assignProperty(pause_resume_button_, "enabled", false); + accumulated_wp_->assignProperty(pause_resume_button_, "toolTip", nft_goal_msg); + + accumulated_wp_->assignProperty(navigation_mode_button_, "text", "Start Waypoint Following"); + accumulated_wp_->assignProperty(navigation_mode_button_, "enabled", false); + accumulated_wp_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg); + + accumulated_nav_through_poses_ = new QState(); + accumulated_nav_through_poses_->setObjectName("accumulated_nav_through_poses"); + accumulated_nav_through_poses_->assignProperty(start_reset_button_, "text", "Cancel"); + accumulated_nav_through_poses_->assignProperty(start_reset_button_, "toolTip", cancel_msg); + accumulated_nav_through_poses_->assignProperty(start_reset_button_, "enabled", true); + + accumulated_nav_through_poses_->assignProperty( + pause_resume_button_, "text", + "Start Nav Through Poses"); + accumulated_nav_through_poses_->assignProperty(pause_resume_button_, "enabled", false); + accumulated_nav_through_poses_->assignProperty(pause_resume_button_, "toolTip", nft_goal_msg); + + accumulated_nav_through_poses_->assignProperty( + navigation_mode_button_, "text", + "Start Waypoint Following"); + accumulated_nav_through_poses_->assignProperty(navigation_mode_button_, "enabled", false); + accumulated_nav_through_poses_->assignProperty( + navigation_mode_button_, "toolTip", + waypoint_goal_msg); // State entered to cancel the navigate_to_pose action canceled_ = new QState(); @@ -176,7 +219,10 @@ Nav2Panel::Nav2Panel(QWidget * parent) QObject::connect(paused_, SIGNAL(entered()), this, SLOT(onPause())); QObject::connect(resumed_, SIGNAL(exited()), this, SLOT(onResume())); QObject::connect(accumulating_, SIGNAL(entered()), this, SLOT(onAccumulating())); - QObject::connect(accumulated_, SIGNAL(entered()), this, SLOT(onAccumulated())); + QObject::connect(accumulated_wp_, SIGNAL(entered()), this, SLOT(onAccumulatedWp())); + QObject::connect( + accumulated_nav_through_poses_, SIGNAL(entered()), this, + SLOT(onAccumulatedNTP())); // Start/Reset button click transitions initial_->addTransition(start_reset_button_, SIGNAL(clicked()), idle_); @@ -184,20 +230,26 @@ Nav2Panel::Nav2Panel(QWidget * parent) running_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_); paused_->addTransition(start_reset_button_, SIGNAL(clicked()), reset_); idle_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulating_); - accumulating_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulated_); + accumulating_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulated_wp_); + accumulating_->addTransition( + pause_resume_button_, SIGNAL( + clicked()), accumulated_nav_through_poses_); accumulating_->addTransition(start_reset_button_, SIGNAL(clicked()), idle_); + accumulated_wp_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_); + accumulated_nav_through_poses_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_); // Internal state transitions canceled_->addTransition(canceled_, SIGNAL(entered()), idle_); reset_->addTransition(reset_, SIGNAL(entered()), initial_); resumed_->addTransition(resumed_, SIGNAL(entered()), idle_); - accumulated_->addTransition(accumulated_, SIGNAL(entered()), idle_); // Pause/Resume button click transitions idle_->addTransition(pause_resume_button_, SIGNAL(clicked()), paused_); paused_->addTransition(pause_resume_button_, SIGNAL(clicked()), resumed_); - // ROSAction Transitions + // ROSAction Transitions: So when actions are updated remotely (failing, succeeding, etc) + // the state of the application will also update. This means that if in the processing + // states and then goes inactive, move back to the idle state. Vise versa as well. ROSActionQTransition * idleTransition = new ROSActionQTransition(QActionState::INACTIVE); idleTransition->setTargetState(running_); idle_->addTransition(idleTransition); @@ -206,6 +258,24 @@ Nav2Panel::Nav2Panel(QWidget * parent) runningTransition->setTargetState(idle_); running_->addTransition(runningTransition); + ROSActionQTransition * idleAccumulatedWpTransition = + new ROSActionQTransition(QActionState::INACTIVE); + idleAccumulatedWpTransition->setTargetState(accumulated_wp_); + idle_->addTransition(idleAccumulatedWpTransition); + + ROSActionQTransition * accumulatedWpTransition = new ROSActionQTransition(QActionState::ACTIVE); + accumulatedWpTransition->setTargetState(idle_); + accumulated_wp_->addTransition(accumulatedWpTransition); + + ROSActionQTransition * idleAccumulatedNTPTransition = + new ROSActionQTransition(QActionState::INACTIVE); + idleAccumulatedNTPTransition->setTargetState(accumulated_nav_through_poses_); + idle_->addTransition(idleAccumulatedNTPTransition); + + ROSActionQTransition * accumulatedNTPTransition = new ROSActionQTransition(QActionState::ACTIVE); + accumulatedNTPTransition->setTargetState(idle_); + accumulated_nav_through_poses_->addTransition(accumulatedNTPTransition); + initial_thread_ = new InitialThread(client_nav_, client_loc_); connect(initial_thread_, &InitialThread::finished, initial_thread_, &QObject::deleteLater); @@ -230,6 +300,8 @@ Nav2Panel::Nav2Panel(QWidget * parent) initial_thread_, &InitialThread::navigationInactive, [this, navigation_inactive] { navigation_status_indicator_->setText(navigation_inactive); + navigation_goal_status_indicator_->setText(getGoalStatusLabel()); + navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel()); }); QObject::connect( initial_thread_, &InitialThread::localizationActive, @@ -251,7 +323,8 @@ Nav2Panel::Nav2Panel(QWidget * parent) state_machine_.addState(paused_); state_machine_.addState(resumed_); state_machine_.addState(accumulating_); - state_machine_.addState(accumulated_); + state_machine_.addState(accumulated_wp_); + state_machine_.addState(accumulated_nav_through_poses_); state_machine_.setInitialState(pre_initial_); @@ -263,6 +336,8 @@ Nav2Panel::Nav2Panel(QWidget * parent) QVBoxLayout * main_layout = new QVBoxLayout; main_layout->addWidget(navigation_status_indicator_); main_layout->addWidget(localization_status_indicator_); + main_layout->addWidget(navigation_goal_status_indicator_); + main_layout->addWidget(navigation_feedback_indicator_); main_layout->addWidget(pause_resume_button_); main_layout->addWidget(start_reset_button_); main_layout->addWidget(navigation_mode_button_); @@ -281,9 +356,14 @@ Nav2Panel::Nav2Panel(QWidget * parent) waypoint_follower_action_client_ = rclcpp_action::create_client( client_node_, - "FollowWaypoints"); + "follow_waypoints"); + nav_through_poses_action_client_ = + rclcpp_action::create_client( + client_node_, + "navigate_through_poses"); navigation_goal_ = nav2_msgs::action::NavigateToPose::Goal(); waypoint_follower_goal_ = nav2_msgs::action::FollowWaypoints::Goal(); + nav_through_poses_goal_ = nav2_msgs::action::NavigateThroughPoses::Goal(); wp_navigation_markers_pub_ = client_node_->create_publisher( @@ -303,6 +383,44 @@ void Nav2Panel::onInitialize() { auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + // create action feedback subscribers + navigation_feedback_sub_ = + node->create_subscription( + "navigate_to_pose/_action/feedback", + rclcpp::SystemDefaultsQoS(), + [this](const nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage::SharedPtr msg) { + navigation_feedback_indicator_->setText(getNavToPoseFeedbackLabel(msg->feedback)); + }); + nav_through_poses_feedback_sub_ = + node->create_subscription( + "navigate_through_poses/_action/feedback", + rclcpp::SystemDefaultsQoS(), + [this](const nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage::SharedPtr msg) { + navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel(msg->feedback)); + }); + + // create action goal status subscribers + navigation_goal_status_sub_ = node->create_subscription( + "navigate_to_pose/_action/status", + rclcpp::SystemDefaultsQoS(), + [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { + navigation_goal_status_indicator_->setText( + getGoalStatusLabel(msg->status_list.back().status)); + if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { + navigation_feedback_indicator_->setText(getNavToPoseFeedbackLabel()); + } + }); + nav_through_poses_goal_status_sub_ = node->create_subscription( + "navigate_through_poses/_action/status", + rclcpp::SystemDefaultsQoS(), + [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { + navigation_goal_status_indicator_->setText( + getGoalStatusLabel(msg->status_list.back().status)); + if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { + navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel()); + } + }); } void @@ -408,7 +526,19 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) void Nav2Panel::onCancelButtonPressed() { - if (state_machine_.configuration().contains(accumulating_)) { + if (navigation_goal_handle_) { + auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); + + if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); + } else { + navigation_goal_handle_.reset(); + } + } + + if (waypoint_follower_goal_handle_) { auto future_cancel = waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_); @@ -416,30 +546,44 @@ Nav2Panel::onCancelButtonPressed() rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); - return; + } else { + waypoint_follower_goal_handle_.reset(); } - } else { - auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); + } + + if (nav_through_poses_goal_handle_) { + auto future_cancel = + nav_through_poses_action_client_->async_cancel_goal(nav_through_poses_goal_handle_); if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); - return; + RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel nav through pose action"); + } else { + nav_through_poses_goal_handle_.reset(); } } + timer_.stop(); } void -Nav2Panel::onAccumulated() +Nav2Panel::onAccumulatedWp() { std::cout << "Start waypoint" << std::endl; startWaypointFollowing(acummulated_poses_); acummulated_poses_.clear(); } +void +Nav2Panel::onAccumulatedNTP() +{ + std::cout << "Start navigate through poses" << std::endl; + startNavThroughPoses(acummulated_poses_); + acummulated_poses_.clear(); +} + void Nav2Panel::onAccumulating() { @@ -449,7 +593,7 @@ Nav2Panel::onAccumulating() void Nav2Panel::timerEvent(QTimerEvent * event) { - if (state_machine_.configuration().contains(accumulating_)) { + if (state_machine_.configuration().contains(accumulated_wp_)) { if (event->timerId() == timer_.timerId()) { if (!waypoint_follower_goal_handle_) { RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); @@ -460,6 +604,27 @@ Nav2Panel::timerEvent(QTimerEvent * event) rclcpp::spin_some(client_node_); auto status = waypoint_follower_goal_handle_->get_status(); + // Check if the goal is still executing + if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) + { + state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE)); + } else { + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + timer_.stop(); + } + } + } else if (state_machine_.configuration().contains(accumulated_nav_through_poses_)) { + if (event->timerId() == timer_.timerId()) { + if (!nav_through_poses_goal_handle_) { + RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + return; + } + + rclcpp::spin_some(client_node_); + auto status = nav_through_poses_goal_handle_->get_status(); + // Check if the goal is still executing if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) @@ -501,7 +666,7 @@ Nav2Panel::startWaypointFollowing(std::vector p waypoint_follower_action_client_->wait_for_action_server(std::chrono::seconds(5)); if (!is_action_server_ready) { RCLCPP_ERROR( - client_node_->get_logger(), "FollowWaypoints action server is not available." + client_node_->get_logger(), "follow_waypoints action server is not available." " Is the initial pose set?"); return; } @@ -509,7 +674,7 @@ Nav2Panel::startWaypointFollowing(std::vector p // Send the goal poses waypoint_follower_goal_.poses = poses; - RCLCPP_INFO( + RCLCPP_DEBUG( client_node_->get_logger(), "Sending a path of %zu waypoints:", waypoint_follower_goal_.poses.size()); for (auto waypoint : waypoint_follower_goal_.poses) { @@ -521,7 +686,9 @@ Nav2Panel::startWaypointFollowing(std::vector p // Enable result awareness by providing an empty lambda function auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = [](auto) {}; + send_goal_options.result_callback = [this](auto) { + waypoint_follower_goal_handle_.reset(); + }; auto future_goal_handle = waypoint_follower_action_client_->async_send_goal(waypoint_follower_goal_, send_goal_options); @@ -542,6 +709,58 @@ Nav2Panel::startWaypointFollowing(std::vector p timer_.start(200, this); } +void +Nav2Panel::startNavThroughPoses(std::vector poses) +{ + auto is_action_server_ready = + nav_through_poses_action_client_->wait_for_action_server(std::chrono::seconds(5)); + if (!is_action_server_ready) { + RCLCPP_ERROR( + client_node_->get_logger(), "navigate_through_poses action server is not available." + " Is the initial pose set?"); + return; + } + + nav_through_poses_goal_.poses = poses; + RCLCPP_INFO( + client_node_->get_logger(), + "NavigateThroughPoses will be called using the BT Navigator's default behavior tree."); + + RCLCPP_DEBUG( + client_node_->get_logger(), "Sending a path of %zu waypoints:", + nav_through_poses_goal_.poses.size()); + for (auto waypoint : nav_through_poses_goal_.poses) { + RCLCPP_DEBUG( + client_node_->get_logger(), + "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); + } + + // Enable result awareness by providing an empty lambda function + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = [this](auto) { + nav_through_poses_goal_handle_.reset(); + }; + + auto future_goal_handle = + nav_through_poses_action_client_->async_send_goal(nav_through_poses_goal_, send_goal_options); + if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); + return; + } + + // Get the goal handle and save so that we can check on completion in the timer callback + nav_through_poses_goal_handle_ = future_goal_handle.get(); + if (!nav_through_poses_goal_handle_) { + RCLCPP_ERROR(client_node_->get_logger(), "Goal was rejected by server"); + return; + } + + timer_.start(200, this); +} + void Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose) { @@ -550,7 +769,7 @@ Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose) if (!is_action_server_ready) { RCLCPP_ERROR( client_node_->get_logger(), - "FollowWaypoints action server is not available." + "navigate_to_pose action server is not available." " Is the initial pose set?"); return; } @@ -558,10 +777,16 @@ Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose) // Send the goal pose navigation_goal_.pose = pose; + RCLCPP_INFO( + client_node_->get_logger(), + "NavigateToPose will be called using the BT Navigator's default behavior tree."); + // Enable result awareness by providing an empty lambda function auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = [](auto) {}; + send_goal_options.result_callback = [this](auto) { + navigation_goal_handle_.reset(); + }; auto future_goal_handle = navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options); @@ -682,6 +907,81 @@ Nav2Panel::updateWpNavigationMarkers() wp_navigation_markers_pub_->publish(std::move(marker_array)); } +inline QString +Nav2Panel::getGoalStatusLabel(int8_t status) +{ + std::string status_str; + switch (status) { + case action_msgs::msg::GoalStatus::STATUS_EXECUTING: + status_str = "active"; + break; + + case action_msgs::msg::GoalStatus::STATUS_SUCCEEDED: + status_str = "reached"; + break; + + case action_msgs::msg::GoalStatus::STATUS_CANCELED: + status_str = "canceled"; + break; + + case action_msgs::msg::GoalStatus::STATUS_ABORTED: + status_str = "aborted"; + break; + + case action_msgs::msg::GoalStatus::STATUS_UNKNOWN: + status_str = "unknown"; + break; + + default: + status_str = "inactive"; + break; + } + return QString( + std::string( + "
Feedback:" + + status_str + "
").c_str()); +} + +inline QString +Nav2Panel::getNavToPoseFeedbackLabel(nav2_msgs::action::NavigateToPose::Feedback msg) +{ + return QString(std::string("" + toLabel(msg) + "
").c_str()); +} + +inline QString +Nav2Panel::getNavThroughPosesFeedbackLabel(nav2_msgs::action::NavigateThroughPoses::Feedback msg) +{ + return QString( + std::string( + "" + toLabel(msg) + "
Poses remaining:" + + std::to_string(msg.number_of_poses_remaining) + + "
").c_str()); +} + +template +inline std::string Nav2Panel::toLabel(T & msg) +{ + return std::string( + "ETA:" + + toString(rclcpp::Duration(msg.estimated_time_remaining).seconds(), 0) + " s" + "Distance remaining:" + + toString(msg.distance_remaining, 2) + " m" + "Time taken:" + + toString(rclcpp::Duration(msg.navigation_time).seconds(), 0) + " s" + "Recoveries:" + + std::to_string(msg.number_of_recoveries) + + ""); +} + +inline std::string +Nav2Panel::toString(double val, int precision) +{ + std::ostringstream out; + out.precision(precision); + out << std::fixed << val; + return out.str(); +} + } // namespace nav2_rviz_plugins #include // NOLINT diff --git a/nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp b/nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp new file mode 100644 index 00000000000..702a6e50dcf --- /dev/null +++ b/nav2_rviz_plugins/src/particle_cloud_display/flat_weighted_arrows_array.cpp @@ -0,0 +1,141 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp" + +#include +#include +#include + +#include +#include + +#include "rviz_rendering/material_manager.hpp" + +namespace nav2_rviz_plugins +{ + +FlatWeightedArrowsArray::FlatWeightedArrowsArray(Ogre::SceneManager * scene_manager) +: scene_manager_(scene_manager), manual_object_(nullptr) {} + +FlatWeightedArrowsArray::~FlatWeightedArrowsArray() +{ + if (manual_object_) { + scene_manager_->destroyManualObject(manual_object_); + } +} + +void FlatWeightedArrowsArray::createAndAttachManualObject(Ogre::SceneNode * scene_node) +{ + manual_object_ = scene_manager_->createManualObject(); + manual_object_->setDynamic(true); + scene_node->attachObject(manual_object_); +} + +void FlatWeightedArrowsArray::updateManualObject( + Ogre::ColourValue color, + float alpha, + float min_length, + float max_length, + const std::vector & poses) +{ + clear(); + + color.a = alpha; + setManualObjectMaterial(); + rviz_rendering::MaterialManager::enableAlphaBlending(material_, alpha); + + manual_object_->begin( + material_->getName(), Ogre::RenderOperation::OT_LINE_LIST, "rviz_rendering"); + setManualObjectVertices(color, min_length, max_length, poses); + manual_object_->end(); +} + +void FlatWeightedArrowsArray::clear() +{ + if (manual_object_) { + manual_object_->clear(); + } +} + +void FlatWeightedArrowsArray::setManualObjectMaterial() +{ + static int material_count = 0; + std::string material_name = "FlatWeightedArrowsMaterial" + std::to_string(material_count++); + material_ = rviz_rendering::MaterialManager::createMaterialWithNoLighting(material_name); +} + +void FlatWeightedArrowsArray::setManualObjectVertices( + const Ogre::ColourValue & color, + float min_length, + float max_length, + const std::vector & poses) +{ + manual_object_->estimateVertexCount(poses.size() * 6); + + float scale = max_length - min_length; + float length; + for (const auto & pose : poses) { + length = std::min(std::max(pose.weight * scale + min_length, min_length), max_length); + Ogre::Vector3 vertices[6]; + vertices[0] = pose.position; // back of arrow + vertices[1] = + pose.position + pose.orientation * Ogre::Vector3(length, 0, 0); // tip of arrow + vertices[2] = vertices[1]; + vertices[3] = pose.position + pose.orientation * Ogre::Vector3( + 0.75f * length, 0.2f * length, 0); + vertices[4] = vertices[1]; + vertices[5] = pose.position + pose.orientation * Ogre::Vector3( + 0.75f * length, -0.2f * length, + 0); + + for (const auto & vertex : vertices) { + manual_object_->position(vertex); + manual_object_->colour(color); + } + } +} + +} // namespace nav2_rviz_plugins diff --git a/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp b/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp new file mode 100644 index 00000000000..f04abad5a61 --- /dev/null +++ b/nav2_rviz_plugins/src/particle_cloud_display/particle_cloud_display.cpp @@ -0,0 +1,423 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2018, Bosch Software Innovations GmbH. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp" + +#include +#include + +#include +#include +#include + +#include "rviz_common/logging.hpp" +#include "rviz_common/msg_conversions.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/color_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/validate_floats.hpp" + +#include "rviz_rendering/objects/arrow.hpp" +#include "rviz_rendering/objects/axes.hpp" + +#include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp" + +namespace nav2_rviz_plugins +{ +namespace +{ +struct ShapeType +{ + enum + { + Arrow2d, + Arrow3d, + Axes, + }; +}; + +} // namespace + +ParticleCloudDisplay::ParticleCloudDisplay( + rviz_common::DisplayContext * display_context, + Ogre::SceneNode * scene_node) +: ParticleCloudDisplay() +{ + context_ = display_context; + scene_node_ = scene_node; + scene_manager_ = context_->getSceneManager(); + + arrows2d_ = std::make_unique(scene_manager_); + arrows2d_->createAndAttachManualObject(scene_node); + arrow_node_ = scene_node_->createChildSceneNode(); + axes_node_ = scene_node_->createChildSceneNode(); + updateShapeChoice(); +} + +ParticleCloudDisplay::ParticleCloudDisplay() +: min_length_(0.02f), max_length_(0.3f) +{ + initializeProperties(); + + shape_property_->addOption("Arrow (Flat)", ShapeType::Arrow2d); + shape_property_->addOption("Arrow (3D)", ShapeType::Arrow3d); + shape_property_->addOption("Axes", ShapeType::Axes); + arrow_alpha_property_->setMin(0); + arrow_alpha_property_->setMax(1); + arrow_min_length_property_->setMax(max_length_); + arrow_max_length_property_->setMin(min_length_); +} + +void ParticleCloudDisplay::initializeProperties() +{ + shape_property_ = new rviz_common::properties::EnumProperty( + "Shape", "Arrow (Flat)", "Shape to display the pose as.", this, SLOT(updateShapeChoice())); + + arrow_color_property_ = new rviz_common::properties::ColorProperty( + "Color", QColor(255, 25, 0), "Color to draw the arrows.", this, SLOT(updateArrowColor())); + + arrow_alpha_property_ = new rviz_common::properties::FloatProperty( + "Alpha", + 1.0f, + "Amount of transparency to apply to the displayed poses.", + this, + SLOT(updateArrowColor())); + + arrow_min_length_property_ = new rviz_common::properties::FloatProperty( + "Min Arrow Length", min_length_, "Minimum length of the arrows.", this, SLOT(updateGeometry())); + + arrow_max_length_property_ = new rviz_common::properties::FloatProperty( + "Max Arrow Length", max_length_, "Maximum length of the arrows.", this, SLOT(updateGeometry())); + + // Scales are set based on initial values + length_scale_ = max_length_ - min_length_; + shaft_radius_scale_ = 0.0435; + head_length_scale_ = 0.3043; + head_radius_scale_ = 0.1304; +} + +ParticleCloudDisplay::~ParticleCloudDisplay() +{ + // because of forward declaration of arrow and axes, destructor cannot be declared in .hpp as + // default +} + +void ParticleCloudDisplay::onInitialize() +{ + MFDClass::onInitialize(); + arrows2d_ = std::make_unique(scene_manager_); + arrows2d_->createAndAttachManualObject(scene_node_); + arrow_node_ = scene_node_->createChildSceneNode(); + axes_node_ = scene_node_->createChildSceneNode(); + updateShapeChoice(); +} + +void ParticleCloudDisplay::processMessage(const nav2_msgs::msg::ParticleCloud::ConstSharedPtr msg) +{ + if (!validateFloats(*msg)) { + setStatus( + rviz_common::properties::StatusProperty::Error, + "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + if (!setTransform(msg->header)) { + return; + } + + poses_.resize(msg->particles.size()); + + for (std::size_t i = 0; i < msg->particles.size(); ++i) { + poses_[i].position = rviz_common::pointMsgToOgre(msg->particles[i].pose.position); + poses_[i].orientation = rviz_common::quaternionMsgToOgre(msg->particles[i].pose.orientation); + poses_[i].weight = static_cast(msg->particles[i].weight); + } + + updateDisplay(); + + context_->queueRender(); +} + +bool ParticleCloudDisplay::validateFloats(const nav2_msgs::msg::ParticleCloud & msg) +{ + for (auto & particle : msg.particles) { + if (!rviz_common::validateFloats(particle.pose) || + !rviz_common::validateFloats(particle.weight)) + { + return false; + } + } + return true; +} + +bool ParticleCloudDisplay::setTransform(std_msgs::msg::Header const & header) +{ + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(header, position, orientation)) { + setMissingTransformToFixedFrame(header.frame_id); + return false; + } + setTransformOk(); + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + return true; +} + +void ParticleCloudDisplay::updateDisplay() +{ + int shape = shape_property_->getOptionInt(); + switch (shape) { + case ShapeType::Arrow2d: + updateArrows2d(); + arrows3d_.clear(); + axes_.clear(); + break; + case ShapeType::Arrow3d: + updateArrows3d(); + arrows2d_->clear(); + axes_.clear(); + break; + case ShapeType::Axes: + updateAxes(); + arrows2d_->clear(); + arrows3d_.clear(); + break; + } +} + +void ParticleCloudDisplay::updateArrows2d() +{ + arrows2d_->updateManualObject( + arrow_color_property_->getOgreColor(), + arrow_alpha_property_->getFloat(), + min_length_, + max_length_, + poses_); +} + +void ParticleCloudDisplay::updateArrows3d() +{ + while (arrows3d_.size() < poses_.size()) { + arrows3d_.push_back(makeArrow3d()); + } + while (arrows3d_.size() > poses_.size()) { + arrows3d_.pop_back(); + } + + Ogre::Quaternion adjust_orientation(Ogre::Degree(-90), Ogre::Vector3::UNIT_Y); + float shaft_length; + for (std::size_t i = 0; i < poses_.size(); ++i) { + shaft_length = std::min( + std::max( + poses_[i].weight * length_scale_ + min_length_, + min_length_), max_length_); + arrows3d_[i]->set( + shaft_length, + shaft_length * shaft_radius_scale_, + shaft_length * head_length_scale_, + shaft_length * head_radius_scale_ + ); + arrows3d_[i]->setPosition(poses_[i].position); + arrows3d_[i]->setOrientation(poses_[i].orientation * adjust_orientation); + } +} + +void ParticleCloudDisplay::updateAxes() +{ + while (axes_.size() < poses_.size()) { + axes_.push_back(makeAxes()); + } + while (axes_.size() > poses_.size()) { + axes_.pop_back(); + } + float shaft_length; + for (std::size_t i = 0; i < poses_.size(); ++i) { + shaft_length = std::min( + std::max( + poses_[i].weight * length_scale_ + min_length_, + min_length_), max_length_); + axes_[i]->set(shaft_length, shaft_length * shaft_radius_scale_); + axes_[i]->setPosition(poses_[i].position); + axes_[i]->setOrientation(poses_[i].orientation); + } +} + +std::unique_ptr ParticleCloudDisplay::makeArrow3d() +{ + Ogre::ColourValue color = arrow_color_property_->getOgreColor(); + color.a = arrow_alpha_property_->getFloat(); + + auto arrow = std::make_unique( + scene_manager_, + arrow_node_, + min_length_, + min_length_ * shaft_radius_scale_, + min_length_ * head_length_scale_, + min_length_ * head_radius_scale_ + ); + + arrow->setColor(color); + return arrow; +} + +std::unique_ptr ParticleCloudDisplay::makeAxes() +{ + return std::make_unique( + scene_manager_, + axes_node_, + min_length_, + min_length_ * shaft_radius_scale_ + ); +} + +void ParticleCloudDisplay::reset() +{ + MFDClass::reset(); + arrows2d_->clear(); + arrows3d_.clear(); + axes_.clear(); +} + +void ParticleCloudDisplay::updateShapeChoice() +{ + int shape = shape_property_->getOptionInt(); + bool use_axes = shape == ShapeType::Axes; + + arrow_color_property_->setHidden(use_axes); + arrow_alpha_property_->setHidden(use_axes); + + if (initialized()) { + updateDisplay(); + } +} + +void ParticleCloudDisplay::updateArrowColor() +{ + int shape = shape_property_->getOptionInt(); + Ogre::ColourValue color = arrow_color_property_->getOgreColor(); + color.a = arrow_alpha_property_->getFloat(); + + if (shape == ShapeType::Arrow2d) { + updateArrows2d(); + } else if (shape == ShapeType::Arrow3d) { + for (const auto & arrow : arrows3d_) { + arrow->setColor(color); + } + } + context_->queueRender(); +} + +void ParticleCloudDisplay::updateGeometry() +{ + min_length_ = arrow_min_length_property_->getFloat(); + max_length_ = arrow_max_length_property_->getFloat(); + length_scale_ = max_length_ - min_length_; + + arrow_min_length_property_->setMax(max_length_); + arrow_max_length_property_->setMin(min_length_); + + int shape = shape_property_->getOptionInt(); + switch (shape) { + case ShapeType::Arrow2d: + updateArrows2d(); + arrows3d_.clear(); + axes_.clear(); + break; + case ShapeType::Arrow3d: + updateArrow3dGeometry(); + arrows2d_->clear(); + axes_.clear(); + break; + case ShapeType::Axes: + updateAxesGeometry(); + arrows2d_->clear(); + arrows3d_.clear(); + break; + } + + context_->queueRender(); +} + +void ParticleCloudDisplay::updateArrow3dGeometry() +{ + float shaft_length; + for (std::size_t i = 0; i < poses_.size() && i < arrows3d_.size(); ++i) { + shaft_length = std::min( + std::max( + poses_[i].weight * length_scale_ + min_length_, + min_length_), max_length_); + arrows3d_[i]->set( + shaft_length, + shaft_length * shaft_radius_scale_, + shaft_length * head_length_scale_, + shaft_length * head_radius_scale_ + ); + } +} + +void ParticleCloudDisplay::updateAxesGeometry() +{ + float shaft_length; + for (std::size_t i = 0; i < poses_.size() && i < axes_.size(); ++i) { + shaft_length = std::min( + std::max( + poses_[i].weight * length_scale_ + min_length_, + min_length_), max_length_); + axes_[i]->set(shaft_length, shaft_length * shaft_radius_scale_); + } +} + +void ParticleCloudDisplay::setShape(QString shape) +{ + shape_property_->setValue(shape); +} + +} // namespace nav2_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::ParticleCloudDisplay, rviz_common::Display) diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md new file mode 100644 index 00000000000..707e3722941 --- /dev/null +++ b/nav2_simple_commander/README.md @@ -0,0 +1,111 @@ +# Nav2 Simple (Python3) Commander + +## Overview + +The goal of this package is to provide a "navigation as a library" capability to Python3 users. We provide an API that handles all the ROS2-y and Action Server-y things for you such that you can focus on building an application leveraging the capabilities of Nav2. We also provide you with demos and examples of API usage to build common basic capabilities in autonomous mobile robotics. + +This was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) at [Samsung Research](https://www.sra.samsung.com/), with initial prototypes being prepared for the Keynote at the [2021 ROS Developers Day](https://www.theconstructsim.com/ros-developers-day-2021/) conference (code can be found [here](https://github.com/SteveMacenski/nav2_rosdevday_2021)). + +![](media/readme.gif) + +## API + +The methods provided by the basic navigator are shown below, with inputs and expected returns. If a server fails, it may throw an exception or return a `None` object, so please be sure to properly wrap your navigation calls in try/catch and check results for `None` type. + +| Robot Navigator Method | Description | +| --------------------------------- | -------------------------------------------------------------------------- | +| setInitialPose(initial_pose) | Sets the initial pose (`PoseStamped`) of the robot to localization. | +| goThroughPoses(poses) | Requests the robot to drive through a set of poses (list of `PoseStamped`).| +| goToPose(pose) | Requests the robot to drive to a pose (`PoseStamped`). | +| followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. | +| cancelNav() | Cancel an ongoing `goThroughPoses` `goToPose` or `followWaypoints` request.| +| isNavComplete() | Checks if navigation is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | +| getFeedback() | Gets feedback from navigation task, returns action server feedback object. | +| getResult() | Gets final result of navigation task, to be called after `isNavComplete` returns `True`. Returns action server result object. | +| getPath(start, goal) | Gets a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | +| getPathThroughPoses(start, goals) | Gets a path through a starting to a set of goals, a list of `PoseStamped`, `nav_msgs/Path`. | +| changeMap(map_filepath) | Requests a change from the current map to `map_filepath`'s yaml. | +| clearAllCostmaps() | Clears both the global and local costmaps. | +| clearLocalCostmap() | Clears the local costmap. | +| clearGlobalCostmap() | Clears the global costmap. | +| getGlobalCostmap() | Returns the global costmap, `nav2_msgs/Costmap` | +| getLocalCostmap() | Returns the local costmap, `nav2_msgs/Costmap` | +| waitUntilNav2Active() | Blocks until Nav2 is completely online and lifecycle nodes are in the active state. To be used in conjunction with autostart or external lifecycle bringup. | +| lifecycleStartup() | Sends a request to all lifecycle management servers to bring them into the active state, to be used if autostart is `false` and you want this program to control Nav2's lifecycle. | +| lifecycleShutdown() | Sends a request to all lifecycle management servers to shut them down. | + +A general template for building applications is as follows: + +``` python3 + +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy + +rclpy.init() + +nav = BasicNavigator() +... +nav.setInitialPose(init_pose) +nav.waitUntilNav2Active() # if autostarted, else use `lifecycleStartup()` +... +nav.goToPose(goal_pose) +while not nav.isNavComplete(): + feedback = nav.getFeedback() + if feedback.navigation_duration > 600: + nav.cancelNav() +... +result = nav.getResult() +if result == NavigationResult.SUCCEEDED: + print('Goal succeeded!') +elif result == NavigationResult.CANCELED: + print('Goal was canceled!') +elif result == NavigationResult.FAILED: + print('Goal failed!') +``` + +## Usage of Demos and Examples + +Make sure to install the `aws_robomaker_small_warehouse_world` package or build it in your local workspace alongside Nav2. It can be found [here](https://github.com/aws-robotics/aws-robomaker-small-warehouse-world). The demonstrations, examples, and launch files assume you're working with this gazebo world (such that the hard-programmed shelf locations and routes highlighting the API are meaningful). + +Make sure you have set the model directory of turtlebot3 simulation and aws warehouse world to the `GAZEBO_MODEL_PATH`. There are 2 main ways to run the demos of the `nav2_simple_commander` API. + +### Automatically + +The main benefit of this is automatically showing the above demonstrations in a single command for the default robot model and world. This will make use of Nav2's default robot and parameters set out in the main simulation launch file in `nav2_bringup`. + +``` bash +# Launch the launch file for the demo / example +ros2 launch nav2_simple_commander demo_security_launch.py +``` + +This will bring up the robot in the AWS Warehouse in a reasonable position, launch the autonomy script, and complete some task to demonstrate the `nav2_simple_commander` API. + +### Manually + +The main benefit of this is to be able to launch alternative robot models or different navigation configurations than the default for a specific technology demonstation. As long as Nav2 and the simulation (or physical robot) is running, the simple python commander examples / demos don't care what the robot is or how it got there. Since the examples / demos do contain hard-programmed item locations or routes, you should still utilize the AWS Warehouse. Obviously these are easy to update if you wish to adapt these examples / demos to another environment. + +``` bash +# Terminal 1: launch your robot navigation and simulation (or physical robot). For example +ros2 launch nav2_bringup tb3_simulation_launch.py world:=/path/to/aws_robomaker_small_warehouse_world/.world map:=/path/to/aws_robomaker_small_warehouse_world/.yaml + +# Terminal 2: launch your autonomy / application demo or example. For example +ros2 run nav2_simple_commander demo_security +``` + +Then you should see the autonomy application running! + +## Examples + +The `nav2_simple_commander` has a few examples to highlight the API functions available to you as a user: + +- `example_nav_to_pose.py` - Demonstrates the navigate to pose capabilities of the navigator, as well as a number of auxiliary methods. +- `example_nav_through_poses.py` - Demonstrates the navigate through poses capabilities of the navigator, as well as a number of auxiliary methods. +- `example_waypoint_follower.py` - Demonstrates the waypoint following capabilities of the navigator, as well as a number of auxiliary methods required. + +## Demos + +The `nav2_simple_commander` has a few demonstrations to highlight a couple of simple autonomy applications you can build using the `nav2_simple_commander` API: + +- `demo_security.py` - A simple security robot application, showing how to have a robot follow a security route using Navigate Through Poses to do a patrol route, indefinitely. +- `demo_picking.py` - A simple item picking application, showing how to have a robot drive to a specific shelf in a warehouse to either pick an item or have a person place an item into a basket and deliver it to a destination for shipping using Navigate To Pose. +- `demo_inspection.py` - A simple shelf inspection application, showing how to use the Waypoint Follower and task executors to take pictures, RFID scans, etc of shelves to analyze the current shelf statuses and locate items in the warehouse. diff --git a/nav2_simple_commander/launch/inspection_demo_launch.py b/nav2_simple_commander/launch/inspection_demo_launch.py new file mode 100644 index 00000000000..3cfbd1a2c3c --- /dev/null +++ b/nav2_simple_commander/launch/inspection_demo_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_inspection', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/nav_through_poses_example_launch.py b/nav2_simple_commander/launch/nav_through_poses_example_launch.py new file mode 100644 index 00000000000..11f39b9a37c --- /dev/null +++ b/nav2_simple_commander/launch/nav_through_poses_example_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_nav_through_poses', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/nav_to_pose_example_launch.py b/nav2_simple_commander/launch/nav_to_pose_example_launch.py new file mode 100644 index 00000000000..7d019884fe6 --- /dev/null +++ b/nav2_simple_commander/launch/nav_to_pose_example_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_nav_to_pose', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/picking_demo_launch.py b/nav2_simple_commander/launch/picking_demo_launch.py new file mode 100644 index 00000000000..5978570243f --- /dev/null +++ b/nav2_simple_commander/launch/picking_demo_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_picking', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/security_demo_launch.py b/nav2_simple_commander/launch/security_demo_launch.py new file mode 100644 index 00000000000..55ef0fe65a2 --- /dev/null +++ b/nav2_simple_commander/launch/security_demo_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='demo_security', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/launch/warehouse.world b/nav2_simple_commander/launch/warehouse.world new file mode 100644 index 00000000000..639965a031d --- /dev/null +++ b/nav2_simple_commander/launch/warehouse.world @@ -0,0 +1,682 @@ + + + + + 0 0 -9.8 + + 0.001 + 1 + 1000 + + + + + + + + model://aws_robomaker_warehouse_ShelfF_01 + + -5.795143 -0.956635 0 0 0 0 + + + + + model://aws_robomaker_warehouse_WallB_01 + + 0.0 0.0 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 0.57943 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -4.827049 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -8.6651 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -1.242668 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -3.038551 0 0 0 0 + + + + + model://aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -6.750542 0 0 0 0 + + + + + + + model://aws_robomaker_warehouse_GroundB_01 + + 0.0 0.0 -0.090092 0 0 0 + + + + + model://aws_robomaker_warehouse_Lamp_01 + + 0 0 -4 0 0 0 + + + + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 9.631706 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_Bucket_01 + + -1.8321 -6.3752 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_Bucket_01 + + 0.433449 8.59 0 0 0 -1.563161 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 5.708138 8.616844 -0.017477 0 0 0 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + 3.408638 8.616844 -0.017477 0 0 0 + + + + + model://aws_robomaker_warehouse_ClutteringA_01 + + -1.491287 5.222435 -0.017477 0 0 -1.583185 + + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.324959 3.822449 -0.012064 0 0 1.563871 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.54171 3.816475 -0.015663 0 0 -1.583191 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 5.384239 6.137154 0 0 0 3.150000 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + 3.236 6.137154 0 0 0 3.150000 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.573677 2.301994 -0.015663 0 0 -3.133191 + + + + + model://aws_robomaker_warehouse_ClutteringC_01 + + -1.2196 9.407 -0.015663 0 0 1.563871 + + + + + model://aws_robomaker_warehouse_ClutteringD_01 + + -1.634682 -7.811813 -0.319559 0 0 0 + + + + + model://aws_robomaker_warehouse_TrashCanC_01 + + -1.592441 7.715420 0 0 0 0 + + + + + + model://aws_robomaker_warehouse_PalletJackB_01 + + -0.276098 -9.481944 0.023266 0 0 0 + + + + + 0 0 9 0 0 0 + 0.5 0.5 0.5 1 + 0.2 0.2 0.2 1 + + 80 + 0.3 + 0.01 + 0.001 + + 1 + 0.1 0.1 -1 + + + + + -4.70385 10.895 16.2659 -0 0.921795 -1.12701 + orbit + perspective + + + + + 3.45 2.15 0.01 0.0 0.0 3.14 + + + + + + + -0.064 0 0.048 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 1.0 + + + + -0.064 0 0.048 0 0 0 + + + 0.265 0.265 0.089 + + + + + + -0.064 0 0 0 0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + + + + + true + 200 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + false + + + ~/out:=imu + + + + + + + + -0.052 0 0.111 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + + -0.052 0 0.111 0 0 0 + + + 0.0508 + 0.055 + + + + + + -0.064 0 0.121 0 0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + + true + true + -0.064 0 0.121 0 0 0 + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + base_scan + + + + + + + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + + + 0.0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/tire.dae + 0.001 0.001 0.001 + + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + + + + + + 30 + + + wheel_left_joint + wheel_right_joint + + + 0.287 + 0.066 + + + 20 + 1.0 + + cmd_vel + + + true + true + false + + odom + odom + base_footprint + + + + + + + ~/out:=joint_states + + 30 + wheel_left_joint + wheel_right_joint + + + + + + diff --git a/nav2_simple_commander/launch/waypoint_follower_example_launch.py b/nav2_simple_commander/launch/waypoint_follower_example_launch.py new file mode 100644 index 00000000000..eddcc0d4b5d --- /dev/null +++ b/nav2_simple_commander/launch/waypoint_follower_example_launch.py @@ -0,0 +1,77 @@ +# Copyright (c) 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + warehouse_dir = get_package_share_directory('aws_robomaker_small_warehouse_world') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + python_commander_dir = get_package_share_directory('nav2_simple_commander') + + map_yaml_file = os.path.join(warehouse_dir, 'maps', '005', 'map.yaml') + world = os.path.join(python_commander_dir, 'warehouse.world') + + # start the simulation + start_gazebo_server_cmd = ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_factory.so', world], + cwd=[warehouse_dir], output='screen') + + start_gazebo_client_cmd = ExecuteProcess( + cmd=['gzclient'], + cwd=[warehouse_dir], output='screen') + + urdf = os.path.join(nav2_bringup_dir, 'urdf', 'turtlebot3_waffle.urdf') + start_robot_state_publisher_cmd = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + arguments=[urdf]) + + # start the visualization + rviz_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False'}.items()) + + # start navigation + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'map': map_yaml_file}.items()) + + # start the demo autonomy task + demo_cmd = Node( + package='nav2_simple_commander', + executable='example_waypoint_follower', + emulate_tty=True, + output='screen') + + ld = LaunchDescription() + ld.add_action(start_gazebo_server_cmd) + ld.add_action(start_gazebo_client_cmd) + ld.add_action(start_robot_state_publisher_cmd) + ld.add_action(rviz_cmd) + ld.add_action(bringup_cmd) + ld.add_action(demo_cmd) + return ld diff --git a/nav2_simple_commander/media/readme.gif b/nav2_simple_commander/media/readme.gif new file mode 100644 index 00000000000..d3a925a252c Binary files /dev/null and b/nav2_simple_commander/media/readme.gif differ diff --git a/nav2_simple_commander/nav2_simple_commander/__init__.py b/nav2_simple_commander/nav2_simple_commander/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/nav2_simple_commander/nav2_simple_commander/demo_inspection.py b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py new file mode 100644 index 00000000000..111e54dc9d3 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_inspection.py @@ -0,0 +1,101 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from copy import deepcopy + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +import rclpy + +""" +Basic stock inspection demo. In this demonstration, the expectation +is that there are cameras or RFID sensors mounted on the robots +collecting information about stock quantity and location. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Inspection route, probably read in from a file for a real application + # from either a map or drive and repeat. + inspection_route = [ + [3.461, -0.450], + [5.531, -0.450], + [3.461, -2.200], + [5.531, -2.200], + [3.661, -4.121], + [5.431, -4.121], + [3.661, -5.850], + [5.431, -5.800]] + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate + navigator.waitUntilNav2Active() + + # Send our route + inspection_points = [] + inspection_pose = PoseStamped() + inspection_pose.header.frame_id = 'map' + inspection_pose.header.stamp = navigator.get_clock().now().to_msg() + inspection_pose.pose.orientation.z = 1.0 + inspection_pose.pose.orientation.w = 0.0 + for pt in inspection_route: + inspection_pose.pose.position.x = pt[0] + inspection_pose.pose.position.y = pt[1] + inspection_points.append(deepcopy(inspection_pose)) + navigator.followWaypoints(inspection_points) + + # Do something during our route (e.x. AI to analyze stock information or upload to the cloud) + # Simply the current waypoint ID for the demonstation + i = 0 + while not navigator.isNavComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Executing current waypoint: ' + + str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points))) + + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Inspection of shelves complete! Returning to start...') + elif result == NavigationResult.CANCELED: + print('Inspection of shelving was canceled. Returning to start...') + exit(1) + elif result == NavigationResult.FAILED: + print('Inspection of shelving failed! Returning to start...') + + # go back to start + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + navigator.goToPose(initial_pose) + while not navigator.isNavComplete(): + pass + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/demo_picking.py b/nav2_simple_commander/nav2_simple_commander/demo_picking.py new file mode 100644 index 00000000000..8dbf0f7b8e9 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_picking.py @@ -0,0 +1,123 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult + +import rclpy +from rclpy.duration import Duration + + +# Shelf positions for picking +shelf_positions = { + 'shelf_A': [-3.829, -7.604], + 'shelf_B': [-3.791, -3.287], + 'shelf_C': [-3.791, 1.254], + 'shelf_D': [-3.24, 5.861]} + +# Shipping destination for picked products +shipping_destinations = { + 'recycling': [-0.205, 7.403], + 'pallet_jack7': [-0.073, -8.497], + 'conveyer_432': [6.217, 2.153], + 'frieght_bay_3': [-6.349, 9.147]} + +""" +Basic item picking demo. In this demonstration, the expectation +is that there is a person at the item shelf to put the item on the robot +and at the pallet jack to remove it +(probably with some kind of button for 'got item, robot go do next task'). +""" + + +def main(): + # Recieved virtual request for picking item at Shelf A and bring to + # worker at the pallet jack 7 for shipping. This request would + # contain the shelf ID ("shelf_A") and shipping destination ("pallet_jack7") + #################### + request_item_location = 'shelf_C' + request_destination = 'pallet_jack7' + #################### + + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate + navigator.waitUntilNav2Active() + + shelf_item_pose = PoseStamped() + shelf_item_pose.header.frame_id = 'map' + shelf_item_pose.header.stamp = navigator.get_clock().now().to_msg() + shelf_item_pose.pose.position.x = shelf_positions[request_item_location][0] + shelf_item_pose.pose.position.y = shelf_positions[request_item_location][1] + shelf_item_pose.pose.orientation.z = 1.0 + shelf_item_pose.pose.orientation.w = 0.0 + print('Received request for item picking at ' + request_item_location + '.') + navigator.goToPose(shelf_item_pose) + + # Do something during our route + # (e.x. queue up future tasks or detect person for fine-tuned positioning) + # Simply print information for workers on the robot's ETA for the demonstation + i = 0 + while not navigator.isNavComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time of arrival at ' + request_item_location + + ' for worker: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Got product from ' + request_item_location + + '! Bringing product to shipping destination (' + request_destination + ')...') + shipping_destination = PoseStamped() + shipping_destination.header.frame_id = 'map' + shipping_destination.header.stamp = navigator.get_clock().now().to_msg() + shipping_destination.pose.position.x = shipping_destinations[request_destination][0] + shipping_destination.pose.position.y = shipping_destinations[request_destination][1] + shipping_destination.pose.orientation.z = 1.0 + shipping_destination.pose.orientation.w = 0.0 + navigator.goToPose(shipping_destination) + + elif result == NavigationResult.CANCELED: + print('Task at ' + request_item_location + ' was canceled. Returning to staging point...') + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + navigator.goToPose(initial_pose) + + elif result == NavigationResult.FAILED: + print('Task at ' + request_item_location + ' failed!') + exit(-1) + + while not navigator.isNavComplete(): + pass + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/demo_security.py b/nav2_simple_commander/nav2_simple_commander/demo_security.py new file mode 100644 index 00000000000..8180bc36b4d --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/demo_security.py @@ -0,0 +1,107 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from copy import deepcopy + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult + +import rclpy +from rclpy.duration import Duration + + +""" +Basic security route patrol demo. In this demonstration, the expectation +is that there are security cameras mounted on the robots recording or being +watched live by security staff. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Security route, probably read in from a file for a real application + # from either a map or drive and repeat. + security_route = [ + [1.792, 2.144], + [1.792, -5.44], + [1.792, -9.427], + [-3.665, -9.427], + [-3.665, -4.303], + [-3.665, 2.330], + [-3.665, 9.283]] + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Wait for navigation to fully activate + navigator.waitUntilNav2Active() + + # Do security route until dead + while rclpy.ok(): + # Send our route + route_poses = [] + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.header.stamp = navigator.get_clock().now().to_msg() + pose.pose.orientation.w = 1.0 + for pt in security_route: + pose.pose.position.x = pt[0] + pose.pose.position.y = pt[1] + route_poses.append(deepcopy(pose)) + navigator.goThroughPoses(route_poses) + + # Do something during our route (e.x. AI detection on camera images for anomalies) + # Simply print ETA for the demonstation + i = 0 + while not navigator.isNavComplete(): + i += 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time to complete current route: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + # Some failure mode, must stop since the robot is clearly stuck + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0): + print('Navigation has exceeded timeout of 180s, canceling request.') + navigator.cancelNav() + + # If at end of route, reverse the route to restart + security_route.reverse() + + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Route complete! Restarting...') + elif result == NavigationResult.CANCELED: + print('Security route was canceled, exiting.') + exit(1) + elif result == NavigationResult.FAILED: + print('Security route failed! Restarting from other side...') + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py new file mode 100644 index 00000000000..f4523b8974f --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py @@ -0,0 +1,139 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +import rclpy +from rclpy.duration import Duration + +""" +Basic navigation demo to go to poses. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # set our demo's goal poses + goal_poses = [] + goal_pose1 = PoseStamped() + goal_pose1.header.frame_id = 'map' + goal_pose1.header.stamp = navigator.get_clock().now().to_msg() + goal_pose1.pose.position.x = 1.5 + goal_pose1.pose.position.y = 0.55 + goal_pose1.pose.orientation.w = 0.707 + goal_pose1.pose.orientation.z = 0.707 + goal_poses.append(goal_pose1) + + # additional goals can be appended + goal_pose2 = PoseStamped() + goal_pose2.header.frame_id = 'map' + goal_pose2.header.stamp = navigator.get_clock().now().to_msg() + goal_pose2.pose.position.x = 1.5 + goal_pose2.pose.position.y = -3.75 + goal_pose2.pose.orientation.w = 0.707 + goal_pose2.pose.orientation.z = 0.707 + goal_poses.append(goal_pose2) + goal_pose3 = PoseStamped() + goal_pose3.header.frame_id = 'map' + goal_pose3.header.stamp = navigator.get_clock().now().to_msg() + goal_pose3.pose.position.x = -3.6 + goal_pose3.pose.position.y = -4.75 + goal_pose3.pose.orientation.w = 0.707 + goal_pose3.pose.orientation.z = 0.707 + goal_poses.append(goal_pose3) + + # sanity check a valid path exists + # path = navigator.getPathThroughPoses(initial_pose, goal_poses) + + navigator.goThroughPoses(goal_poses) + + i = 0 + while not navigator.isNavComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i = i + 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time of arrival: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + # Some navigation timeout to demo cancellation + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): + navigator.cancelNav() + + # Some navigation request change to demo preemption + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=35.0): + goal_pose4 = PoseStamped() + goal_pose4.header.frame_id = 'map' + goal_pose4.header.stamp = navigator.get_clock().now().to_msg() + goal_pose4.pose.position.x = -5.0 + goal_pose4.pose.position.y = -4.75 + goal_pose4.pose.orientation.w = 0.707 + goal_pose4.pose.orientation.z = 0.707 + navigator.goThroughPoses([goal_pose4]) + + # Do something depending on the return code + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Goal succeeded!') + elif result == NavigationResult.CANCELED: + print('Goal was canceled!') + elif result == NavigationResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py new file mode 100644 index 00000000000..cd302e418c9 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py @@ -0,0 +1,112 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +import rclpy +from rclpy.duration import Duration + +""" +Basic navigation demo to go to pose. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # Go to our demos first goal pose + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = -2.0 + goal_pose.pose.position.y = -0.5 + goal_pose.pose.orientation.w = 1.0 + + # sanity check a valid path exists + # path = navigator.getPath(initial_pose, goal_pose) + + navigator.goToPose(goal_pose) + + i = 0 + while not navigator.isNavComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i = i + 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Estimated time of arrival: ' + '{0:.0f}'.format( + Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9) + + ' seconds.') + + # Some navigation timeout to demo cancellation + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): + navigator.cancelNav() + + # Some navigation request change to demo preemption + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=18.0): + goal_pose.pose.position.x = -3.0 + navigator.goToPose(goal_pose) + + # Do something depending on the return code + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Goal succeeded!') + elif result == NavigationResult.CANCELED: + print('Goal was canceled!') + elif result == NavigationResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py new file mode 100644 index 00000000000..8ed0fc3d16b --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py @@ -0,0 +1,142 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, NavigationResult +import rclpy +from rclpy.duration import Duration + +""" +Basic navigation demo to go to poses. +""" + + +def main(): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 3.45 + initial_pose.pose.position.y = 2.15 + initial_pose.pose.orientation.z = 1.0 + initial_pose.pose.orientation.w = 0.0 + navigator.setInitialPose(initial_pose) + + # Activate navigation, if not autostarted. This should be called after setInitialPose() + # or this will initialize at the origin of the map and update the costmap with bogus readings. + # If autostart, you should `waitUntilNav2Active()` instead. + # navigator.lifecycleStartup() + + # Wait for navigation to fully activate, since autostarting nav2 + navigator.waitUntilNav2Active() + + # If desired, you can change or load the map as well + # navigator.changeMap('/path/to/map.yaml') + + # You may use the navigator to clear or obtain costmaps + # navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap() + # global_costmap = navigator.getGlobalCostmap() + # local_costmap = navigator.getLocalCostmap() + + # set our demo's goal poses to follow + goal_poses = [] + goal_pose1 = PoseStamped() + goal_pose1.header.frame_id = 'map' + goal_pose1.header.stamp = navigator.get_clock().now().to_msg() + goal_pose1.pose.position.x = 1.5 + goal_pose1.pose.position.y = 0.55 + goal_pose1.pose.orientation.w = 0.707 + goal_pose1.pose.orientation.z = 0.707 + goal_poses.append(goal_pose1) + + # additional goals can be appended + goal_pose2 = PoseStamped() + goal_pose2.header.frame_id = 'map' + goal_pose2.header.stamp = navigator.get_clock().now().to_msg() + goal_pose2.pose.position.x = 1.5 + goal_pose2.pose.position.y = -3.75 + goal_pose2.pose.orientation.w = 0.707 + goal_pose2.pose.orientation.z = 0.707 + goal_poses.append(goal_pose2) + goal_pose3 = PoseStamped() + goal_pose3.header.frame_id = 'map' + goal_pose3.header.stamp = navigator.get_clock().now().to_msg() + goal_pose3.pose.position.x = -3.6 + goal_pose3.pose.position.y = -4.75 + goal_pose3.pose.orientation.w = 0.707 + goal_pose3.pose.orientation.z = 0.707 + goal_poses.append(goal_pose3) + + # sanity check a valid path exists + # path = navigator.getPath(initial_pose, goal_pose1) + + nav_start = navigator.get_clock().now() + navigator.followWaypoints(goal_poses) + + i = 0 + while not navigator.isNavComplete(): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback + i = i + 1 + feedback = navigator.getFeedback() + if feedback and i % 5 == 0: + print('Executing current waypoint: ' + + str(feedback.current_waypoint + 1) + '/' + str(len(goal_poses))) + now = navigator.get_clock().now() + + # Some navigation timeout to demo cancellation + if now - nav_start > Duration(seconds=600.0): + navigator.cancelNav() + + # Some follow waypoints request change to demo preemption + if now - nav_start > Duration(seconds=35.0): + goal_pose4 = PoseStamped() + goal_pose4.header.frame_id = 'map' + goal_pose4.header.stamp = now.to_msg() + goal_pose4.pose.position.x = -5.0 + goal_pose4.pose.position.y = -4.75 + goal_pose4.pose.orientation.w = 0.707 + goal_pose4.pose.orientation.z = 0.707 + goal_poses = [goal_pose4] + nav_start = now + navigator.followWaypoints(goal_poses) + + # Do something depending on the return code + result = navigator.getResult() + if result == NavigationResult.SUCCEEDED: + print('Goal succeeded!') + elif result == NavigationResult.CANCELED: + print('Goal was canceled!') + elif result == NavigationResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') + + navigator.lifecycleShutdown() + + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py new file mode 100644 index 00000000000..20428fdf1b3 --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -0,0 +1,421 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from enum import Enum +import time + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose +from nav2_msgs.action import FollowWaypoints, NavigateThroughPoses, NavigateToPose +from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy +from rclpy.qos import QoSProfile, QoSReliabilityPolicy + + +class NavigationResult(Enum): + UKNOWN = 0 + SUCCEEDED = 1 + CANCELED = 2 + FAILED = 3 + + +class BasicNavigator(Node): + + def __init__(self): + super().__init__(node_name='basic_navigator') + self.initial_pose = PoseStamped() + self.initial_pose.header.frame_id = 'map' + self.goal_handle = None + self.result_future = None + self.feedback = None + self.status = None + + amcl_pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1) + + self.initial_pose_received = False + self.nav_through_poses_client = ActionClient(self, + NavigateThroughPoses, + 'navigate_through_poses') + self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') + self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, + 'compute_path_to_pose') + self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses, + 'compute_path_through_poses') + self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped, + 'amcl_pose', + self._amclPoseCallback, + amcl_pose_qos) + self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, + 'initialpose', + 10) + self.change_maps_srv = self.create_client(LoadMap, '/map_server/load_map') + self.clear_costmap_global_srv = self.create_client( + ClearEntireCostmap, '/global_costmap/clear_entirely_global_costmap') + self.clear_costmap_local_srv = self.create_client( + ClearEntireCostmap, '/local_costmap/clear_entirely_local_costmap') + self.get_costmap_global_srv = self.create_client(GetCostmap, '/global_costmap/get_costmap') + self.get_costmap_local_srv = self.create_client(GetCostmap, '/local_costmap/get_costmap') + + def setInitialPose(self, initial_pose): + """Set the initial pose to the localization system.""" + self.initial_pose_received = False + self.initial_pose = initial_pose + self._setInitialPose() + + def goThroughPoses(self, poses): + """Send a `NavThroughPoses` action request.""" + self.debug("Waiting for 'NavigateThroughPoses' action server") + while not self.nav_through_poses_client.wait_for_server(timeout_sec=1.0): + self.info("'NavigateThroughPoses' action server not available, waiting...") + + goal_msg = NavigateThroughPoses.Goal() + goal_msg.poses = poses + + self.info('Navigating with ' + str(len(goal_msg.poses)) + ' goals.' + '...') + send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg, + self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Goal with ' + str(len(poses)) + ' poses was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def goToPose(self, pose): + """Send a `NavToPose` action request.""" + self.debug("Waiting for 'NavigateToPose' action server") + while not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0): + self.info("'NavigateToPose' action server not available, waiting...") + + goal_msg = NavigateToPose.Goal() + goal_msg.pose = pose + + self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' + + str(pose.pose.position.y) + '...') + send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg, + self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Goal to ' + str(pose.pose.position.x) + ' ' + + str(pose.pose.position.y) + ' was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def followWaypoints(self, poses): + """Send a `FollowWaypoints` action request.""" + self.debug("Waiting for 'FollowWaypoints' action server") + while not self.follow_waypoints_client.wait_for_server(timeout_sec=1.0): + self.info("'FollowWaypoints' action server not available, waiting...") + + goal_msg = FollowWaypoints.Goal() + goal_msg.poses = poses + + self.info('Following ' + str(len(goal_msg.poses)) + ' goals.' + '...') + send_goal_future = self.follow_waypoints_client.send_goal_async(goal_msg, + self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Following ' + str(len(poses)) + ' waypoints request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + + def cancelNav(self): + """Cancel pending navigation request of any type.""" + self.info('Canceling current goal.') + if self.result_future: + future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, future) + return + + def isNavComplete(self): + """Check if the navigation request of any type is complete yet.""" + if not self.result_future: + # task was cancelled or completed + return True + rclpy.spin_until_future_complete(self, self.result_future, timeout_sec=0.10) + if self.result_future.result(): + self.status = self.result_future.result().status + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.debug('Goal with failed with status code: {0}'.format(self.status)) + return True + else: + # Timed out, still processing, not complete yet + return False + + self.debug('Goal succeeded!') + return True + + def getFeedback(self): + """Get the pending action feedback message.""" + return self.feedback + + def getResult(self): + """Get the pending action result message.""" + if self.status == GoalStatus.STATUS_SUCCEEDED: + return NavigationResult.SUCCEEDED + elif self.status == GoalStatus.STATUS_ABORTED: + return NavigationResult.FAILED + elif self.status == GoalStatus.STATUS_CANCELED: + return NavigationResult.CANCELED + else: + return NavigationResult.UNKNOWN + + def waitUntilNav2Active(self): + """Block until the full navigation system is up and running.""" + self._waitForNodeToActivate('amcl') + self._waitForInitialPose() + self._waitForNodeToActivate('bt_navigator') + self.info('Nav2 is ready for use!') + return + + def getPath(self, start, goal): + """Send a `ComputePathToPose` action request.""" + self.debug("Waiting for 'ComputePathToPose' action server") + while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0): + self.info("'ComputePathToPose' action server not available, waiting...") + + goal_msg = ComputePathToPose.Goal() + goal_msg.goal = goal + goal_msg.start = start + + self.info('Getting path...') + send_goal_future = self.compute_path_to_pose_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Get path was rejected!') + return None + + self.result_future = self.goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, self.result_future) + self.status = self.result_future.result().status + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.warn('Getting path failed with status code: {0}'.format(self.status)) + return None + + return self.result_future.result().result.path + + def getPathThroughPoses(self, start, goals): + """Send a `ComputePathThroughPoses` action request.""" + self.debug("Waiting for 'ComputePathThroughPoses' action server") + while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): + self.info("'ComputePathThroughPoses' action server not available, waiting...") + + goal_msg = ComputePathThroughPoses.Goal() + goal_msg.goals = goals + goal_msg.start = start + + self.info('Getting path...') + send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error('Get path was rejected!') + return None + + self.result_future = self.goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, self.result_future) + self.status = self.result_future.result().status + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.warn('Getting path failed with status code: {0}'.format(self.status)) + return None + + return self.result_future.result().result.path + + def changeMap(self, map_filepath): + """Change the current static map in the map server.""" + while not self.change_maps_srv.wait_for_service(timeout_sec=1.0): + self.info('change map service not available, waiting...') + req = LoadMap.Request() + req.map_url = map_filepath + future = self.change_maps_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + status = future.result().result + if status != LoadMap.Response().RESULT_SUCCESS: + self.error('Change map request failed!') + else: + self.info('Change map request was successful!') + return + + def clearAllCostmaps(self): + """Clear all costmaps.""" + self.clearLocalCostmap() + self.clearGlobalCostmap() + return + + def clearLocalCostmap(self): + """Clear local costmap.""" + while not self.clear_costmap_local_srv.wait_for_service(timeout_sec=1.0): + self.info('Clear local costmaps service not available, waiting...') + req = ClearEntireCostmap.Request() + future = self.clear_costmap_local_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return + + def clearGlobalCostmap(self): + """Clear global costmap.""" + while not self.clear_costmap_global_srv.wait_for_service(timeout_sec=1.0): + self.info('Clear global costmaps service not available, waiting...') + req = ClearEntireCostmap.Request() + future = self.clear_costmap_global_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return + + def getGlobalCostmap(self): + """Get the global costmap.""" + while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0): + self.info('Get global costmaps service not available, waiting...') + req = GetCostmap.Request() + future = self.get_costmap_global_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result().map + + def getLocalCostmap(self): + """Get the local costmap.""" + while not self.get_costmap_local_srv.wait_for_service(timeout_sec=1.0): + self.info('Get local costmaps service not available, waiting...') + req = GetCostmap.Request() + future = self.get_costmap_local_srv.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result().map + + def lifecycleStartup(self): + """Startup nav2 lifecycle system.""" + self.info('Starting up lifecycle nodes based on lifecycle_manager.') + for srv_name, srv_type in self.get_service_names_and_types(): + if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes': + self.info('Starting up ' + srv_name) + mgr_client = self.create_client(ManageLifecycleNodes, srv_name) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info(srv_name + ' service not available, waiting...') + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().STARTUP + future = mgr_client.call_async(req) + + # starting up requires a full map->odom->base_link TF tree + # so if we're not successful, try forwarding the initial pose + while True: + rclpy.spin_until_future_complete(self, future, timeout_sec=0.10) + if not future: + self._waitForInitialPose() + else: + break + self.info('Nav2 is ready for use!') + return + + def lifecycleShutdown(self): + """Shutdown nav2 lifecycle system.""" + self.info('Shutting down lifecycle nodes based on lifecycle_manager.') + for srv_name, srv_type in self.get_service_names_and_types(): + if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes': + self.info('Shutting down ' + srv_name) + mgr_client = self.create_client(ManageLifecycleNodes, srv_name) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info(srv_name + ' service not available, waiting...') + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + future.result() + return + + def _waitForNodeToActivate(self, node_name): + # Waits for the node within the tester namespace to become active + self.debug('Waiting for ' + node_name + ' to become active..') + node_service = node_name + '/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info(node_service + ' service not available, waiting...') + + req = GetState.Request() + state = 'unknown' + while state != 'active': + self.debug('Getting ' + node_name + ' state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.debug('Result of get_state: %s' % state) + time.sleep(2) + return + + def _waitForInitialPose(self): + while not self.initial_pose_received: + self.info('Setting initial pose') + self._setInitialPose() + self.info('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1.0) + return + + def _amclPoseCallback(self, msg): + self.debug('Received amcl pose') + self.initial_pose_received = True + return + + def _feedbackCallback(self, msg): + self.debug('Received action feedback message') + self.feedback = msg.feedback + return + + def _setInitialPose(self): + msg = PoseWithCovarianceStamped() + msg.pose.pose = self.initial_pose.pose + msg.header.frame_id = self.initial_pose.header.frame_id + msg.header.stamp = self.initial_pose.header.stamp + self.info('Publishing Initial Pose') + self.initial_pose_pub.publish(msg) + return + + def info(self, msg): + self.get_logger().info(msg) + return + + def warn(self, msg): + self.get_logger().warn(msg) + return + + def error(self, msg): + self.get_logger().error(msg) + return + + def debug(self, msg): + self.get_logger().debug(msg) + return diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml new file mode 100644 index 00000000000..a30175bf7d9 --- /dev/null +++ b/nav2_simple_commander/package.xml @@ -0,0 +1,25 @@ + + + + nav2_simple_commander + 1.0.0 + An importable library for writing mobile robot applications in python3 + steve + Apache-2.0 + + python-enum34 + rclpy + geometry_msgs + nav2_msgs + action_msgs + lifecycle_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/nav2_simple_commander/resource/nav2_simple_commander b/nav2_simple_commander/resource/nav2_simple_commander new file mode 100644 index 00000000000..e69de29bb2d diff --git a/nav2_simple_commander/setup.cfg b/nav2_simple_commander/setup.cfg new file mode 100644 index 00000000000..2d91d903923 --- /dev/null +++ b/nav2_simple_commander/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/nav2_simple_commander +[install] +install-scripts=$base/lib/nav2_simple_commander diff --git a/nav2_simple_commander/setup.py b/nav2_simple_commander/setup.py new file mode 100644 index 00000000000..ca44856c5ad --- /dev/null +++ b/nav2_simple_commander/setup.py @@ -0,0 +1,36 @@ +from glob import glob +import os + +from setuptools import setup + + +package_name = 'nav2_simple_commander' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='steve', + maintainer_email='stevenmacenski@gmail.com', + description='An importable library for writing mobile robot applications in python3', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'example_nav_to_pose = nav2_simple_commander.example_nav_to_pose:main', + 'example_nav_through_poses = nav2_simple_commander.example_nav_through_poses:main', + 'example_waypoint_follower = nav2_simple_commander.example_waypoint_follower:main', + 'demo_picking = nav2_simple_commander.demo_picking:main', + 'demo_inspection = nav2_simple_commander.demo_inspection:main', + 'demo_security = nav2_simple_commander.demo_security:main', + ], + }, +) diff --git a/nav2_simple_commander/test/test_copyright.py b/nav2_simple_commander/test/test_copyright.py new file mode 100644 index 00000000000..cc8ff03f79a --- /dev/null +++ b/nav2_simple_commander/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/nav2_simple_commander/test/test_flake8.py b/nav2_simple_commander/test/test_flake8.py new file mode 100644 index 00000000000..27ee1078ff0 --- /dev/null +++ b/nav2_simple_commander/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/nav2_simple_commander/test/test_pep257.py b/nav2_simple_commander/test/test_pep257.py new file mode 100644 index 00000000000..b234a3840f4 --- /dev/null +++ b/nav2_simple_commander/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index e15c474d5f4..05b84e4f8b0 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -19,14 +19,13 @@ find_package(builtin_interfaces REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(pluginlib REQUIRED) -find_package(Ceres REQUIRED COMPONENTS SuiteSparse) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(ompl REQUIRED) find_package(OpenMP REQUIRED) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") @@ -35,12 +34,16 @@ add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) include_directories( include - ${CERES_INCLUDES} ${OMPL_INCLUDE_DIRS} ${OpenMP_INCLUDE_DIRS} ) -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") -set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") + +find_package(OpenMP) +if(OPENMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +endif() set(library_name nav2_smac_planner) @@ -62,16 +65,16 @@ set(dependencies eigen3_cmake_module ) -# SE2 plugin +# Hybrid plugin add_library(${library_name} SHARED - src/smac_planner.cpp + src/smac_planner_hybrid.cpp src/a_star.cpp - src/node_se2.cpp + src/node_hybrid.cpp src/costmap_downsampler.cpp src/node_2d.cpp ) -target_link_libraries(${library_name} ${CERES_LIBRARIES} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES} OpenMP::OpenMP_CXX) +target_link_libraries(${library_name} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES} OpenMP::OpenMP_CXX) target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS}) ament_target_dependencies(${library_name} @@ -82,12 +85,12 @@ ament_target_dependencies(${library_name} add_library(${library_name}_2d SHARED src/smac_planner_2d.cpp src/a_star.cpp - src/node_se2.cpp + src/node_hybrid.cpp src/costmap_downsampler.cpp src/node_2d.cpp ) -target_link_libraries(${library_name}_2d ${CERES_LIBRARIES} ${OMPL_LIBRARIES}) +target_link_libraries(${library_name}_2d ${OMPL_LIBRARIES}) target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS}) ament_target_dependencies(${library_name}_2d diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index b7f1552cbed..5d36fb47e4f 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -1,14 +1,14 @@ # Smac Planner The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 2 distinct plugins: -- `SmacPlanner`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models. +- `SmacPlannerHybrid`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models (ackermann and car models). - `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting Moore and Von Neumann models. It also introduces the following basic building blocks: - `CostmapDownsampler`: A library to take in a costmap object and downsample it to another resolution. -- `AStar`: A generic and highly optimized A* template library used by the planning plugins to search. Template implementations are provided for grid-A* and SE2 Hybrid-A* planning. Additional template for 3D planning also could be made available. +- `AStar`: A generic and highly optimized A* template library used by the planning plugins to search. Template implementations are provided for grid-A*, SE2 Hybrid-A* planning. Additional template for 3D planning also could be made available. - `CollisionChecker`: Collision check based on a robot's radius or footprint. -- `Smoother`: A Conjugate-gradient (CG) smoother with several optional cost function implementations for use. This is a cost-aware smoother unlike b-splines or bezier curves. +- `Smoother`: A path smoother to smooth out 2D, Hybrid-A\* paths. We have users reporting using this on: - Delivery robots @@ -16,16 +16,16 @@ We have users reporting using this on: ## Introduction -The `nav2_smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support cars, car-like, and ackermann vehicles using the `SmacPlanner` plugin which implements a Hybrid-A\* planner. This plugin is also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking. +The `nav2_smac_planner` package contains an optimized templated A* search algorithm used to create multiple A\*-based planners for multiple types of robot platforms. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). We support **circular** differential-drive and omni-directional drive robots using the `SmacPlanner2D` planner which implements a cost-aware A\* planner. We support **cars, car-like, and ackermann vehicles** using the `SmacPlannerHybrid` plugin which implements a Hybrid-A\* planner. These plugins are also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking. -The `SmacPlanner` fully-implements the Hybrid-A* planner as proposed in [Practical Search Techniques in Path Planning for Autonomous Driving](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf), including hybrid searching, CG smoothing, analytic expansions and hueristic functions. +The `SmacPlannerHybrid` implements the Hybrid-A* planner as proposed in [Practical Search Techniques in Path Planning for Autonomous Driving](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf), including hybrid searching, gradient descent smoothing, analytic expansions and hueristic functions. In summary... -The `SmacPlanner` is designed to work with: +The `SmacPlannerHybrid` is designed to work with: - Ackermann, car, and car-like robots - High speed or curvature constrained robots (as to not flip over, skid, or dump load at high speeds) -- Arbitrary shaped, non-circular differential or omnidirectional robots requring SE2 collision checking +- Arbitrary shaped, non-circular differential or omnidirectional robots requiring SE2 collision checking with constrained curvatures The `SmacPlanner2D` is designed to work with: - Circular, differential or omnidirectional robots @@ -33,26 +33,27 @@ The `SmacPlanner2D` is designed to work with: ## Features -We further improve on the Hybrid-A\* work in the following ways: +We further improve in the following ways: - Remove need for upsampling by searching with 10x smaller motion primitives (same as their upsampling ratio). - Multi-resolution search allowing planning to occur at a coarser resolution for wider spaces (O(N^2) faster). - Cost-aware penalty functions in search resulting in far smoother plans (further reducing requirement to smooth). -- Additional cost functions in the CG smoother including path deflection. -- Approximated smoother Voronoi field using costmap inflation function. -- Fixed 3 mathematical issues in the original paper resulting in higher quality smoothing. +- Gradient descent smoother - Faster planning than original paper by highly optimizing the template A\* algorithm. +- Faster planning via precomputed heuristic, motion primitive, and other values. - Automatically adjusted search motion model sizes by motion model, costmap resolution, and bin sizing. - Closest path on approach within tolerance if exact path cannot be found or in invalid space. - Multi-model hybrid searching including Dubin and Reeds-Shepp models. More models may be trivially added. -- Time monitoring of planning to dynamically scale the maximum CG smoothing time based on remaining planning duration requested. - High unit and integration test coverage, doxygen documentation. - Uses modern C++14 language features and individual components are easily reusable. -- Speed optimizations: Fast approximation of shortest paths with wavefront hueristic, no data structure graph lookups in main loop, near-zero copy main loop, Voronoi field approximation, et al. +- Speed optimizations: no data structure graph lookups in main loop, near-zero copy main loop, dynamically generated graph and dynamic programming-based obstacle heuristic, optional recomputation of heuristics for subsiquent planning requests of the same goal, etc. - Templated Nodes and A\* implementation to support additional robot extensions. +- Selective re-evaluation of the obstacle heuristic per goal/map or each iteration, which can speed up subsiquent replanning 20x or more. -All of these features (multi-resolution, models, smoother, etc) are also available in the 2D `SmacPlanner2D` plugin. +All of these features (multi-resolution, models, smoother, etc) are also available in the `SmacPlanner2D` plugin. -The 2D A\* implementation also does not have any of the weird artifacts introduced by the gradient wavefront-based 2D A\* implementation in the NavFn Planner. While this 2D A\* planner is slightly slower, I believe it's well worth the increased quality in paths. Though the `SmacPlanner2D` is grid-based, any reasonable local trajectory planner - including those supported by Navigation2 - will not have any issue with grid-based plans. +The 2D A\* implementation also does not have any of the weird artifacts introduced by the gradient wavefront-based 2D A\* implementation in the NavFn Planner. While this 2D A\* planner is slightly slower, I believe it's well worth the increased quality in paths. Though the `SmacPlanner2D` is grid-based, any reasonable local trajectory planner - including those supported by Nav2 - will not have any issue with grid-based plans. + +Note: In prior releases, a CG smoother largely implementing the original Hybrid-A\* paper's. However, this smoother failed to consistently provide useful results, took too much compute time, and was deprecated. While smoothing a path 95% of the time seems like a "good" solution, we need something far more reliable for practical use. Since we are working with mobile robots and not autonomous cars at 60 mph, we can take some different liberties in smoothing knowing that our local trajectory planners are pretty smart. If you are looking for it, it now lives in the `test/` directory in a depreciated folder. This smoother has been replaced by a B-Spline inspired solution which is faster, far more consistent, and simpler to understand. While this smoother is **not** cost-aware, we have added cost-aware penalty functions in the planners themselves to push the plans away from high-cost spaces and we do check for validity of smoothed sections to ensure feasibility. ## Metrics @@ -69,17 +70,15 @@ For example, the following path (roughly 85 meters) path took 33ms to compute. The basic design centralizes a templated A\* implementation that handles the search of a graph of nodes. The implementation is templated by the nodes, `NodeT`, which contain the methods needed to compute the hueristics, travel costs, and search neighborhoods. The outcome of this design is then a standard A\* implementation that can be used to traverse any type of graph as long as a node template can be created for it. -We provide by default a 2D node template (`Node2D`) which does 2D grid-search with either 4 or 8-connected neighborhoods, but the smoother can be used to smooth it out. We also provide a SE2 node template (`NodeSE2`) which does SE2 (X, Y, theta) search and collision checking on Dubin or Reeds-Shepp motion models. Additional templates could be easily made and included for 3D grid search and non-grid base searching like routing. - -Additional node templates could be added into the future to better support other types of robot path planning, such as including a state lattice motion primitive node and 3D path planning. Pull requests or discussions aroudn this point is encouraged. +We provide by default a 2D node template (`Node2D`) which does 2D grid-search with either 4 or 8-connected neighborhoods, but the smoother smooths it out on turns. We also provide a Hybrid A\* node template (`NodeHybrid`) which does SE2 (X, Y, theta) search and collision checking on Dubin or Reeds-Shepp motion models. Additional templates could be easily made and included for 3D grid search and non-grid base searching like routing. -In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path. For the SE2 and `SmacPlanner` plugins, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. +In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A\* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A\* path. For the `SmacPlannerHybrid` plugin, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A\* is also promised to be feasible for differential and omni-directional robots. We isolated the A\*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves. ## Parameters -See inline description of parameters in the `SmacPlanner`. This includes comments as specific parameters apply to `SmacPlanner2D` and `SmacPlanner` in SE2 place. +See inline description of parameters in the `SmacPlanner`. This includes comments as specific parameters apply to `SmacPlanner2D` and `SmacPlanner` in place. ``` planner_server: @@ -89,44 +88,31 @@ planner_server: GridBased: plugin: "nav2_smac_planner/SmacPlanner" - tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: false # allow traveling in unknown space - max_iterations: -1 # maximum total iterations to search for before failing - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only - max_planning_time: 2.0 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. - smooth_path: false # Whether to smooth searched path - motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; SE2 Dubin, Redds-Shepp - angle_quantization_bins: 72 # For SE2 node: Number of angle bins for search, must be 1 for 2D node (no angle search) - minimum_turning_radius: 0.20 # For SE2 node & smoother: minimum turning radius in m of path / vehicle - reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.20 # For SE2 node: penalty to apply if motion is changing directions, must be >= 0 - non_straight_penalty: 1.05 # For SE2 node: penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 1.3 # For SE2 node: penalty to apply to higher cost zones - + tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: false # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only + max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. + motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; Hybrid Dubin, Redds-Shepp; State Lattice set internally + cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) + minimum_turning_radius: 0.40 # For Hybrid nodes: minimum turning radius in m of path / vehicle + angle_quantization_bins: 64 # For Hybrid/Lattice nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) + analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach. + minimum_turning_radius: 0.40 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle + reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.15 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0 + non_straight_penalty: 1.50 # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 1.7 # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + lookup_table_size: 20 # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. smoother: - smoother: - w_curve: 30.0 # weight to minimize curvature of path - w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight - w_smooth: 30000.0 # weight to maximize smoothness of path - w_cost: 0.025 # weight to steer robot away from collision and cost - cost_scaling_factor: 10.0 # this should match the inflation layer's parameter - - # I do not recommend users mess with this unless they're doing production tuning - optimizer: - max_time: 0.10 # maximum compute time for smoother - max_iterations: 500 # max iterations of smoother - debug_optimizer: false # print debug info - gradient_tol: 1.0e-10 - fn_tol: 1.0e-20 - param_tol: 1.0e-15 - advanced: - min_line_search_step_size: 1.0e-20 - max_num_line_search_step_size_iterations: 50 - line_search_sufficient_function_decrease: 1.0e-20 - max_num_line_search_direction_restarts: 10 - max_line_search_step_expansion: 50 + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1e-10 ``` ## Topics @@ -148,26 +134,65 @@ sudo apt-get install ros--smac-planner Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. -Some of the most popular tuning guides for Navigation / Navigation2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. +Some of the most popular tuning guides for Navigation / Nav2 even [call this out specifically](https://arxiv.org/pdf/1706.09068.pdf) that there's substantial benefit to creating a gentle potential field across the width of the map - after inscribed costs are applied - yet very few users do this. -This habit actually results in paths produced by NavFn, Global Planner, and now SmacPlanner to be very suboptimal. They really want to look for a smooth potential field rather than wide open 0-cost spaces in order to stay in the middle of spaces and deal with close-by moving obstacles better. +This habit actually results in paths produced by NavFn, Global Planner, and now SmacPlanner to be somewhat suboptimal. They really want to look for a smooth potential field rather than wide open 0-cost spaces in order to stay in the middle of spaces and deal with close-by moving obstacles better. So it is my recommendation in using this package, as well as all other cost-aware search planners available in ROS, to increase your inflation layer cost scale in order to adequately produce a smooth potential across the entire map. For very large open spaces, its fine to have 0-cost areas in the middle, but for halls, aisles, and similar; **please create a smooth potential to provide the best performance**. -### 2D Search and Smoothing +### Hybrid-A* Turning Radius' -While the 2D planner has the smoother available (albeit, default parameters are tuned for the Hybrid-A\* planner, so you may need to play with that), my recommendation is not to use it. +A very reasonable and logical assumption would be to set the `minimum_turning_radius` to the kinematic limits of your vehicle. For an ackermann car, that's a physical quantity; while for differential or omni robots, its a bit of a dance around what kind of turns you'd like your robot to be able to make. Obviously setting this to something insanely small (like 20 cm) means you have alot of options, but also probably means the raw output plans won't be very straight and smooth when you have 2+ meter wide aisles to work in. -The 2D planner provides a 4-connected or 8-connected neighborhood path. This path may have little zig-zags in order to get at another non-90 or non-45 degree heading. That is totally fine. Your local trajectory planner such as DWB and TEB take these points into account to follow, but you won't see any zig-zag behaviors of your final robot motion after given to a trajectory planner. +I assert that you should also consider the environment you operate within when setting this. While you should **absolutely not** set this to be any smaller than the actual limits of your vehicle, there are some useful side effects of increasing this value in practical use. If you work in an area wider than the turning circle of your robot, you have some options that will ultimately improve the performance of the planner (in terms of CPU and compute time) as well as generate paths that are more "smooth" directly out of the planner -- not requiring any explicit path smoothing. -The smoothing is more "pleasing" to human eyes, but you don't want to be owning additional compute when it doesn't largely impact the output. However, if you have a more sensitive local trajectory planner like a carrot follower (e.g. pure pursuit), then you will want to smooth out the paths in order to have something more easily followable. - -Take this advise into account. Some good numbers to potentially start with would be `cost_scaling_factor: 10.0` and `inflation_radius: 5.5`. +By default, `0.4m` is the setting which I think is "reasonable" for the smaller scale industrial grade robots (think Simbe, the small Fetch, or Locus robots) resulting in faster plans and less "wobbly" motions that do not require post-smoothing -- further improving CPU performance. I selected `0.4m` as a trade off between practical robots mentioned above and hobbyist users with a tiny-little-turtlebot-3 which might still need to navigate around some smaller cavities. ### Costmap Resolutions -We provide for both the Hybrid-A\* and 2D A\* implementations a costmap downsampler option. This can be **incredible** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For 60m paths in an office space, I was able to get it << 100ms at a 2-3x downsample rate. +We provide for the Hybrid-A\* and 2D A\* implementations a costmap downsampler option. This can be **incredible** beneficial when planning very long paths in larger spaces. The motion models for SE2 planning and neighborhood search in 2D planning is proportional to the costmap resolution. By downsampling it, you can N^2 reduce the number of expansions required to achieve a particular goal. However, the lower the resolution, the larger small obstacles appear and you won't be able to get super close to obstacles. This is a trade-off to make and test. Some numbers I've seen are 2-4x drops in planning CPU time for a 2-3x downsample rate. For long and complex paths, I was able to get it << 100ms at only a 2x downsample rate from a plan that otherwise took upward of 400ms. I recommend users using a 5cm resolution costmap and playing with the different values of downsampling rate until they achieve what they think is optimal performance (lowest number of expansions vs. necessity to achieve fine goal poses). Then, I would recommend to change the global costmap resolution to this new value. That way you don't own the compute of downsampling and maintaining a higher-resolution costmap that isn't used. Remember, the global costmap is **only** there to provide an environment for the planner to work in. It is not there for human-viewing even if a more fine resolution costmap is more human "pleasing". If you use multiple planners in the planner server, then you will want to use the highest resolution for the most needed planner and then use the downsampler to downsample to the Hybrid-A* resolution. + +### No path found for clearly valid goals or long compute times + +Before addressing the section below, make sure you have an appropriately set max iterations parameter. If you have a 1 km2 sized warehouse, clearly 5000 expansions will be insufficient. Try increasing this value if you're unable to achieve goals or disable it with the `-1` value to see if you are now able to plan within a reasonable time period. If you still have issues, there is a secondary effect which could be happening that is good to be aware of. + +In maps with small gaps or holes, you may see an issue planning to certain regions. If there are gaps small enough to be untraversible yet large enough that inflation doesn't close it up with inflated costs, then it is recommended to lightly touch up the map or increase your inflation to remove those spaces from non-lethal space. + +Seeing the figures below, you'll see an attempt to plan into a "U" shaped region across the map. The first figure shows the small gap in the map (from an imperfect SLAM session) which is nearly traversible, but not quite. From the starting location, that gap yeilds the shortest path to the goal, so the heuristics will attempt to route the paths in that direction. However, it is not possible to actually pass with a kinematically valid path with the footprint set. As a result, the planner expands all of its maximum 1,000,000 iterations attempting to fit through it (visualized in red). If an infinite number of iterations were allowed, eventually a valid path would be found, but might take significant time. + +By simply increasing the footprint (a bit hackier, the best solution is to edit the map to make this area impassible), then that gap is now properly blocked as un-navigable. In the second figure, you can see that the heuristics influence the expansion down a navigable route and is able to find a path in less than 10,000 iterations (or about 110ms). It is easy now! + +As such, it is recommended if you have sparse SLAM maps, gaps or holes in your map, that you lightly post-process them to fill those gaps or increasing your footprint's padding or radius to make these areas invalid. Without it, it might waste expansions on this small corridor that: A) you dont want your robot actually using B) probably isnt actually valid and a SLAM artifact and C) if there's a more open space, you'd rather it use that. + +![](media/A.png) +![](media/B.png) + +One interesting thing to note from the second figure is that you see a number of expansions in open space. This is due to travel / heuristic values being so similar, tuning values of the penalty weights can have a decent impact there. The defaults are set as a good middle ground between large open spaces and confined aisles (environment specific tuning could be done to reduce the number of expansions for a specific map, speeding up the planner). The planner actually runs substantially faster the more confined the areas of search / environments are -- but still plenty fast for even wide open areas! + +Sometimes visualizing the expansions is very useful to debug potential concerns (why does this goal take longer to compute, why can't I find a path, etc), should you on rare occasion run into an issue. The following snippet is what I used to visualize the expansion in the images above which may help you in future endevours. + +``` cpp +// In createPath() +static auto node = std::make_shared("test"); +static auto pub = node->create_publisher("expansions", 1); +geometry_msgs::msg::PoseArray msg; +geometry_msgs::msg::Pose msg_pose; +msg.header.stamp = node->now(); +msg.header.frame_id = "map"; + +... + +// Each time we expand a new node +msg_pose.position.x = _costmap->getOriginX() + (current_node->pose.x * _costmap->getResolution()); +msg_pose.position.y = _costmap->getOriginY() + (current_node->pose.y * _costmap->getResolution()); +msg.poses.push_back(msg_pose); + +... + +// On backtrace or failure +pub->publish(msg); +``` 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 6158422a7d7..39d5788b97d 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -27,7 +27,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_smac_planner/node_2d.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_basic.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" @@ -35,15 +35,6 @@ namespace nav2_smac_planner { -inline double squaredDistance( - const Eigen::Vector2d & p1, - const Eigen::Vector2d & p2) -{ - const double & dx = p1[0] - p2[0]; - const double & dy = p1[1] - p2[1]; - return hypot(dx, dy); -} - /** * @class nav2_smac_planner::AStarAlgorithm * @brief An A* implementation for planning in a costmap. Templated based on the Node type. @@ -61,6 +52,27 @@ class AStarAlgorithm typedef typename NodeVector::iterator NeighborIterator; typedef std::function NodeGetter; + /** + * @struct nav2_smac_planner::AnalyticExpansionNodes + * @brief Analytic expansion nodes and associated metadata + */ + + struct AnalyticExpansionNode + { + AnalyticExpansionNode( + NodePtr & node_in, + Coordinates & initial_coords_in, + Coordinates & proposed_coords_in) + : node(node_in), initial_coords(initial_coords_in), proposed_coords(proposed_coords_in) + {} + + NodePtr node; + Coordinates initial_coords; + Coordinates proposed_coords; + }; + + typedef std::vector AnalyticExpansionNodes; + /** * @struct nav2_smac_planner::NodeComparator * @brief Node comparison for priority queue sorting @@ -97,7 +109,9 @@ class AStarAlgorithm void initialize( const bool & allow_unknown, int & max_iterations, - const int & max_on_approach_iterations); + const int & max_on_approach_iterations, + const float & lookup_table_size, + const unsigned int & dim_3_size); /** * @brief Creating path from given costmap, start, and goal @@ -109,18 +123,10 @@ class AStarAlgorithm bool createPath(CoordinateVector & path, int & num_iterations, const float & tolerance); /** - * @brief Create the graph based on the node type. For 2D nodes, a cost grid. - * For 3D nodes, a SE2 grid without cost info as needs collision detector for footprint. - * @param x The total number of nodes in the X direction - * @param y The total number of nodes in the X direction - * @param dim_3 The total number of nodes in the theta or Z direction - * @param costmap Costmap to convert into the graph + * @brief Sets the collision checker to use + * @param collision_checker Collision checker to use for checking state validity */ - void createGraph( - const unsigned int & x, - const unsigned int & y, - const unsigned int & dim_3, - nav2_costmap_2d::Costmap2D * & costmap); + void setCollisionChecker(GridCollisionChecker * collision_checker); /** * @brief Set the goal for planning, as a node index @@ -144,29 +150,6 @@ class AStarAlgorithm const unsigned int & my, const unsigned int & dim_3); - /** - * @brief Set the footprint - * @param footprint footprint of robot - * @param use_radius Whether this footprint is a circle with radius - */ - void setFootprint(nav2_costmap_2d::Footprint footprint, bool use_radius); - - /** - * @brief Perform an analytic path expansion to the goal - * @param node The node to start the analytic path from - * @param getter The function object that gets valid nodes from the graph - * @return Node pointer to goal node if successful, else return nullptr - */ - NodePtr getAnalyticPath(const NodePtr & node, const NodeGetter & getter); - - /** - * @brief Set the starting pose for planning, as a node index - * @param node Node pointer to the goal node to backtrace - * @param path Reference to a vector of indicies of generated path - * @return whether the path was able to be backtraced - */ - bool backtracePath(NodePtr & node, CoordinateVector & path); - /** * @brief Get maximum number of iterations to plan * @return Reference to Maximum iterations parameter @@ -227,7 +210,7 @@ class AStarAlgorithm * @param cost The cost to sort into the open set of the node * @param node Node pointer reference to add to open set */ - inline void addNode(const float cost, NodePtr & node); + inline void addNode(const float & cost, NodePtr & node); /** * @brief Adds node to graph @@ -244,19 +227,12 @@ class AStarAlgorithm inline bool isGoal(NodePtr & node); /** - * @brief Get cost of traversal between nodes - * @param current_node Pointer to current node - * @param new_node Pointer to new node - * @return Reference traversal cost between the nodes - */ - inline float getTraversalCost(NodePtr & current_node, NodePtr & new_node); - - /** - * @brief Get total cost of traversal for a node - * @param node Pointer to current node - * @return Reference accumulated cost between the nodes + * @brief Set the starting pose for planning, as a node index + * @param node Node pointer to the goal node to backtrace + * @param path Reference to a vector of indicies of generated path + * @return whether the path was able to be backtraced */ - inline float & getAccumulatedCost(NodePtr & node); + bool backtracePath(NodePtr node, CoordinateVector & path); /** * @brief Get cost of heuristic of node @@ -287,10 +263,26 @@ class AStarAlgorithm * @return Node pointer reference to goal node if successful, else * return nullptr */ - inline NodePtr tryAnalyticExpansion( + NodePtr tryAnalyticExpansion( const NodePtr & current_node, const NodeGetter & getter, int & iterations, int & best_cost); + /** + * @brief Perform an analytic path expansion to the goal + * @param node The node to start the analytic path from + * @param getter The function object that gets valid nodes from the graph + * @return A set of analytically expanded nodes to the goal from current node, if possible + */ + AnalyticExpansionNodes getAnalyticPath(const NodePtr & node, const NodeGetter & getter); + + /** + * @brief Takes final analytic expansion and appends to current expanded node + * @param node The node to start the analytic path from + * @param expanded_nodes Expanded nodes to append to end of current search path + * @return Node pointer to goal node if successful, else return nullptr + */ + NodePtr setAnalyticPath(const NodePtr & node, const AnalyticExpansionNodes & expanded_nodes); + bool _traverse_unknown; int _max_iterations; int _max_on_approach_iterations; @@ -310,9 +302,7 @@ class AStarAlgorithm MotionModel _motion_model; NodeHeuristicPair _best_heuristic_node; - GridCollisionChecker _collision_checker; - nav2_costmap_2d::Footprint _footprint; - bool _is_radius_footprint; + GridCollisionChecker * _collision_checker; nav2_costmap_2d::Costmap2D * _costmap; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 6b77d13a55a..78f6c2e4adc 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -11,7 +11,7 @@ // 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 "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_smac_planner/constants.hpp" @@ -32,10 +32,14 @@ class GridCollisionChecker /** * @brief A constructor for nav2_smac_planner::GridCollisionChecker * @param costmap The costmap to collision check against + * @param num_quantizations The number of quantizations to precompute footprint + * orientations for to speed up collision checking */ GridCollisionChecker( - nav2_costmap_2d::Costmap2D * costmap) - : FootprintCollisionChecker(costmap) + nav2_costmap_2d::Costmap2D * costmap, + unsigned int num_quantizations) + : FootprintCollisionChecker(costmap), + num_quantizations_(num_quantizations) { } @@ -44,10 +48,47 @@ class GridCollisionChecker * @param footprint The footprint to collision check against * @param radius Whether or not the footprint is a circle and use radius collision checking */ - void setFootprint(const nav2_costmap_2d::Footprint & footprint, const bool & radius) + void setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_inscribed_cost) { - unoriented_footprint_ = footprint; + possible_inscribed_cost_ = possible_inscribed_cost; footprint_is_radius_ = radius; + + // Use radius, no caching required + if (radius) { + return; + } + + // No change, no updates required + if (footprint == unoriented_footprint_) { + return; + } + + bin_size_ = 2.0 * M_PI / static_cast(num_quantizations_); + oriented_footprints_.reserve(num_quantizations_); + double sin_th, cos_th; + geometry_msgs::msg::Point new_pt; + const unsigned int footprint_size = footprint.size(); + + // Precompute the orientation bins for checking to use + for (unsigned int i = 0; i != num_quantizations_; i++) { + sin_th = sin(i * bin_size_); + cos_th = cos(i * bin_size_); + nav2_costmap_2d::Footprint oriented_footprint; + oriented_footprint.reserve(footprint_size); + + for (unsigned int j = 0; j < footprint_size; j++) { + new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; + new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; + oriented_footprint.push_back(new_pt); + } + + oriented_footprints_.push_back(oriented_footprint); + } + + unoriented_footprint_ = footprint; } /** @@ -69,15 +110,47 @@ class GridCollisionChecker costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); if (!footprint_is_radius_) { - // if footprint, then we check for the footprint's points - footprint_cost_ = footprintCostAtPose( - wx, wy, static_cast(theta), unoriented_footprint_); + // if footprint, then we check for the footprint's points, but first see + // if the robot is even potentially in an inscribed collision + footprint_cost_ = costmap_->getCost( + static_cast(x), static_cast(y)); + + if (footprint_cost_ < possible_inscribed_cost_) { + return false; + } + + // If its inscribed, in collision, or unknown in the middle, + // no need to even check the footprint, its invalid + if (footprint_cost_ == UNKNOWN && !traverse_unknown) { + return true; + } + + if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { + return true; + } + + // if possible inscribed, need to check actual footprint pose. + // Use precomputed oriented footprints are done on initialization, + // offset by translation value to collision check + int angle_bin = theta / bin_size_; + geometry_msgs::msg::Point new_pt; + const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; + nav2_costmap_2d::Footprint current_footprint; + current_footprint.reserve(oriented_footprint.size()); + for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { + new_pt.x = wx + oriented_footprint[i].x; + new_pt.y = wy + oriented_footprint[i].y; + current_footprint.push_back(new_pt); + } + + footprint_cost_ = footprintCost(current_footprint); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= OCCUPIED; + return footprint_cost_ >= INSCRIBED; } else { // if radius, then we can check the center of the cost assuming inflation is used footprint_cost_ = costmap_->getCost( @@ -92,6 +165,25 @@ class GridCollisionChecker } } + /** + * @brief Check if in collision with costmap and footprint at pose + * @param i Index to search collision status of + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const unsigned int & i, + const bool & traverse_unknown) + { + footprint_cost_ = costmap_->getCost(i); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= INSCRIBED; + } + /** * @brief Get cost at footprint pose in costmap * @return the cost at the pose in costmap @@ -103,9 +195,13 @@ class GridCollisionChecker } protected: + std::vector oriented_footprints_; nav2_costmap_2d::Footprint unoriented_footprint_; double footprint_cost_; bool footprint_is_radius_; + unsigned int num_quantizations_; + double bin_size_; + double possible_inscribed_cost_{-1}; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index 1a67c746e24..8bd0db55754 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -26,6 +26,7 @@ enum class MotionModel MOORE = 2, DUBIN = 3, REEDS_SHEPP = 4, + STATE_LATTICE = 5, }; inline std::string toString(const MotionModel & n) @@ -39,6 +40,8 @@ inline std::string toString(const MotionModel & n) return "Dubin"; case MotionModel::REEDS_SHEPP: return "Reeds-Shepp"; + case MotionModel::STATE_LATTICE: + return "State Lattice"; default: return "Unknown"; } @@ -54,15 +57,17 @@ inline MotionModel fromString(const std::string & n) return MotionModel::DUBIN; } else if (n == "REEDS_SHEPP") { return MotionModel::REEDS_SHEPP; + } else if (n == "STATE_LATTICE") { + return MotionModel::STATE_LATTICE; } else { return MotionModel::UNKNOWN; } } -const float UNKNOWN = 255; -const float OCCUPIED = 254; -const float INSCRIBED = 253; -const float MAX_NON_OBSTACLE = 252; +const float UNKNOWN = 255.0; +const float OCCUPIED = 254.0; +const float INSCRIBED = 253.0; +const float MAX_NON_OBSTACLE = 252.0; const float FREE = 0; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 688e6ea5b97..f5a43d0fef9 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -24,8 +24,10 @@ #include #include +#include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" namespace nav2_smac_planner { @@ -58,10 +60,9 @@ class Node2D /** * @brief A constructor for nav2_smac_planner::Node2D - * @param cost_in The costmap cost at this node * @param index The index of this node for self-reference */ - explicit Node2D(unsigned char & cost_in, const unsigned int index); + explicit Node2D(const unsigned int index); /** * @brief A destructor for nav2_smac_planner::Node2D @@ -80,9 +81,8 @@ class Node2D /** * @brief Reset method for new search - * @param cost_in The costmap cost at this node */ - void reset(const unsigned char & cost); + void reset(); /** * @brief Gets the accumulated cost at this node * @return accumulated cost @@ -96,7 +96,7 @@ class Node2D * @brief Sets the accumulated cost at this node * @param reference to accumulated cost */ - inline void setAccumulatedCost(const float cost_in) + inline void setAccumulatedCost(const float & cost_in) { _accumulated_cost = cost_in; } @@ -110,6 +110,15 @@ class Node2D return _cell_cost; } + /** + * @brief Gets the costmap cost at this node + * @return costmap cost + */ + inline void setCost(const float & cost) + { + _cell_cost = cost; + } + /** * @brief Gets if cell has been visited in search * @param If cell was visited @@ -160,7 +169,7 @@ class Node2D * @param collision_checker Pointer to collision checker object * @return whether this node is valid and collision free */ - bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker); + bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker * collision_checker); /** * @brief get traversal cost from this node to child node @@ -199,40 +208,60 @@ class Node2D return Coordinates(index % width, index / width); } + /** + * @brief Get index + * @param Index Index of point + * @return coordinates of point + */ + static inline Coordinates getCoords(const unsigned int & index) + { + const unsigned int & size_x = _neighbors_grid_offsets[3]; + return Coordinates(index % size_x, index / size_x); + } + /** * @brief Get cost of heuristic of node * @param node Node index current * @param node Node index of new + * @param costmap Costmap ptr to use * @return Heuristic cost between the nodes */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const Coordinates & goal_coordinates, + const nav2_costmap_2d::Costmap2D * costmap); /** * @brief Initialize the neighborhood to be used in A* * We support 4-connect (VON_NEUMANN) and 8-connect (MOORE) - * @param x_size_uint The total x size to find neighbors * @param neighborhood The desired neighborhood type + * @param x_size_uint The total x size to find neighbors + * @param y_size The total y size to find neighbors + * @param num_angle_quantization Number of quantizations, must be 0 + * @param search_info Search parameters, unused by 2D node */ - static void initNeighborhood( - const unsigned int & x_size_uint, - const MotionModel & neighborhood); + static void initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & num_angle_quantization, + SearchInfo & search_info); + /** * @brief Retrieve all valid neighbors of a node. - * @param node Pointer to the node we are currently exploring in A* - * @param graph Reference to graph to discover new nodes + * @param validity_checker Functor for state validity checking + * @param collision_checker Collision checker to use + * @param traverse_unknown If unknown costs are valid to traverse * @param neighbors Vector of neighbors to be filled */ - static void getNeighbors( - NodePtr & node, + void getNeighbors( std::function & validity_checker, - GridCollisionChecker collision_checker, + GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors); Node2D * parent; - static double neutral_cost; + static float cost_travel_multiplier; static std::vector _neighbors_grid_offsets; private: diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index 290ca131624..0b69771ef45 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -28,7 +28,7 @@ #include "ompl/base/StateSpace.h" #include "nav2_smac_planner/constants.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/collision_checker.hpp" @@ -55,13 +55,13 @@ class NodeBasic { } - typename NodeT::Coordinates pose; // Used by NodeSE2 + typename NodeT::Coordinates pose; // Used by NodeHybrid NodeT * graph_node_ptr; unsigned int index; }; template class NodeBasic; -template class NodeBasic; +template class NodeBasic; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp similarity index 65% rename from nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp rename to nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index 4ea431c1875..e2b85f18a2f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_se2.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_SMAC_PLANNER__NODE_SE2_HPP_ -#define NAV2_SMAC_PLANNER__NODE_SE2_HPP_ +#ifndef NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ +#define NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ #include #include @@ -30,10 +30,14 @@ #include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/collision_checker.hpp" +#include "nav2_smac_planner/costmap_downsampler.hpp" namespace nav2_smac_planner { +typedef std::vector LookupTable; +typedef std::pair TrigValues; + // Need seperate pose struct for motion table operations /** @@ -65,18 +69,18 @@ struct MotionPose typedef std::vector MotionPoses; // Must forward declare -class NodeSE2; +class NodeHybrid; /** - * @struct nav2_smac_planner::MotionTable + * @struct nav2_smac_planner::HybridMotionTable * @brief A table of motion primitives and related functions */ -struct MotionTable +struct HybridMotionTable { /** - * @brief A constructor for nav2_smac_planner::MotionTable + * @brief A constructor for nav2_smac_planner::HybridMotionTable */ - MotionTable() {} + HybridMotionTable() {} /** * @brief Initializing using Dubin model @@ -106,53 +110,51 @@ struct MotionTable /** * @brief Get projections of motion models - * @param node Ptr to SE2 node + * @param node Ptr to NodeHybrid * @return A set of motion poses */ - MotionPoses getProjections(const NodeSE2 * node); - - /** - * @brief Get a projection of motion model - * @param node Ptr to SE2 node - * @return A motion pose - */ - MotionPose getProjection(const NodeSE2 * node, const unsigned int & motion_index); + MotionPoses getProjections(const NodeHybrid * node); MotionPoses projections; unsigned int size_x; unsigned int num_angle_quantization; float num_angle_quantization_float; + float min_turning_radius; float bin_size; float change_penalty; float non_straight_penalty; float cost_penalty; float reverse_penalty; ompl::base::StateSpacePtr state_space; + std::vector> delta_xs; + std::vector> delta_ys; + std::vector trig_values; }; /** - * @class nav2_smac_planner::NodeSE2 - * @brief NodeSE2 implementation for graph + * @class nav2_smac_planner::NodeHybrid + * @brief NodeHybrid implementation for graph, Hybrid-A* */ -class NodeSE2 +class NodeHybrid { public: - typedef NodeSE2 * NodePtr; - typedef std::unique_ptr> Graph; + typedef NodeHybrid * NodePtr; + typedef std::unique_ptr> Graph; typedef std::vector NodeVector; + /** - * @class nav2_smac_planner::NodeSE2::Coordinates - * @brief NodeSE2 implementation of coordinate structure + * @class nav2_smac_planner::NodeHybrid::Coordinates + * @brief NodeHybrid implementation of coordinate structure */ struct Coordinates { /** - * @brief A constructor for nav2_smac_planner::NodeSE2::Coordinates + * @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates */ Coordinates() {} /** - * @brief A constructor for nav2_smac_planner::NodeSE2::Coordinates + * @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates * @param x_in X coordinate * @param y_in Y coordinate * @param theta_in Theta coordinate @@ -161,28 +163,38 @@ class NodeSE2 : x(x_in), y(y_in), theta(theta_in) {} + inline bool operator==(const Coordinates & rhs) + { + return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; + } + + inline bool operator!=(const Coordinates & rhs) + { + return !(*this == rhs); + } + float x, y, theta; }; typedef std::vector CoordinateVector; /** - * @brief A constructor for nav2_smac_planner::NodeSE2 + * @brief A constructor for nav2_smac_planner::NodeHybrid * @param index The index of this node for self-reference */ - explicit NodeSE2(const unsigned int index); + explicit NodeHybrid(const unsigned int index); /** - * @brief A destructor for nav2_smac_planner::NodeSE2 + * @brief A destructor for nav2_smac_planner::NodeHybrid */ - ~NodeSE2(); + ~NodeHybrid(); /** * @brief operator== for comparisons - * @param NodeSE2 right hand side node reference + * @param NodeHybrid right hand side node reference * @return If cell indicies are equal */ - bool operator==(const NodeSE2 & rhs) + bool operator==(const NodeHybrid & rhs) { return this->_index == rhs._index; } @@ -214,7 +226,7 @@ class NodeSE2 * @brief Sets the accumulated cost at this node * @param reference to accumulated cost */ - inline void setAccumulatedCost(const float cost_in) + inline void setAccumulatedCost(const float & cost_in) { _accumulated_cost = cost_in; } @@ -261,24 +273,6 @@ class NodeSE2 inline void visited() { _was_visited = true; - _is_queued = false; - } - - /** - * @brief Gets if cell is currently queued in search - * @param If cell was queued - */ - inline bool & isQueued() - { - return _is_queued; - } - - /** - * @brief Sets if cell is currently queued in search - */ - inline void queued() - { - _is_queued = true; } /** @@ -295,7 +289,7 @@ class NodeSE2 * @param traverse_unknown If we can explore unknown nodes on the graph * @return whether this node is valid and collision free */ - bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker); + bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker * collision_checker); /** * @brief Get traversal cost of parent node to child node @@ -315,7 +309,7 @@ class NodeSE2 */ static inline unsigned int getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & angle, - const unsigned int & width, const unsigned int angle_quantization) + const unsigned int & width, const unsigned int & angle_quantization) { return angle + x * angle_quantization + y * width * angle_quantization; } @@ -344,7 +338,7 @@ class NodeSE2 */ static inline Coordinates getCoords( const unsigned int & index, - const unsigned int & width, const unsigned int angle_quantization) + const unsigned int & width, const unsigned int & angle_quantization) { return Coordinates( (index / angle_quantization) % width, // x @@ -356,11 +350,13 @@ class NodeSE2 * @brief Get cost of heuristic of node * @param node Node index current * @param node Node index of new + * @param costmap Costmap ptr to use * @return Heuristic cost between the nodes */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const Coordinates & goal_coordinates, + const nav2_costmap_2d::Costmap2D * costmap); /** * @brief Initialize motion models @@ -378,46 +374,87 @@ class NodeSE2 SearchInfo & search_info); /** - * @brief Compute the wavefront heuristic - * @param costmap Costmap to use to compute heuristic - * @param start_x Coordinate of Start X - * @param start_y Coordinate of Start Y - * @param goal_x Coordinate of Goal X - * @param goal_y Coordinate of Goal Y + * @brief Compute the SE2 distance heuristic + * @param lookup_table_dim Size, in costmap pixels, of the + * each lookup table dimension to populate + * @param motion_model Motion model to use for state space + * @param dim_3_size Number of quantization bins for caching + * @param search_info Info containing minimum radius to use + */ + static void precomputeDistanceHeuristic( + const float & lookup_table_dim, + const MotionModel & motion_model, + const unsigned int & dim_3_size, + const SearchInfo & search_info); + + /** + * @brief Compute the Obstacle heuristic + * @param node_coords Coordinates to get heuristic at + * @param goal_coords Coordinates to compute heuristic to + * @return heuristic Heuristic value */ - static void computeWavefrontHeuristic( - nav2_costmap_2d::Costmap2D * & costmap, - const unsigned int & start_x, const unsigned int & start_y, + static float getObstacleHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords); + + /** + * @brief Compute the Distance heuristic + * @param node_coords Coordinates to get heuristic at + * @param goal_coords Coordinates to compute heuristic to + * @param obstacle_heuristic Value of the obstacle heuristic to compute + * additional motion heuristics if required + * @return heuristic Heuristic value + */ + static float getDistanceHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const float & obstacle_heuristic); + + /** + * @brief reset the obstacle heuristic state + * @param costmap Costmap to use + * @param goal_coords Coordinates to start heuristic expansion at + */ + static void resetObstacleHeuristic( + nav2_costmap_2d::Costmap2D * costmap, const unsigned int & goal_x, const unsigned int & goal_y); /** * @brief Retrieve all valid neighbors of a node. - * @param node Pointer to the node we are currently exploring in A* * @param validity_checker Functor for state validity checking + * @param collision_checker Collision checker to use + * @param traverse_unknown If unknown costs are valid to traverse * @param neighbors Vector of neighbors to be filled */ - static void getNeighbors( - const NodePtr & node, - std::function & validity_checker, - GridCollisionChecker collision_checker, + void getNeighbors( + std::function & validity_checker, + GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors); - NodeSE2 * parent; + NodeHybrid * parent; Coordinates pose; - static double neutral_cost; - static MotionTable motion_table; + + // Constants required across all nodes but don't want to allocate more than once + static double travel_distance_cost; + static HybridMotionTable motion_table; + // Wavefront lookup and queue for continuing to expand as needed + static LookupTable obstacle_heuristic_lookup_table; + static std::queue obstacle_heuristic_queue; + static nav2_costmap_2d::Costmap2D * sampled_costmap; + static CostmapDownsampler downsampler; + // Dubin / Reeds-Shepp lookup and size for dereferencing + static LookupTable dist_heuristic_lookup_table; + static float size_lookup; private: float _cell_cost; float _accumulated_cost; unsigned int _index; bool _was_visited; - bool _is_queued; unsigned int _motion_primitive_index; - static std::vector _wavefront_heuristic; }; } // namespace nav2_smac_planner -#endif // NAV2_SMAC_PLANNER__NODE_SE2_HPP_ +#endif // NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index 31f10496092..ff235b02cfd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -21,6 +21,7 @@ #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/smoother.hpp" +#include "nav2_smac_planner/utils.hpp" #include "nav2_smac_planner/costmap_downsampler.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" @@ -85,24 +86,9 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; - /** - * @brief Create an Eigen Vector2D of world poses from continuous map coords - * @param mx float of map X coordinate - * @param my float of map Y coordinate - * @param costmap Costmap pointer - * @return Eigen::Vector2d eigen vector of the generated path - */ - Eigen::Vector2d getWorldCoords( - const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap); - - /** - * @brief Remove hooking at end of paths - * @param path Path to remove hooking from - */ - void removeHook(std::vector & path); - protected: std::unique_ptr> _a_star; + GridCollisionChecker _collision_checker; std::unique_ptr _smoother; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _costmap_downsampler; @@ -113,8 +99,6 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner int _downsampling_factor; bool _downsample_costmap; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; - SmootherParams _smoother_params; - OptimizerParams _optimizer_params; double _max_planning_time; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp similarity index 69% rename from nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp rename to nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 0f49dc7a4c7..109913b7fd0 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_ -#define NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_ +#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ +#define NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ #include #include @@ -21,6 +21,7 @@ #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/smoother.hpp" +#include "nav2_smac_planner/utils.hpp" #include "nav2_smac_planner/costmap_downsampler.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" @@ -35,18 +36,18 @@ namespace nav2_smac_planner { -class SmacPlanner : public nav2_core::GlobalPlanner +class SmacPlannerHybrid : public nav2_core::GlobalPlanner { public: /** * @brief constructor */ - SmacPlanner(); + SmacPlannerHybrid(); /** * @brief destructor */ - ~SmacPlanner(); + ~SmacPlannerHybrid(); /** * @brief Configuring plugin @@ -85,34 +86,12 @@ class SmacPlanner : public nav2_core::GlobalPlanner const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; - /** - * @brief Create an Eigen Vector2D of world poses from continuous map coords - * @param mx float of map X coordinate - * @param my float of map Y coordinate - * @param costmap Costmap pointer - * @return Eigen::Vector2d eigen vector of the generated path - */ - Eigen::Vector2d getWorldCoords( - const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap); - - /** - * @brief Create quaternion from A* coord bins - * @param theta continuous bin coordinates angle - * @return quaternion orientation in map frame - */ - geometry_msgs::msg::Quaternion getWorldOrientation(const float & theta); - - /** - * @brief Remove hooking at end of paths - * @param path Path to remove hooking from - */ - void removeHook(std::vector & path); - protected: - std::unique_ptr> _a_star; + std::unique_ptr> _a_star; + GridCollisionChecker _collision_checker; std::unique_ptr _smoother; rclcpp::Clock::SharedPtr _clock; - rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner")}; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlannerHybrid")}; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _costmap_downsampler; std::string _global_frame, _name; @@ -122,11 +101,9 @@ class SmacPlanner : public nav2_core::GlobalPlanner double _angle_bin_size; bool _downsample_costmap; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; - SmootherParams _smoother_params; - OptimizerParams _optimizer_params; double _max_planning_time; }; } // namespace nav2_smac_planner -#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_HPP_ +#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index b3b0fe82375..a9c45d4cda2 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2021, Samsung Research America // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,17 +23,15 @@ #include #include "nav2_smac_planner/types.hpp" -#include "nav2_smac_planner/smoother_cost_function.hpp" - -#include "ceres/ceres.h" -#include "Eigen/Core" +#include "nav2_util/geometry_utils.hpp" +#include "nav_msgs/msg/path.hpp" namespace nav2_smac_planner { /** * @class nav2_smac_planner::Smoother - * @brief A Conjugate Gradient 2D path smoother implementation + * @brief A path smoother implementation */ class Smoother { @@ -41,7 +39,13 @@ class Smoother /** * @brief A constructor for nav2_smac_planner::Smoother */ - Smoother() {} + explicit Smoother(const SmootherParams & params) + { + tolerance_ = params.tolerance_; + max_its_ = params.max_its_; + data_w_ = params.w_data_; + smooth_w_ = params.w_smooth_; + } /** * @brief A destructor for nav2_smac_planner::Smoother @@ -50,90 +54,199 @@ class Smoother /** * @brief Initialization of the smoother - * @param params OptimizerParam struct + * @param min_turning_radius Minimum turning radius (m) */ - void initialize(const OptimizerParams params) + void initialize(const double & min_turning_radius) { - _debug = params.debug; - - // General Params - - // 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT - _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT; - _options.line_search_type = ceres::WOLFE; - _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE; - _options.line_search_interpolation_type = ceres::CUBIC; - - _options.max_num_iterations = params.max_iterations; - _options.max_solver_time_in_seconds = params.max_time; - - _options.function_tolerance = params.fn_tol; - _options.gradient_tolerance = params.gradient_tol; - _options.parameter_tolerance = params.param_tol; - - _options.min_line_search_step_size = params.advanced.min_line_search_step_size; - _options.max_num_line_search_step_size_iterations = - params.advanced.max_num_line_search_step_size_iterations; - _options.line_search_sufficient_function_decrease = - params.advanced.line_search_sufficient_function_decrease; - _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction; - _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction; - _options.max_num_line_search_direction_restarts = - params.advanced.max_num_line_search_direction_restarts; - _options.line_search_sufficient_curvature_decrease = - params.advanced.line_search_sufficient_curvature_decrease; - _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion; - - if (_debug) { - _options.minimizer_progress_to_stdout = true; - } else { - _options.logging_type = ceres::SILENT; - } + min_turning_rad_ = min_turning_radius; } /** * @brief Smoother method * @param path Reference to path * @param costmap Pointer to minimal costmap - * @param smoother parameters weights + * @param max_time Maximum time to compute, stop early if over limit * @return If smoothing was successful */ bool smooth( - std::vector & path, - nav2_costmap_2d::Costmap2D * costmap, - const SmootherParams & params) + nav_msgs::msg::Path & path, + const nav2_costmap_2d::Costmap2D * costmap, + const double & max_time, + const bool do_refinement = true) { - _options.max_solver_time_in_seconds = params.max_time; + using namespace std::chrono; // NOLINT + steady_clock::time_point a = steady_clock::now(); + rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time); + + int its = 0; + double change = tolerance_; + const unsigned int & path_size = path.poses.size(); + double x_i, y_i, y_m1, y_ip1, y_im2, y_ip2, y_i_org, curvature; + unsigned int mx, my; + + // Adding 5% margin due to floating point error + const double max_curvature = (1.0 / min_turning_rad_) * 1.05; + nav_msgs::msg::Path new_path = path; + nav_msgs::msg::Path last_path = path; + + while (change >= tolerance_) { + its += 1; + change = 0.0; + + // Make sure the smoothing function will converge + if (its >= max_its_) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Number of iterations has exceeded limit of %i.", max_its_); + path = last_path; + updateApproximatePathOrientations(path); + return false; + } + + // Make sure still have time left to process + steady_clock::time_point b = steady_clock::now(); + rclcpp::Duration timespan(duration_cast>(b - a)); + if (timespan > max_dur) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Smoothing time exceeded allowed duration of %0.2f.", max_time); + path = last_path; + updateApproximatePathOrientations(path); + return false; + } + + for (unsigned int i = 2; i != path_size - 2; i++) { + for (unsigned int j = 0; j != 2; j++) { + x_i = getFieldByDim(path.poses[i], j); + y_i = getFieldByDim(new_path.poses[i], j); + y_m1 = getFieldByDim(new_path.poses[i - 1], j); + y_ip1 = getFieldByDim(new_path.poses[i + 1], j); + y_i_org = y_i; + + if (i > 2 && i < path_size - 2) { + // Smooth based on local 5 point neighborhood and original data locations + y_im2 = getFieldByDim(new_path.poses[i - 2], j); + y_ip2 = getFieldByDim(new_path.poses[i + 2], j); + y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_im2 + y_ip2 + y_ip1 + y_m1 - (4.0 * y_i)); + } else { + // Smooth based on local 3 point neighborhood and original data locations + // At boundary conditions, need to use a more local neighborhood because the first + // and last 2 points cannot move to ensure the boundry conditions are upheld + y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_ip1 + y_m1 - (2.0 * y_i)); + } + + setFieldByDim(new_path.poses[i], j, y_i); + change += abs(y_i - y_i_org); + } + + // validate update is admissible, only checks cost if a valid costmap pointer is provided + float cost = 0.0; + if (costmap) { + costmap->worldToMap( + getFieldByDim(new_path.poses[i], 0), + getFieldByDim(new_path.poses[i], 1), + mx, my); + cost = static_cast(costmap->getCost(mx, my)); + } + if (cost > MAX_NON_OBSTACLE) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Smoothing process resulted in an infeasible collision. " + "Returning the last path before the infeasibility was introduced."); + path = last_path; + updateApproximatePathOrientations(path); + return false; + } + } + + last_path = new_path; + } - double parameters[path.size() * 2]; // NOLINT - for (uint i = 0; i != path.size(); i++) { - parameters[2 * i] = path[i][0]; - parameters[2 * i + 1] = path[i][1]; + // Lets do additional refinement, it shouldn't take more than a couple milliseconds + // but really puts the path quality over the top. + if (do_refinement) { + smooth(new_path, costmap, max_time, false); } - ceres::GradientProblemSolver::Summary summary; - ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params)); - ceres::Solve(_options, problem, parameters, &summary); + for (unsigned int i = 3; i != path_size - 3; i++) { + if (getCurvature(new_path, i) > max_curvature) { + RCLCPP_DEBUG( + rclcpp::get_logger("SmacPlannerSmoother"), + "Smoothing process resulted in an infeasible curvature somewhere on the path. " + "This is most likely at the end point boundary conditions which will be further " + "refined as a perfect curve as you approach the goal. If this becomes a practical " + "issue for you, please file a ticket mentioning this message."); + updateApproximatePathOrientations(new_path); + path = new_path; + return false; + } + } - if (_debug) { - std::cout << summary.FullReport() << '\n'; + updateApproximatePathOrientations(new_path); + path = new_path; + return true; + } + +protected: + inline double getFieldByDim(const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim) + { + if (dim == 0) { + return msg.pose.position.x; + } else if (dim == 1) { + return msg.pose.position.y; + } else { + return msg.pose.position.z; } + } - if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) { - return false; + inline void setFieldByDim( + geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, + const double & value) + { + if (dim == 0) { + msg.pose.position.x = value; + } else if (dim == 1) { + msg.pose.position.y = value; + } else { + msg.pose.position.z = value; } + } - for (uint i = 0; i != path.size(); i++) { - path[i][0] = parameters[2 * i]; - path[i][1] = parameters[2 * i + 1]; + inline double getCurvature(const nav_msgs::msg::Path & path, const unsigned int i) + { + // k = 1 / r = acos(delta_phi) / |xi|, where delta_phi = (xi dot xi+1) / (|xi| * |xi+1|) + const double dxi_x = getFieldByDim(path.poses[i], 0) - getFieldByDim(path.poses[i - 1], 0); + const double dxi_y = getFieldByDim(path.poses[i], 1) - getFieldByDim(path.poses[i - 1], 1); + const double dxip1_x = getFieldByDim(path.poses[i + 1], 0) - getFieldByDim(path.poses[i], 0); + const double dxip1_y = getFieldByDim(path.poses[i + 1], 1) - getFieldByDim(path.poses[i], 1); + const double norm_dx_i = hypot(dxi_x, dxi_y); + const double norm_dx_ip1 = hypot(dxip1_x, dxip1_y); + double arg = (dxi_x * dxip1_x + dxi_y * dxip1_y) / (norm_dx_i * norm_dx_ip1); + + // In case of small out of bounds issues from floating point error + if (arg > 1.0) { + arg = 1.0; + } else if (arg < -1.0) { + arg = -1.0; } - return true; + return acos(arg) / norm_dx_i; + } + + inline void updateApproximatePathOrientations(nav_msgs::msg::Path & path) + { + using namespace nav2_util::geometry_utils; // NOLINT + double dx, dy, theta; + for (unsigned int i = 0; i != path.poses.size() - 1; i++) { + dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; + dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; + theta = atan2(dy, dx); + path.poses[i].pose.orientation = orientationAroundZAxis(theta); + } } -private: - bool _debug; - ceres::GradientProblemSolver::Options _options; + double min_turning_rad_, tolerance_, data_w_, smooth_w_; + int max_its_; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index eeeb9218737..dffc88062bc 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -17,6 +17,11 @@ #include #include +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_smac_planner { @@ -35,6 +40,51 @@ struct SearchInfo float reverse_penalty; float cost_penalty; float analytic_expansion_ratio; + std::string lattice_filepath; + bool cache_obstacle_heuristic; +}; + +/** + * @struct nav2_smac_planner::SmootherParams + * @brief Parameters for the smoother + */ +struct SmootherParams +{ + /** + * @brief A constructor for nav2_smac_planner::SmootherParams + */ + SmootherParams() + { + } + + /** + * @brief Get params from ROS parameter + * @param node Ptr to node + * @param name Name of plugin + */ + void get(std::shared_ptr node, const std::string & name) + { + std::string local_name = name + std::string(".smoother."); + + // Smoother params + nav2_util::declare_parameter_if_not_declared( + node, local_name + "tolerance", rclcpp::ParameterValue(1e-10)); + node->get_parameter(local_name + "tolerance", tolerance_); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "max_iterations", rclcpp::ParameterValue(1000)); + node->get_parameter(local_name + "max_iterations", max_its_); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_data", rclcpp::ParameterValue(0.2)); + node->get_parameter(local_name + "w_data", w_data_); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "w_smooth", rclcpp::ParameterValue(0.3)); + node->get_parameter(local_name + "w_smooth", w_smooth_); + } + + double tolerance_; + int max_its_; + double w_data_; + double w_smooth_; }; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp new file mode 100644 index 00000000000..0c58fbfc012 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -0,0 +1,109 @@ +// Copyright (c) 2021, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef NAV2_SMAC_PLANNER__UTILS_HPP_ +#define NAV2_SMAC_PLANNER__UTILS_HPP_ + +#include +#include + +#include "Eigen/Core" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "tf2/utils.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + +namespace nav2_smac_planner +{ + +/** +* @brief Create an Eigen Vector2D of world poses from continuous map coords +* @param mx float of map X coordinate +* @param my float of map Y coordinate +* @param costmap Costmap pointer +* @return Eigen::Vector2d eigen vector of the generated path +*/ +inline geometry_msgs::msg::Pose getWorldCoords( + const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) +{ + geometry_msgs::msg::Pose msg; + msg.position.x = + static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); + msg.position.y = + static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); + return msg; +} + +/** +* @brief Create quaternion from A* coord bins +* @param theta continuous bin coordinates angle +* @return quaternion orientation in map frame +*/ +inline geometry_msgs::msg::Quaternion getWorldOrientation( + const float & theta, + const float & bin_size) +{ + // theta is in continuous bin coordinates, must convert to world orientation + tf2::Quaternion q; + q.setEuler(0.0, 0.0, theta * static_cast(bin_size)); + return tf2::toMsg(q); +} + +/** +* @brief Find the min cost of the inflation decay function for which the robot MAY be +* in collision in any orientation +* @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) +* @return double circumscribed cost, any higher than this and need to do full footprint collision checking +* since some element of the robot could be in collision +*/ +inline double findCircumscribedCost(std::shared_ptr costmap) +{ + double result = -1.0; + bool inflation_layer_found = false; + std::vector>::iterator layer; + + // check if the costmap has an inflation layer + for (layer = costmap->getLayeredCostmap()->getPlugins()->begin(); + layer != costmap->getLayeredCostmap()->getPlugins()->end(); + ++layer) + { + std::shared_ptr inflation_layer = + std::dynamic_pointer_cast(*layer); + if (!inflation_layer) { + continue; + } + + inflation_layer_found = true; + double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + double resolution = costmap->getCostmap()->getResolution(); + result = static_cast(inflation_layer->computeCost(circum_radius / resolution)); + } + + if (!inflation_layer_found) { + RCLCPP_WARN( + rclcpp::get_logger("computeCircumscribedCost"), + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times!"); + } + + return result; +} + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__UTILS_HPP_ diff --git a/nav2_smac_planner/media/A.png b/nav2_smac_planner/media/A.png new file mode 100644 index 00000000000..8e04208118d Binary files /dev/null and b/nav2_smac_planner/media/A.png differ diff --git a/nav2_smac_planner/media/B.png b/nav2_smac_planner/media/B.png new file mode 100644 index 00000000000..e9098ed5bf2 Binary files /dev/null and b/nav2_smac_planner/media/B.png differ diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index a266e7d50a6..e8a37b23f91 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,8 +2,8 @@ nav2_smac_planner - 0.3.0 - Smac global planning plugin + 1.0.0 + Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 @@ -23,13 +23,14 @@ nav2_costmap_2d nav2_core pluginlib - libceres-dev eigen3_cmake_module eigen ompl ament_lint_common ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest ament_cmake diff --git a/nav2_smac_planner/smac_plugin.xml b/nav2_smac_planner/smac_plugin.xml index c28cb32c7f9..7b52bb24d76 100644 --- a/nav2_smac_planner/smac_plugin.xml +++ b/nav2_smac_planner/smac_plugin.xml @@ -1,5 +1,5 @@ - - SE2 version of the SMAC planner + + Hybrid-A* SMAC planner diff --git a/nav2_smac_planner/smac_plugin_2d.xml b/nav2_smac_planner/smac_plugin_2d.xml index aab22602e58..5bae6874721 100644 --- a/nav2_smac_planner/smac_plugin_2d.xml +++ b/nav2_smac_planner/smac_plugin_2d.xml @@ -1,5 +1,5 @@ - 2D version of the SMAC Planner + 2D A* SMAC Planner diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 788f71cba09..cd09cc79432 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -48,8 +48,7 @@ AStarAlgorithm::AStarAlgorithm( _goal_coordinates(Coordinates()), _start(nullptr), _goal(nullptr), - _motion_model(motion_model), - _collision_checker(nullptr) + _motion_model(motion_model) { _graph.reserve(100000); } @@ -63,75 +62,59 @@ template void AStarAlgorithm::initialize( const bool & allow_unknown, int & max_iterations, - const int & max_on_approach_iterations) + const int & max_on_approach_iterations, + const float & lookup_table_size, + const unsigned int & dim_3_size) { _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; + NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + _dim3_size = dim_3_size; } template<> -void AStarAlgorithm::createGraph( - const unsigned int & x_size, - const unsigned int & y_size, - const unsigned int & dim_3_size, - nav2_costmap_2d::Costmap2D * & costmap) +void AStarAlgorithm::initialize( + const bool & allow_unknown, + int & max_iterations, + const int & max_on_approach_iterations, + const float & /*lookup_table_size*/, + const unsigned int & dim_3_size) { + _traverse_unknown = allow_unknown; + _max_iterations = max_iterations; + _max_on_approach_iterations = max_on_approach_iterations; + if (dim_3_size != 1) { throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization."); } - _costmap = costmap; - _dim3_size = dim_3_size; // 2D search MUST be 2D, not 3D or SE2. - clearGraph(); - - if (getSizeX() != x_size || getSizeY() != y_size) { - _x_size = x_size; - _y_size = y_size; - Node2D::initNeighborhood(_x_size, _motion_model); - } + _dim3_size = dim_3_size; } -template<> -void AStarAlgorithm::createGraph( - const unsigned int & x_size, - const unsigned int & y_size, - const unsigned int & dim_3_size, - nav2_costmap_2d::Costmap2D * & costmap) +template +void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision_checker) { - _costmap = costmap; - _collision_checker = GridCollisionChecker(costmap); - _collision_checker.setFootprint(_footprint, _is_radius_footprint); + _collision_checker = collision_checker; + _costmap = collision_checker->getCostmap(); + unsigned int x_size = _costmap->getSizeInCellsX(); + unsigned int y_size = _costmap->getSizeInCellsY(); - _dim3_size = dim_3_size; - unsigned int index; clearGraph(); if (getSizeX() != x_size || getSizeY() != y_size) { _x_size = x_size; _y_size = y_size; - NodeSE2::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info); + NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info); } } template -void AStarAlgorithm::setFootprint(nav2_costmap_2d::Footprint footprint, bool use_radius) -{ - _footprint = footprint; - _is_radius_footprint = use_radius; -} - -template<> -typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( +typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( const unsigned int & index) { - return &(_graph.emplace(index, Node2D(_costmap->getCharMap()[index], index)).first->second); -} - -template<> -typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( - const unsigned int & index) -{ - return &(_graph.emplace(index, NodeSE2(index)).first->second); + // Emplace will only create a new object if it doesn't already exist. + // If an element exists, it will return the existing object, not create a new one. + return &(_graph.emplace(index, NodeT(index)).first->second); } template<> @@ -146,13 +129,13 @@ void AStarAlgorithm::setStart( _start = addToGraph(Node2D::getIndex(mx, my, getSizeX())); } -template<> -void AStarAlgorithm::setStart( +template +void AStarAlgorithm::setStart( const unsigned int & mx, const unsigned int & my, const unsigned int & dim_3) { - _start = addToGraph(NodeSE2::getIndex(mx, my, dim_3, getSizeX(), getSizeDim3())); + _start = addToGraph(NodeT::getIndex(mx, my, dim_3)); _start->setPose( Coordinates( static_cast(mx), @@ -174,24 +157,25 @@ void AStarAlgorithm::setGoal( _goal_coordinates = Node2D::Coordinates(mx, my); } -template<> -void AStarAlgorithm::setGoal( +template +void AStarAlgorithm::setGoal( const unsigned int & mx, const unsigned int & my, const unsigned int & dim_3) { - _goal = addToGraph(NodeSE2::getIndex(mx, my, dim_3, getSizeX(), getSizeDim3())); - _goal_coordinates = NodeSE2::Coordinates( + _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); + + typename NodeT::Coordinates goal_coords( static_cast(mx), static_cast(my), static_cast(dim_3)); - _goal->setPose(_goal_coordinates); - NodeSE2::computeWavefrontHeuristic( - _costmap, - static_cast(getStart()->pose.x), - static_cast(getStart()->pose.y), - mx, my); + if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { + NodeT::resetObstacleHeuristic(_costmap, mx, my); + } + + _goal_coordinates = goal_coords; + _goal->setPose(_goal_coordinates); } template @@ -227,7 +211,7 @@ bool AStarAlgorithm::createPath( CoordinateVector & path, int & iterations, const float & tolerance) { - _tolerance = tolerance * NodeT::neutral_cost; + _tolerance = tolerance; _best_heuristic_node = {std::numeric_limits::max(), 0}; clearQueue(); @@ -242,6 +226,7 @@ bool AStarAlgorithm::createPath( // Optimization: preallocate all variables NodePtr current_node = nullptr; NodePtr neighbor = nullptr; + NodePtr expansion_result = nullptr; float g_cost = 0.0; NodeVector neighbors; int approach_iterations = 0; @@ -277,13 +262,12 @@ bool AStarAlgorithm::createPath( // 2) Mark Nbest as visited current_node->visited(); - // 2.a) Use an analytic expansion (if available) to generate a path - // to the goal. - NodePtr result = tryAnalyticExpansion( - current_node, neighborGetter, analytic_iterations, - closest_distance); - if (result != nullptr) { - current_node = result; + // 2.1) Use an analytic expansion (if available) to generate a path + expansion_result = nullptr; + expansion_result = tryAnalyticExpansion( + current_node, neighborGetter, analytic_iterations, closest_distance); + if (expansion_result != nullptr) { + current_node = expansion_result; } // 3) Check if we're at the goal, backtrace if required @@ -292,18 +276,14 @@ bool AStarAlgorithm::createPath( } else if (_best_heuristic_node.first < getToleranceHeuristic()) { // Optimization: Let us find when in tolerance and refine within reason approach_iterations++; - if (approach_iterations > getOnApproachMaxIterations() || - iterations + 1 == getMaxIterations()) - { - NodePtr node = &_graph.at(_best_heuristic_node.second); - return backtracePath(node, path); + if (approach_iterations >= getOnApproachMaxIterations()) { + return backtracePath(&_graph.at(_best_heuristic_node.second), path); } } // 4) Expand neighbors of Nbest not visited neighbors.clear(); - NodeT::getNeighbors( - current_node, neighborGetter, _collision_checker, _traverse_unknown, neighbors); + current_node->getNeighbors(neighborGetter, _collision_checker, _traverse_unknown, neighbors); for (neighbor_iterator = neighbors.begin(); neighbor_iterator != neighbors.end(); ++neighbor_iterator) @@ -311,15 +291,14 @@ bool AStarAlgorithm::createPath( neighbor = *neighbor_iterator; // 4.1) Compute the cost to go to this node - g_cost = getAccumulatedCost(current_node) + getTraversalCost(current_node, neighbor); + g_cost = current_node->getAccumulatedCost() + current_node->getTraversalCost(neighbor); // 4.2) If this is a lower cost than prior, we set this as the new cost and new approach - if (g_cost < getAccumulatedCost(neighbor)) { + if (g_cost < neighbor->getAccumulatedCost()) { neighbor->setAccumulatedCost(g_cost); neighbor->parent = current_node; - // 4.3) If not in queue or visited, add it, `getNeighbors()` handles - neighbor->queued(); + // 4.3) Add to queue with heuristic cost addNode(g_cost + getHeuristicCost(neighbor), neighbor); } } @@ -335,109 +314,7 @@ bool AStarAlgorithm::isGoal(NodePtr & node) } template<> -AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( - const NodePtr & node, - const NodeGetter & node_getter) -{ - ompl::base::ScopedState<> from(node->motion_table.state_space), to( - node->motion_table.state_space), s(node->motion_table.state_space); - const NodeSE2::Coordinates & node_coords = node->pose; - from[0] = node_coords.x; - from[1] = node_coords.y; - from[2] = node_coords.theta * node->motion_table.bin_size; - to[0] = _goal_coordinates.x; - to[1] = _goal_coordinates.y; - to[2] = _goal_coordinates.theta * node->motion_table.bin_size; - - float d = node->motion_table.state_space->distance(from(), to()); - NodePtr prev(node); - // A move of sqrt(2) is guaranteed to be in a new cell - static const float sqrt_2 = std::sqrt(2.); - unsigned int num_intervals = std::floor(d / sqrt_2); - - using PossibleNode = std::pair; - std::vector possible_nodes; - possible_nodes.reserve(num_intervals - 1); // We won't store this node or the goal - std::vector reals; - // Pre-allocate - unsigned int index = 0; - NodePtr next(nullptr); - float angle = 0.0; - Coordinates proposed_coordinates; - // Don't generate the first point because we are already there! - // And the last point is the goal, so ignore it too! - for (float i = 1; i < num_intervals; i++) { - node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); - reals = s.reals(); - angle = reals[2] / node->motion_table.bin_size; - while (angle >= node->motion_table.num_angle_quantization_float) { - angle -= node->motion_table.num_angle_quantization_float; - } - while (angle < 0.0) { - angle += node->motion_table.num_angle_quantization_float; - } - // Turn the pose into a node, and check if it is valid - index = NodeSE2::getIndex( - static_cast(reals[0]), - static_cast(reals[1]), - static_cast(angle)); - // Get the node from the graph - if (node_getter(index, next)) { - Coordinates initial_node_coords = next->pose; - proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; - next->setPose(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); - prev = next; - } else { - next->setPose(initial_node_coords); - for (const auto & node_pose : possible_nodes) { - const auto & n = node_pose.first; - n->setPose(node_pose.second); - } - return NodePtr(nullptr); - } - } else { - // Abort - for (const auto & node_pose : possible_nodes) { - const auto & n = node_pose.first; - n->setPose(node_pose.second); - } - return NodePtr(nullptr); - } - } - // Legitimate path - set the parent relationships - poses already set - prev = node; - for (const auto & node_pose : possible_nodes) { - const auto & n = node_pose.first; - if (!n->wasVisited()) { - // Make sure this node has not been visited by the regular algorithm. - // If it has been, there is the (slight) chance that it is in the path we are expanding - // from, so we should skip it. - // Skipping to the next node will still create a kinematically feasible path. - n->parent = prev; - n->visited(); - prev = n; - } - } - if (_goal != prev) { - _goal->parent = prev; - _goal->visited(); - } - return _goal; -} - -template -typename AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( - const NodePtr & node, - const NodeGetter & node_getter) -{ - return NodePtr(nullptr); -} - -template<> -bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & path) +bool AStarAlgorithm::backtracePath(NodePtr node, CoordinateVector & path) { if (!node->parent) { return false; @@ -455,8 +332,8 @@ bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & pa return path.size() > 1; } -template<> -bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & path) +template +bool AStarAlgorithm::backtracePath(NodePtr node, CoordinateVector & path) { if (!node->parent) { return false; @@ -484,20 +361,23 @@ typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() return _goal; } -template -typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() { - NodeBasic node = _queue.top().second; + NodeBasic node = _queue.top().second; _queue.pop(); return node.graph_node_ptr; } -template<> -typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() { - NodeBasic node = _queue.top().second; + NodeBasic node = _queue.top().second; _queue.pop(); + // We only want to override the node's pose if it has not yet been visited + // to prevent the case that a node has been queued multiple times and + // a new branch is overriding one of lower cost already visited. if (!node.graph_node_ptr->wasVisited()) { node.graph_node_ptr->pose = node.pose; } @@ -505,44 +385,30 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() return node.graph_node_ptr; } -template -void AStarAlgorithm::addNode(const float cost, NodePtr & node) +template<> +void AStarAlgorithm::addNode(const float & cost, NodePtr & node) { - NodeBasic queued_node(node->getIndex()); + NodeBasic queued_node(node->getIndex()); queued_node.graph_node_ptr = node; _queue.emplace(cost, queued_node); } -template<> -void AStarAlgorithm::addNode(const float cost, NodePtr & node) +template +void AStarAlgorithm::addNode(const float & cost, NodePtr & node) { - NodeBasic queued_node(node->getIndex()); + NodeBasic queued_node(node->getIndex()); queued_node.pose = node->pose; queued_node.graph_node_ptr = node; _queue.emplace(cost, queued_node); } -template -float AStarAlgorithm::getTraversalCost( - NodePtr & current_node, - NodePtr & new_node) -{ - return current_node->getTraversalCost(new_node); -} - -template -float & AStarAlgorithm::getAccumulatedCost(NodePtr & node) -{ - return node->getAccumulatedCost(); -} - template float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); float heuristic = NodeT::getHeuristicCost( - node_coords, _goal_coordinates); + node_coords, _goal_coordinates, _costmap); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; @@ -562,8 +428,8 @@ template void AStarAlgorithm::clearGraph() { Graph g; - g.reserve(100000); std::swap(_graph, g); + _graph.reserve(100000); } template @@ -607,26 +473,24 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::tryAnalyticExpans const NodePtr & current_node, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { - if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP) { - // This must be a NodeSE2 node if we are using these motion models - + // This must be a NodeHybrid or NodeLattice if we are using these motion models + if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP || + _motion_model == MotionModel::STATE_LATTICE) + { // See if we are closer and should be expanding more often const Coordinates node_coords = NodeT::getCoords(current_node->getIndex(), getSizeX(), getSizeDim3()); - closest_distance = - std::min( + closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost( - node_coords, - _goal_coordinates) / NodeT::neutral_cost) - ); + static_cast(NodeT::getHeuristicCost(node_coords, _goal_coordinates, _costmap))); + // We want to expand at a rate of d/expansion_ratio, // but check to see if we are so close that we would be expanding every iteration // If so, limit it to the expansion ratio (rounded up) int desired_iterations = std::max( static_cast(closest_distance / _search_info.analytic_expansion_ratio), - static_cast(std::ceil(_search_info.analytic_expansion_ratio)) - ); + static_cast(std::ceil(_search_info.analytic_expansion_ratio))); + // If we are closer now, we should update the target number of iterations to go analytic_iterations = std::min(analytic_iterations, desired_iterations); @@ -634,18 +498,172 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::tryAnalyticExpans // Always run the expansion on the first run in case there is a // trivial path to be found if (analytic_iterations <= 0) { - // Reset the counter, and try the analytic path expansion + // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; - return getAnalyticPath(current_node, getter); + AnalyticExpansionNodes analytic_nodes = getAnalyticPath(current_node, getter); + if (!analytic_nodes.empty()) { + // If we have a valid path, attempt to refine it + NodePtr node = current_node; + NodePtr test_node = current_node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) + { + test_node = test_node->parent->parent->parent->parent->parent; + refined_analytic_nodes = getAnalyticPath(test_node, getter); + if (refined_analytic_nodes.empty()) { + break; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { + break; + } + } + + return setAnalyticPath(node, analytic_nodes); + } } + analytic_iterations--; } + // No valid motion model - return nullptr return NodePtr(nullptr); } +template +typename AStarAlgorithm::AnalyticExpansionNodes AStarAlgorithm::getAnalyticPath( + const NodePtr & node, + const NodeGetter & node_getter) +{ + static ompl::base::ScopedState<> from(node->motion_table.state_space), to( + node->motion_table.state_space), s(node->motion_table.state_space); + from[0] = node->pose.x; + from[1] = node->pose.y; + from[2] = node->pose.theta * node->motion_table.bin_size; + to[0] = _goal_coordinates.x; + to[1] = _goal_coordinates.y; + to[2] = _goal_coordinates.theta * node->motion_table.bin_size; + + float d = node->motion_table.state_space->distance(from(), to()); + + // A move of sqrt(2) is guaranteed to be in a new cell + static const float sqrt_2 = std::sqrt(2.); + unsigned int num_intervals = std::floor(d / sqrt_2); + + AnalyticExpansionNodes possible_nodes; + possible_nodes.reserve(num_intervals - 1); // We won't store this node or the goal + std::vector reals; + + // Pre-allocate + NodePtr prev(node); + unsigned int index = 0; + NodePtr next(nullptr); + float angle = 0.0; + Coordinates proposed_coordinates; + bool failure = false; + + // Check intermediary poses (non-goal, non-start) + for (float i = 1; i < num_intervals; i++) { + node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); + reals = s.reals(); + angle = reals[2] / node->motion_table.bin_size; + while (angle >= node->motion_table.num_angle_quantization_float) { + angle -= node->motion_table.num_angle_quantization_float; + } + while (angle < 0.0) { + angle += node->motion_table.num_angle_quantization_float; + } + // Turn the pose into a node, and check if it is valid + index = NodeT::getIndex( + static_cast(reals[0]), + static_cast(reals[1]), + static_cast(angle)); + // Get the node from the graph + if (node_getter(index, next)) { + Coordinates initial_node_coords = next->pose; + proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; + next->setPose(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); + prev = next; + } else { + // Abort + next->setPose(initial_node_coords); + failure = true; + break; + } + } else { + // Abort + failure = true; + break; + } + } + + // Reset to initial poses to not impact future searches + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.node; + n->setPose(node_pose.initial_coords); + } + + if (failure) { + return AnalyticExpansionNodes(); + } + + return possible_nodes; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::setAnalyticPath( + const NodePtr & node, + const AnalyticExpansionNodes & expanded_nodes) +{ + // Legitimate final path - set the parent relationships & poses + NodePtr prev = node; + for (const auto & node_pose : expanded_nodes) { + const auto & n = node_pose.node; + if (!n->wasVisited() && n->getIndex() != _goal->getIndex()) { + // Make sure this node has not been visited by the regular algorithm. + // If it has been, there is the (slight) chance that it is in the path we are expanding + // from, so we should skip it. + // Skipping to the next node will still create a kinematically feasible path. + n->parent = prev; + n->pose = node_pose.proposed_coords; + n->visited(); + prev = n; + } + } + if (_goal != prev) { + _goal->parent = prev; + _goal->visited(); + } + return _goal; +} + +template<> +typename AStarAlgorithm::AnalyticExpansionNodes AStarAlgorithm::getAnalyticPath( + const NodePtr & node, + const NodeGetter & node_getter) +{ + return AnalyticExpansionNodes(); +} + +template<> +typename AStarAlgorithm::NodePtr AStarAlgorithm::setAnalyticPath( + const NodePtr & node, + const AnalyticExpansionNodes & expanded_nodes) +{ + return NodePtr(nullptr); +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; -template class AStarAlgorithm; +template class AStarAlgorithm; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/costmap_downsampler.cpp b/nav2_smac_planner/src/costmap_downsampler.cpp index c7225234232..32bc05b36ac 100644 --- a/nav2_smac_planner/src/costmap_downsampler.cpp +++ b/nav2_smac_planner/src/costmap_downsampler.cpp @@ -48,18 +48,24 @@ void CostmapDownsampler::on_configure( _downsampled_size_x, _downsampled_size_y, _downsampled_resolution, _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN); - _downsampled_costmap_pub = std::make_unique( - node, _downsampled_costmap.get(), global_frame, topic_name, false); + if (!node.expired()) { + _downsampled_costmap_pub = std::make_unique( + node, _downsampled_costmap.get(), global_frame, topic_name, false); + } } void CostmapDownsampler::on_activate() { - _downsampled_costmap_pub->on_activate(); + if (_downsampled_costmap_pub) { + _downsampled_costmap_pub->on_activate(); + } } void CostmapDownsampler::on_deactivate() { - _downsampled_costmap_pub->on_deactivate(); + if (_downsampled_costmap_pub) { + _downsampled_costmap_pub->on_deactivate(); + } } void CostmapDownsampler::on_cleanup() @@ -90,7 +96,9 @@ nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( } } - _downsampled_costmap_pub->publishCostmap(); + if (_downsampled_costmap_pub) { + _downsampled_costmap_pub->publishCostmap(); + } return _downsampled_costmap.get(); } diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index cd8a2238791..a9ed0abf580 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -23,11 +23,11 @@ namespace nav2_smac_planner // defining static member for all instance to share std::vector Node2D::_neighbors_grid_offsets; -double Node2D::neutral_cost = 50.0; +float Node2D::cost_travel_multiplier = 2.0; -Node2D::Node2D(unsigned char & cost_in, const unsigned int index) +Node2D::Node2D(const unsigned int index) : parent(nullptr), - _cell_cost(static_cast(cost_in)), + _cell_cost(std::numeric_limits::quiet_NaN()), _accumulated_cost(std::numeric_limits::max()), _index(index), _was_visited(false), @@ -40,10 +40,10 @@ Node2D::~Node2D() parent = nullptr; } -void Node2D::reset(const unsigned char & cost) +void Node2D::reset() { parent = nullptr; - _cell_cost = static_cast(cost); + _cell_cost = std::numeric_limits::quiet_NaN(); _accumulated_cost = std::numeric_limits::max(); _was_visited = false; _is_queued = false; @@ -51,54 +51,53 @@ void Node2D::reset(const unsigned char & cost) bool Node2D::isNodeValid( const bool & traverse_unknown, - GridCollisionChecker /*collision_checker*/) + GridCollisionChecker * collision_checker) { - // NOTE(stevemacenski): Right now, we do not check if the node has wrapped around - // the regular grid (e.g. your node is on the edge of the costmap and i+1 - // goes to the other side). This check would add compute time and my assertion is - // that if you do wrap around, the heuristic will be so high it'll be added far - // in the queue that it will never be called if a valid path exists. - // This is intentionally un-included to increase speed, but be aware. If this causes - // trouble, please file a ticket and we can address it then. - - auto & cost = this->getCost(); - - // occupied node - if (cost == OCCUPIED || cost == INSCRIBED) { - return false; - } - - // unknown node - if (cost == UNKNOWN && !traverse_unknown) { + if (collision_checker->inCollision(this->getIndex(), traverse_unknown)) { return false; } + _cell_cost = collision_checker->getCost(); return true; } float Node2D::getTraversalCost(const NodePtr & child) { - // cost to travel will be the cost of the cell's code + float normalized_cost = child->getCost() / 252.0; + const Coordinates A = getCoords(child->getIndex()); + const Coordinates B = getCoords(this->getIndex()); + const float & dx = A.x - B.x; + const float & dy = A.y - B.y; + static float sqrt_2 = sqrt(2); + + // If a diagonal move, travel cost is sqrt(2) not 1.0. + if ((dx * dx + dy * dy) > 1.05) { + return sqrt_2 + cost_travel_multiplier * normalized_cost; + } - // neutral_cost is neutral cost for cost just to travel anywhere (50) - // 0.8 is a scale factor to remap costs [0, 252] evenly from [50, 252] - return Node2D::neutral_cost + 0.8 * child->getCost(); + return 1.0 + cost_travel_multiplier * normalized_cost; } float Node2D::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates) + const Coordinates & goal_coordinates, + const nav2_costmap_2d::Costmap2D * /*costmap*/) { - return hypotf( - goal_coordinates.x - node_coords.x, - goal_coordinates.y - node_coords.y) * Node2D::neutral_cost; + // Using Moore distance as it more accurately represents the distances + // even a Van Neumann neighborhood robot can navigate. + return fabs(goal_coordinates.x - node_coords.x) + + fabs(goal_coordinates.y - node_coords.y); } -void Node2D::initNeighborhood( - const unsigned int & x_size_uint, - const MotionModel & neighborhood) +void Node2D::initMotionModel( + const MotionModel & neighborhood, + unsigned int & x_size_uint, + unsigned int & /*size_y*/, + unsigned int & /*num_angle_quantization*/, + SearchInfo & search_info) { int x_size = static_cast(x_size_uint); + cost_travel_multiplier = search_info.cost_penalty; switch (neighborhood) { case MotionModel::UNKNOWN: throw std::runtime_error("Unknown neighborhood type selected."); @@ -117,15 +116,14 @@ void Node2D::initNeighborhood( } void Node2D::getNeighbors( - NodePtr & node, std::function & NeighborGetter, - GridCollisionChecker collision_checker, + GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors) { // NOTE(stevemacenski): Irritatingly, the order here matters. If you start in free // space and then expand 8-connected, the first set of neighbors will be all cost - // _neutral_cost. Then its expansion will all be 2 * _neutral_cost but now multiple + // 1.0. Then its expansion will all be 2 * 1.0 but now multiple // nodes are touching that node so the last cell to update the back pointer wins. // Thusly, the ordering ends with the cardinal directions for both sets such that // behavior is consistent in large free spaces between them. @@ -136,7 +134,7 @@ void Node2D::getNeighbors( // rather than a small inflation around the obstacles int index; NodePtr neighbor; - int node_i = node->getIndex(); + int node_i = this->getIndex(); for (unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) { index = node_i + _neighbors_grid_offsets[i]; diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp new file mode 100644 index 00000000000..059f9f5445f --- /dev/null +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -0,0 +1,648 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ompl/base/ScopedState.h" +#include "ompl/base/spaces/DubinsStateSpace.h" +#include "ompl/base/spaces/ReedsSheppStateSpace.h" + +#include "nav2_smac_planner/node_hybrid.hpp" + +using namespace std::chrono; // NOLINT + +namespace nav2_smac_planner +{ + +// defining static member for all instance to share +LookupTable NodeHybrid::obstacle_heuristic_lookup_table; +std::queue NodeHybrid::obstacle_heuristic_queue; +double NodeHybrid::travel_distance_cost = sqrt(2); +HybridMotionTable NodeHybrid::motion_table; +float NodeHybrid::size_lookup = 25; +LookupTable NodeHybrid::dist_heuristic_lookup_table; +nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr; +CostmapDownsampler NodeHybrid::downsampler; + +// Each of these tables are the projected motion models through +// time and space applied to the search on the current node in +// continuous map-coordinates (e.g. not meters but partial map cells) +// Currently, these are set to project *at minimum* into a neighboring +// cell. Though this could be later modified to project a certain +// amount of time or particular distance forward. + +// http://planning.cs.uiuc.edu/node821.html +// Model for ackermann style vehicle with minimum radius restriction +void HybridMotionTable::initDubin( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + + // if nothing changed, no need to re-compute primitives + if (num_angle_quantization_in == num_angle_quantization && + min_turning_radius == search_info.minimum_turning_radius) + { + return; + } + + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + min_turning_radius = search_info.minimum_turning_radius; + + // angle must meet 3 requirements: + // 1) be increment of quantized bin size + // 2) chord length must be greater than sqrt(2) to leave current cell + // 3) maximum curvature must be respected, represented by minimum turning angle + // Thusly: + // On circle of radius minimum turning angle, we need select motion primatives + // with chord length > sqrt(2) and be an increment of our bin size + // + // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size + // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R)) + float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); + // Now make sure angle is an increment of the quantized bin size + // And since its based on the minimum chord, we need to make sure its always larger + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + // Search dimensions are clean multiples of quantization - this prevents + // paths with loops in them + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + // find deflections + // If we make a right triangle out of the chord in circle of radius + // min turning angle, we can see that delta X = R * sin (angle) + float delta_x = min_turning_radius * sin(angle); + // Using that same right triangle, we can see that the complement + // to delta Y is R * cos (angle). If we subtract R, we get the actual value + float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); + + projections.clear(); + projections.reserve(3); + projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward + projections.emplace_back(delta_x, delta_y, increments); // Left + projections.emplace_back(delta_x, -delta_y, -increments); // Right + + // Create the correct OMPL state space + if (!state_space) { + state_space = std::make_unique(min_turning_radius); + } + + // Precompute projection deltas + delta_xs.resize(projections.size()); + delta_ys.resize(projections.size()); + trig_values.resize(num_angle_quantization); + + for (unsigned int i = 0; i != projections.size(); i++) { + delta_xs[i].resize(num_angle_quantization); + delta_ys[i].resize(num_angle_quantization); + + for (unsigned int j = 0; j != num_angle_quantization; j++) { + double cos_theta = cos(bin_size * j); + double sin_theta = sin(bin_size * j); + if (i == 0) { + // if first iteration, cache the trig values for later + trig_values[j] = {cos_theta, sin_theta}; + } + delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; + delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; + } + } +} + +// http://planning.cs.uiuc.edu/node822.html +// Same as Dubin model but now reverse is valid +// See notes in Dubin for explanation +void HybridMotionTable::initReedsShepp( + unsigned int & size_x_in, + unsigned int & /*size_y_in*/, + unsigned int & num_angle_quantization_in, + SearchInfo & search_info) +{ + size_x = size_x_in; + change_penalty = search_info.change_penalty; + non_straight_penalty = search_info.non_straight_penalty; + cost_penalty = search_info.cost_penalty; + reverse_penalty = search_info.reverse_penalty; + + // if nothing changed, no need to re-compute primitives + if (num_angle_quantization_in == num_angle_quantization && + min_turning_radius == search_info.minimum_turning_radius) + { + return; + } + + num_angle_quantization = num_angle_quantization_in; + num_angle_quantization_float = static_cast(num_angle_quantization); + min_turning_radius = search_info.minimum_turning_radius; + + float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius)); + bin_size = + 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); + float increments; + if (angle < bin_size) { + increments = 1.0f; + } else { + increments = ceil(angle / bin_size); + } + angle = increments * bin_size; + + float delta_x = min_turning_radius * sin(angle); + float delta_y = min_turning_radius - (min_turning_radius * cos(angle)); + + projections.clear(); + projections.reserve(6); + projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward + projections.emplace_back(delta_x, delta_y, increments); // Forward + Left + projections.emplace_back(delta_x, -delta_y, -increments); // Forward + Right + projections.emplace_back(-hypotf(delta_x, delta_y), 0.0, 0.0); // Backward + projections.emplace_back(-delta_x, delta_y, -increments); // Backward + Left + projections.emplace_back(-delta_x, -delta_y, increments); // Backward + Right + + // Create the correct OMPL state space + if (!state_space) { + state_space = std::make_unique( + min_turning_radius); + } + + // Precompute projection deltas + delta_xs.resize(projections.size()); + delta_ys.resize(projections.size()); + trig_values.resize(num_angle_quantization); + + for (unsigned int i = 0; i != projections.size(); i++) { + delta_xs[i].resize(num_angle_quantization); + delta_ys[i].resize(num_angle_quantization); + + for (unsigned int j = 0; j != num_angle_quantization; j++) { + double cos_theta = cos(bin_size * j); + double sin_theta = sin(bin_size * j); + if (i == 0) { + // if first iteration, cache the trig values for later + trig_values[j] = {cos_theta, sin_theta}; + } + delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta; + delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta; + } + } +} + +MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) +{ + MotionPoses projection_list; + projection_list.reserve(projections.size()); + + for (unsigned int i = 0; i != projections.size(); i++) { + const MotionPose & motion_model = projections[i]; + + // normalize theta, I know its overkill, but I've been burned before... + const float & node_heading = node->pose.theta; + float new_heading = node_heading + motion_model._theta; + + if (new_heading >= num_angle_quantization_float) { + new_heading -= num_angle_quantization_float; + } + + if (new_heading < 0.0) { + new_heading += num_angle_quantization_float; + } + + projection_list.emplace_back( + delta_xs[i][node_heading] + node->pose.x, + delta_ys[i][node_heading] + node->pose.y, + new_heading); + } + + return projection_list; +} + +NodeHybrid::NodeHybrid(const unsigned int index) +: parent(nullptr), + pose(0.0f, 0.0f, 0.0f), + _cell_cost(std::numeric_limits::quiet_NaN()), + _accumulated_cost(std::numeric_limits::max()), + _index(index), + _was_visited(false), + _motion_primitive_index(std::numeric_limits::max()) +{ +} + +NodeHybrid::~NodeHybrid() +{ + parent = nullptr; +} + +void NodeHybrid::reset() +{ + parent = nullptr; + _cell_cost = std::numeric_limits::quiet_NaN(); + _accumulated_cost = std::numeric_limits::max(); + _was_visited = false; + _motion_primitive_index = std::numeric_limits::max(); + pose.x = 0.0f; + pose.y = 0.0f; + pose.theta = 0.0f; +} + +bool NodeHybrid::isNodeValid( + const bool & traverse_unknown, + GridCollisionChecker * collision_checker) +{ + if (collision_checker->inCollision( + this->pose.x, this->pose.y, this->pose.theta * motion_table.bin_size, traverse_unknown)) + { + return false; + } + + _cell_cost = collision_checker->getCost(); + return true; +} + +float NodeHybrid::getTraversalCost(const NodePtr & child) +{ + const float normalized_cost = child->getCost() / 252.0; + if (std::isnan(normalized_cost)) { + throw std::runtime_error( + "Node attempted to get traversal " + "cost without a known SE2 collision cost!"); + } + + // this is the first node + if (getMotionPrimitiveIndex() == std::numeric_limits::max()) { + return NodeHybrid::travel_distance_cost; + } + + // Note(stevemacenski): `travel_cost_raw` at one point contained a term: + // `+ motion_table.cost_penalty * normalized_cost;` + // It has been removed, but we may want to readdress this point and determine + // the technically and theoretically correctness of that choice. I feel technically speaking + // that term has merit, but it doesn't seem to impact performance or path quality. + // W/o it lowers the travel cost, which would drive the heuristics up proportionally where I + // would expect it to plan much faster in all cases, but I only see it in some cases. Since + // this term would weight against moving to high cost zones, I would expect to see more smooth + // central motion, but I only see it in some cases, possibly because the obstacle heuristic is + // already driving the robot away from high cost areas; implicitly handling this. However, + // then I would expect that not adding it here would make it unbalanced enough that path quality + // would suffer, which I did not see in my limited experimentation, possibly due to the smoother. + float travel_cost = 0.0; + float travel_cost_raw = NodeHybrid::travel_distance_cost; + + if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) { + // New motion is a straight motion, no additional costs to be applied + travel_cost = travel_cost_raw; + } else { + if (getMotionPrimitiveIndex() == child->getMotionPrimitiveIndex()) { + // Turning motion but keeps in same direction: encourages to commit to turning if starting it + travel_cost = travel_cost_raw * motion_table.non_straight_penalty; + } else { + // Turning motion and changing direction: penalizes wiggling + travel_cost = travel_cost_raw * + (motion_table.non_straight_penalty + motion_table.change_penalty); + } + } + + if (getMotionPrimitiveIndex() > 2) { + // reverse direction + travel_cost *= motion_table.reverse_penalty; + } + + return travel_cost; +} + +float NodeHybrid::getHeuristicCost( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const nav2_costmap_2d::Costmap2D * /*costmap*/) +{ + const float obstacle_heuristic = getObstacleHeuristic(node_coords, goal_coords); + const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); + return std::max(obstacle_heuristic, dist_heuristic); +} + +void NodeHybrid::initMotionModel( + const MotionModel & motion_model, + unsigned int & size_x, + unsigned int & size_y, + unsigned int & num_angle_quantization, + SearchInfo & search_info) +{ + // find the motion model selected + switch (motion_model) { + case MotionModel::DUBIN: + motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); + break; + case MotionModel::REEDS_SHEPP: + motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); + break; + default: + throw std::runtime_error( + "Invalid motion model for Hybrid A*. Please select between" + " Dubin (Ackermann forward only)," + " Reeds-Shepp (Ackermann forward and back)."); + } + + travel_distance_cost = motion_table.projections[0]._x; +} + +void NodeHybrid::resetObstacleHeuristic( + nav2_costmap_2d::Costmap2D * costmap, + const unsigned int & goal_x, const unsigned int & goal_y) +{ + // Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up + // the planner considerably to search through 75% less cells with no detectable + // erosion of path quality after even modest smoothing. The error would be no more + // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality + std::weak_ptr ptr; + downsampler.on_configure(ptr, "fake_frame", "fake_topic", costmap, 2.0); + downsampler.on_activate(); + sampled_costmap = downsampler.downsample(2.0); + + // Clear lookup table + unsigned int size = sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY(); + if (obstacle_heuristic_lookup_table.size() == size) { + // must reset all values + std::fill( + obstacle_heuristic_lookup_table.begin(), + obstacle_heuristic_lookup_table.end(), 0.0); + } else { + unsigned int obstacle_size = obstacle_heuristic_lookup_table.size(); + obstacle_heuristic_lookup_table.resize(size, 0.0); + // must reset values for non-constructed indices + std::fill_n( + obstacle_heuristic_lookup_table.begin(), obstacle_size, 0.0); + } + + // Set initial goal point to queue from. Divided by 2 due to downsampled costmap. + std::queue q; + std::swap(obstacle_heuristic_queue, q); + obstacle_heuristic_queue.emplace( + ceil(goal_y / 2.0) * sampled_costmap->getSizeInCellsX() + ceil(goal_x / 2.0)); +} + +float NodeHybrid::getObstacleHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords) +{ + // If already expanded, return the cost + unsigned int size_x = sampled_costmap->getSizeInCellsX(); + // Divided by 2 due to downsampled costmap. + const unsigned int start_index = ceil(node_coords.y / 2.0) * size_x + ceil(node_coords.x / 2.0); + const float & starting_cost = obstacle_heuristic_lookup_table[start_index]; + if (starting_cost > 0.0) { + // costs are doubled due to downsampling + return 2.0 * starting_cost; + } + + // If not, expand until it is included. This dynamic programming ensures that + // we only expand the MINIMUM spanning set of the costmap per planning request. + // Rather than naively expanding the entire (potentially massive) map for a limited + // path, we only expand to the extent required for the furthest expansion in the + // search-planning request that dynamically updates during search as needed. + const int size_x_int = static_cast(size_x); + const unsigned int size_y = sampled_costmap->getSizeInCellsY(); + const float sqrt_2 = sqrt(2); + unsigned int mx, my, mx_idx, my_idx; + unsigned int idx = 0, new_idx = 0; + float last_accumulated_cost = 0.0, travel_cost = 0.0; + float heuristic_cost = 0.0, current_accumulated_cost = 0.0; + float cost = 0.0, existing_cost = 0.0; + + const std::vector neighborhood = {1, -1, // left right + size_x_int, -size_x_int, // up down + size_x_int + 1, size_x_int - 1, // upper diagonals + -size_x_int + 1, -size_x_int - 1}; // lower diagonals + + while (!obstacle_heuristic_queue.empty()) { + // get next one + idx = obstacle_heuristic_queue.front(); + obstacle_heuristic_queue.pop(); + last_accumulated_cost = obstacle_heuristic_lookup_table[idx]; + + + if (idx == start_index) { + // costs are doubled due to downsampling + return 2.0 * last_accumulated_cost; + } + + my_idx = idx / size_x; + mx_idx = idx - (my_idx * size_x); + + // find neighbors + for (unsigned int i = 0; i != neighborhood.size(); i++) { + new_idx = static_cast(static_cast(idx) + neighborhood[i]); + cost = static_cast(sampled_costmap->getCost(idx)); + travel_cost = + ((i <= 3) ? 1.0 : sqrt_2) + (motion_table.cost_penalty * cost / 252.0); + current_accumulated_cost = last_accumulated_cost + travel_cost; + + // if neighbor path is better and non-lethal, set new cost and add to queue + if (new_idx > 0 && new_idx < size_x * size_y && cost < INSCRIBED) { + my = new_idx / size_x; + mx = new_idx - (my * size_x); + + if (mx == 0 && mx_idx >= size_x - 1 || mx >= size_x - 1 && mx_idx == 0) { + continue; + } + if (my == 0 && my_idx >= size_y - 1 || my >= size_y - 1 && my_idx == 0) { + continue; + } + + existing_cost = obstacle_heuristic_lookup_table[new_idx]; + if (existing_cost == 0.0 || existing_cost > current_accumulated_cost) { + obstacle_heuristic_lookup_table[new_idx] = current_accumulated_cost; + obstacle_heuristic_queue.emplace(new_idx); + } + } + } + } + + // costs are doubled due to downsampling + return 2.0 * obstacle_heuristic_lookup_table[start_index]; +} + +float NodeHybrid::getDistanceHeuristic( + const Coordinates & node_coords, + const Coordinates & goal_coords, + const float & obstacle_heuristic) +{ + // rotate and translate node_coords such that goal_coords relative is (0,0,0) + // Due to the rounding involved in exact cell increments for caching, + // this is not an exact replica of a live heuristic, but has bounded error. + // (Usually less than 1 cell) + + // This angle is negative since we are de-rotating the current node + // by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th) + const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta]; + const float cos_th = trig_vals.first; + const float sin_th = -trig_vals.second; + const float dx = node_coords.x - goal_coords.x; + const float dy = node_coords.y - goal_coords.y; + + double dtheta_bin = node_coords.theta - goal_coords.theta; + if (dtheta_bin > motion_table.num_angle_quantization) { + dtheta_bin -= motion_table.num_angle_quantization; + } else if (dtheta_bin < 0) { + dtheta_bin += motion_table.num_angle_quantization; + } + + Coordinates node_coords_relative( + round(dx * cos_th - dy * sin_th), + round(dx * sin_th + dy * cos_th), + round(dtheta_bin)); + + // Check if the relative node coordinate is within the localized window around the goal + // to apply the distance heuristic. Since the lookup table is contains only the positive + // X axis, we mirror the Y and theta values across the X axis to find the heuristic values. + float motion_heuristic = 0.0; + const int floored_size = floor(size_lookup / 2.0); + const int ceiling_size = ceil(size_lookup / 2.0); + const float mirrored_relative_y = abs(node_coords_relative.y); + if (abs(node_coords_relative.x) < floored_size && mirrored_relative_y < floored_size) { + // Need to mirror angle if Y coordinate was mirrored + int theta_pos; + if (node_coords_relative.y < 0.0) { + theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta; + } else { + theta_pos = node_coords_relative.theta; + } + const int x_pos = node_coords_relative.x + floored_size; + const int y_pos = static_cast(mirrored_relative_y); + const int index = + x_pos * ceiling_size * motion_table.num_angle_quantization + + y_pos * motion_table.num_angle_quantization + + theta_pos; + motion_heuristic = dist_heuristic_lookup_table[index]; + } else if (obstacle_heuristic == 0.0) { + // If no obstacle heuristic value, must have some H to use + // In nominal situations, this should never be called. + static ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); + to[0] = goal_coords.x; + to[1] = goal_coords.y; + to[2] = goal_coords.theta * motion_table.num_angle_quantization; + from[0] = node_coords.x; + from[1] = node_coords.y; + from[2] = node_coords.theta * motion_table.num_angle_quantization; + motion_heuristic = motion_table.state_space->distance(from(), to()); + } + + return motion_heuristic; +} + +void NodeHybrid::precomputeDistanceHeuristic( + const float & lookup_table_dim, + const MotionModel & motion_model, + const unsigned int & dim_3_size, + const SearchInfo & search_info) +{ + // Dubin or Reeds-Shepp shortest distances + if (motion_model == MotionModel::DUBIN) { + motion_table.state_space = std::make_unique( + search_info.minimum_turning_radius); + } else if (motion_model == MotionModel::REEDS_SHEPP) { + motion_table.state_space = std::make_unique( + search_info.minimum_turning_radius); + } else { + throw std::runtime_error( + "Node attempted to precompute distance heuristics " + "with invalid motion model!"); + } + + ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); + to[0] = 0.0; + to[1] = 0.0; + to[2] = 0.0; + size_lookup = lookup_table_dim; + float motion_heuristic = 0.0; + unsigned int index = 0; + int dim_3_size_int = static_cast(dim_3_size); + float angular_bin_size = 2 * M_PI / static_cast(dim_3_size); + + // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal + // to help drive the search towards admissible approaches. Deu to symmetries in the + // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror + // around the X axis any relative node lookup. This reduces memory overhead and increases + // the size of a window a platform can store in memory. + dist_heuristic_lookup_table.resize(size_lookup * ceil(size_lookup / 2.0) * dim_3_size_int); + for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) { + for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) { + for (int heading = 0; heading != dim_3_size_int; heading++) { + from[0] = x; + from[1] = y; + from[2] = heading * angular_bin_size; + motion_heuristic = motion_table.state_space->distance(from(), to()); + dist_heuristic_lookup_table[index] = motion_heuristic; + index++; + } + } + } +} + +void NodeHybrid::getNeighbors( + std::function & NeighborGetter, + GridCollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors) +{ + unsigned int index = 0; + NodePtr neighbor = nullptr; + Coordinates initial_node_coords; + const MotionPoses motion_projections = motion_table.getProjections(this); + + for (unsigned int i = 0; i != motion_projections.size(); i++) { + index = NodeHybrid::getIndex( + static_cast(motion_projections[i]._x), + static_cast(motion_projections[i]._y), + static_cast(motion_projections[i]._theta), + motion_table.size_x, motion_table.num_angle_quantization); + + if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { + // Cache the initial pose in case it was visited but valid + // don't want to disrupt continuous coordinate expansion + initial_node_coords = neighbor->pose; + neighbor->setPose( + Coordinates( + motion_projections[i]._x, + motion_projections[i]._y, + motion_projections[i]._theta)); + if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { + neighbor->setMotionPrimitiveIndex(i); + neighbors.push_back(neighbor); + } else { + neighbor->setPose(initial_node_coords); + } + } + } +} + +} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_se2.cpp b/nav2_smac_planner/src/node_se2.cpp deleted file mode 100644 index 9bc2516d12b..00000000000 --- a/nav2_smac_planner/src/node_se2.cpp +++ /dev/null @@ -1,432 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include -#include -#include -#include -#include -#include -#include - -#include "ompl/base/ScopedState.h" -#include "ompl/base/spaces/DubinsStateSpace.h" -#include "ompl/base/spaces/ReedsSheppStateSpace.h" - -#include "nav2_smac_planner/node_se2.hpp" - -using namespace std::chrono; // NOLINT - -namespace nav2_smac_planner -{ - -// defining static member for all instance to share -std::vector NodeSE2::_wavefront_heuristic; -double NodeSE2::neutral_cost = sqrt(2); -MotionTable NodeSE2::motion_table; - -// Each of these tables are the projected motion models through -// time and space applied to the search on the current node in -// continuous map-coordinates (e.g. not meters but partial map cells) -// Currently, these are set to project *at minimum* into a neighboring -// cell. Though this could be later modified to project a certain -// amount of time or particular distance forward. - -// http://planning.cs.uiuc.edu/node821.html -// Model for ackermann style vehicle with minimum radius restriction -void MotionTable::initDubin( - unsigned int & size_x_in, - unsigned int & /*size_y_in*/, - unsigned int & num_angle_quantization_in, - SearchInfo & search_info) -{ - size_x = size_x_in; - num_angle_quantization = num_angle_quantization_in; - num_angle_quantization_float = static_cast(num_angle_quantization); - change_penalty = search_info.change_penalty; - non_straight_penalty = search_info.non_straight_penalty; - cost_penalty = search_info.cost_penalty; - reverse_penalty = search_info.reverse_penalty; - - // angle must meet 3 requirements: - // 1) be increment of quantized bin size - // 2) chord length must be greater than sqrt(2) to leave current cell - // 3) maximum curvature must be respected, represented by minimum turning angle - // Thusly: - // On circle of radius minimum turning angle, we need select motion primatives - // with chord length > sqrt(2) and be an increment of our bin size - // - // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size - // Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R)) - float angle = 2.0 * asin(sqrt(2.0) / (2 * search_info.minimum_turning_radius)); - // Now make sure angle is an increment of the quantized bin size - // And since its based on the minimum chord, we need to make sure its always larger - bin_size = - 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); - float increments; - if (angle < bin_size) { - increments = 1.0f; - } else { - // Search dimensions are clean multiples of quantization - this prevents - // paths with loops in them - increments = ceil(angle / bin_size); - } - angle = increments * bin_size; - - // find deflections - // If we make a right triangle out of the chord in circle of radius - // min turning angle, we can see that delta X = R * sin (angle) - float delta_x = search_info.minimum_turning_radius * sin(angle); - // Using that same right triangle, we can see that the complement - // to delta Y is R * cos (angle). If we subtract R, we get the actual value - float delta_y = search_info.minimum_turning_radius - - (search_info.minimum_turning_radius * cos(angle)); - - projections.clear(); - projections.reserve(3); - projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward - projections.emplace_back(delta_x, delta_y, increments); // Left - projections.emplace_back(delta_x, -delta_y, -increments); // Right - - // Create the correct OMPL state space - state_space = std::make_unique(search_info.minimum_turning_radius); -} - -// http://planning.cs.uiuc.edu/node822.html -// Same as Dubin model but now reverse is valid -// See notes in Dubin for explanation -void MotionTable::initReedsShepp( - unsigned int & size_x_in, - unsigned int & /*size_y_in*/, - unsigned int & num_angle_quantization_in, - SearchInfo & search_info) -{ - size_x = size_x_in; - num_angle_quantization = num_angle_quantization_in; - num_angle_quantization_float = static_cast(num_angle_quantization); - change_penalty = search_info.change_penalty; - non_straight_penalty = search_info.non_straight_penalty; - cost_penalty = search_info.cost_penalty; - reverse_penalty = search_info.reverse_penalty; - - float angle = 2.0 * asin(sqrt(2.0) / (2 * search_info.minimum_turning_radius)); - bin_size = - 2.0f * static_cast(M_PI) / static_cast(num_angle_quantization); - float increments; - if (angle < bin_size) { - increments = 1.0f; - } else { - increments = ceil(angle / bin_size); - } - angle = increments * bin_size; - - float delta_x = search_info.minimum_turning_radius * sin(angle); - float delta_y = search_info.minimum_turning_radius - - (search_info.minimum_turning_radius * cos(angle)); - - projections.clear(); - projections.reserve(6); - projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward - projections.emplace_back(delta_x, delta_y, increments); // Forward + Left - projections.emplace_back(delta_x, -delta_y, -increments); // Forward + Right - projections.emplace_back(-hypotf(delta_x, delta_y), 0.0, 0.0); // Backward - projections.emplace_back(-delta_x, delta_y, -increments); // Backward + Left - projections.emplace_back(-delta_x, -delta_y, increments); // Backward + Right - - // Create the correct OMPL state space - state_space = std::make_unique( - search_info.minimum_turning_radius); -} - -MotionPoses MotionTable::getProjections(const NodeSE2 * node) -{ - MotionPoses projection_list; - for (unsigned int i = 0; i != projections.size(); i++) { - projection_list.push_back(getProjection(node, i)); - } - - return projection_list; -} - -MotionPose MotionTable::getProjection(const NodeSE2 * node, const unsigned int & motion_index) -{ - const MotionPose & motion_model = projections[motion_index]; - - // transform delta X, Y, and Theta into local coordinates - const float & node_heading = node->pose.theta; - const float cos_theta = cos(node_heading * bin_size); // needs actual angle [0, 2PI] - const float sin_theta = sin(node_heading * bin_size); - const float delta_x = motion_model._x * cos_theta - motion_model._y * sin_theta; - const float delta_y = motion_model._x * sin_theta + motion_model._y * cos_theta; - float new_heading = node_heading + motion_model._theta; - - // normalize theta - while (new_heading >= num_angle_quantization_float) { - new_heading -= num_angle_quantization_float; - } - while (new_heading < 0.0) { - new_heading += num_angle_quantization_float; - } - - return MotionPose(delta_x + node->pose.x, delta_y + node->pose.y, new_heading); -} - -NodeSE2::NodeSE2(const unsigned int index) -: parent(nullptr), - pose(0.0f, 0.0f, 0.0f), - _cell_cost(std::numeric_limits::quiet_NaN()), - _accumulated_cost(std::numeric_limits::max()), - _index(index), - _was_visited(false), - _is_queued(false), - _motion_primitive_index(std::numeric_limits::max()) -{ -} - -NodeSE2::~NodeSE2() -{ - parent = nullptr; -} - -void NodeSE2::reset() -{ - parent = nullptr; - _cell_cost = std::numeric_limits::quiet_NaN(); - _accumulated_cost = std::numeric_limits::max(); - _was_visited = false; - _is_queued = false; - _motion_primitive_index = std::numeric_limits::max(); - pose.x = 0.0f; - pose.y = 0.0f; - pose.theta = 0.0f; -} - -bool NodeSE2::isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker) -{ - if (collision_checker.inCollision( - this->pose.x, this->pose.y, this->pose.theta * motion_table.bin_size, traverse_unknown)) - { - return false; - } - - _cell_cost = collision_checker.getCost(); - return true; -} - -float NodeSE2::getTraversalCost(const NodePtr & child) -{ - const float normalized_cost = child->getCost() / 252.0; - if (std::isnan(normalized_cost)) { - throw std::runtime_error( - "Node attempted to get traversal " - "cost without a known SE2 collision cost!"); - } - - // this is the first node - if (getMotionPrimitiveIndex() == std::numeric_limits::max()) { - return NodeSE2::neutral_cost; - } - - float travel_cost = 0.0; - float travel_cost_raw = NodeSE2::neutral_cost + motion_table.cost_penalty * normalized_cost; - - if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) { - // straight motion, no additional costs to be applied - travel_cost = travel_cost_raw; - } else { - if (getMotionPrimitiveIndex() == child->getMotionPrimitiveIndex()) { - // Turning motion but keeps in same direction: encourages to commit to turning if starting it - travel_cost = travel_cost_raw * motion_table.non_straight_penalty; - } else { - // Turning motion and changing direction: penalizes wiggling - travel_cost = travel_cost_raw * motion_table.change_penalty; - travel_cost += travel_cost_raw * motion_table.non_straight_penalty; - } - } - - if (getMotionPrimitiveIndex() > 2) { - // reverse direction - travel_cost *= motion_table.reverse_penalty; - } - - return travel_cost; -} - -float NodeSE2::getHeuristicCost( - const Coordinates & node_coords, - const Coordinates & goal_coords) -{ - // Dubin or Reeds-Shepp shortest distances - // Create OMPL states for checking - ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); - from[0] = node_coords.x; - from[1] = node_coords.y; - from[2] = node_coords.theta * motion_table.bin_size; - to[0] = goal_coords.x; - to[1] = goal_coords.y; - to[2] = goal_coords.theta * motion_table.bin_size; - - const float motion_heuristic = motion_table.state_space->distance(from(), to()); - - const unsigned int & wavefront_idx = static_cast(node_coords.y) * - motion_table.size_x + static_cast(node_coords.x); - const unsigned int & wavefront_value = _wavefront_heuristic[wavefront_idx]; - - // if lethal or didn't visit, use the motion heuristic instead. - if (wavefront_value == 0) { - return NodeSE2::neutral_cost * motion_heuristic; - } - - // -2 because wavefront starts at 2 - const float wavefront_heuristic = static_cast(wavefront_value - 2); - - return NodeSE2::neutral_cost * std::max(wavefront_heuristic, motion_heuristic); -} - -void NodeSE2::initMotionModel( - const MotionModel & motion_model, - unsigned int & size_x, - unsigned int & size_y, - unsigned int & num_angle_quantization, - SearchInfo & search_info) -{ - // find the motion model selected - switch (motion_model) { - case MotionModel::DUBIN: - motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); - break; - case MotionModel::REEDS_SHEPP: - motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); - break; - default: - throw std::runtime_error( - "Invalid motion model for SE2 node. Please select between" - " Dubin (Ackermann forward only)," - " Reeds-Shepp (Ackermann forward and back)."); - } -} - -void NodeSE2::computeWavefrontHeuristic( - nav2_costmap_2d::Costmap2D * & costmap, - const unsigned int & start_x, const unsigned int & start_y, - const unsigned int & goal_x, const unsigned int & goal_y) -{ - unsigned int size = costmap->getSizeInCellsX() * costmap->getSizeInCellsY(); - if (_wavefront_heuristic.size() == size) { - // must reset all values - for (unsigned int i = 0; i != _wavefront_heuristic.size(); i++) { - _wavefront_heuristic[i] = 0; - } - } else { - unsigned int wavefront_size = _wavefront_heuristic.size(); - _wavefront_heuristic.resize(size, 0); - // must reset values for non-constructed indices - for (unsigned int i = 0; i != wavefront_size; i++) { - _wavefront_heuristic[i] = 0; - } - } - - const unsigned int & size_x = motion_table.size_x; - const int size_x_int = static_cast(size_x); - const unsigned int size_y = costmap->getSizeInCellsY(); - const unsigned int goal_index = goal_y * size_x + goal_x; - const unsigned int start_index = start_y * size_x + start_x; - unsigned int mx, my, mx_idx, my_idx; - - std::queue q; - q.emplace(goal_index); - - unsigned int idx = goal_index; - _wavefront_heuristic[idx] = 2; - - static const std::vector neighborhood = {1, -1, // left right - size_x_int, -size_x_int, // up down - size_x_int + 1, size_x_int - 1, // upper diagonals - -size_x_int + 1, -size_x_int - 1}; // lower diagonals - - while (!q.empty() || idx == start_index) { - // get next one - idx = q.front(); - q.pop(); - - my_idx = idx / size_x; - mx_idx = idx - (my_idx * size_x); - - // find neighbors - for (unsigned int i = 0; i != neighborhood.size(); i++) { - unsigned int new_idx = static_cast(static_cast(idx) + neighborhood[i]); - unsigned int last_wave_cost = _wavefront_heuristic[idx]; - - // if neighbor is unvisited and non-lethal, set N and add to queue - if (new_idx > 0 && new_idx < size_x * size_y && - _wavefront_heuristic[new_idx] == 0 && - static_cast(costmap->getCost(idx)) < INSCRIBED) - { - my = new_idx / size_x; - mx = new_idx - (my * size_x); - - if (mx == 0 && mx_idx >= size_x - 1 || mx >= size_x - 1 && mx_idx == 0) { - continue; - } - if (my == 0 && my_idx >= size_y - 1 || my >= size_y - 1 && my_idx == 0) { - continue; - } - - _wavefront_heuristic[new_idx] = last_wave_cost + 1; - q.emplace(idx + neighborhood[i]); - } - } - } -} - -void NodeSE2::getNeighbors( - const NodePtr & node, - std::function & NeighborGetter, - GridCollisionChecker collision_checker, - const bool & traverse_unknown, - NodeVector & neighbors) -{ - unsigned int index = 0; - NodePtr neighbor = nullptr; - Coordinates initial_node_coords; - const MotionPoses motion_projections = motion_table.getProjections(node); - - for (unsigned int i = 0; i != motion_projections.size(); i++) { - index = NodeSE2::getIndex( - static_cast(motion_projections[i]._x), - static_cast(motion_projections[i]._y), - static_cast(motion_projections[i]._theta), - motion_table.size_x, motion_table.num_angle_quantization); - - if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { - // Cache the initial pose in case it was visited but valid - // don't want to disrupt continuous coordinate expansion - initial_node_coords = neighbor->pose; - neighbor->setPose( - Coordinates( - motion_projections[i]._x, - motion_projections[i]._y, - motion_projections[i]._theta)); - if (neighbor->isNodeValid(traverse_unknown, collision_checker)) { - neighbor->setMotionPrimitiveIndex(i); - neighbors.push_back(neighbor); - } else { - neighbor->setPose(initial_node_coords); - } - } - } -} - -} // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 16da85f15ee..dfd295bcdfc 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -28,6 +28,7 @@ using namespace std::chrono; // NOLINT SmacPlanner2D::SmacPlanner2D() : _a_star(nullptr), + _collision_checker(nullptr, 1), _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) @@ -56,8 +57,7 @@ void SmacPlanner2D::configure( bool allow_unknown; int max_iterations; int max_on_approach_iterations; - bool smooth_path; - double minimum_turning_radius; + SearchInfo search_info; std::string motion_model_for_search; // General planner params @@ -70,22 +70,19 @@ void SmacPlanner2D::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + nav2_util::declare_parameter_if_not_declared( + node, name + ".cost_travel_multiplier", rclcpp::ParameterValue(2.0)); + node->get_parameter(name + ".cost_travel_multiplier", search_info.cost_penalty); nav2_util::declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", allow_unknown); nav2_util::declare_parameter_if_not_declared( - node, name + ".max_iterations", rclcpp::ParameterValue(-1)); + node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", max_iterations); nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", max_on_approach_iterations); - nav2_util::declare_parameter_if_not_declared( - node, name + ".smooth_path", rclcpp::ParameterValue(false)); - node->get_parameter(name + ".smooth_path", smooth_path); - nav2_util::declare_parameter_if_not_declared( - node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); - node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius); nav2_util::declare_parameter_if_not_declared( node, name + ".max_planning_time", rclcpp::ParameterValue(1.0)); @@ -117,20 +114,29 @@ void SmacPlanner2D::configure( max_iterations = std::numeric_limits::max(); } - _a_star = std::make_unique>(motion_model, SearchInfo()); + // Initialize collision checker + _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/); + _collision_checker.setFootprint( + costmap_ros->getRobotFootprint(), + true /*for 2D, most use radius*/, + 0.0 /*for 2D cost at inscribed isn't relevent*/); + + // Initialize A* template + _a_star = std::make_unique>(motion_model, search_info); _a_star->initialize( allow_unknown, max_iterations, - max_on_approach_iterations); - - if (smooth_path) { - _smoother = std::make_unique(); - _optimizer_params.get(node.get(), name); - _smoother_params.get(node.get(), name); - _smoother_params.max_curvature = 1.0f / minimum_turning_radius; - _smoother->initialize(_optimizer_params); - } + max_on_approach_iterations, + 0.0 /*unused for 2D*/, + 1.0 /*unused for 2D*/); + + // Initialize path smoother + SmootherParams params; + params.get(node, name); + _smoother = std::make_unique(params); + _smoother->initialize(1e-50 /*No valid minimum turning radius for 2D*/); + // Initialize costmap downsampler if (_downsample_costmap && _downsampling_factor > 1) { std::string topic_name = "downsampled_costmap"; _costmap_downsampler = std::make_unique(); @@ -195,14 +201,11 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( nav2_costmap_2d::Costmap2D * costmap = _costmap; if (_costmap_downsampler) { costmap = _costmap_downsampler->downsample(_downsampling_factor); + _collision_checker.setCostmap(costmap); } - // Set Costmap - _a_star->createGraph( - costmap->getSizeInCellsX(), - costmap->getSizeInCellsY(), - 1, - costmap); + // Set collision checker and costmap information + _a_star->setCollisionChecker(&_collision_checker); // Set starting point unsigned int mx, my; @@ -252,21 +255,10 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( return plan; } - // Convert to world coordinates and downsample path for smoothing if necesssary - // We're going to downsample by 4x to give terms room to move. - const int downsample_ratio = 4; - std::vector path_world; - path_world.reserve(_smoother ? path.size() / downsample_ratio : path.size()); - plan.poses.reserve(_smoother ? path.size() / downsample_ratio : path.size()); - + // Convert to world coordinates + plan.poses.reserve(path.size()); for (int i = path.size() - 1; i >= 0; --i) { - if (_smoother && i % downsample_ratio != 0) { - continue; - } - - path_world.push_back(getWorldCoords(path[i].x, path[i].y, costmap)); - pose.pose.position.x = path_world.back().x(); - pose.pose.position.y = path_world.back().y(); + pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); plan.poses.push_back(pose); } @@ -275,67 +267,24 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( _raw_plan_publisher->publish(plan); } - // If not smoothing or too short to smooth, return path - if (!_smoother || path_world.size() < 4) { -#ifdef BENCHMARK_TESTING - steady_clock::time_point b = steady_clock::now(); - duration time_span = duration_cast>(b - a); - std::cout << "It took " << time_span.count() * 1000 << - " milliseconds with " << num_iterations << " iterations." << std::endl; -#endif - return plan; - } - // Find how much time we have left to do smoothing steady_clock::time_point b = steady_clock::now(); duration time_span = duration_cast>(b - a); double time_remaining = _max_planning_time - static_cast(time_span.count()); - _smoother_params.max_time = std::min(time_remaining, _optimizer_params.max_time); - - // Smooth plan - if (!_smoother->smooth(path_world, costmap, _smoother_params)) { - RCLCPP_WARN( - _logger, - "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", - _name.c_str()); - return plan; - } - removeHook(path_world); +#ifdef BENCHMARK_TESTING + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif - // populate final path - for (uint i = 0; i != path_world.size(); i++) { - pose.pose.position.x = path_world[i][0]; - pose.pose.position.y = path_world[i][1]; - plan.poses[i] = pose; + // Smooth plan + if (plan.poses.size() > 6) { + _smoother->smooth(plan, costmap, time_remaining); } return plan; } -void SmacPlanner2D::removeHook(std::vector & path) -{ - // Removes the end "hooking" since goal is locked in place - Eigen::Vector2d interpolated_second_to_last_point; - interpolated_second_to_last_point = (path.end()[-3] + path.end()[-1]) / 2.0; - if ( - squaredDistance(path.end()[-2], path.end()[-1]) > - squaredDistance(interpolated_second_to_last_point, path.end()[-1])) - { - path.end()[-2] = interpolated_second_to_last_point; - } -} - -Eigen::Vector2d SmacPlanner2D::getWorldCoords( - const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) -{ - float world_x = - static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); - float world_y = - static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); - return Eigen::Vector2d(world_x, world_y); -} - } // namespace nav2_smac_planner #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_smac_planner/src/smac_planner.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp similarity index 62% rename from nav2_smac_planner/src/smac_planner.cpp rename to nav2_smac_planner/src/smac_planner_hybrid.cpp index d9fb2c7d14a..1581bfbedcb 100644 --- a/nav2_smac_planner/src/smac_planner.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -19,31 +19,32 @@ #include #include "Eigen/Core" -#include "nav2_smac_planner/smac_planner.hpp" +#include "nav2_smac_planner/smac_planner_hybrid.hpp" -#define BENCHMARK_TESTING +// #define BENCHMARK_TESTING namespace nav2_smac_planner { using namespace std::chrono; // NOLINT -SmacPlanner::SmacPlanner() +SmacPlannerHybrid::SmacPlannerHybrid() : _a_star(nullptr), + _collision_checker(nullptr, 1), _smoother(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) { } -SmacPlanner::~SmacPlanner() +SmacPlannerHybrid::~SmacPlannerHybrid() { RCLCPP_INFO( - _logger, "Destroying plugin %s of type SmacPlanner", + _logger, "Destroying plugin %s of type SmacPlannerHybrid", _name.c_str()); } -void SmacPlanner::configure( +void SmacPlannerHybrid::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, std::shared_ptr/*tf*/, std::shared_ptr costmap_ros) @@ -57,25 +58,21 @@ void SmacPlanner::configure( bool allow_unknown; int max_iterations; - int max_on_approach_iterations = std::numeric_limits::max(); int angle_quantizations; + double lookup_table_size; SearchInfo search_info; - bool smooth_path; std::string motion_model_for_search; // General planner params nav2_util::declare_parameter_if_not_declared( - node, name + ".tolerance", rclcpp::ParameterValue(0.125)); - _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); - nav2_util::declare_parameter_if_not_declared( - node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); + node, name + ".downsample_costmap", rclcpp::ParameterValue(false)); node->get_parameter(name + ".downsample_costmap", _downsample_costmap); nav2_util::declare_parameter_if_not_declared( node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); node->get_parameter(name + ".downsampling_factor", _downsampling_factor); nav2_util::declare_parameter_if_not_declared( - node, name + ".angle_quantization_bins", rclcpp::ParameterValue(1)); + node, name + ".angle_quantization_bins", rclcpp::ParameterValue(72)); node->get_parameter(name + ".angle_quantization_bins", angle_quantizations); _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); @@ -84,35 +81,37 @@ void SmacPlanner::configure( node, name + ".allow_unknown", rclcpp::ParameterValue(true)); node->get_parameter(name + ".allow_unknown", allow_unknown); nav2_util::declare_parameter_if_not_declared( - node, name + ".max_iterations", rclcpp::ParameterValue(-1)); + node, name + ".max_iterations", rclcpp::ParameterValue(1000000)); node->get_parameter(name + ".max_iterations", max_iterations); - nav2_util::declare_parameter_if_not_declared( - node, name + ".smooth_path", rclcpp::ParameterValue(false)); - node->get_parameter(name + ".smooth_path", smooth_path); nav2_util::declare_parameter_if_not_declared( - node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4)); node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius); - + nav2_util::declare_parameter_if_not_declared( + node, name + ".cache_obstacle_heuristic", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".cache_obstacle_heuristic", search_info.cache_obstacle_heuristic); nav2_util::declare_parameter_if_not_declared( node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0)); node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".change_penalty", rclcpp::ParameterValue(0.5)); + node, name + ".change_penalty", rclcpp::ParameterValue(0.15)); node->get_parameter(name + ".change_penalty", search_info.change_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05)); + node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.50)); node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".cost_penalty", rclcpp::ParameterValue(1.2)); + node, name + ".cost_penalty", rclcpp::ParameterValue(1.7)); node->get_parameter(name + ".cost_penalty", search_info.cost_penalty); nav2_util::declare_parameter_if_not_declared( - node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0)); + node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5)); node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); nav2_util::declare_parameter_if_not_declared( node, name + ".max_planning_time", rclcpp::ParameterValue(5.0)); node->get_parameter(name + ".max_planning_time", _max_planning_time); + nav2_util::declare_parameter_if_not_declared( + node, name + ".lookup_table_size", rclcpp::ParameterValue(20.0)); + node->get_parameter(name + ".lookup_table_size", lookup_table_size); nav2_util::declare_parameter_if_not_declared( node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); @@ -122,17 +121,10 @@ void SmacPlanner::configure( RCLCPP_WARN( _logger, "Unable to get MotionModel search type. Given '%s', " - "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", + "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.", motion_model_for_search.c_str()); } - if (max_on_approach_iterations <= 0) { - RCLCPP_INFO( - _logger, "On approach iteration selected as <= 0, " - "disabling tolerance and on approach iterations."); - max_on_approach_iterations = std::numeric_limits::max(); - } - if (max_iterations <= 0) { RCLCPP_INFO( _logger, "maximum iteration selected as <= 0, " @@ -144,22 +136,45 @@ void SmacPlanner::configure( const double minimum_turning_radius_global_coords = search_info.minimum_turning_radius; search_info.minimum_turning_radius = search_info.minimum_turning_radius / (_costmap->getResolution() * _downsampling_factor); + float lookup_table_dim = + static_cast(lookup_table_size) / + static_cast(_costmap->getResolution() * _downsampling_factor); + + // Make sure its a whole number + lookup_table_dim = static_cast(static_cast(lookup_table_dim)); - _a_star = std::make_unique>(motion_model, search_info); + // Make sure its an odd number + if (static_cast(lookup_table_dim) % 2 == 0) { + RCLCPP_INFO( + _logger, + "Even sized heuristic lookup table size set %f, increasing size by 1 to make odd", + lookup_table_dim); + lookup_table_dim += 1.0; + } + + // Initialize collision checker + _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations); + _collision_checker.setFootprint( + costmap_ros->getRobotFootprint(), + costmap_ros->getUseRadius(), + findCircumscribedCost(costmap_ros)); + + // Initialize A* template + _a_star = std::make_unique>(motion_model, search_info); _a_star->initialize( allow_unknown, max_iterations, - max_on_approach_iterations); - _a_star->setFootprint(costmap_ros->getRobotFootprint(), costmap_ros->getUseRadius()); - - if (smooth_path) { - _smoother = std::make_unique(); - _optimizer_params.get(node.get(), name); - _smoother_params.get(node.get(), name); - _smoother_params.max_curvature = 1.0f / minimum_turning_radius_global_coords; - _smoother->initialize(_optimizer_params); - } + std::numeric_limits::max(), + lookup_table_dim, + _angle_quantizations); + + // Initialize path smoother + SmootherParams params; + params.get(node, name); + _smoother = std::make_unique(params); + _smoother->initialize(minimum_turning_radius_global_coords); + // Initialize costmap downsampler if (_downsample_costmap && _downsampling_factor > 1) { std::string topic_name = "downsampled_costmap"; _costmap_downsampler = std::make_unique(); @@ -170,18 +185,17 @@ void SmacPlanner::configure( _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO( - _logger, "Configured plugin %s of type SmacPlanner with " - "tolerance %.2f, maximum iterations %i, " - "max on approach iterations %i, and %s. Using motion model: %s.", - _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, + _logger, "Configured plugin %s of type SmacPlannerHybrid with " + "maximum iterations %i, and %s. Using motion model: %s.", + _name.c_str(), max_iterations, allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", toString(motion_model).c_str()); } -void SmacPlanner::activate() +void SmacPlannerHybrid::activate() { RCLCPP_INFO( - _logger, "Activating plugin %s of type SmacPlanner", + _logger, "Activating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_activate(); if (_costmap_downsampler) { @@ -189,10 +203,10 @@ void SmacPlanner::activate() } } -void SmacPlanner::deactivate() +void SmacPlannerHybrid::deactivate() { RCLCPP_INFO( - _logger, "Deactivating plugin %s of type SmacPlanner", + _logger, "Deactivating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_deactivate(); if (_costmap_downsampler) { @@ -200,10 +214,10 @@ void SmacPlanner::deactivate() } } -void SmacPlanner::cleanup() +void SmacPlannerHybrid::cleanup() { RCLCPP_INFO( - _logger, "Cleaning up plugin %s of type SmacPlanner", + _logger, "Cleaning up plugin %s of type SmacPlannerHybrid", _name.c_str()); _a_star.reset(); _smoother.reset(); @@ -212,7 +226,7 @@ void SmacPlanner::cleanup() _raw_plan_publisher.reset(); } -nav_msgs::msg::Path SmacPlanner::createPlan( +nav_msgs::msg::Path SmacPlannerHybrid::createPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) { @@ -224,14 +238,11 @@ nav_msgs::msg::Path SmacPlanner::createPlan( nav2_costmap_2d::Costmap2D * costmap = _costmap; if (_costmap_downsampler) { costmap = _costmap_downsampler->downsample(_downsampling_factor); + _collision_checker.setCostmap(costmap); } - // Set Costmap - _a_star->createGraph( - costmap->getSizeInCellsX(), - costmap->getSizeInCellsY(), - _angle_quantizations, - costmap); + // Set collision checker and costmap information + _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates unsigned int mx, my; @@ -265,13 +276,11 @@ nav_msgs::msg::Path SmacPlanner::createPlan( pose.pose.orientation.w = 1.0; // Compute plan - NodeSE2::CoordinateVector path; + NodeHybrid::CoordinateVector path; int num_iterations = 0; std::string error; try { - if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) - { + if (!_a_star->createPath(path, num_iterations, 0.0)) { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found"); } else { @@ -291,18 +300,11 @@ nav_msgs::msg::Path SmacPlanner::createPlan( return plan; } - // Convert to world coordinates and downsample path for smoothing if necesssary - // We're going to downsample by 4x to give terms room to move. - const int downsample_ratio = 4; - std::vector path_world; - path_world.reserve(path.size()); + // Convert to world coordinates plan.poses.reserve(path.size()); - for (int i = path.size() - 1; i >= 0; --i) { - path_world.push_back(getWorldCoords(path[i].x, path[i].y, costmap)); - pose.pose.position.x = path_world.back().x(); - pose.pose.position.y = path_world.back().y(); - pose.pose.orientation = getWorldOrientation(path[i].theta); + pose.pose = getWorldCoords(path[i].x, path[i].y, costmap); + pose.pose.orientation = getWorldOrientation(path[i].theta, _angle_bin_size); plan.poses.push_back(pose); } @@ -311,78 +313,32 @@ nav_msgs::msg::Path SmacPlanner::createPlan( _raw_plan_publisher->publish(plan); } - // If not smoothing or too short to smooth, return path - if (!_smoother || path_world.size() < 4) { -#ifdef BENCHMARK_TESTING - steady_clock::time_point b = steady_clock::now(); - duration time_span = duration_cast>(b - a); - std::cout << "It took " << time_span.count() * 1000 << - " milliseconds with " << num_iterations << " iterations." << std::endl; -#endif - return plan; - } - // Find how much time we have left to do smoothing steady_clock::time_point b = steady_clock::now(); duration time_span = duration_cast>(b - a); double time_remaining = _max_planning_time - static_cast(time_span.count()); - _smoother_params.max_time = std::min(time_remaining, _optimizer_params.max_time); + +#ifdef BENCHMARK_TESTING + std::cout << "It took " << time_span.count() * 1000 << + " milliseconds with " << num_iterations << " iterations." << std::endl; +#endif // Smooth plan - if (!_smoother->smooth(path_world, costmap, _smoother_params)) { - RCLCPP_WARN( - _logger, - "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", - _name.c_str()); - return plan; + if (num_iterations > 1 && plan.poses.size() > 6) { + _smoother->smooth(plan, costmap, time_remaining); } - removeHook(path_world); - - // populate final path - // TODO(stevemacenski): set orientation to tangent of path - for (uint i = 0; i != path_world.size(); i++) { - pose.pose.position.x = path_world[i][0]; - pose.pose.position.y = path_world[i][1]; - plan.poses[i] = pose; - } +#ifdef BENCHMARK_TESTING + steady_clock::time_point c = steady_clock::now(); + duration time_span2 = duration_cast>(c - b); + std::cout << "It took " << time_span2.count() * 1000 << + " milliseconds to smooth path." << std::endl; +#endif return plan; } -void SmacPlanner::removeHook(std::vector & path) -{ - // Removes the end "hooking" since goal is locked in place - Eigen::Vector2d interpolated_second_to_last_point; - interpolated_second_to_last_point = (path.end()[-3] + path.end()[-1]) / 2.0; - if ( - squaredDistance(path.end()[-2], path.end()[-1]) > - squaredDistance(interpolated_second_to_last_point, path.end()[-1])) - { - path.end()[-2] = interpolated_second_to_last_point; - } -} - -Eigen::Vector2d SmacPlanner::getWorldCoords( - const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap) -{ - // mx, my are in continuous grid coordinates, must convert to world coordinates - double world_x = - static_cast(costmap->getOriginX()) + (mx + 0.5) * costmap->getResolution(); - double world_y = - static_cast(costmap->getOriginY()) + (my + 0.5) * costmap->getResolution(); - return Eigen::Vector2d(world_x, world_y); -} - -geometry_msgs::msg::Quaternion SmacPlanner::getWorldOrientation(const float & theta) -{ - // theta is in continuous bin coordinates, must convert to world orientation - tf2::Quaternion q; - q.setEuler(0.0, 0.0, theta * static_cast(_angle_bin_size)); - return tf2::toMsg(q); -} - } // namespace nav2_smac_planner #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlannerHybrid, nav2_core::GlobalPlanner) diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index c5ff757d1c8..1409c86debe 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -20,14 +20,14 @@ target_link_libraries(test_node2d ${library_name} ) -# Test NodeSE2 -ament_add_gtest(test_nodese2 - test_nodese2.cpp +# Test NodeHybrid +ament_add_gtest(test_nodehybrid + test_nodehybrid.cpp ) -ament_target_dependencies(test_nodese2 +ament_target_dependencies(test_nodehybrid ${dependencies} ) -target_link_libraries(test_nodese2 +target_link_libraries(test_nodehybrid ${library_name} ) @@ -64,14 +64,14 @@ target_link_libraries(test_a_star ${library_name} ) -# Test SMAC SE2 -ament_add_gtest(test_smac_se2 - test_smac_se2.cpp +# Test SMAC Hybrid +ament_add_gtest(test_smac_hybrid + test_smac_hybrid.cpp ) -ament_target_dependencies(test_smac_se2 +ament_target_dependencies(test_smac_hybrid ${dependencies} ) -target_link_libraries(test_smac_se2 +target_link_libraries(test_smac_hybrid ${library_name} ) @@ -86,13 +86,3 @@ target_link_libraries(test_smac_2d ${library_name}_2d ) -# Test smoother -ament_add_gtest(test_smoother - test_smoother.cpp -) -ament_target_dependencies(test_smoother - ${dependencies} -) -target_link_libraries(test_smoother - ${library_name}_2d -) diff --git a/nav2_smac_planner/include/nav2_smac_planner/options.hpp b/nav2_smac_planner/test/deprecated/options.hpp similarity index 98% rename from nav2_smac_planner/include/nav2_smac_planner/options.hpp rename to nav2_smac_planner/test/deprecated/options.hpp index 159ab8b53aa..5d33223dc41 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/options.hpp +++ b/nav2_smac_planner/test/deprecated/options.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_SMAC_PLANNER__OPTIONS_HPP_ -#define NAV2_SMAC_PLANNER__OPTIONS_HPP_ +#ifndef DEPRECATED__OPTIONS_HPP_ +#define DEPRECATED__OPTIONS_HPP_ #include #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -204,4 +204,4 @@ struct OptimizerParams } // namespace nav2_smac_planner -#endif // NAV2_SMAC_PLANNER__OPTIONS_HPP_ +#endif // DEPRECATED__OPTIONS_HPP_ diff --git a/nav2_smac_planner/test/deprecated/smoother.hpp b/nav2_smac_planner/test/deprecated/smoother.hpp new file mode 100644 index 00000000000..e523eaea1bb --- /dev/null +++ b/nav2_smac_planner/test/deprecated/smoother.hpp @@ -0,0 +1,141 @@ +// Copyright (c) 2020, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef DEPRECATED__SMOOTHER_HPP_ +#define DEPRECATED__SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "nav2_smac_planner/types.hpp" +#include "nav2_smac_planner/smoother_cost_function.hpp" + +#include "ceres/ceres.h" +#include "Eigen/Core" + +namespace nav2_smac_planner +{ + +/** + * @class nav2_smac_planner::Smoother + * @brief A Conjugate Gradient 2D path smoother implementation + */ +class Smoother +{ +public: + /** + * @brief A constructor for nav2_smac_planner::Smoother + */ + Smoother() {} + + /** + * @brief A destructor for nav2_smac_planner::Smoother + */ + ~Smoother() {} + + /** + * @brief Initialization of the smoother + * @param params OptimizerParam struct + */ + void initialize(const OptimizerParams params) + { + _debug = params.debug; + + // General Params + + // 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT + _options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT; + _options.line_search_type = ceres::WOLFE; + _options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE; + _options.line_search_interpolation_type = ceres::CUBIC; + + _options.max_num_iterations = params.max_iterations; + _options.max_solver_time_in_seconds = params.max_time; + + _options.function_tolerance = params.fn_tol; + _options.gradient_tolerance = params.gradient_tol; + _options.parameter_tolerance = params.param_tol; + + _options.min_line_search_step_size = params.advanced.min_line_search_step_size; + _options.max_num_line_search_step_size_iterations = + params.advanced.max_num_line_search_step_size_iterations; + _options.line_search_sufficient_function_decrease = + params.advanced.line_search_sufficient_function_decrease; + _options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction; + _options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction; + _options.max_num_line_search_direction_restarts = + params.advanced.max_num_line_search_direction_restarts; + _options.line_search_sufficient_curvature_decrease = + params.advanced.line_search_sufficient_curvature_decrease; + _options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion; + + if (_debug) { + _options.minimizer_progress_to_stdout = true; + } else { + _options.logging_type = ceres::SILENT; + } + } + + /** + * @brief Smoother method + * @param path Reference to path + * @param costmap Pointer to minimal costmap + * @param smoother parameters weights + * @return If smoothing was successful + */ + bool smooth( + std::vector & path, + nav2_costmap_2d::Costmap2D * costmap, + const SmootherParams & params) + { + _options.max_solver_time_in_seconds = params.max_time; + + double parameters[path.size() * 2]; // NOLINT + for (uint i = 0; i != path.size(); i++) { + parameters[2 * i] = path[i][0]; + parameters[2 * i + 1] = path[i][1]; + } + + ceres::GradientProblemSolver::Summary summary; + ceres::GradientProblem problem(new UnconstrainedSmootherCostFunction(&path, costmap, params)); + ceres::Solve(_options, problem, parameters, &summary); + + if (_debug) { + std::cout << summary.FullReport() << '\n'; + } + + if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost <= 0.0) { + return false; + } + + for (uint i = 0; i != path.size(); i++) { + path[i][0] = parameters[2 * i]; + path[i][1] = parameters[2 * i + 1]; + } + + return true; + } + +private: + bool _debug; + ceres::GradientProblemSolver::Options _options; +}; + +} // namespace nav2_smac_planner + +#endif // DEPRECATED__SMOOTHER_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp b/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp similarity index 94% rename from nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp rename to nav2_smac_planner/test/deprecated/smoother_cost_function.hpp index eec8c61b4e3..7e5349c894e 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother_cost_function.hpp +++ b/nav2_smac_planner/test/deprecated/smoother_cost_function.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ -#define NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ +#ifndef DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_ +#define DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_ #include #include @@ -56,6 +56,22 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction _costmap(costmap), _params(params) { + // int height = costmap->getSizeInCellsX(); + // int width = costmap->getSizeInCellsY(); + // bool** binMap; + // binMap = new bool*[width]; + + // for (int x = 0; x < width; x++) { binMap[x] = new bool[height]; } + + // for (int x = 0; x < width; ++x) { + // for (int y = 0; y < height; ++y) { + // binMap[x][y] = costmap->getCost(x,y) >= 253 ? true : false; + // } + // } + + // voronoiDiagram.initializeMap(width, height, binMap); + // voronoiDiagram.update(); + // voronoiDiagram.visualize(); } /** @@ -138,7 +154,7 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction if (valid_coords = _costmap->worldToMap(xi[0], xi[1], mx, my)) { costmap_cost = _costmap->getCost(mx, my); - addCostResidual(_params.costmap_weight, costmap_cost, cost_raw); + addCostResidual(_params.costmap_weight, costmap_cost, cost_raw, xi); } if (gradient != NULL) { @@ -379,13 +395,23 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction inline void addCostResidual( const double & weight, const double & value, - double & r) const + double & r, + Eigen::Vector2d & xi) const { if (value == FREE) { return; } r += weight * value * value; // objective function value + + + // float obsDst = voronoiDiagram.getDistance((int)xi[0], (int)xi[1]); + + // if (abs(obsDst) > 0.3) { + // return; + // } + + // r += weight * (abs(obsDst) - 0.3) * (abs(obsDst) - 0.3); } /** @@ -508,8 +534,9 @@ class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction int _num_params; nav2_costmap_2d::Costmap2D * _costmap{nullptr}; SmootherParams _params; + // DynamicVoronoi voronoiDiagram; }; } // namespace nav2_smac_planner -#endif // NAV2_SMAC_PLANNER__SMOOTHER_COST_FUNCTION_HPP_ +#endif // DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_ diff --git a/nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp b/nav2_smac_planner/test/deprecated/upsampler.hpp similarity index 98% rename from nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp rename to nav2_smac_planner/test/deprecated/upsampler.hpp index 84ba625fa11..90e0f8b4642 100644 --- a/nav2_smac_planner/test/deprecated_upsampler/upsampler.hpp +++ b/nav2_smac_planner/test/deprecated/upsampler.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ -#define DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ +#ifndef DEPRECATED__UPSAMPLER_HPP_ +#define DEPRECATED__UPSAMPLER_HPP_ #include #include @@ -210,4 +210,4 @@ class Upsampler } // namespace nav2_smac_planner -#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_HPP_ +#endif // DEPRECATED__UPSAMPLER_HPP_ diff --git a/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp b/nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp similarity index 98% rename from nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp rename to nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp index 44df1c3f228..4a89800b5b5 100644 --- a/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function.hpp +++ b/nav2_smac_planner/test/deprecated/upsampler_cost_function.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ -#define DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ +#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_ +#define DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_ #include #include @@ -363,4 +363,4 @@ class UpsamplerCostFunction : public ceres::FirstOrderFunction } // namespace nav2_smac_planner -#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_HPP_ +#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_ diff --git a/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp b/nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp similarity index 98% rename from nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp rename to nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp index 17204694b1b..f1599bcd785 100644 --- a/nav2_smac_planner/test/deprecated_upsampler/upsampler_cost_function_nlls.hpp +++ b/nav2_smac_planner/test/deprecated/upsampler_cost_function_nlls.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ -#define DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ +#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ +#define DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ #include #include @@ -331,4 +331,4 @@ class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1 } // namespace nav2_smac_planner -#endif // DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ +#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 721eee47100..17e10708f21 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -22,7 +22,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/collision_checker.hpp" @@ -45,8 +45,7 @@ TEST(AStarTest, test_a_star_2d) int it_on_approach = 10; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach); - a_star.setFootprint(nav2_costmap_2d::Footprint(), true); + a_star.initialize(false, max_iterations, it_on_approach, 0.0, 1); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -58,12 +57,15 @@ TEST(AStarTest, test_a_star_2d) } // functional case testing - a_star.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA); + std::unique_ptr checker = + std::make_unique(costmapA, 1); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + a_star.setCollisionChecker(checker.get()); a_star.setStart(20u, 20u, 0); a_star.setGoal(80u, 80u, 0); nav2_smac_planner::Node2D::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); - EXPECT_EQ(num_it, 556); + EXPECT_EQ(num_it, 102); // check path is the right size and collision free EXPECT_EQ(path.size(), 81u); @@ -72,9 +74,6 @@ TEST(AStarTest, test_a_star_2d) } // setting non-zero dim 3 for 2D search - EXPECT_THROW( - a_star.createGraph( - costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 10, costmapA), std::runtime_error); EXPECT_THROW(a_star.setGoal(0, 0, 10), std::runtime_error); EXPECT_THROW(a_star.setStart(0, 0, 10), std::runtime_error); @@ -82,11 +81,10 @@ TEST(AStarTest, test_a_star_2d) // failure cases with invalid inputs nav2_smac_planner::AStarAlgorithm a_star_2( nav2_smac_planner::MotionModel::VON_NEUMANN, info); - a_star_2.initialize(false, max_iterations, it_on_approach); - a_star_2.setFootprint(nav2_costmap_2d::Footprint(), true); + a_star_2.initialize(false, max_iterations, it_on_approach, 0, 1); num_it = 0; EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); - a_star_2.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA); + a_star_2.setCollisionChecker(checker.get()); num_it = 0; EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); a_star_2.setStart(50, 50, 0); // invalid @@ -102,7 +100,7 @@ TEST(AStarTest, test_a_star_2d) a_star_2.setStart(20, 20, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); - EXPECT_EQ(path.size(), 32u); + EXPECT_EQ(path.size(), 42u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -112,7 +110,7 @@ TEST(AStarTest, test_a_star_2d) EXPECT_EQ(a_star_2.getSizeX(), 100u); EXPECT_EQ(a_star_2.getSizeY(), 100u); EXPECT_EQ(a_star_2.getSizeDim3(), 1u); - EXPECT_EQ(a_star_2.getToleranceHeuristic(), 1000.0); + EXPECT_EQ(a_star_2.getToleranceHeuristic(), 20.0); EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10); delete costmapA; @@ -121,20 +119,20 @@ TEST(AStarTest, test_a_star_2d) TEST(AStarTest, test_a_star_se2) { nav2_smac_planner::SearchInfo info; - info.change_penalty = 1.2; - info.non_straight_penalty = 1.4; - info.reverse_penalty = 2.1; - info.minimum_turning_radius = 2.0; // in grid coordinates + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.minimum_turning_radius = 8; // in grid coordinates unsigned int size_theta = 72; - nav2_smac_planner::AStarAlgorithm a_star( + info.cost_penalty = 1.7; + nav2_smac_planner::AStarAlgorithm a_star( nav2_smac_planner::MotionModel::DUBIN, info); int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach); - a_star.setFootprint(nav2_costmap_2d::Footprint(), true); + a_star.initialize(false, max_iterations, it_on_approach, 401, size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -145,17 +143,20 @@ TEST(AStarTest, test_a_star_se2) } } + std::unique_ptr checker = + std::make_unique(costmapA, size_theta); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + // functional case testing - a_star.createGraph( - costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), size_theta, costmapA); + a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); a_star.setGoal(80u, 80u, 40u); - nav2_smac_planner::NodeSE2::CoordinateVector path; + nav2_smac_planner::NodeHybrid::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); // check path is the right size and collision free - EXPECT_EQ(num_it, 44); - EXPECT_EQ(path.size(), 75u); + EXPECT_EQ(num_it, 351); + EXPECT_EQ(path.size(), 73u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp index 687b4b5cc35..40e73c82ada 100644 --- a/nav2_smac_planner/test/test_collision_checker.cpp +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -41,8 +41,8 @@ TEST(collision_footprint, test_basic) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); - collision_checker.setFootprint(footprint, false /*use footprint*/); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(5.0, 5.0, 0.0, false); float cost = collision_checker.getCost(); EXPECT_NEAR(cost, 0.0, 0.001); @@ -53,9 +53,9 @@ TEST(collision_footprint, test_point_cost) { nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); nav2_costmap_2d::Footprint footprint; - collision_checker.setFootprint(footprint, true /*radius / pointcose*/); + collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0); collision_checker.inCollision(5.0, 5.0, 0.0, false); float cost = collision_checker.getCost(); @@ -67,9 +67,9 @@ TEST(collision_footprint, test_world_to_map) { nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0); - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); nav2_costmap_2d::Footprint footprint; - collision_checker.setFootprint(footprint, true /*radius / point cost*/); + collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0); unsigned int x, y; @@ -94,7 +94,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) for (unsigned int i = 40; i <= 60; ++i) { for (unsigned int j = 40; j <= 60; ++j) { - costmap_->setCost(i, j, 0); + costmap_->setCost(i, j, 128); } } @@ -113,12 +113,12 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); - collision_checker.setFootprint(footprint, false /*use footprint*/); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); float cost = collision_checker.getCost(); - EXPECT_NEAR(cost, 0.0, 0.001); + EXPECT_NEAR(cost, 128.0, 0.001); collision_checker.inCollision(50, 49, 0.0, false); float up_value = collision_checker.getCost(); @@ -133,8 +133,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement) TEST(collision_footprint, test_point_and_line_cost) { nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( - 100, 100, 0.10000, 0, 0.0, - 0.0); + 100, 100, 0.10000, 0, 0.0, 128.0); costmap_->setCost(62, 50, 254); costmap_->setCost(39, 60, 254); @@ -154,12 +153,12 @@ TEST(collision_footprint, test_point_and_line_cost) nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; - nav2_smac_planner::GridCollisionChecker collision_checker(costmap_); - collision_checker.setFootprint(footprint, false /*use footprint*/); + nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72); + collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0); collision_checker.inCollision(50, 50, 0.0, false); float value = collision_checker.getCost(); - EXPECT_NEAR(value, 0.0, 0.001); + EXPECT_NEAR(value, 128.0, 0.001); collision_checker.inCollision(49, 50, 0.0, false); float left_value = collision_checker.getCost(); diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 33377254a76..f4520bd4961 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -35,41 +35,45 @@ RclCppFixture g_rclcppfixture; TEST(Node2DTest, test_node_2d) { nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); - nav2_smac_planner::GridCollisionChecker checker(&costmapA); + std::unique_ptr checker = + std::make_unique(&costmapA, 72); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); // test construction unsigned char cost = static_cast(1); - nav2_smac_planner::Node2D testA(cost, 1); - nav2_smac_planner::Node2D testB(cost, 1); + nav2_smac_planner::Node2D testA(1); + testA.setCost(cost); + nav2_smac_planner::Node2D testB(1); + testB.setCost(cost); EXPECT_EQ(testA.getCost(), 1.0f); + nav2_smac_planner::SearchInfo info; + info.cost_penalty = 1.0; + unsigned int size = 10; + nav2_smac_planner::Node2D::initMotionModel( + nav2_smac_planner::MotionModel::MOORE, size, size, size, info); // test reset - testA.reset(10); - EXPECT_EQ(testA.getCost(), 10.0f); - - // Check constants - EXPECT_EQ(testA.neutral_cost, 50.0f); + testA.reset(); + EXPECT_TRUE(std::isnan(testA.getCost())); // check collision checking - EXPECT_EQ(testA.isNodeValid(false, checker), true); - testA.reset(254); - EXPECT_EQ(testA.isNodeValid(false, checker), false); - testA.reset(255); - EXPECT_EQ(testA.isNodeValid(true, checker), true); - EXPECT_EQ(testA.isNodeValid(false, checker), false); - testA.reset(10); + EXPECT_EQ(testA.isNodeValid(false, checker.get()), true); + testA.setCost(255); + EXPECT_EQ(testA.isNodeValid(true, checker.get()), true); + testA.setCost(10); // check traversal cost computation - EXPECT_EQ(testB.getTraversalCost(&testA), 58.0f); + EXPECT_NEAR(testB.getTraversalCost(&testA), 1.03f, 0.1f); // check heuristic cost computation nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0); nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0); - EXPECT_NEAR(testB.getHeuristicCost(A, B), 559.016, 0.01); + EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 15., 0.01); // check operator== works on index unsigned char costC = '2'; - nav2_smac_planner::Node2D testC(costC, 1); + nav2_smac_planner::Node2D testC(1); + testC.setCost(costC); EXPECT_TRUE(testA == testC); // check accumulated costs are set @@ -97,15 +101,24 @@ TEST(Node2DTest, test_node_2d) TEST(Node2DTest, test_node_2d_neighbors) { + nav2_smac_planner::SearchInfo info; + unsigned int size_x = 10u; + unsigned int size_y = 10u; + unsigned int quant = 0u; // test neighborhood computation - nav2_smac_planner::Node2D::initNeighborhood(10u, nav2_smac_planner::MotionModel::VON_NEUMANN); + nav2_smac_planner::Node2D::initMotionModel( + nav2_smac_planner::MotionModel::VON_NEUMANN, size_x, + size_y, quant, info); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -10); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 10); - nav2_smac_planner::Node2D::initNeighborhood(100u, nav2_smac_planner::MotionModel::MOORE); + size_x = 100u; + nav2_smac_planner::Node2D::initMotionModel( + nav2_smac_planner::MotionModel::MOORE, size_x, size_y, + quant, info); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1); @@ -117,17 +130,19 @@ TEST(Node2DTest, test_node_2d_neighbors) EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[7], 101); nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); - nav2_smac_planner::GridCollisionChecker checker(&costmapA); + std::unique_ptr checker = + std::make_unique(&costmapA, 72); unsigned char cost = static_cast(1); - nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(cost, 1); + nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1); + node->setCost(cost); std::function neighborGetter = [&, this](const unsigned int & index, nav2_smac_planner::Node2D * & neighbor_rtn) -> bool { - return true; + return false; }; nav2_smac_planner::Node2D::NodeVector neighbors; - nav2_smac_planner::Node2D::getNeighbors(node, neighborGetter, checker, false, neighbors); + node->getNeighbors(neighborGetter, checker.get(), false, neighbors); delete node; // should be empty since totally invalid diff --git a/nav2_smac_planner/test/test_nodebasic.cpp b/nav2_smac_planner/test/test_nodebasic.cpp index 23d445ce547..965246d67c0 100644 --- a/nav2_smac_planner/test/test_nodebasic.cpp +++ b/nav2_smac_planner/test/test_nodebasic.cpp @@ -24,7 +24,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_smac_planner/node_basic.hpp" #include "nav2_smac_planner/node_2d.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/collision_checker.hpp" class RclCppFixture @@ -37,7 +37,7 @@ RclCppFixture g_rclcppfixture; TEST(NodeBasicTest, test_node_basic) { - nav2_smac_planner::NodeBasic node(50); + nav2_smac_planner::NodeBasic node(50); EXPECT_EQ(node.index, 50u); EXPECT_EQ(node.graph_node_ptr, nullptr); diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp new file mode 100644 index 00000000000..9954a011ef0 --- /dev/null +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -0,0 +1,219 @@ +// Copyright (c) 2020, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" +#include "nav2_smac_planner/collision_checker.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(NodeHybridTest, test_node_hybrid) +{ + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.minimum_turning_radius = 8; // 0.4m/5cm resolution costmap + info.cost_penalty = 1.7; + unsigned int size_x = 10; + unsigned int size_y = 10; + unsigned int size_theta = 72; + + // Check defaulted constants + nav2_smac_planner::NodeHybrid testA(49); + EXPECT_EQ(testA.travel_distance_cost, sqrt(2)); + + nav2_smac_planner::NodeHybrid::initMotionModel( + nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); + + nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( + 10, 10, 0.05, 0.0, 0.0, 0); + std::unique_ptr checker = + std::make_unique(costmapA, 72); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + // test construction + nav2_smac_planner::NodeHybrid testB(49); + EXPECT_TRUE(std::isnan(testA.getCost())); + + // test node valid and cost + testA.pose.x = 5; + testA.pose.y = 5; + testA.pose.theta = 0; + EXPECT_EQ(testA.isNodeValid(true, checker.get()), true); + EXPECT_EQ(testA.isNodeValid(false, checker.get()), true); + EXPECT_EQ(testA.getCost(), 0.0f); + + // test reset + testA.reset(); + EXPECT_TRUE(std::isnan(testA.getCost())); + + // Check motion-specific constants + EXPECT_NEAR(testA.travel_distance_cost, 2.08842, 0.1); + + // check collision checking + EXPECT_EQ(testA.isNodeValid(false, checker.get()), true); + + // check traversal cost computation + // simulated first node, should return neutral cost + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088, 0.1); + // now with straight motion, cost is 0, so will be sqrt(2) as well + testB.setMotionPrimitiveIndex(1); + testA.setMotionPrimitiveIndex(0); + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.088, 0.1); + // same direction as parent, testB + testA.setMotionPrimitiveIndex(1); + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.297f, 0.01); + // opposite direction as parent, testB + testA.setMotionPrimitiveIndex(2); + EXPECT_NEAR(testB.getTraversalCost(&testA), 2.506f, 0.01); + + // will throw because never collision checked testB + EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error); + + // check motion primitives + EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u); + + // check operator== works on index + nav2_smac_planner::NodeHybrid testC(49); + EXPECT_TRUE(testA == testC); + + // check accumulated costs are set + testC.setAccumulatedCost(100); + EXPECT_EQ(testC.getAccumulatedCost(), 100.0f); + + // check visiting state + EXPECT_EQ(testC.wasVisited(), false); + testC.visited(); + EXPECT_EQ(testC.wasVisited(), true); + + // check index + EXPECT_EQ(testC.getIndex(), 49u); + + // check set pose and pose + testC.setPose(nav2_smac_planner::NodeHybrid::Coordinates(10.0, 5.0, 4)); + EXPECT_EQ(testC.pose.x, 10.0); + EXPECT_EQ(testC.pose.y, 5.0); + EXPECT_EQ(testC.pose.theta, 4); + + // check static index functions + EXPECT_EQ(nav2_smac_planner::NodeHybrid::getIndex(1u, 1u, 4u, 10u, 72u), 796u); + EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).x, 1u); + EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).y, 1u); + EXPECT_EQ(nav2_smac_planner::NodeHybrid::getCoords(796u, 10u, 72u).theta, 4u); + + delete costmapA; +} + +TEST(NodeHybridTest, test_node_debin_neighbors) +{ + nav2_smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 4; // 0.2 in grid coordinates + unsigned int size_x = 100; + unsigned int size_y = 100; + unsigned int size_theta = 72; + nav2_smac_planner::NodeHybrid::initMotionModel( + nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); + + + // test neighborhood computation + EXPECT_EQ(nav2_smac_planner::NodeHybrid::motion_table.projections.size(), 3u); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 1.731517, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 1.69047, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.3747, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 5, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 1.69047, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.3747, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -5, 0.01); +} + +TEST(NodeHybridTest, test_node_reeds_neighbors) +{ + nav2_smac_planner::SearchInfo info; + info.change_penalty = 1.2; + info.non_straight_penalty = 1.4; + info.reverse_penalty = 2.1; + info.minimum_turning_radius = 8; // 0.4 in grid coordinates + unsigned int size_x = 100; + unsigned int size_y = 100; + unsigned int size_theta = 72; + nav2_smac_planner::NodeHybrid::initMotionModel( + nav2_smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info); + + EXPECT_EQ(nav2_smac_planner::NodeHybrid::motion_table.projections.size(), 6u); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 2.088, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 2.070, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.272, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 3, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 2.070, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.272, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -3, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._x, -2.088, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._y, 0, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[3]._theta, 0, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._x, -2.07, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._y, 0.272, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[4]._theta, -3, 0.01); + + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._x, -2.07, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._y, -0.272, 0.01); + EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._theta, 3, 0.01); + + nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); + std::unique_ptr checker = + std::make_unique(&costmapA, 72); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49); + std::function neighborGetter = + [&, this](const unsigned int & index, nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool + { + // because we don't return a real object + return false; + }; + + nav2_smac_planner::NodeHybrid::NodeVector neighbors; + node->getNeighbors(neighborGetter, checker.get(), false, neighbors); + delete node; + + // should be empty since totally invalid + EXPECT_EQ(neighbors.size(), 0u); +} diff --git a/nav2_smac_planner/test/test_nodese2.cpp b/nav2_smac_planner/test/test_nodese2.cpp deleted file mode 100644 index 39b9aa2dfa9..00000000000 --- a/nav2_smac_planner/test/test_nodese2.cpp +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include -#include -#include -#include - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_smac_planner/node_se2.hpp" -#include "nav2_smac_planner/collision_checker.hpp" - -class RclCppFixture -{ -public: - RclCppFixture() {rclcpp::init(0, nullptr);} - ~RclCppFixture() {rclcpp::shutdown();} -}; -RclCppFixture g_rclcppfixture; - -TEST(NodeSE2Test, test_node_se2) -{ - nav2_smac_planner::SearchInfo info; - info.change_penalty = 1.2; - info.non_straight_penalty = 1.4; - info.reverse_penalty = 2.1; - info.minimum_turning_radius = 0.20; - unsigned int size_x = 10; - unsigned int size_y = 10; - unsigned int size_theta = 72; - - nav2_smac_planner::NodeSE2::initMotionModel( - nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); - - nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D( - 10, 10, 0.05, 0.0, 0.0, 0); - nav2_smac_planner::GridCollisionChecker checker(costmapA); - checker.setFootprint(nav2_costmap_2d::Footprint(), true); - - // test construction - nav2_smac_planner::NodeSE2 testA(49); - nav2_smac_planner::NodeSE2 testB(49); - EXPECT_TRUE(std::isnan(testA.getCost())); - - // test node valid and cost - testA.pose.x = 5; - testA.pose.y = 5; - testA.pose.theta = 0; - EXPECT_EQ(testA.isNodeValid(true, checker), true); - EXPECT_EQ(testA.isNodeValid(false, checker), true); - EXPECT_EQ(testA.getCost(), 0.0f); - - // test reset - testA.reset(); - EXPECT_TRUE(std::isnan(testA.getCost())); - - // Check constants - EXPECT_EQ(testA.neutral_cost, sqrt(2)); - - // check collision checking - EXPECT_EQ(testA.isNodeValid(false, checker), true); - - // check traversal cost computation - // simulated first node, should return neutral cost - EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); - // now with straight motion, cost is 0, so will be sqrt(2) as well - testB.setMotionPrimitiveIndex(1); - testA.setMotionPrimitiveIndex(0); - EXPECT_NEAR(testB.getTraversalCost(&testA), sqrt(2), 0.01); - // same direction as parent, testB - testA.setMotionPrimitiveIndex(1); - EXPECT_NEAR(testB.getTraversalCost(&testA), 1.9799f, 0.01); - // opposite direction as parent, testB - testA.setMotionPrimitiveIndex(2); - EXPECT_NEAR(testB.getTraversalCost(&testA), 3.67696f, 0.01); - - // will throw because never collision checked testB - EXPECT_THROW(testA.getTraversalCost(&testB), std::runtime_error); - - // check motion primitives - EXPECT_EQ(testA.getMotionPrimitiveIndex(), 2u); - - // check heuristic cost computation - nav2_smac_planner::NodeSE2::computeWavefrontHeuristic( - costmapA, - static_cast(10.0), - static_cast(5.0), - 0.0, 0.0); - nav2_smac_planner::NodeSE2::Coordinates A(0.0, 0.0, 4.2); - nav2_smac_planner::NodeSE2::Coordinates B(10.0, 5.0, 54.1); - EXPECT_NEAR(testB.getHeuristicCost(B, A), 16.723, 0.01); - - // check operator== works on index - nav2_smac_planner::NodeSE2 testC(49); - EXPECT_TRUE(testA == testC); - - // check accumulated costs are set - testC.setAccumulatedCost(100); - EXPECT_EQ(testC.getAccumulatedCost(), 100.0f); - - // check visiting state - EXPECT_EQ(testC.wasVisited(), false); - testC.queued(); - EXPECT_EQ(testC.isQueued(), true); - testC.visited(); - EXPECT_EQ(testC.wasVisited(), true); - EXPECT_EQ(testC.isQueued(), false); - - // check index - EXPECT_EQ(testC.getIndex(), 49u); - - // check set pose and pose - testC.setPose(nav2_smac_planner::NodeSE2::Coordinates(10.0, 5.0, 4)); - EXPECT_EQ(testC.pose.x, 10.0); - EXPECT_EQ(testC.pose.y, 5.0); - EXPECT_EQ(testC.pose.theta, 4); - - // check static index functions - EXPECT_EQ(nav2_smac_planner::NodeSE2::getIndex(1u, 1u, 4u, 10u, 72u), 796u); - EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).x, 1u); - EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).y, 1u); - EXPECT_EQ(nav2_smac_planner::NodeSE2::getCoords(796u, 10u, 72u).theta, 4u); - - delete costmapA; -} - -TEST(NodeSE2Test, test_node_2d_neighbors) -{ - nav2_smac_planner::SearchInfo info; - info.change_penalty = 1.2; - info.non_straight_penalty = 1.4; - info.reverse_penalty = 2.1; - info.minimum_turning_radius = 4; // 0.2 in grid coordinates - unsigned int size_x = 100; - unsigned int size_y = 100; - unsigned int size_theta = 72; - nav2_smac_planner::NodeSE2::initMotionModel( - nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); - - - // test neighborhood computation - EXPECT_EQ(nav2_smac_planner::NodeSE2::motion_table.projections.size(), 3u); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); - - nav2_smac_planner::NodeSE2::initMotionModel( - nav2_smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info); - - EXPECT_EQ(nav2_smac_planner::NodeSE2::motion_table.projections.size(), 6u); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._x, 1.731517, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._x, 1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._y, 0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[1]._theta, 5, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._x, 1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._y, -0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[2]._theta, -5, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._x, -1.731517, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._y, 0, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[3]._theta, 0, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._x, -1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._y, 0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[4]._theta, -5, 0.01); - - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._x, -1.69047, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._y, -0.3747, 0.01); - EXPECT_NEAR(nav2_smac_planner::NodeSE2::motion_table.projections[5]._theta, 5, 0.01); - - nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); - nav2_smac_planner::GridCollisionChecker checker(&costmapA); - nav2_smac_planner::NodeSE2 * node = new nav2_smac_planner::NodeSE2(49); - std::function neighborGetter = - [&, this](const unsigned int & index, nav2_smac_planner::NodeSE2 * & neighbor_rtn) -> bool - { - // because we don't return a real object - return false; - }; - - nav2_smac_planner::NodeSE2::NodeVector neighbors; - nav2_smac_planner::NodeSE2::getNeighbors(node, neighborGetter, checker, false, neighbors); - delete node; - - // should be empty since totally invalid - EXPECT_EQ(neighbors.size(), 0u); -} diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index 59a31eb17ba..f3391d42404 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -23,10 +23,10 @@ #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/collision_checker.hpp" -#include "nav2_smac_planner/smac_planner.hpp" +#include "nav2_smac_planner/smac_planner_hybrid.hpp" #include "nav2_smac_planner/smac_planner_2d.hpp" class RclCppFixture diff --git a/nav2_smac_planner/test/test_smac_se2.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp similarity index 85% rename from nav2_smac_planner/test/test_smac_se2.cpp rename to nav2_smac_planner/test/test_smac_hybrid.cpp index c0a4cf8e8e2..9c6d50a0b51 100644 --- a/nav2_smac_planner/test/test_smac_se2.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -23,10 +23,10 @@ #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_smac_planner/node_se2.hpp" +#include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/a_star.hpp" #include "nav2_smac_planner/collision_checker.hpp" -#include "nav2_smac_planner/smac_planner.hpp" +#include "nav2_smac_planner/smac_planner_hybrid.hpp" #include "nav2_smac_planner/smac_planner_2d.hpp" class RclCppFixture @@ -50,8 +50,6 @@ TEST(SmacTest, test_smac_se2) std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); - nodeSE2->declare_parameter("test.smooth_path", true); - nodeSE2->set_parameter(rclcpp::Parameter("test.smooth_path", true)); nodeSE2->declare_parameter("test.downsample_costmap", true); nodeSE2->set_parameter(rclcpp::Parameter("test.downsample_costmap", true)); nodeSE2->declare_parameter("test.downsampling_factor", 2); @@ -61,8 +59,10 @@ TEST(SmacTest, test_smac_se2) start.pose.position.x = 0.0; start.pose.position.y = 0.0; start.pose.orientation.w = 1.0; - goal = start; - auto planner = std::make_unique(); + goal.pose.position.x = 1.0; + goal.pose.position.y = 1.0; + goal.pose.orientation.w = 1.0; + auto planner = std::make_unique(); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -79,14 +79,3 @@ TEST(SmacTest, test_smac_se2) costmap_ros.reset(); nodeSE2.reset(); } - -TEST(SmacTestSE2, test_dist) -{ - Eigen::Vector2d p1; - p1[0] = 0.0; - p1[1] = 0.0; - Eigen::Vector2d p2; - p2[0] = 0.0; - p2[1] = 1.0; - EXPECT_NEAR(nav2_smac_planner::squaredDistance(p1, p2), 1.0, 0.001); -} diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp deleted file mode 100644 index 41c8280950e..00000000000 --- a/nav2_smac_planner/test/test_smoother.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include -#include -#include -#include - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_smac_planner/a_star.hpp" -#include "nav2_smac_planner/smoother.hpp" - -class RclCppFixture -{ -public: - RclCppFixture() {rclcpp::init(0, nullptr);} - ~RclCppFixture() {rclcpp::shutdown();} -}; -RclCppFixture g_rclcppfixture; - -TEST(SmootherTest, test_smoother) -{ - rclcpp_lifecycle::LifecycleNode::SharedPtr node2D = - std::make_shared("SmootherTest"); - - // create and populate costmap - nav2_costmap_2d::Costmap2D * costmap = new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0, 0, 0); - - for (unsigned int i = 40; i <= 60; ++i) { - for (unsigned int j = 40; j <= 60; ++j) { - costmap->setCost(i, j, 254); - } - } - - // compute path to use - nav2_smac_planner::SearchInfo info; - info.change_penalty = 1.2; - info.non_straight_penalty = 1.4; - info.reverse_penalty = 2.1; - info.minimum_turning_radius = 4.0; // in grid coordinates - unsigned int size_theta = 72; - nav2_smac_planner::AStarAlgorithm a_star( - nav2_smac_planner::MotionModel::DUBIN, info); - int max_iterations = 1000000; - float tolerance = 0.0; - int it_on_approach = 1000000000; - int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach); - a_star.setFootprint(nav2_costmap_2d::Footprint(), true); - a_star.createGraph(costmap->getSizeInCellsX(), costmap->getSizeInCellsY(), size_theta, costmap); - a_star.setStart(10u, 10u, 0u); - a_star.setGoal(80u, 80u, 40u); - nav2_smac_planner::NodeSE2::CoordinateVector plan; - EXPECT_TRUE(a_star.createPath(plan, num_it, tolerance)); - - // populate our smoothing paths - std::vector path; - std::vector initial_path; - for (unsigned int i = 0; i != plan.size(); i++) { - path.push_back(Eigen::Vector2d(plan[i].x, plan[i].y)); - initial_path.push_back(Eigen::Vector2d(plan[i].x, plan[i].y)); - } - - nav2_smac_planner::OptimizerParams params; - params.debug = true; - params.get(node2D.get(), "test"); - - nav2_smac_planner::SmootherParams smoother_params; - smoother_params.get(node2D.get(), "test"); - smoother_params.max_curvature = 5.0; - smoother_params.curvature_weight = 30.0; - smoother_params.distance_weight = 0.0; - smoother_params.smooth_weight = 00.0; - smoother_params.costmap_weight = 0.025; - - nav2_smac_planner::Smoother smoother; - smoother.initialize(params); - smoother.smooth(path, costmap, smoother_params); - - // kept at the right size - EXPECT_EQ(path.size(), 73u); - - for (unsigned int i = 1; i != path.size() - 1; i++) { - // check distance between points is in a good range - EXPECT_NEAR( - hypot(path[i][0] - path[i + 1][0], path[i][1] - path[i + 1][1]), 1.407170, 0.5); - } - - delete costmap; -} diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 99d03699726..96d932136b5 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -7,6 +7,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_map_server REQUIRED) +find_package(nav2_behavior_tree REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(visualization_msgs REQUIRED) @@ -21,6 +22,7 @@ find_package(nav2_navfn_planner REQUIRED) find_package(nav2_planner REQUIRED) find_package(navigation2) find_package(angles REQUIRED) +find_package(behaviortree_cpp_v3 REQUIRED) nav2_package() @@ -33,6 +35,7 @@ set(dependencies visualization_msgs nav2_amcl nav2_lifecycle_manager + nav2_behavior_tree gazebo_ros_pkgs geometry_msgs std_msgs @@ -41,6 +44,7 @@ set(dependencies nav2_planner nav2_navfn_planner angles + behaviortree_cpp_v3 ) if(BUILD_TESTING) @@ -50,6 +54,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) + add_subdirectory(src/behavior_tree) add_subdirectory(src/planning) add_subdirectory(src/localization) add_subdirectory(src/system) diff --git a/nav2_system_tests/models/turtlebot3_waffle/model-1_4.sdf b/nav2_system_tests/models/turtlebot3_waffle/model-1_4.sdf index 3f4ce11b0a8..c409a6e3e3e 100644 --- a/nav2_system_tests/models/turtlebot3_waffle/model-1_4.sdf +++ b/nav2_system_tests/models/turtlebot3_waffle/model-1_4.sdf @@ -87,6 +87,7 @@ + false ~/out:=imu diff --git a/nav2_system_tests/models/turtlebot3_waffle/model.sdf b/nav2_system_tests/models/turtlebot3_waffle/model.sdf index 897d69699c8..fdf2b1fef07 100644 --- a/nav2_system_tests/models/turtlebot3_waffle/model.sdf +++ b/nav2_system_tests/models/turtlebot3_waffle/model.sdf @@ -87,6 +87,7 @@ + false ~/out:=imu diff --git a/nav2_system_tests/models/turtlebot3_waffle_depth_camera/model.sdf b/nav2_system_tests/models/turtlebot3_waffle_depth_camera/model.sdf index 26b923b5086..0c7dfa22daf 100644 --- a/nav2_system_tests/models/turtlebot3_waffle_depth_camera/model.sdf +++ b/nav2_system_tests/models/turtlebot3_waffle_depth_camera/model.sdf @@ -77,6 +77,7 @@ + false ~/out:=imu diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 51ed254b9c4..ec3f0d2f42a 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.3 + 1.0.0 TODO Carlos Orduno Apache-2.0 @@ -17,6 +17,7 @@ nav2_msgs nav2_lifecycle_manager nav2_navfn_planner + nav2_behavior_tree nav_msgs visualization_msgs nav2_amcl @@ -40,6 +41,7 @@ nav2_msgs nav2_lifecycle_manager nav2_navfn_planner + nav2_behavior_tree nav_msgs visualization_msgs geometry_msgs diff --git a/nav2_system_tests/scripts/ctest_loop.bash b/nav2_system_tests/scripts/ctest_loop.bash index 3a3fc36d496..29f0e278289 100755 --- a/nav2_system_tests/scripts/ctest_loop.bash +++ b/nav2_system_tests/scripts/ctest_loop.bash @@ -1,7 +1,7 @@ #!/bin/bash # -# Simple bash script to loop over the navigation2 "bt_navigator" system test +# Simple bash script to loop over the Nav2 "bt_navigator" system test # # options: # -c <#> - number of times to loop diff --git a/nav2_system_tests/src/behavior_tree/CMakeLists.txt b/nav2_system_tests/src/behavior_tree/CMakeLists.txt new file mode 100644 index 00000000000..19eca116e07 --- /dev/null +++ b/nav2_system_tests/src/behavior_tree/CMakeLists.txt @@ -0,0 +1,16 @@ +find_package(Boost COMPONENTS system filesystem REQUIRED) + +ament_add_gtest(test_behavior_tree_node + test_behavior_tree_node.cpp + server_handler.cpp +) + +ament_target_dependencies(test_behavior_tree_node + ${dependencies} +) + +target_include_directories(test_behavior_tree_node PUBLIC ${Boost_INCLUDE_DIRS}) +target_link_libraries(test_behavior_tree_node + ${Boost_FILESYSTEM_LIBRARY} + ${Boost_SYSTEM_LIBRARY} +) diff --git a/nav2_system_tests/src/behavior_tree/dummy_servers.hpp b/nav2_system_tests/src/behavior_tree/dummy_servers.hpp new file mode 100644 index 00000000000..ce8d26599e1 --- /dev/null +++ b/nav2_system_tests/src/behavior_tree/dummy_servers.hpp @@ -0,0 +1,207 @@ +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef BEHAVIOR_TREE__DUMMY_SERVERS_HPP_ +#define BEHAVIOR_TREE__DUMMY_SERVERS_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; // NOLINT +using namespace std::chrono; // NOLINT +using namespace std::placeholders; // NOLINT + +template +class DummyService +{ +public: + explicit DummyService( + const rclcpp::Node::SharedPtr & node, + std::string service_name) + : node_(node), + service_name_(service_name), + request_count_(0), + disabled_(false) + { + server_ = node->create_service( + service_name, + std::bind(&DummyService::handle_service, this, _1, _2, _3)); + } + + void disable() + { + server_.reset(); + disabled_ = true; + } + + void enable() + { + if (disabled_) { + server_ = node_->create_service( + service_name_, + std::bind(&DummyService::handle_service, this, _1, _2, _3)); + disabled_ = false; + } + } + + void reset() + { + enable(); + request_count_ = 0; + } + + int getRequestCount() const + { + return request_count_; + } + +protected: + virtual void fillResponse( + const std::shared_ptr/*request*/, + const std::shared_ptr/*response*/) {} + + void handle_service( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response) + { + request_count_++; + fillResponse(request, response); + } + +private: + rclcpp::Node::SharedPtr node_; + typename rclcpp::Service::SharedPtr server_; + std::string service_name_; + int request_count_; + bool disabled_; +}; + +template +class DummyActionServer +{ +public: + explicit DummyActionServer( + const rclcpp::Node::SharedPtr & node, + std::string action_name) + : action_name_(action_name), + goal_count_(0) + { + this->action_server_ = rclcpp_action::create_server( + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + action_name, + std::bind(&DummyActionServer::handle_goal, this, _1, _2), + std::bind(&DummyActionServer::handle_cancel, this, _1), + std::bind(&DummyActionServer::handle_accepted, this, _1)); + } + + void setFailureRanges(const std::vector> & failureRanges) + { + failure_ranges_ = failureRanges; + } + + void setRunningRanges(const std::vector> & runningRanges) + { + running_ranges_ = runningRanges; + } + + void reset() + { + failure_ranges_.clear(); + running_ranges_.clear(); + goal_count_ = 0; + } + + int getGoalCount() const + { + return goal_count_; + } + +protected: + virtual std::shared_ptr fillResult() + { + return std::make_shared(); + } + + virtual rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr/*goal*/) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + virtual rclcpp_action::CancelResponse handle_cancel( + const typename std::shared_ptr>) + { + return rclcpp_action::CancelResponse::ACCEPT; + } + + void execute( + const typename std::shared_ptr> goal_handle) + { + goal_count_++; + auto result = fillResult(); + + // if current goal index exists in running range, the thread sleeps for 1 second + // to simulate a long running action + for (auto & index : running_ranges_) { + if (goal_count_ >= index.first && goal_count_ <= index.second) { + std::this_thread::sleep_for(1s); + break; + } + } + + // if current goal index exists in failure range, the goal will be aborted + for (auto & index : failure_ranges_) { + if (goal_count_ >= index.first && goal_count_ <= index.second) { + goal_handle->abort(result); + return; + } + } + + // goal succeeds for all other indices + goal_handle->succeed(result); + } + + void handle_accepted( + const std::shared_ptr> goal_handle) + { + using namespace std::placeholders; // NOLINT + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&DummyActionServer::execute, this, _1), goal_handle}.detach(); + } + +protected: + typename rclcpp_action::Server::SharedPtr action_server_; + std::string action_name_; + + // contains pairs of indices which define a range for which the + // requested action goal will return running for 1s or be aborted + // for all other indices, the action server will return success + std::vector> failure_ranges_; + std::vector> running_ranges_; + + int goal_count_; +}; + +#endif // BEHAVIOR_TREE__DUMMY_SERVERS_HPP_ diff --git a/nav2_system_tests/src/behavior_tree/server_handler.cpp b/nav2_system_tests/src/behavior_tree/server_handler.cpp new file mode 100644 index 00000000000..0952f607121 --- /dev/null +++ b/nav2_system_tests/src/behavior_tree/server_handler.cpp @@ -0,0 +1,93 @@ +// Copyright (c) 2020 Vinny Ruia +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2018 Intel Corporation +// +// 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 "server_handler.hpp" + +using namespace std::chrono_literals; // NOLINT +using namespace std::chrono; // NOLINT + +ServerHandler::ServerHandler() +: is_active_(false) +{ + node_ = rclcpp::Node::make_shared("behavior_tree_tester"); + + clear_local_costmap_server = std::make_unique>( + node_, "local_costmap/clear_entirely_local_costmap"); + clear_global_costmap_server = std::make_unique>( + node_, "global_costmap/clear_entirely_global_costmap"); + compute_path_to_pose_server = std::make_unique(node_); + follow_path_server = std::make_unique>( + node_, "follow_path"); + spin_server = std::make_unique>( + node_, "spin"); + wait_server = std::make_unique>( + node_, "wait"); + backup_server = std::make_unique>( + node_, "backup"); + ntp_server = std::make_unique>( + node_, "compute_path_through_poses"); +} + +ServerHandler::~ServerHandler() +{ + if (is_active_) { + deactivate(); + } +} + +void ServerHandler::activate() +{ + if (is_active_) { + throw std::runtime_error("Trying to activate while already activated"); + } + + is_active_ = true; + server_thread_ = + std::make_shared(std::bind(&ServerHandler::spinThread, this)); + + std::cout << "Server handler is active!" << std::endl; +} + +void ServerHandler::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + + is_active_ = false; + server_thread_->join(); + + std::cout << "Server handler has been deactivated!" << std::endl; +} + +void ServerHandler::reset() const +{ + clear_global_costmap_server->reset(); + clear_local_costmap_server->reset(); + compute_path_to_pose_server->reset(); + follow_path_server->reset(); + spin_server->reset(); + wait_server->reset(); + backup_server->reset(); +} + +void ServerHandler::spinThread() +{ + rclcpp::spin(node_); +} diff --git a/nav2_system_tests/src/behavior_tree/server_handler.hpp b/nav2_system_tests/src/behavior_tree/server_handler.hpp new file mode 100644 index 00000000000..c9fc50f38cc --- /dev/null +++ b/nav2_system_tests/src/behavior_tree/server_handler.hpp @@ -0,0 +1,106 @@ +// Copyright (c) 2020 Vinny Ruia +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef BEHAVIOR_TREE__SERVER_HANDLER_HPP_ +#define BEHAVIOR_TREE__SERVER_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "nav2_msgs/srv/clear_entire_costmap.hpp" +#include "nav2_msgs/action/compute_path_to_pose.hpp" +#include "nav2_msgs/action/follow_path.hpp" +#include "nav2_msgs/action/spin.hpp" +#include "nav2_msgs/action/back_up.hpp" +#include "nav2_msgs/action/wait.hpp" +#include "nav2_msgs/action/compute_path_through_poses.hpp" + +#include "geometry_msgs/msg/point_stamped.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include "dummy_servers.hpp" + +class ComputePathToPoseActionServer + : public DummyActionServer +{ +public: + explicit ComputePathToPoseActionServer(const rclcpp::Node::SharedPtr & node) + : DummyActionServer(node, "compute_path_to_pose") + { + result_ = std::make_shared(); + geometry_msgs::msg::PoseStamped pose; + pose.header = result_->path.header; + pose.pose.position.x = 0.0; + pose.pose.position.y = 0.0; + pose.pose.position.z = 0.0; + pose.pose.orientation.x = 0.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 1.0; + for (int i = 0; i < 6; ++i) { + result_->path.poses.push_back(pose); + } + } + + std::shared_ptr fillResult() override + { + return result_; + } + +private: + std::shared_ptr result_; +}; + +class ServerHandler +{ +public: + ServerHandler(); + ~ServerHandler(); + + void activate(); + + void deactivate(); + + bool isActive() const + { + return is_active_; + } + + void reset() const; + +public: + std::unique_ptr> clear_local_costmap_server; + std::unique_ptr> clear_global_costmap_server; + std::unique_ptr compute_path_to_pose_server; + std::unique_ptr> follow_path_server; + std::unique_ptr> spin_server; + std::unique_ptr> wait_server; + std::unique_ptr> backup_server; + std::unique_ptr> ntp_server; + +private: + void spinThread(); + + bool is_active_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr server_thread_; +}; + +#endif // BEHAVIOR_TREE__SERVER_HANDLER_HPP_ diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp new file mode 100644 index 00000000000..2b19740ee4f --- /dev/null +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -0,0 +1,632 @@ +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include + +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp_v3/utils/shared_library.h" + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/create_timer_ros.h" + +#include "rclcpp/rclcpp.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +#include "server_handler.hpp" + +using namespace std::chrono_literals; +namespace fs = boost::filesystem; + +class BehaviorTreeHandler +{ +public: + BehaviorTreeHandler() + { + node_ = rclcpp::Node::make_shared("behavior_tree_handler"); + + tf_ = std::make_shared(node_->get_clock()); + auto timer_interface = std::make_shared( + node_->get_node_base_interface(), node_->get_node_timers_interface()); + tf_->setCreateTimerInterface(timer_interface); + tf_->setUsingDedicatedThread(true); + tf_listener_ = std::make_shared(*tf_, node_, false); + + const std::vector plugin_libs = { + "nav2_compute_path_to_pose_action_bt_node", + "nav2_compute_path_through_poses_action_bt_node", + "nav2_follow_path_action_bt_node", + "nav2_back_up_action_bt_node", + "nav2_spin_action_bt_node", + "nav2_wait_action_bt_node", + "nav2_clear_costmap_service_bt_node", + "nav2_is_stuck_condition_bt_node", + "nav2_goal_reached_condition_bt_node", + "nav2_initial_pose_received_condition_bt_node", + "nav2_goal_updated_condition_bt_node", + "nav2_reinitialize_global_localization_service_bt_node", + "nav2_rate_controller_bt_node", + "nav2_distance_controller_bt_node", + "nav2_speed_controller_bt_node", + "nav2_truncate_path_action_bt_node", + "nav2_goal_updater_node_bt_node", + "nav2_recovery_node_bt_node", + "nav2_pipeline_sequence_bt_node", + "nav2_round_robin_node_bt_node", + "nav2_transform_available_condition_bt_node", + "nav2_time_expired_condition_bt_node", + "nav2_distance_traveled_condition_bt_node", + "nav2_single_trigger_bt_node", + "nav2_is_battery_low_condition_bt_node", + "nav2_navigate_through_poses_action_bt_node", + "nav2_navigate_to_pose_action_bt_node", + "nav2_remove_passed_goals_action_bt_node", + "nav2_planner_selector_bt_node", + "nav2_controller_selector_bt_node", + "nav2_goal_checker_selector_bt_node" + }; + for (const auto & p : plugin_libs) { + factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); + } + } + + bool loadBehaviorTree(const std::string & filename) + { + // Read the input BT XML from the specified file into a string + std::ifstream xml_file(filename); + + if (!xml_file.good()) { + RCLCPP_ERROR(node_->get_logger(), "Couldn't open input XML file: %s", filename.c_str()); + return false; + } + + auto xml_string = std::string( + std::istreambuf_iterator(xml_file), + std::istreambuf_iterator()); + + // Create the blackboard that will be shared by all of the nodes in the tree + blackboard = BT::Blackboard::create(); + + // Put items on the blackboard + blackboard->set("node", node_); // NOLINT + blackboard->set( + "server_timeout", std::chrono::milliseconds(20)); // NOLINT + blackboard->set( + "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT + blackboard->set>("tf_buffer", tf_); // NOLINT + blackboard->set("initial_pose_received", false); // NOLINT + blackboard->set("number_recoveries", 0); // NOLINT + + // set dummy goal on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.header.frame_id = "map"; + goal.pose.position.x = 0.0; + goal.pose.position.y = 0.0; + goal.pose.position.z = 0.0; + goal.pose.orientation.x = 0.0; + goal.pose.orientation.y = 0.0; + goal.pose.orientation.z = 0.0; + goal.pose.orientation.w = 1.0; + + blackboard->set("goal", goal); // NOLINT + + // Create the Behavior Tree from the XML input + try { + tree = factory_.createTreeFromText(xml_string, blackboard); + } catch (BT::RuntimeError & exp) { + RCLCPP_ERROR(node_->get_logger(), "%s: %s", filename.c_str(), exp.what()); + return false; + } + + return true; + } + +public: + BT::Blackboard::Ptr blackboard; + BT::Tree tree; + +private: + rclcpp::Node::SharedPtr node_; + + std::shared_ptr tf_; + std::shared_ptr tf_listener_; + + BT::BehaviorTreeFactory factory_; +}; + +class BehaviorTreeTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + // initialize ROS + rclcpp::init(0, nullptr); + + server_handler = std::make_shared(); + if (!server_handler->isActive()) { + server_handler->activate(); + } + } + + static void TearDownTestCase() + { + // shutdown ROS + rclcpp::shutdown(); + + server_handler.reset(); + bt_handler.reset(); + } + + void SetUp() override + { + server_handler->reset(); + bt_handler = std::make_shared(); + } + + void TearDown() override + { + bt_handler.reset(); + } + +protected: + static std::shared_ptr server_handler; + static std::shared_ptr bt_handler; +}; + +std::shared_ptr BehaviorTreeTestFixture::server_handler = nullptr; +std::shared_ptr BehaviorTreeTestFixture::bt_handler = nullptr; + +TEST_F(BehaviorTreeTestFixture, TestBTXMLFiles) +{ + fs::path root = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + root /= "behavior_trees/"; + + if (boost::filesystem::exists(root) && boost::filesystem::is_directory(root)) { + for (auto const & entry : boost::filesystem::recursive_directory_iterator(root)) { + if (boost::filesystem::is_regular_file(entry) && entry.path().extension() == ".xml") { + EXPECT_EQ(bt_handler->loadBehaviorTree(entry.path().string()), true); + } + } + } +} + +/** + * Test scenario: + * + * ComputePathToPose and FollowPath return SUCCESS + * The behavior tree should execute correctly and return SUCCESS + */ +TEST_F(BehaviorTreeTestFixture, TestAllSuccess) +{ + // Load behavior tree from file + fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + bt_file /= "behavior_trees/"; + bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + while (result == BT::NodeStatus::RUNNING) { + result = bt_handler->tree.tickRoot(); + std::this_thread::sleep_for(10ms); + } + + // The final result should be success since all action servers returned success + EXPECT_EQ(result, BT::NodeStatus::SUCCESS); + + // Goal count should be 1 since only one goal is sent to ComputePathToPose and FollowPath servers + EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 1); + EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 1); + + // Goal count should be 0 since no goal is sent to all other servers + EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0); + EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0); + EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0); + EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 0); + EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 0); +} + +/** + * Test scenario: + * + * ComputePathToPose returns FAILURE and ClearGlobalCostmap-Context returns FAILURE + * PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback + * GoalUpdated returns FAILURE and RoundRobin is triggered + * RoundRobin triggers ClearingActions Sequence which returns FAILURE + * RoundRobin triggers Spin, Wait, and BackUp which return FAILURE + * RoundRobin returns FAILURE hence RecoveryCallbackk returns FAILURE + * Finally NavigateRecovery returns FAILURE + * The behavior tree should also return FAILURE + */ +TEST_F(BehaviorTreeTestFixture, TestAllFailure) +{ + // Load behavior tree from file + fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + bt_file /= "behavior_trees/"; + bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + + // Set all action server to fail the first 100 times + std::vector> failureRange; + failureRange.emplace_back(std::pair(0, 100)); + server_handler->compute_path_to_pose_server->setFailureRanges(failureRange); + server_handler->follow_path_server->setFailureRanges(failureRange); + server_handler->spin_server->setFailureRanges(failureRange); + server_handler->wait_server->setFailureRanges(failureRange); + server_handler->backup_server->setFailureRanges(failureRange); + + // Disable services + server_handler->clear_global_costmap_server->disable(); + server_handler->clear_local_costmap_server->disable(); + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + while (result == BT::NodeStatus::RUNNING) { + result = bt_handler->tree.tickRoot(); + std::this_thread::sleep_for(10ms); + } + + // The final result should be failure + EXPECT_EQ(result, BT::NodeStatus::FAILURE); + + // Goal count should be 1 since only one goal is sent to ComputePathToPose + EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 1); + + // Goal count should be 0 since no goal is sent to FollowPath action server + EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 0); + + // All recovery action servers were sent 1 goal + EXPECT_EQ(server_handler->spin_server->getGoalCount(), 1); + EXPECT_EQ(server_handler->wait_server->getGoalCount(), 1); + EXPECT_EQ(server_handler->backup_server->getGoalCount(), 1); + + // Service count is 0 since the server was disabled + EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 0); + EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 0); +} + +/** + * Test scenario: + * + * ComputePathToPose returns FAILURE on the first try triggering the planner recovery + * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns SUCCESS when retried + * FollowPath returns FAILURE on the first try triggering the controller recovery + * ClearLocalCostmap-Context returns SUCCESS and FollowPath returns SUCCESS when retried + * The behavior tree should return SUCCESS + */ +TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) +{ + // Load behavior tree from file + fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + bt_file /= "behavior_trees/"; + bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + + // Set ComputePathToPose and FollowPath action servers to fail for the first action + std::vector> failureRange; + failureRange.emplace_back(std::pair(0, 1)); + server_handler->compute_path_to_pose_server->setFailureRanges(failureRange); + server_handler->follow_path_server->setFailureRanges(failureRange); + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + while (result == BT::NodeStatus::RUNNING) { + result = bt_handler->tree.tickRoot(); + std::this_thread::sleep_for(10ms); + } + + // The final result should be success + EXPECT_EQ(result, BT::NodeStatus::SUCCESS); + + // Goal count should be 2 since only two goals were sent to ComputePathToPose and FollowPath + EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 2); + EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 2); + + // Navigate subtree recovery services are called once each + EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 1); + EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 1); + + // Goal count should be 0 since no goal is sent to all other servers + EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0); + EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0); + EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0); +} + +/** + * Test scenario: + * + * ComputePathToPose returns FAILURE on the first try triggering the planner recovery + * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns SUCCESS when retried + * FollowPath returns FAILURE on the first try triggering the controller recovery + * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried + * FollowPath returns FAILURE again and PipelineSequence returns FAILURE + * NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE + * RoundRobin triggers ClearingActions Sequence which returns SUCCESS + * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS + * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS + * FollowPath returns FAILURE on the third try triggering the controller recovery + * ClearLocalCostmap-Context returns SUCCESS and FollowPath returns SUCCESS on the fourth try + * The behavior tree should return SUCCESS + */ +TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) +{ + // Load behavior tree from file + fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + bt_file /= "behavior_trees/"; + bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + + // Set ComputePathToPose action server to fail for the first action + std::vector> plannerFailureRange; + plannerFailureRange.emplace_back(std::pair(0, 1)); + server_handler->compute_path_to_pose_server->setFailureRanges(plannerFailureRange); + + // Set FollowPath action server to fail for the first 3 actions + std::vector> controllerFailureRange; + controllerFailureRange.emplace_back(std::pair(0, 3)); + server_handler->follow_path_server->setFailureRanges(controllerFailureRange); + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + while (result == BT::NodeStatus::RUNNING) { + result = bt_handler->tree.tickRoot(); + std::this_thread::sleep_for(10ms); + } + + // The final result should be success + EXPECT_EQ(result, BT::NodeStatus::SUCCESS); + + // FollowPath is called 4 times + EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 4); + + // ComputePathToPose is called 3 times + EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 3); + + // Local costmap is cleared 3 times + EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 3); + + // Global costmap is cleared 2 times + EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 2); + + // Goal count should be 0 since only no goal is sent to all other servers + EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0); + EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0); + EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0); +} + +/** + * Test scenario: + * + * ComputePathToPose returns FAILURE on the first try triggering the planner recovery + * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried + * PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback + * GoalUpdated returns FAILURE, RoundRobin triggers ClearingActions Sequence which returns SUCCESS + * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS + * + * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS (retry #1) + * FollowPath returns FAILURE on the first try triggering the controller recovery + * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried + * FollowPath returns FAILURE again and PipelineSequence returns FAILURE + * NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE + * RoundRobin triggers Spin which returns FAILURE + * RoundRobin triggers Wait which returns SUCCESS + * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS + * + * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #2) + * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried + * PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback + * GoalUpdated returns FAILURE and RoundRobin triggers BackUp which returns FAILURE + * RoundRobin triggers ClearingActions Sequence which returns SUCCESS + * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS + * + * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #3) + * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried + * PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback + * GoalUpdated returns FAILURE and RoundRobin triggers Spin which returns SUCCESS + * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS + * + * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #4) + * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried + * PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback + * GoalUpdated returns FAILURE and RoundRobin triggers Wait which returns FAILURE + * RoundRobin triggers BackUp which returns SUCCESS + * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS + * + * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS (retry #5) + * FollowPath returns FAILURE on the first try triggering the controller recovery + * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried + * FollowPath returns FAILURE again and PipelineSequence returns FAILURE + * NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE + * RoundRobin triggers ClearingActions Sequence which returns SUCCESS + * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS + * + * PipelineSequence is triggered again and ComputePathToPose returns FAILURE (retry #6) + * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried + * PipelineSequence returns FAILURE and NavigateRecovery finally also returns FAILURE + * + * The behavior tree should return FAILURE + */ +TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) +{ + // Load behavior tree from file + fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + bt_file /= "behavior_trees/"; + bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + + // Set ComputePathToPose action server to fail for the first 2 actions + std::vector> plannerFailureRange; + plannerFailureRange.emplace_back(std::pair(0, 2)); + plannerFailureRange.emplace_back(std::pair(4, 9)); + plannerFailureRange.emplace_back(std::pair(11, 12)); + server_handler->compute_path_to_pose_server->setFailureRanges(plannerFailureRange); + + // Set FollowPath action server to fail for the first 2 actions + std::vector> controllerFailureRange; + controllerFailureRange.emplace_back(std::pair(0, 4)); + server_handler->follow_path_server->setFailureRanges(controllerFailureRange); + + // Set Spin action server to fail for the first action + std::vector> spinFailureRange; + spinFailureRange.emplace_back(std::pair(0, 1)); + server_handler->spin_server->setFailureRanges(spinFailureRange); + + // Set Wait action server to fail for the first action + std::vector> waitFailureRange; + waitFailureRange.emplace_back(std::pair(2, 2)); + server_handler->wait_server->setFailureRanges(waitFailureRange); + + // Set BackUp action server to fail for the first action + std::vector> backupFailureRange; + backupFailureRange.emplace_back(std::pair(0, 1)); + server_handler->backup_server->setFailureRanges(backupFailureRange); + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + while (result == BT::NodeStatus::RUNNING) { + result = bt_handler->tree.tickRoot(); + std::this_thread::sleep_for(10ms); + } + + // The final result should be success + EXPECT_EQ(result, BT::NodeStatus::FAILURE); + + // ComputePathToPose is called 12 times + EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 12); + + // FollowPath is called 4 times + EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 4); + + // Local costmap is cleared 5 times + EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 5); + + // Global costmap is cleared 8 times + EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 8); + + // All recovery action servers receive 2 goals + EXPECT_EQ(server_handler->spin_server->getGoalCount(), 2); + EXPECT_EQ(server_handler->wait_server->getGoalCount(), 2); + EXPECT_EQ(server_handler->backup_server->getGoalCount(), 2); +} + +/** + * Test scenario: + * + * ComputePathToPose returns FAILURE on the first try triggering the planner recovery + * ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried + * PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback + * GoalUpdated returns FAILURE, RoundRobin triggers ClearingActions Sequence which returns SUCCESS + * RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS + * PipelineSequence is triggered again and ComputePathToPose returns SUCCESS + * FollowPath returns FAILURE on the first try triggering the controller recovery + * ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried + * FollowPath returns FAILURE and PipelineSequence returns FAILURE + * NavigateRecovery triggers RecoveryFallback which triggers GoalUpdated + * GoalUpdated returns FAILURE and RecoveryFallback triggers RoundRobin + * RoundRobin triggers Spin which returns RUNNING + * + * At this point a new goal is updated on the blackboard + * + * RecoveryFallback triggers GoalUpdated which returns SUCCESS this time + * Since GoalUpdated returned SUCCESS, RoundRobin and hence Spin is halted + * RecoveryFallback also returns SUCCESS and PipelineSequence is retried + * PipelineSequence triggers ComputePathToPose which returns SUCCESS + * FollowPath returns SUCCESS and NavigateRecovery finally also returns SUCCESS + * + * The behavior tree should return SUCCESS + */ +TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated) +{ + // Load behavior tree from file + fs::path bt_file = ament_index_cpp::get_package_share_directory("nav2_bt_navigator"); + bt_file /= "behavior_trees/"; + bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml"; + EXPECT_EQ(bt_handler->loadBehaviorTree(bt_file.string()), true); + + // Set ComputePathToPose action server to fail for the first 2 actions + std::vector> plannerFailureRange; + plannerFailureRange.emplace_back(std::pair(0, 2)); + server_handler->compute_path_to_pose_server->setFailureRanges(plannerFailureRange); + + // Set FollowPath action server to fail for the first 2 actions + std::vector> controllerFailureRange; + controllerFailureRange.emplace_back(std::pair(0, 2)); + server_handler->follow_path_server->setFailureRanges(controllerFailureRange); + + // Set Spin action server to return running for the first action + std::vector> spinRunningRange; + spinRunningRange.emplace_back(std::pair(1, 1)); + server_handler->spin_server->setRunningRanges(spinRunningRange); + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + while (result == BT::NodeStatus::RUNNING) { + result = bt_handler->tree.tickRoot(); + + // Update goal on blackboard after Spin has been triggered once + // to simulate a goal update during a recovery action + if (server_handler->spin_server->getGoalCount() > 0) { + geometry_msgs::msg::PoseStamped goal; + goal.pose.position.x = 1.0; + goal.pose.position.y = 1.0; + goal.pose.position.z = 1.0; + goal.pose.orientation.x = 0.0; + goal.pose.orientation.y = 0.0; + goal.pose.orientation.z = 0.0; + goal.pose.orientation.w = 1.0; + bt_handler->blackboard->set("goal", goal); // NOLINT + } + + std::this_thread::sleep_for(10ms); + } + + // The final result should be success + EXPECT_EQ(result, BT::NodeStatus::SUCCESS); + + // ComputePathToPose is called 4 times + EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 4); + + // FollowPath is called 3 times + EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 3); + + // Local costmap is cleared 2 times + EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 2); + + // Global costmap is cleared 2 times + EXPECT_EQ(server_handler->clear_global_costmap_server->getRequestCount(), 2); + + // Spin server receives 1 action + EXPECT_EQ(server_handler->spin_server->getGoalCount(), 1); + + // All recovery action servers receive 0 goals + EXPECT_EQ(server_handler->wait_server->getGoalCount(), 0); + EXPECT_EQ(server_handler->backup_server->getGoalCount(), 0); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + bool all_successful = RUN_ALL_TESTS(); + return all_successful; +} diff --git a/nav2_system_tests/src/costmap_filters/CMakeLists.txt b/nav2_system_tests/src/costmap_filters/CMakeLists.txt index a8cdcfe4352..e05620b366f 100644 --- a/nav2_system_tests/src/costmap_filters/CMakeLists.txt +++ b/nav2_system_tests/src/costmap_filters/CMakeLists.txt @@ -9,7 +9,7 @@ ament_add_test(test_keepout_filter TEST_MASK=${PROJECT_SOURCE_DIR}/maps/keepout_mask.yaml TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ASTAR=False ) @@ -25,7 +25,7 @@ ament_add_test(test_speed_filter_global TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world PARAMS_FILE=${CMAKE_CURRENT_SOURCE_DIR}/speed_global_params.yaml GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ASTAR=False ) @@ -41,6 +41,6 @@ ament_add_test(test_speed_filter_local TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world PARAMS_FILE=${CMAKE_CURRENT_SOURCE_DIR}/speed_local_params.yaml GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ASTAR=False ) diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 539ac94ea6f..666a1b93787 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -53,9 +53,13 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom - default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch + # files to allow for a commandline change default used is the + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node - nav2_follow_path_action_bt_node - nav2_back_up_action_bt_node - nav2_spin_action_bt_node @@ -77,6 +81,11 @@ bt_navigator: - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -146,6 +155,8 @@ controller_server: RotateToGoal.scale: 32.0 RotateToGoal.slowing_factor: 5.0 RotateToGoal.lookahead_time: -1.0 + publish_cost_grid_pc: True + controller_server_rclcpp_node: ros__parameters: @@ -164,7 +175,8 @@ local_costmap: height: 3 resolution: 0.05 robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer", "keepout_filter"] + plugins: ["voxel_layer", "inflation_layer"] + filters: ["keepout_filter"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -185,6 +197,10 @@ local_costmap: clearing: True marking: True data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 static_layer: map_subscribe_transient_local: True keepout_filter: @@ -210,7 +226,8 @@ global_costmap: robot_radius: 0.22 resolution: 0.05 track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer", "keepout_filter"] + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + filters: ["keepout_filter"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -261,8 +278,8 @@ planner_server: GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" tolerance: 0.5 - use_astar: false - allow_unknown: true + use_astar: False + allow_unknown: True planner_server_rclcpp_node: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index 345424a8733..71fa2a02a16 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -53,9 +53,13 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom - default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch + # files to allow for a commandline change default used is the + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node - nav2_follow_path_action_bt_node - nav2_back_up_action_bt_node - nav2_spin_action_bt_node @@ -77,6 +81,11 @@ bt_navigator: - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -188,11 +197,6 @@ local_costmap: data_type: "LaserScan" static_layer: map_subscribe_transient_local: True - speed_filter: - plugin: "nav2_costmap_2d::SpeedFilter" - enabled: True - filter_info_topic: "/costmap_filter_info" - speed_limit_topic: "/speed_limit" always_send_full_costmap: True local_costmap_client: ros__parameters: @@ -212,7 +216,8 @@ global_costmap: robot_radius: 0.22 resolution: 0.05 track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer", "speed_filter"] + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + filters: ["speed_filter"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True @@ -264,8 +269,8 @@ planner_server: GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" tolerance: 0.5 - use_astar: false - allow_unknown: true + use_astar: False + allow_unknown: True planner_server_rclcpp_node: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index 96dc6383367..1dd66e8fc6c 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -53,9 +53,13 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom - default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch + # files to allow for a commandline change default used is the + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node - nav2_follow_path_action_bt_node - nav2_back_up_action_bt_node - nav2_spin_action_bt_node @@ -77,6 +81,11 @@ bt_navigator: - nav2_transform_available_condition_bt_node - nav2_time_expired_condition_bt_node - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -165,7 +174,8 @@ local_costmap: height: 3 resolution: 0.05 robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer", "speed_filter"] + plugins: ["voxel_layer", "inflation_layer"] + filters: ["speed_filter"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 @@ -259,8 +269,8 @@ planner_server: GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" tolerance: 0.5 - use_astar: false - allow_unknown: true + use_astar: False + allow_unknown: True planner_server_rclcpp_node: ros__parameters: 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 63727519643..c2c7e093230 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -23,6 +23,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -54,8 +55,11 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) + context = LaunchContext() + new_yaml = configured_params.perform(context) return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation ExecuteProcess( @@ -91,14 +95,14 @@ def generate_launch_description(): executable='map_server', name='filter_mask_server', output='screen', - parameters=[configured_params]), + parameters=[new_yaml]), Node( package='nav2_map_server', executable='costmap_filter_info_server', name='costmap_filter_info_server', output='screen', - parameters=[configured_params]), + parameters=[new_yaml]), IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -107,9 +111,16 @@ def generate_launch_description(): 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', - 'params_file': configured_params, + 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True'}.items()), + + Node( + package='nav2_costmap_2d', + executable='nav2_costmap_2d_cloud', + name='costmap_2d_cloud', + output='screen', + remappings=[('voxel_grid', 'local_costmap/voxel_grid')]), ]) 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 6e52142261f..32f6f9a4f48 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -23,6 +23,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -52,9 +53,11 @@ def generate_launch_description(): root_key='', param_rewrites=param_substitutions, convert_types=True) - + context = LaunchContext() + new_yaml = configured_params.perform(context) return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation ExecuteProcess( @@ -90,14 +93,14 @@ def generate_launch_description(): executable='map_server', name='filter_mask_server', output='screen', - parameters=[configured_params]), + parameters=[new_yaml]), Node( package='nav2_map_server', executable='costmap_filter_info_server', name='costmap_filter_info_server', output='screen', - parameters=[configured_params]), + parameters=[new_yaml]), IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -106,7 +109,7 @@ def generate_launch_description(): 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', - 'params_file': configured_params, + 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/costmap_filters/tester_node.py b/nav2_system_tests/src/costmap_filters/tester_node.py index 14a601adb62..ca0b585b665 100755 --- a/nav2_system_tests/src/costmap_filters/tester_node.py +++ b/nav2_system_tests/src/costmap_filters/tester_node.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python3 +#!/usr/bin/env python3 # Copyright (c) 2018 Intel Corporation. # Copyright (c) 2020 Samsung Research Russia @@ -34,11 +34,11 @@ from nav_msgs.msg import Path import rclpy - from rclpy.action import ActionClient from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy from rclpy.qos import QoSProfile +from sensor_msgs.msg import PointCloud2 class TestType(Enum): @@ -96,26 +96,46 @@ def __init__( self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) transient_local_qos = QoSProfile( - durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, - history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, - depth=1) + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1) volatile_qos = QoSProfile( - durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE, - reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, - history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, - depth=1) + durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + depth=1) self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, transient_local_qos) - + self.clearing_ep_sub = self.create_subscription(PointCloud2, + 'local_costmap/clearing_endpoints', + self.clearingEndpointsCallback, + transient_local_qos) self.test_type = test_type self.filter_test_result = True + self.clearing_endpoints_received = False + self.voxel_marked_received = False + self.voxel_unknown_received = False + self.cost_cloud_received = False + if self.test_type == TestType.KEEPOUT: self.plan_sub = self.create_subscription(Path, 'plan', self.planCallback, volatile_qos) + self.voxel_marked_sub = self.create_subscription(PointCloud2, + 'voxel_marked_cloud', + self.voxelMarkedCallback, + 1) + self.voxel_unknown_sub = self.create_subscription(PointCloud2, + 'voxel_unknown_cloud', + self.voxelUnknownCallback, + 1) + self.cost_cloud_sub = self.create_subscription(PointCloud2, + 'cost_cloud', + self.dwbCostCloudCallback, + 1) elif self.test_type == TestType.SPEED: self.speed_it = 0 # Expected chain of speed limits @@ -132,7 +152,8 @@ def __init__( self.initial_pose_received = False self.initial_pose = initial_pose self.goal_pose = goal_pose - self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + self.action_client = ActionClient( + self, NavigateToPose, 'navigate_to_pose') def info_msg(self, msg: str): self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') @@ -165,7 +186,8 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): # Sends a `NavToPose` action request and waits for completion self.info_msg("Waiting for 'NavigateToPose' action server") while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg("'NavigateToPose' action server not available, waiting...") + self.info_msg( + "'NavigateToPose' action server not available, waiting...") self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() @@ -208,7 +230,8 @@ def checkKeepout(self, x, y): self.warn_msg('Filter mask was not received') elif self.isInKeepout(x, y): self.filter_test_result = False - self.error_msg('Pose (' + str(x) + ', ' + str(y) + ') belongs to keepout zone') + self.error_msg('Pose (' + str(x) + ', ' + + str(y) + ') belongs to keepout zone') return False return True @@ -245,6 +268,23 @@ def planCallback(self, msg): self.error_msg('Path plan intersects with keepout zone') return + def clearingEndpointsCallback(self, msg): + if len(msg.data) > 0: + self.clearing_endpoints_received = True + + def voxelMarkedCallback(self, msg): + if len(msg.data) > 0: + self.voxel_marked_received = True + + def voxelUnknownCallback(self, msg): + if len(msg.data) > 0: + self.voxel_unknown_received = True + + def dwbCostCloudCallback(self, msg): + self.info_msg('Received cost_cloud points') + if len(msg.data) > 0: + self.cost_cloud_received = True + def speedLimitCallback(self, msg): self.info_msg('Received speed limit: ' + str(msg.speed_limit)) self.checkSpeed(self.speed_it, msg.speed_limit) @@ -266,6 +306,21 @@ def wait_for_filter_mask(self, timeout): return False return True + def wait_for_pointcloud_subscribers(self, timeout): + start_time = time.time() + while not self.voxel_unknown_received or not self.voxel_marked_received \ + or not self.clearing_endpoints_received: + self.info_msg( + 'Waiting for voxel_marked_cloud/voxel_unknown_cloud/\ + clearing_endpoints msg to be received ...') + rclpy.spin_once(self, timeout_sec=1) + if (time.time() - start_time) > timeout: + self.error_msg( + 'Time out to waiting for voxel_marked_cloud/voxel_unknown_cloud/\ + clearing_endpoints msgs') + return False + return True + def reachesGoal(self, timeout, distance): goalReached = False start_time = time.time() @@ -305,7 +360,8 @@ def wait_for_node_active(self, node_name: str): state = future.result().current_state.label self.info_msg('Result of get_state: %s' % state) else: - self.error_msg('Exception while calling service: %r' % future.exception()) + self.error_msg('Exception while calling service: %r' % + future.exception()) time.sleep(5) def shutdown(self): @@ -313,9 +369,11 @@ def shutdown(self): self.action_client.destroy() transition_service = 'lifecycle_manager_navigation/manage_nodes' - mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + mgr_client = self.create_client( + ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(transition_service + + ' service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -324,13 +382,16 @@ def shutdown(self): self.info_msg('Shutting down navigation lifecycle manager...') rclpy.spin_until_future_complete(self, future) future.result() - self.info_msg('Shutting down navigation lifecycle manager complete.') - except Exception as e: + self.info_msg( + 'Shutting down navigation lifecycle manager complete.') + except Exception as e: # noqa: B902 self.error_msg('Service call failed %r' % (e,)) transition_service = 'lifecycle_manager_localization/manage_nodes' - mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + mgr_client = self.create_client( + ManageLifecycleNodes, transition_service) while not mgr_client.wait_for_service(timeout_sec=1.0): - self.info_msg(transition_service + ' service not available, waiting...') + self.info_msg(transition_service + + ' service not available, waiting...') req = ManageLifecycleNodes.Request() req.command = ManageLifecycleNodes.Request().SHUTDOWN @@ -339,8 +400,9 @@ def shutdown(self): self.info_msg('Shutting down localization lifecycle manager...') rclpy.spin_until_future_complete(self, future) future.result() - self.info_msg('Shutting down localization lifecycle manager complete') - except Exception as e: + self.info_msg( + 'Shutting down localization lifecycle manager complete') + except Exception as e: # noqa: B902 self.error_msg('Service call failed %r' % (e,)) def wait_for_initial_pose(self): @@ -382,16 +444,19 @@ def run_all_tests(robot_tester): robot_tester.wait_for_initial_pose() robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.wait_for_filter_mask(10) - if (result): result = robot_tester.runNavigateAction() + if robot_tester.test_type == TestType.KEEPOUT: + result = result and robot_tester.wait_for_pointcloud_subscribers(10) + if (result): result = test_RobotMovesToGoal(robot_tester) if (result): if robot_tester.test_type == TestType.KEEPOUT: result = robot_tester.filter_test_result + result = result and robot_tester.cost_cloud_received elif robot_tester.test_type == TestType.SPEED: result = test_SpeedLimitsAllCorrect(robot_tester) @@ -438,7 +503,8 @@ def get_tester(args): def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments - parser = argparse.ArgumentParser(description='System-level costmap filters tester node') + parser = argparse.ArgumentParser( + description='System-level costmap filters tester node') parser.add_argument('-t', '--type', type=str, action='store', dest='type', help='Type of costmap filter being tested.') group = parser.add_mutually_exclusive_group(required=True) diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index cfc5cbc3af0..130f420bc0d 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -344,7 +344,7 @@ bool PlannerTester::defaultPlannerRandomTests( RCLCPP_INFO( this->get_logger(), - "Tested with %u tests. Planner failed on %u. Test time %u ms", + "Tested with %u tests. Planner failed on %u. Test time %ld ms", number_tests, num_fail, elapsed.count()); if ((num_fail / number_tests) > acceptable_fail_ratio) { @@ -368,7 +368,7 @@ bool PlannerTester::plannerTest( // Then request to compute a path TaskStatus status = createPlan(goal, path); - RCLCPP_DEBUG(this->get_logger(), "Path request status: %d", status); + RCLCPP_DEBUG(this->get_logger(), "Path request status: %d", static_cast(status)); if (status == TaskStatus::FAILED) { return false; diff --git a/nav2_system_tests/src/recoveries/backup/CMakeLists.txt b/nav2_system_tests/src/recoveries/backup/CMakeLists.txt index d21387dec46..ca3f35402ae 100644 --- a/nav2_system_tests/src/recoveries/backup/CMakeLists.txt +++ b/nav2_system_tests/src/recoveries/backup/CMakeLists.txt @@ -19,5 +19,5 @@ ament_add_test(test_backup_recovery TEST_EXECUTABLE=$ TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ) diff --git a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py index 82739e30968..97affc65168 100755 --- a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py +++ b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py @@ -47,7 +47,8 @@ def generate_launch_description(): convert_types=True) return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation ExecuteProcess( diff --git a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp index f5efe16f9dc..8802755ec0b 100644 --- a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp @@ -72,10 +72,9 @@ TEST_P(BackupRecoveryTestFixture, testBackupRecovery) success = backup_recovery_tester->defaultBackupRecoveryTest(target_dist, tolerance); // if intentionally backing into an obstacle, should fail. - // TODO(stevemacenski): uncomment this once note below completed. - // if (target_dist < -1.0) { - // success = !success; - // } + if (target_dist < -0.1) { + success = !success; + } EXPECT_EQ(true, success); } @@ -89,7 +88,7 @@ INSTANTIATE_TEST_SUITE_P( BackupRecoveryTests, BackupRecoveryTestFixture, ::testing::Values( - std::make_tuple(-0.1, 0.1), + std::make_tuple(-0.05, 0.01), std::make_tuple(-0.2, 0.1), std::make_tuple(-2.0, 0.1)), testNameGenerator); diff --git a/nav2_system_tests/src/recoveries/spin/CMakeLists.txt b/nav2_system_tests/src/recoveries/spin/CMakeLists.txt index 34abacca01f..804468dda00 100644 --- a/nav2_system_tests/src/recoveries/spin/CMakeLists.txt +++ b/nav2_system_tests/src/recoveries/spin/CMakeLists.txt @@ -19,5 +19,16 @@ ament_add_test(test_spin_recovery TEST_EXECUTABLE=$ TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) + +ament_add_test(test_spin_recovery_fake + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_recovery_fake_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + MAKE_FAKE_COSTMAP=true ) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index ee9f6b63bc2..5a0337b4074 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -36,8 +36,16 @@ SpinRecoveryTester::SpinRecoveryTester() { node_ = rclcpp::Node::make_shared("spin_recovery_test"); + tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); + tf_broadcaster_ = std::make_shared(node_); + if (std::getenv("MAKE_FAKE_COSTMAP") != NULL) { + // if this variable is set, make a fake costmap + make_fake_costmap_ = true; + } else { + make_fake_costmap_ = false; + } client_ptr_ = rclcpp_action::create_client( node_->get_node_base_interface(), @@ -48,6 +56,8 @@ SpinRecoveryTester::SpinRecoveryTester() publisher_ = node_->create_publisher("initialpose", 10); + fake_costmap_publisher_ = + node_->create_publisher("local_costmap/costmap_raw", 10); subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), @@ -67,12 +77,15 @@ void SpinRecoveryTester::activate() throw std::runtime_error("Trying to activate while already active"); return; } - - while (!initial_pose_received_) { - RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); - sendInitialPose(); - std::this_thread::sleep_for(100ms); - rclcpp::spin_some(node_); + if (!make_fake_costmap_) { + while (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); + sendInitialPose(); + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node_); + } + } else { + sendFakeOdom(0.0); } // Wait for lifecycle_manager_navigation to activate recoveries_server @@ -114,10 +127,18 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( // Sleep to let recovery server be ready for serving in multiple runs std::this_thread::sleep_for(5s); + if (make_fake_costmap_) { + sendFakeOdom(0.0); + } + auto goal_msg = Spin::Goal(); goal_msg.target_yaw = target_yaw; - RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); + // Intialize fake costmap + if (make_fake_costmap_) { + sendFakeCostmap(target_yaw); + sendFakeOdom(0.0); + } geometry_msgs::msg::PoseStamped initial_pose; if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) { @@ -125,7 +146,11 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( return false; } RCLCPP_INFO(node_->get_logger(), "Found current robot pose"); - + RCLCPP_INFO( + node_->get_logger(), + "Init Yaw is %lf", + fabs(tf2::getYaw(initial_pose.pose.orientation))); + RCLCPP_INFO(node_->get_logger(), "Before sending goal"); auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != @@ -145,6 +170,26 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( auto result_future = client_ptr_->async_get_result(goal_handle); RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); + + if (make_fake_costmap_) { // if we are faking the costmap, we will fake success. + sendFakeOdom(0.0); + sendFakeCostmap(target_yaw); + RCLCPP_INFO(node_->get_logger(), "target_yaw %lf", target_yaw); + // Slowly increment command yaw by increment to simulate the robot slowly spinning into place + float step_size = tolerance / 4.0; + for (float command_yaw = 0.0; + abs(command_yaw) < abs(target_yaw); + command_yaw = command_yaw + step_size) + { + sendFakeOdom(command_yaw); + sendFakeCostmap(target_yaw); + rclcpp::sleep_for(std::chrono::milliseconds(1)); + } + sendFakeOdom(target_yaw); + sendFakeCostmap(target_yaw); + RCLCPP_INFO(node_->get_logger(), "After sending goal"); + } if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) { @@ -182,6 +227,14 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( goal_yaw, tf2::getYaw(current_pose.pose.orientation)); if (fabs(dyaw) > tolerance) { + RCLCPP_ERROR( + node_->get_logger(), + "Init Yaw is %lf (tolerance %lf)", + fabs(tf2::getYaw(initial_pose.pose.orientation)), tolerance); + RCLCPP_ERROR( + node_->get_logger(), + "Current Yaw is %lf (tolerance %lf)", + fabs(tf2::getYaw(current_pose.pose.orientation)), tolerance); RCLCPP_ERROR( node_->get_logger(), "Angular distance from goal is %lf (tolerance %lf)", @@ -192,6 +245,32 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( return true; } +void SpinRecoveryTester::sendFakeCostmap(float angle) +{ + nav2_msgs::msg::Costmap fake_costmap; + + fake_costmap.header.frame_id = "odom"; + fake_costmap.header.stamp = rclcpp::Time(); + fake_costmap.metadata.layer = "master"; + fake_costmap.metadata.resolution = .1; + fake_costmap.metadata.size_x = 100; + fake_costmap.metadata.size_y = 100; + fake_costmap.metadata.origin.position.x = 0; + fake_costmap.metadata.origin.position.y = 0; + fake_costmap.metadata.origin.orientation.w = 1.0; + float costmap_val = 0; + for (int ix = 0; ix < 100; ix++) { + for (int iy = 0; iy < 100; iy++) { + if (abs(angle) > M_PI_2f32) { + // fake obstacles in the way so we get failure due to potential collision + costmap_val = 100; + } + fake_costmap.data.push_back(costmap_val); + } + } + fake_costmap_publisher_->publish(fake_costmap); +} + void SpinRecoveryTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; @@ -215,6 +294,25 @@ void SpinRecoveryTester::sendInitialPose() RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); } +void SpinRecoveryTester::sendFakeOdom(float angle) +{ + geometry_msgs::msg::TransformStamped transformStamped; + + transformStamped.header.stamp = rclcpp::Time(); + transformStamped.header.frame_id = "odom"; + transformStamped.child_frame_id = "base_link"; + transformStamped.transform.translation.x = 0.0; + transformStamped.transform.translation.y = 0.0; + transformStamped.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(0, 0, angle); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + + tf_broadcaster_->sendTransform(transformStamped); +} void SpinRecoveryTester::amclPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) { diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp index 29622efbe7c..4ffd429001a 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp @@ -26,13 +26,20 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "angles/angles.h" #include "nav2_msgs/action/spin.hpp" +#include "nav2_msgs/msg/costmap.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/node_thread.hpp" +#include "geometry_msgs/msg/point32.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/quaternion.hpp" #include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" +#include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_listener.h" namespace nav2_system_tests @@ -64,19 +71,30 @@ class SpinRecoveryTester private: void sendInitialPose(); + void sendFakeCostmap(float angle); + void sendFakeOdom(float angle); + void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); bool is_active_; bool initial_pose_received_; + bool make_fake_costmap_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; + std::shared_ptr tf_broadcaster_; rclcpp::Node::SharedPtr node_; // Publisher to publish initial pose rclcpp::Publisher::SharedPtr publisher_; + // Publisher to publish fake costmap raw + rclcpp::Publisher::SharedPtr fake_costmap_publisher_; + + // Publisher to publish fake costmap footprint + rclcpp::Publisher::SharedPtr fake_footprint_publisher_; + // Subscriber for amcl pose rclcpp::Subscription::SharedPtr subscription_; diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_fake_launch.py b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_fake_launch.py new file mode 100755 index 00000000000..a773d91e09b --- /dev/null +++ b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_fake_launch.py @@ -0,0 +1,161 @@ +#! /usr/bin/env python3 +# Copyright (c) 2019 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import DeclareLaunchArgument, ExecuteProcess, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + default_nav_through_poses_bt_xml = LaunchConfiguration('default_nav_through_poses_bt_xml') + default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml') + map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'default_nav_through_poses_bt_xml': default_nav_through_poses_bt_xml, + 'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml, + 'autostart': autostart, + 'map_subscribe_transient_local': map_subscribe_transient_local} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) + + lifecycle_nodes = ['recoveries_server'] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), + + DeclareLaunchArgument( + 'namespace', default_value='', + description='Top-level namespace'), + + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation (Gazebo) clock if true'), + + DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack'), + + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use'), + + DeclareLaunchArgument( + 'default_nav_through_poses_bt_xml', + default_value=os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', 'navigate_through_poses_w_replanning_and_recovery.xml'), + description='Full path to the behavior tree xml file to use'), + + DeclareLaunchArgument( + 'default_nav_to_pose_bt_xml', + default_value=os.path.join( + get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', 'navigate_to_pose_w_replanning_and_recovery.xml'), + description='Full path to the behavior tree xml file to use'), + + DeclareLaunchArgument( + 'map_subscribe_transient_local', default_value='false', + description='Whether to set the map subscriber QoS to transient local'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + Node( + package='nav2_recoveries', + executable='recoveries_server', + name='recoveries_server', + output='screen', + parameters=[configured_params], + remappings=remappings), + + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_spin_recovery_fake_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py index f1b7c40a782..9759d20c8a8 100755 --- a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py +++ b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py @@ -47,7 +47,8 @@ def generate_launch_description(): convert_types=True) return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation ExecuteProcess( diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp index d0e899029e9..059331eaae8 100644 --- a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp @@ -71,8 +71,13 @@ TEST_P(SpinRecoveryTestFixture, testSpinRecovery) break; } } - - EXPECT_EQ(true, success); + if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) { + // if this variable is set, make a fake costmap + // in the fake spin test, we expect a collision for angles > M_PI_2 + EXPECT_EQ(false, success); + } else { + EXPECT_EQ(true, success); + } } INSTANTIATE_TEST_SUITE_P( diff --git a/nav2_system_tests/src/recoveries/wait/CMakeLists.txt b/nav2_system_tests/src/recoveries/wait/CMakeLists.txt index 06755020d5c..6f751cf6aff 100644 --- a/nav2_system_tests/src/recoveries/wait/CMakeLists.txt +++ b/nav2_system_tests/src/recoveries/wait/CMakeLists.txt @@ -19,5 +19,5 @@ ament_add_test(test_wait_recovery TEST_EXECUTABLE=$ TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ) diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py index 484c16fda1d..57a6cf327d9 100755 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py +++ b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py @@ -47,7 +47,8 @@ def generate_launch_description(): convert_types=True) return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation ExecuteProcess( diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 3c38e2fb428..8c637cf8649 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -1,6 +1,3 @@ -# NOTE: The ASTAR=True does not work currently due to remapping not functioning -# All set to false, A* testing of NavFn happens in the planning test portion - ament_add_test(test_bt_navigator GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" @@ -11,8 +8,11 @@ ament_add_test(test_bt_navigator TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=nav_to_pose_tester_node.py ASTAR=True + CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + PLANNER=nav2_navfn_planner/NavfnPlanner ) ament_add_test(test_bt_navigator_with_dijkstra @@ -25,8 +25,11 @@ ament_add_test(test_bt_navigator_with_dijkstra TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=nav_to_pose_tester_node.py ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner ) ament_add_test(test_bt_navigator_with_groot_monitoring @@ -39,9 +42,12 @@ ament_add_test(test_bt_navigator_with_groot_monitoring TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=nav_to_pose_tester_node.py ASTAR=False GROOT_MONITORING=True + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner ) ament_add_test(test_dynamic_obstacle @@ -54,8 +60,28 @@ ament_add_test(test_dynamic_obstacle TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml + TESTER=nav_to_pose_tester_node.py + ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner +) + +ament_add_test(test_nav_through_poses + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml + TESTER=nav_through_poses_tester_node.py ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner ) # ament_add_test(test_multi_robot @@ -69,5 +95,8 @@ ament_add_test(test_dynamic_obstacle # TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/world_only.model # TEST_URDF=${PROJECT_SOURCE_DIR}/urdf/turtlebot3_waffle.urdf # TEST_SDF=${PROJECT_SOURCE_DIR}/models/turtlebot3_waffle/model.sdf -# BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +# BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +# CONTROLLER=dwb_core::DWBLocalPlanner +# PLANNER=nav2_navfn_planner/NavfnPlanner +# TESTER=nav_to_pose_tester_node.py # ) diff --git a/nav2_system_tests/src/system/README.md b/nav2_system_tests/src/system/README.md index d78e9284650..7160a7bdf46 100644 --- a/nav2_system_tests/src/system/README.md +++ b/nav2_system_tests/src/system/README.md @@ -1,9 +1,9 @@ -# Navigation2 System Tests +# Nav2 System Tests This is a 'top level' system test which will use Gazebo to simulate a Robot moving from an known initial starting position to a goal pose. ## To run the test -First, you must build navigation2 including this package: +First, you must build Nav2 including this package: ``` colcon build --symlink-install ``` diff --git a/nav2_system_tests/src/system/nav_through_poses_tester_node.py b/nav2_system_tests/src/system/nav_through_poses_tester_node.py new file mode 100755 index 00000000000..c25e26d49eb --- /dev/null +++ b/nav2_system_tests/src/system/nav_through_poses_tester_node.py @@ -0,0 +1,272 @@ +#! /usr/bin/env python3 +# Copyright 2018 Intel Corporation. +# Copyright 2020 Florian Gramss +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +import sys +import time + +from typing import Optional + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import NavigateThroughPoses +from nav2_msgs.srv import ManageLifecycleNodes + +import rclpy + +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + + +class NavTester(Node): + + def __init__( + self, + initial_pose: Pose, + goal_pose: Pose, + namespace: str = '' + ): + super().__init__(node_name='nav2_tester', namespace=namespace) + self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, + 'initialpose', 10) + + pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1) + + self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, + 'amcl_pose', self.poseCallback, pose_qos) + self.initial_pose_received = False + self.initial_pose = initial_pose + self.goal_pose = goal_pose + self.action_client = ActionClient(self, NavigateThroughPoses, 'navigate_through_poses') + + def info_msg(self, msg: str): + self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + + def warn_msg(self, msg: str): + self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + + def error_msg(self, msg: str): + self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + + def setInitialPose(self): + msg = PoseWithCovarianceStamped() + msg.pose.pose = self.initial_pose + msg.header.frame_id = 'map' + self.info_msg('Publishing Initial Pose') + self.initial_pose_pub.publish(msg) + self.currentPose = self.initial_pose + + def getStampedPoseMsg(self, pose: Pose): + msg = PoseStamped() + msg.header.frame_id = 'map' + msg.pose = pose + return msg + + def runNavigateAction(self, goal_pose: Optional[Pose] = None): + # Sends a `NavToPose` action request and waits for completion + self.info_msg("Waiting for 'NavigateThroughPoses' action server") + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'NavigateThroughPoses' action server not available, waiting...") + + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + goal_msg = NavigateThroughPoses.Goal() + goal_msg.poses = [self.getStampedPoseMsg(self.goal_pose)] + + self.info_msg('Sending goal request...') + send_goal_future = self.action_client.send_goal_async(goal_msg) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + self.info_msg("Waiting for 'NavigateToPose' action to complete") + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg('Goal failed with status code: {0}'.format(status)) + return False + + self.info_msg('Goal succeeded!') + return True + + def poseCallback(self, msg): + self.info_msg('Received amcl_pose') + self.current_pose = msg.pose.pose + self.initial_pose_received = True + + def wait_for_node_active(self, node_name: str): + # Waits for the node within the tester namespace to become active + self.info_msg('Waiting for ' + node_name + ' to become active') + node_service = node_name + '/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info_msg(node_service + ' service not available, waiting...') + req = GetState.Request() # empty request + state = 'UNKNOWN' + while (state != 'active'): + self.info_msg('Getting ' + node_name + ' state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.info_msg('Result of get_state: %s' % state) + else: + self.error_msg('Exception while calling service: %r' % future.exception()) + time.sleep(5) + + def shutdown(self): + self.info_msg('Shutting down') + self.action_client.destroy() + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(transition_service + ' service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down navigation lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down navigation lifecycle manager complete.') + except Exception as e: # noqa: B902 + self.error_msg('Service call failed %r' % (e,)) + transition_service = 'lifecycle_manager_localization/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(transition_service + ' service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down localization lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down localization lifecycle manager complete') + except Exception as e: # noqa: B902 + self.error_msg('Service call failed %r' % (e,)) + + def wait_for_initial_pose(self): + self.initial_pose_received = False + while not self.initial_pose_received: + self.info_msg('Setting initial pose') + self.setInitialPose() + self.info_msg('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1) + + +def run_all_tests(robot_tester): + # set transforms to use_sim_time + result = True + if (result): + robot_tester.wait_for_node_active('amcl') + robot_tester.wait_for_initial_pose() + robot_tester.wait_for_node_active('bt_navigator') + result = robot_tester.runNavigateAction() + + # Add more tests here if desired + + if (result): + robot_tester.info_msg('Test PASSED') + else: + robot_tester.error_msg('Test FAILED') + + return result + + +def fwd_pose(x=0.0, y=0.0, z=0.01): + initial_pose = Pose() + initial_pose.position.x = x + initial_pose.position.y = y + initial_pose.position.z = z + initial_pose.orientation.x = 0.0 + initial_pose.orientation.y = 0.0 + initial_pose.orientation.z = 0.0 + initial_pose.orientation.w = 1.0 + return initial_pose + + +def get_testers(args): + testers = [] + + init_x, init_y, final_x, final_y = args.robot[0] + tester = NavTester( + initial_pose=fwd_pose(float(init_x), float(init_y)), + goal_pose=fwd_pose(float(final_x), float(final_y))) + tester.info_msg( + 'Starting tester, robot going from ' + init_x + ', ' + init_y + + ' to ' + final_x + ', ' + final_y + '.') + testers.append(tester) + return testers + + +def main(argv=sys.argv[1:]): + # The robot(s) positions from the input arguments + parser = argparse.ArgumentParser(description='System-level navigation tester node') + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument('-r', '--robot', action='append', nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.') + + args, unknown = parser.parse_known_args() + + rclpy.init() + + # Create testers for each robot + testers = get_testers(args) + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + for tester in testers: + passed = run_all_tests(tester) + if not passed: + break + + for tester in testers: + # stop and shutdown the nav stack to exit cleanly + tester.shutdown() + + testers[0].info_msg('Done Shutting Down.') + + if not passed: + testers[0].info_msg('Exiting failed') + exit(1) + else: + testers[0].info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/src/system/tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py similarity index 97% rename from nav2_system_tests/src/system/tester_node.py rename to nav2_system_tests/src/system/nav_to_pose_tester_node.py index d84724ea5c8..176f2e44908 100755 --- a/nav2_system_tests/src/system/tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -54,9 +54,9 @@ def __init__( self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, - history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, depth=1) self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, @@ -132,7 +132,7 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): if not self.grootMonitoringGetStatus(): self.error_msg('Failed GROOT_BT - Get Status from ZMQ Publisher') future_return = False - except Exception as e: + except Exception as e: # noqa: B902 self.error_msg('Failed GROOT_BT - ZMQ Tests: ' + e.__doc__ + e.message) future_return = False @@ -282,7 +282,7 @@ def shutdown(self): rclpy.spin_until_future_complete(self, future) future.result() self.info_msg('Shutting down navigation lifecycle manager complete.') - except Exception as e: + except Exception as e: # noqa: B902 self.error_msg('Service call failed %r' % (e,)) transition_service = 'lifecycle_manager_localization/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) @@ -297,7 +297,7 @@ def shutdown(self): rclpy.spin_until_future_complete(self, future) future.result() self.info_msg('Shutting down localization lifecycle manager complete') - except Exception as e: + except Exception as e: # noqa: B902 self.error_msg('Service call failed %r' % (e,)) def wait_for_initial_pose(self): diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index 87be85d613e..b0104804ef9 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -109,7 +109,8 @@ def generate_launch_description(): nav_instances_cmds.append(group) ld = LaunchDescription() - ld.add_action(SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),) + ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),) + ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),) ld.add_action(start_gazebo_cmd) for spawn_robot in spawn_robots_cmds: ld.add_action(spawn_robot) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index c56d23c41c7..dd0f97f9dd1 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -53,6 +53,11 @@ def generate_launch_description(): if (os.getenv('GROOT_MONITORING') == 'True'): param_substitutions.update({'enable_groot_monitoring': 'True'}) + param_substitutions.update( + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}) + param_substitutions.update( + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}) + configured_params = RewrittenYaml( source_file=params_file, root_key='', @@ -60,8 +65,10 @@ def generate_launch_description(): convert_types=True) new_yaml = configured_params.perform(context) + return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation ExecuteProcess( @@ -100,7 +107,7 @@ def main(argv=sys.argv[1:]): ld = generate_launch_description() test1_action = ExecuteProcess( - cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + cmd=[os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')), '-r', '-2.0', '-0.5', '0.0', '2.0'], name='tester_node', output='screen') diff --git a/nav2_system_tests/src/system_failure/CMakeLists.txt b/nav2_system_tests/src/system_failure/CMakeLists.txt index 885234adc21..6bc4620535e 100644 --- a/nav2_system_tests/src/system_failure/CMakeLists.txt +++ b/nav2_system_tests/src/system_failure/CMakeLists.txt @@ -8,5 +8,5 @@ ament_add_test(test_failure_navigator TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ) diff --git a/nav2_system_tests/src/system_failure/README.md b/nav2_system_tests/src/system_failure/README.md index 4afd4eaac34..1671ff9da4e 100644 --- a/nav2_system_tests/src/system_failure/README.md +++ b/nav2_system_tests/src/system_failure/README.md @@ -1,3 +1,3 @@ -# Navigation2 System Tests - Failure +# Nav2 System Tests - Failure High level system failures tests 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 c41e78f3048..56d4d44fdb1 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 @@ -23,6 +23,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -50,8 +51,11 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) + context = LaunchContext() + new_yaml = configured_params.perform(context) return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation ExecuteProcess( @@ -80,7 +84,7 @@ def generate_launch_description(): 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', - 'params_file': configured_params, + 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py index eac29bc6540..3635f70a1f8 100755 --- a/nav2_system_tests/src/system_failure/tester_node.py +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -50,9 +50,9 @@ def __init__( self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, - history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, depth=1) self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, @@ -186,7 +186,7 @@ def shutdown(self): rclpy.spin_until_future_complete(self, future) future.result() self.info_msg('Shutting down navigation lifecycle manager complete.') - except Exception as e: + except Exception as e: # noqa: B902 self.error_msg('Service call failed %r' % (e,)) transition_service = 'lifecycle_manager_localization/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) @@ -201,7 +201,7 @@ def shutdown(self): rclpy.spin_until_future_complete(self, future) future.result() self.info_msg('Shutting down localization lifecycle manager complete') - except Exception as e: + except Exception as e: # noqa: B902 self.error_msg('Service call failed %r' % (e,)) def wait_for_initial_pose(self): diff --git a/nav2_system_tests/src/updown/CMakeLists.txt b/nav2_system_tests/src/updown/CMakeLists.txt index a56bff34b23..273e1828ea5 100644 --- a/nav2_system_tests/src/updown/CMakeLists.txt +++ b/nav2_system_tests/src/updown/CMakeLists.txt @@ -6,6 +6,6 @@ ament_target_dependencies(test_updown ${dependencies} ) -install(TARGETS test_updown RUNTIME DESTINATION share/${PROJECT_NAME}) +install(TARGETS test_updown RUNTIME DESTINATION lib/${PROJECT_NAME}) install(FILES test_updown_launch.py DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_system_tests/src/updown/README.md b/nav2_system_tests/src/updown/README.md index abfbf0a9006..6756d524659 100644 --- a/nav2_system_tests/src/updown/README.md +++ b/nav2_system_tests/src/updown/README.md @@ -1,4 +1,4 @@ -# Navigation2 Updown Test +# Nav2 Updown Test This is a 'top level' system test which tests the lifecycle bringup and shutdown of the system. diff --git a/nav2_system_tests/src/waypoint_follower/CMakeLists.txt b/nav2_system_tests/src/waypoint_follower/CMakeLists.txt index 2cd3c53a925..6ba85bc7bfb 100644 --- a/nav2_system_tests/src/waypoint_follower/CMakeLists.txt +++ b/nav2_system_tests/src/waypoint_follower/CMakeLists.txt @@ -8,5 +8,5 @@ ament_add_test(test_waypoint_follower TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ) diff --git a/nav2_system_tests/src/waypoint_follower/test_case_py.launch b/nav2_system_tests/src/waypoint_follower/test_case_py.launch index 06d1298f15e..98819e6e630 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_py.launch +++ b/nav2_system_tests/src/waypoint_follower/test_case_py.launch @@ -21,6 +21,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch import LaunchService from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -46,8 +47,11 @@ def generate_launch_description(): param_rewrites='', convert_types=True) + context = LaunchContext() + new_yaml = configured_params.perform(context) return LaunchDescription([ - SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'), # Launch gazebo server for simulation ExecuteProcess( @@ -75,7 +79,7 @@ def generate_launch_description(): launch_arguments={ 'map': map_yaml_file, 'use_sim_time': 'True', - 'params_file': configured_params, + 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 5a8c340cb76..beb27a0b753 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -33,16 +33,16 @@ class WaypointFollowerTest(Node): def __init__(self): super().__init__(node_name='nav2_waypoint_tester', namespace='') self.waypoints = None - self.action_client = ActionClient(self, FollowWaypoints, 'FollowWaypoints') + self.action_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, 'initialpose', 10) self.initial_pose_received = False self.goal_handle = None pose_qos = QoSProfile( - durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, - history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RELIABLE, + history=QoSHistoryPolicy.KEEP_LAST, depth=1) self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, @@ -76,7 +76,7 @@ def run(self, block): return False while not self.action_client.wait_for_server(timeout_sec=1.0): - self.info_msg("'FollowWaypoints' action server not available, waiting...") + self.info_msg("'follow_waypoints' action server not available, waiting...") action_request = FollowWaypoints.Goal() action_request.poses = self.waypoints @@ -86,7 +86,7 @@ def run(self, block): try: rclpy.spin_until_future_complete(self, send_goal_future) self.goal_handle = send_goal_future.result() - except Exception as e: + except Exception as e: # noqa: B902 self.error_msg('Service call failed %r' % (e,)) if not self.goal_handle.accepted: @@ -99,12 +99,12 @@ def run(self, block): get_result_future = self.goal_handle.get_result_async() - self.info_msg("Waiting for 'FollowWaypoints' action to complete") + self.info_msg("Waiting for 'follow_waypoints' action to complete") try: rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status result = get_result_future.result().result - except Exception as e: + except Exception as e: # noqa: B902 self.error_msg('Service call failed %r' % (e,)) if status != GoalStatus.STATUS_SUCCEEDED: @@ -125,7 +125,7 @@ def shutdown(self): self.info_msg('Shutting down') self.action_client.destroy() - self.info_msg('Destroyed FollowWaypoints action client') + self.info_msg('Destroyed follow_waypoints action client') transition_service = 'lifecycle_manager_navigation/manage_nodes' mgr_client = self.create_client(ManageLifecycleNodes, transition_service) @@ -138,7 +138,7 @@ def shutdown(self): try: rclpy.spin_until_future_complete(self, future) future.result() - except Exception as e: + except Exception as e: # noqa: B902 self.error_msg('%s service call failed %r' % (transition_service, e,)) self.info_msg('{} finished'.format(transition_service)) @@ -154,7 +154,7 @@ def shutdown(self): try: rclpy.spin_until_future_complete(self, future) future.result() - except Exception as e: + except Exception as e: # noqa: B902 self.error_msg('%s service call failed %r' % (transition_service, e,)) self.info_msg('{} finished'.format(transition_service)) diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world index 52020409c99..3a70206ab82 100644 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world +++ b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world @@ -137,6 +137,7 @@ + false ~/out:=imu diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world index b14f46fd139..5488938e0ee 100644 --- a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world +++ b/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world @@ -137,6 +137,7 @@ + false ~/out:=imu diff --git a/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp b/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp index 570feb79c86..8b401e8a595 100644 --- a/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp +++ b/nav2_util/include/nav2_util/clear_entirely_costmap_service_client.hpp @@ -22,11 +22,17 @@ namespace nav2_util { - +/** + * @class nav2_util::ClearEntirelyCostmapServiceClient + * @brief A service client to clear costmaps entirely + */ class ClearEntirelyCostmapServiceClient : public nav2_util::ServiceClient { public: + /** +* @brief A constructor for nav2_util::ClearEntirelyCostmapServiceClient + */ explicit ClearEntirelyCostmapServiceClient(const std::string & service_name) : nav2_util::ServiceClient(service_name) { diff --git a/nav2_util/include/nav2_util/costmap.hpp b/nav2_util/include/nav2_util/costmap.hpp index c811007e4ce..0d868df6e03 100644 --- a/nav2_util/include/nav2_util/costmap.hpp +++ b/nav2_util/include/nav2_util/costmap.hpp @@ -36,28 +36,65 @@ enum class TestCostmap maze2 }; -// Class for a single layered costmap initialized from an -// occupancy grid representing the map. +/** + * @class nav2_util::Costmap + * @brief Class for a single layered costmap initialized from an + * occupancy grid representing the map. + */ class Costmap { public: typedef uint8_t CostValue; + /** + * @brief A constructor for nav2_util::Costmap + * @param node Ptr to a node + * @param trinary_costmap Whether the costmap should be trinary + * @param track_unknown_space Whether to track unknown space in costmap + * @param lethal_threshold The lethal space cost threshold to use + * @param unknown_cost_value Internal costmap cell value for unknown space + */ Costmap( rclcpp::Node * node, bool trinary_costmap = true, bool track_unknown_space = true, int lethal_threshold = 100, int unknown_cost_value = -1); Costmap() = delete; ~Costmap(); + /** + * @brief Set the static map of this costmap + * @param occupancy_grid Occupancy grid to populate this costmap with + */ void set_static_map(const nav_msgs::msg::OccupancyGrid & occupancy_grid); + /** + * @brief Set the test costmap type of this costmap + * @param testCostmapType Type of stored costmap to use + */ void set_test_costmap(const TestCostmap & testCostmapType); + /** + * @brief Get a costmap message from this object + * @param specifications Parameters of costmap + * @return Costmap msg of this costmap + */ nav2_msgs::msg::Costmap get_costmap(const nav2_msgs::msg::CostmapMetaData & specifications); + /** + * @brief Get a metadata message from this object + * @return Costmap metadata of this costmap + */ nav2_msgs::msg::CostmapMetaData get_properties() {return costmap_properties_;} + /** + * @brief Get whether some coordinates are free + * @return bool if free + */ bool is_free(const unsigned int x_coordinate, const unsigned int y_coordinate) const; + + /** + * @brief Get whether some index in the costmap is free + * @return bool if free + */ bool is_free(const unsigned int index) const; // Mapping for often used cost values @@ -68,8 +105,16 @@ class Costmap static const CostValue free_space; private: + /** + * @brief Get data from the test + * @return data + */ std::vector get_test_data(const TestCostmap configuration); + /** + * @brief Get the interpreted value in the costmap + * @return uint value + */ uint8_t interpret_value(const int8_t value) const; // Costmap isn't itself a node diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 52bd5ebb417..17e654fc287 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -21,13 +21,19 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav_msgs/msg/path.hpp" namespace nav2_util { namespace geometry_utils { +/** + * @brief Get a geometry_msgs Quaternion from a yaw angle + * @param angle Yaw angle to generate a quaternion from + * @return geometry_msgs Quaternion + */ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle) { tf2::Quaternion q; @@ -35,6 +41,12 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle) return tf2::toMsg(q); } +/** + * @brief Get the L2 distance between 2 geometry_msgs::Points + * @param pos1 First point + * @param pos1 Second point + * @return double L2 distance + */ inline double euclidean_distance( const geometry_msgs::msg::Point & pos1, const geometry_msgs::msg::Point & pos2) @@ -45,6 +57,12 @@ inline double euclidean_distance( return std::sqrt(dx * dx + dy * dy + dz * dz); } +/** + * @brief Get the L2 distance between 2 geometry_msgs::Poses + * @param pos1 First pose + * @param pos1 Second pose + * @return double L2 distance + */ inline double euclidean_distance( const geometry_msgs::msg::Pose & pos1, const geometry_msgs::msg::Pose & pos2) @@ -55,6 +73,12 @@ inline double euclidean_distance( return std::sqrt(dx * dx + dy * dy + dz * dz); } +/** + * @brief Get the L2 distance between 2 geometry_msgs::PoseStamped + * @param pos1 First pose + * @param pos1 Second pose + * @return double L2 distance + */ inline double euclidean_distance( const geometry_msgs::msg::PoseStamped & pos1, const geometry_msgs::msg::PoseStamped & pos2) @@ -62,6 +86,47 @@ inline double euclidean_distance( return euclidean_distance(pos1.pose, pos2.pose); } +/** + * Find element in iterator with the minimum calculated value + */ +template +inline Iter min_by(Iter begin, Iter end, Getter getCompareVal) +{ + if (begin == end) { + return end; + } + auto lowest = getCompareVal(*begin); + Iter lowest_it = begin; + for (Iter it = ++begin; it != end; ++it) { + auto comp = getCompareVal(*it); + if (comp < lowest) { + lowest = comp; + lowest_it = it; + } + } + return lowest_it; +} + +/** + * @brief Calculate the length of the provided path, starting at the provided index + * @param path Path containing the poses that are planned + * @param start_index Optional argument specifying the starting index for + * the calculation of path length. Provide this if you want to calculate length of a + * subset of the path. + * @return double Path length + */ +inline double calculate_path_length(const nav_msgs::msg::Path & path, size_t start_index = 0) +{ + if (start_index + 1 >= path.poses.size()) { + return 0.0; + } + double path_length = 0.0; + for (size_t idx = start_index; idx < path.poses.size() - 1; ++idx) { + path_length += euclidean_distance(path.poses[idx].pose, path.poses[idx + 1].pose); + } + return path_length; +} + } // namespace geometry_utils } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 9180133817f..5543b5fffc4 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -35,12 +35,24 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface // to interface to classes, such as MessageFilter and TransformListener, that don't yet // support lifecycle nodes. Once we get the fixes into ROS2, this class will be removed. +/** + * @class nav2_util::LifecycleNode + * @brief A lifecycle node wrapper to enable common Nav2 needs such as background node threads + * and manipulating parameters + */ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: + /** + * @brief A lifecycle node constructor + * @param node_name Name for the node + * @param namespace Namespace for the node, if any + * @param use_rclcpp_node Whether to create an internal client node + * @param options Node options + */ LifecycleNode( const std::string & node_name, - const std::string & namespace_ = "", + const std::string & ns = "", bool use_rclcpp_node = false, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); virtual ~LifecycleNode(); @@ -59,7 +71,14 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode int step; } integer_range; - // Declare a parameter that has no integer or floating point range constraints + /** + * @brief Declare a parameter that has no integer or floating point range constraints + * @param node_name Name of parameter + * @param default_value Default node value to add + * @param description Node description + * @param additional_constraints Any additional constraints on the parameters to list + * @param read_only Whether this param should be considered read only + */ void add_parameter( const std::string & name, const rclcpp::ParameterValue & default_value, const std::string & description = "", const std::string & additional_constraints = "", @@ -75,7 +94,15 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode declare_parameter(descriptor.name, default_value, descriptor); } - // Declare a parameter that has a floating point range constraint + /** + * @brief Declare a parameter that has a floating point range constraint + * @param node_name Name of parameter + * @param default_value Default node value to add + * @param fp_range floating point range + * @param description Node description + * @param additional_constraints Any additional constraints on the parameters to list + * @param read_only Whether this param should be considered read only + */ void add_parameter( const std::string & name, const rclcpp::ParameterValue & default_value, const floating_point_range fp_range, @@ -96,7 +123,15 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode declare_parameter(descriptor.name, default_value, descriptor); } - // Declare a parameter that has an integer range constraint + /** + * @brief Declare a parameter that has an integer range constraint + * @param node_name Name of parameter + * @param default_value Default node value to add + * @param integer_range Integer range + * @param description Node description + * @param additional_constraints Any additional constraints on the parameters to list + * @param read_only Whether this param should be considered read only + */ void add_parameter( const std::string & name, const rclcpp::ParameterValue & default_value, const integer_range int_range, @@ -117,12 +152,21 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode declare_parameter(descriptor.name, default_value, descriptor); } + /** + * @brief Get a shared pointer of this + */ std::shared_ptr shared_from_this() { return std::static_pointer_cast( rclcpp_lifecycle::LifecycleNode::shared_from_this()); } + /** + * @brief Abstracted on_error state transition callback, since unimplemented as of 2020 + * in the managed ROS2 node state machine + * @param state State prior to error transition + * @return Return type for success or failed transition to error state + */ nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_FATAL( @@ -131,11 +175,20 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode return nav2_util::CallbackReturn::SUCCESS; } - // bond connection to lifecycle manager + /** + * @brief Create bond connection to lifecycle manager + */ void createBond(); + + /** + * @brief Destroy bond connection to lifecycle manager + */ void destroyBond(); protected: + /** + * @brief Print notifications for lifecycle node + */ void print_lifecycle_node_notification(); // Whether or not to create a local rclcpp::Node which can be used for ROS2 classes that don't diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp index 4bfaead087a..c11bbf7ce48 100644 --- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp +++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp @@ -22,6 +22,7 @@ #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" #include "nav2_util/service_client.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_util { diff --git a/nav2_util/include/nav2_util/line_iterator.hpp b/nav2_util/include/nav2_util/line_iterator.hpp index a9ff0c1f435..82ea12425ea 100644 --- a/nav2_util/include/nav2_util/line_iterator.hpp +++ b/nav2_util/include/nav2_util/line_iterator.hpp @@ -38,10 +38,21 @@ namespace nav2_util { -/** An iterator implementing Bresenham Ray-Tracing. */ + +/** + * @class nav2_util::LineIterator + * @brief An iterator implementing Bresenham Ray-Tracing. + */ class LineIterator { public: + /** + * @brief A constructor for LineIterator + * @param x0 Starting x + * @param y0 Starting y + * @param x1 Ending x + * @param y1 Ending y + */ LineIterator(int x0, int y0, int x1, int y1) : x0_(x0), y0_(y0), @@ -86,11 +97,18 @@ class LineIterator } } + /** + * @brief If the iterator is valid + * @return bool If valid + */ bool isValid() const { return curpixel_ <= numpixels_; } + /** + * @brief Advance iteration along the line + */ void advance() { num_ += numadd_; // Increase the numerator by the top of the fraction @@ -105,28 +123,55 @@ class LineIterator curpixel_++; } + /** + * @brief Get current X value + * @return X + */ int getX() const { return x_; } + + /** + * @brief Get current Y value + * @return Y + */ int getY() const { return y_; } + /** + * @brief Get initial X value + * @return X + */ int getX0() const { return x0_; } + + /** + * @brief Get initial Y value + * @return Y + */ int getY0() const { return y0_; } + /** + * @brief Get terminal X value + * @return X + */ int getX1() const { return x1_; } + + /** + * @brief Get terminal Y value + * @return Y + */ int getY1() const { return y1_; diff --git a/nav2_util/include/nav2_util/node_thread.hpp b/nav2_util/include/nav2_util/node_thread.hpp index 97ab46cee6a..634b4acd5bc 100644 --- a/nav2_util/include/nav2_util/node_thread.hpp +++ b/nav2_util/include/nav2_util/node_thread.hpp @@ -21,17 +21,31 @@ namespace nav2_util { - +/** + * @class nav2_util::NodeThread + * @brief A background thread to process node callbacks + */ class NodeThread { public: + /** + * @brief A background thread to process node callbacks constructor + * @param node_base Interface to Node to spin in thread + */ explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); + /** + * @brief A background thread to process node callbacks constructor + * @param node Node pointer to spin in thread + */ template explicit NodeThread(NodeT node) : NodeThread(node->get_node_base_interface()) {} + /** + * @brief A destructor + */ ~NodeThread(); protected: diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 51281ff10a6..484ff35dd15 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -80,11 +80,20 @@ std::string time_to_string(size_t len); rclcpp::NodeOptions get_node_options_default(bool allow_undeclared = true, bool declare_initial_params = true); +/// 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] 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 rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(), + const rclcpp::ParameterValue & default_value, const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor()) { @@ -93,17 +102,54 @@ void declare_parameter_if_not_declared( } } +/// Declares static ROS2 parameter with given type if it was not already 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] 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 rclcpp::ParameterType & param_type, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor()) +{ + if (!node->has_parameter(param_name)) { + node->declare_parameter(param_name, param_type, parameter_descriptor); + } +} + +/// Gets the type of plugin for the selected node and its plugin +/** + * Gets the type of plugin for the selected node and its plugin. + * Actually seeks for the value of ".plugin" parameter. + * + * \param[in] node Selected node + * \param[in] plugin_name The name of plugin the type of which is being searched for + * \return A string containing the type of plugin (the value of ".plugin" parameter) + */ template std::string get_plugin_type_param( NodeT node, const std::string & plugin_name) { - declare_parameter_if_not_declared(node, plugin_name + ".plugin"); + declare_parameter_if_not_declared(node, plugin_name + ".plugin", rclcpp::PARAMETER_STRING); std::string plugin_type; - if (!node->get_parameter(plugin_name + ".plugin", plugin_type)) { + try { + if (!node->get_parameter(plugin_name + ".plugin", plugin_type)) { + RCLCPP_FATAL( + node->get_logger(), "Can not get 'plugin' param value for %s", plugin_name.c_str()); + exit(-1); + } + } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { RCLCPP_FATAL(node->get_logger(), "'plugin' param not defined for %s", plugin_name.c_str()); exit(-1); } + return plugin_type; } diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index f75d01ebefb..185a86d1471 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -52,11 +52,40 @@ class OdomSmoother double filter_duration = 0.3, const std::string & odom_topic = "odom"); + /** + * @brief Overloadded Constructor for nav_util::LifecycleNode parent + * that subscribes to an Odometry topic + * @param parent NodeHandle for creating subscriber + * @param filter_duration Duration for odom history (seconds) + * @param odom_topic Topic on which odometry should be received + */ + explicit OdomSmoother( + const nav2_util::LifecycleNode::WeakPtr & parent, + double filter_duration = 0.3, + const std::string & odom_topic = "odom"); + + /** + * @brief Get twist msg from smoother + * @return twist Twist msg + */ inline geometry_msgs::msg::Twist getTwist() {return vel_smooth_.twist;} + + /** + * @brief Get twist stamped msg from smoother + * @return twist TwistStamped msg + */ inline geometry_msgs::msg::TwistStamped getTwistStamped() {return vel_smooth_;} protected: + /** + * @brief Callback of odometry subscriber to process + * @param msg Odometry msg to smooth + */ void odomCallback(nav_msgs::msg::Odometry::SharedPtr msg); + + /** + * @brief Update internal state of the smoother after getting new data + */ void updateState(); rclcpp::Subscription::SharedPtr odom_sub_; diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 0575dad5d9e..11368d70304 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -25,17 +25,40 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" namespace nav2_util { - +/** +* @brief get the current pose of the robot +* @param global_pose Pose to transform +* @param tf_buffer TF buffer to use for the transformation +* @param global_frame Frame to transform into +* @param robot_frame Frame to transform from +* @param transform_timeout TF Timeout to use for transformation +* @return bool Whether it could be transformed successfully +*/ bool getCurrentPose( geometry_msgs::msg::PoseStamped & global_pose, tf2_ros::Buffer & tf_buffer, const std::string global_frame = "map", const std::string robot_frame = "base_link", const double transform_timeout = 0.1); +/** +* @brief get an arbitrary pose in a target frame +* @param input_pose Pose to transform +* @param transformed_pose Output transformation +* @param tf_buffer TF buffer to use for the transformation +* @param target_frame Frame to transform into +* @param transform_timeout TF Timeout to use for transformation +* @return bool Whether it could be transformed successfully +*/ +bool transformPoseInTargetFrame( + const geometry_msgs::msg::PoseStamped & input_pose, + geometry_msgs::msg::PoseStamped & transformed_pose, + tf2_ros::Buffer & tf_buffer, const std::string target_frame, + const double transform_timeout = 0.1); + } // end namespace nav2_util #endif // NAV2_UTIL__ROBOT_UTILS_HPP_ diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index e1eb6983817..4af107abf17 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -17,42 +17,51 @@ #include -#include "nav2_util/node_utils.hpp" #include "rclcpp/rclcpp.hpp" namespace nav2_util { +/** + * @class nav2_util::ServiceClient + * @brief A simple wrapper on ROS2 services for invoke() and block-style calling + */ template class ServiceClient { public: + /** + * @brief A constructor + * @param service_name name of the service to call + * @param provided_node Node to create the service client off of + */ explicit ServiceClient( const std::string & service_name, - const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr()) - : service_name_(service_name) + const rclcpp::Node::SharedPtr & provided_node) + : service_name_(service_name), node_(provided_node) { - if (provided_node) { - node_ = provided_node; - } else { - node_ = generate_internal_node(service_name + "_Node"); - } - client_ = node_->create_client(service_name); - } - - ServiceClient(const std::string & service_name, const std::string & parent_name) - : service_name_(service_name) - { - node_ = generate_internal_node(parent_name + std::string("_") + service_name + "_client"); - client_ = node_->create_client(service_name); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + client_ = node_->create_client( + service_name, + rmw_qos_profile_services_default, + callback_group_); } using RequestType = typename ServiceT::Request; using ResponseType = typename ServiceT::Response; + /** + * @brief Invoke the service and block until completed or timed out + * @param request The request object to call the service using + * @param timeout Maximum timeout to wait for, default infinite + * @return Response A pointer to the service response from the request + */ typename ResponseType::SharedPtr invoke( typename RequestType::SharedPtr & request, - const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) + const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) { while (!client_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { @@ -69,7 +78,7 @@ class ServiceClient service_name_.c_str()); auto future_result = client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != + if (callback_group_executor_.spin_until_future_complete(future_result, timeout) != rclcpp::FutureReturnCode::SUCCESS) { throw std::runtime_error(service_name_ + " service client: async_send_request failed"); @@ -78,6 +87,12 @@ class ServiceClient return future_result.get(); } + /** + * @brief Invoke the service and block until completed + * @param request The request object to call the service using + * @param Response A pointer to the service response from the request + * @return bool Whether it was successfully called + */ bool invoke( typename RequestType::SharedPtr & request, typename ResponseType::SharedPtr & response) @@ -97,7 +112,7 @@ class ServiceClient service_name_.c_str()); auto future_result = client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result) != + if (callback_group_executor_.spin_until_future_complete(future_result) != rclcpp::FutureReturnCode::SUCCESS) { return false; @@ -107,21 +122,21 @@ class ServiceClient return response.get(); } - void wait_for_service(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) + /** + * @brief Block until a service is available or timeout + * @param timeout Maximum timeout to wait for, default infinite + * @return bool true if service is available + */ + bool wait_for_service(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) { - auto sleep_dur = std::chrono::milliseconds(10); - while (!client_->wait_for_service(timeout)) { - if (!rclcpp::ok()) { - throw std::runtime_error( - service_name_ + " service client: interrupted while waiting for service"); - } - rclcpp::sleep_for(sleep_dur); - } + return client_->wait_for_service(timeout); } protected: std::string service_name_; rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; typename rclcpp::Client::SharedPtr client_; }; diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index f0c01b71acb..9894d8d47a0 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -28,25 +28,54 @@ namespace nav2_util { +/** + * @class nav2_util::SimpleActionServer + * @brief An action server wrapper to make applications simpler using Actions + */ template class SimpleActionServer { public: + // Callback function to complete main work. This should itself deal with its + // own exceptions, but if for some reason one is thrown, it will be caught + // in SimpleActionServer and terminate the action itself. typedef std::function ExecuteCallback; + // Callback function to notify the user that an exception was thrown that + // the simple action server caught (or another failure) and the action was + // terminated. To avoid using, catch exceptions in your application such that + // the SimpleActionServer will never need to terminate based on failed action + // ExecuteCallback. + typedef std::function CompletionCallback; + + /** + * @brief An constructor for SimpleActionServer + * @param node Ptr to node to make actions + * @param action_name Name of the action to call + * @param execute_callback Execution callback function of Action + * @param server_timeout Timeout to to react to stop or preemption requests + */ explicit SimpleActionServer( typename nodeT::SharedPtr node, const std::string & action_name, ExecuteCallback execute_callback, + CompletionCallback completion_callback = nullptr, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500)) : SimpleActionServer( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), - action_name, execute_callback, server_timeout) + action_name, execute_callback, completion_callback, server_timeout) {} + /** + * @brief An constructor for SimpleActionServer + * @param Abstract node interfaces to make actions + * @param action_name Name of the action to call + * @param execute_callback Execution callback function of Action + * @param server_timeout Timeout to to react to stop or preemption requests + */ explicit SimpleActionServer( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, @@ -54,6 +83,7 @@ class SimpleActionServer rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, const std::string & action_name, ExecuteCallback execute_callback, + CompletionCallback completion_callback = nullptr, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500)) : node_base_interface_(node_base_interface), node_clock_interface_(node_clock_interface), @@ -61,6 +91,7 @@ class SimpleActionServer node_waitables_interface_(node_waitables_interface), action_name_(action_name), execute_callback_(execute_callback), + completion_callback_(completion_callback), server_timeout_(server_timeout) { using namespace std::placeholders; // NOLINT @@ -75,6 +106,12 @@ class SimpleActionServer std::bind(&SimpleActionServer::handle_accepted, this, _1)); } + /** + * @brief handle the goal requested: accept or reject. This implementation always accepts. + * @param uuid Goal ID + * @param Goal A shared pointer to the specific goal + * @return GoalResponse response of the goal processed + */ rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & /*uuid*/, std::shared_ptr/*goal*/) @@ -89,14 +126,32 @@ class SimpleActionServer return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } + /** + * @brief Accepts cancellation requests of action server. + * @param uuid Goal ID + * @param Goal A server goal handle to cancel + * @return CancelResponse response of the goal cancelled + */ rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr>/*handle*/) + const std::shared_ptr> handle) { std::lock_guard lock(update_mutex_); + + if (!handle->is_active()) { + warn_msg( + "Received request for goal cancellation," + "but the handle is inactive, so reject the request"); + return rclcpp_action::CancelResponse::REJECT; + } + debug_msg("Received request for goal cancellation"); return rclcpp_action::CancelResponse::ACCEPT; } + /** + * @brief Handles accepted goals and adds to preempted queue to switch to + * @param Goal A server goal handle to cancel + */ void handle_accepted(const std::shared_ptr> handle) { std::lock_guard lock(update_mutex_); @@ -129,6 +184,9 @@ class SimpleActionServer } } + /** + * @brief Computed background work and processes stop requests + */ void work() { while (rclcpp::ok() && !stop_execution_ && is_active(current_handle_)) { @@ -140,6 +198,7 @@ class SimpleActionServer node_logging_interface_->get_logger(), "Action server failed while executing action callback: \"%s\"", ex.what()); terminate_all(); + completion_callback_(); return; } @@ -149,12 +208,14 @@ class SimpleActionServer if (stop_execution_) { warn_msg("Stopping the thread per request."); terminate_all(); + completion_callback_(); break; } if (is_active(current_handle_)) { warn_msg("Current goal was not completed successfully."); terminate(current_handle_); + completion_callback_(); } if (is_active(pending_handle_)) { @@ -168,6 +229,9 @@ class SimpleActionServer debug_msg("Worker thread done."); } + /** + * @brief Active action server + */ void activate() { std::lock_guard lock(update_mutex_); @@ -175,6 +239,9 @@ class SimpleActionServer stop_execution_ = false; } + /** + * @brief Deactive action server + */ void deactivate() { debug_msg("Deactivating..."); @@ -201,6 +268,7 @@ class SimpleActionServer info_msg("Waiting for async process to finish."); if (steady_clock::now() - start_time >= server_timeout_) { terminate_all(); + completion_callback_(); throw std::runtime_error("Action callback is still running and missed deadline to stop"); } } @@ -208,6 +276,10 @@ class SimpleActionServer debug_msg("Deactivation completed."); } + /** + * @brief Whether the action server is munching on a goal + * @return bool If its running or not + */ bool is_running() { return execution_future_.valid() && @@ -215,18 +287,30 @@ class SimpleActionServer std::future_status::timeout); } + /** + * @brief Whether the action server is active or not + * @return bool If its active or not + */ bool is_server_active() { std::lock_guard lock(update_mutex_); return server_active_; } + /** + * @brief Whether the action server has been asked to be preempted with a new goal + * @return bool If there's a preemption request or not + */ bool is_preempt_requested() const { std::lock_guard lock(update_mutex_); return preempt_requested_; } + /** + * @brief Accept pending goals + * @return Goal Ptr to the goal that's going to be accepted + */ const std::shared_ptr accept_pending_goal() { std::lock_guard lock(update_mutex_); @@ -250,6 +334,28 @@ class SimpleActionServer return current_handle_->get_goal(); } + /** + * @brief Terminate pending goals + */ + void terminate_pending_goal() + { + std::lock_guard lock(update_mutex_); + + if (!pending_handle_ || !pending_handle_->is_active()) { + error_msg("Attempting to terminate pending goal when not available"); + return; + } + + terminate(pending_handle_); + preempt_requested_ = false; + + debug_msg("Pending goal terminated"); + } + + /** + * @brief Get the current goal object + * @return Goal Ptr to the goal that's being processed currently + */ const std::shared_ptr get_current_goal() const { std::lock_guard lock(update_mutex_); @@ -262,6 +368,26 @@ class SimpleActionServer return current_handle_->get_goal(); } + /** + * @brief Get the pending goal object + * @return Goal Ptr to the goal that's pending + */ + const std::shared_ptr get_pending_goal() const + { + std::lock_guard lock(update_mutex_); + + if (!pending_handle_ || !pending_handle_->is_active()) { + error_msg("Attempting to get pending goal when not available"); + return std::shared_ptr(); + } + + return pending_handle_->get_goal(); + } + + /** + * @brief Whether or not a cancel command has come in + * @return bool Whether a cancel command has been requested or not + */ bool is_cancel_requested() const { std::lock_guard lock(update_mutex_); @@ -280,6 +406,10 @@ class SimpleActionServer return current_handle_->is_canceling(); } + /** + * @brief Terminate all pending and active actions + * @param result A result object to send to the terminated actions + */ void terminate_all( typename std::shared_ptr result = std::make_shared()) @@ -290,6 +420,10 @@ class SimpleActionServer preempt_requested_ = false; } + /** + * @brief Terminate the active action + * @param result A result object to send to the terminated action + */ void terminate_current( typename std::shared_ptr result = std::make_shared()) @@ -298,6 +432,10 @@ class SimpleActionServer terminate(current_handle_, result); } + /** + * @brief Return success of the active action + * @param result A result object to send to the terminated actions + */ void succeeded_current( typename std::shared_ptr result = std::make_shared()) @@ -311,6 +449,10 @@ class SimpleActionServer } } + /** + * @brief Publish feedback to the action server clients + * @param feedback A feedback object to send to the clients + */ void publish_feedback(typename std::shared_ptr feedback) { if (!is_active(current_handle_)) { @@ -330,6 +472,7 @@ class SimpleActionServer std::string action_name_; ExecuteCallback execute_callback_; + CompletionCallback completion_callback_; std::future execution_future_; bool stop_execution_{false}; @@ -343,17 +486,30 @@ class SimpleActionServer typename rclcpp_action::Server::SharedPtr action_server_; + /** + * @brief Generate an empty result object for an action type + */ constexpr auto empty_result() const { return std::make_shared(); } + /** + * @brief Whether a given goal handle is currently active + * @param handle Goal handle to check + * @return Whether this goal handle is active + */ constexpr bool is_active( const std::shared_ptr> handle) const { return handle != nullptr && handle->is_active(); } + /** + * @brief Terminate a particular action with a result + * @param handle goal handle to terminate + * @param the Results object to terminate the action with + */ void terminate( std::shared_ptr> handle, typename std::shared_ptr result = @@ -373,6 +529,9 @@ class SimpleActionServer } } + /** + * @brief Info logging + */ void info_msg(const std::string & msg) const { RCLCPP_INFO( @@ -380,6 +539,9 @@ class SimpleActionServer "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str()); } + /** + * @brief Debug logging + */ void debug_msg(const std::string & msg) const { RCLCPP_DEBUG( @@ -387,6 +549,9 @@ class SimpleActionServer "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str()); } + /** + * @brief Error logging + */ void error_msg(const std::string & msg) const { RCLCPP_ERROR( @@ -394,6 +559,9 @@ class SimpleActionServer "[%s] [ActionServer] %s", action_name_.c_str(), msg.c_str()); } + /** + * @brief Warn logging + */ void warn_msg(const std::string & msg) const { RCLCPP_WARN( diff --git a/nav2_util/include/nav2_util/string_utils.hpp b/nav2_util/include/nav2_util/string_utils.hpp index 1f3478a68d0..06d93b82a26 100644 --- a/nav2_util/include/nav2_util/string_utils.hpp +++ b/nav2_util/include/nav2_util/string_utils.hpp @@ -23,9 +23,20 @@ namespace nav2_util typedef std::vector Tokens; +/* + * @brief Remove leading slash from a topic name + * @param in String of topic in + * @return String out without slash +*/ std::string strip_leading_slash(const std::string & in); -/// Split a string at the delimiters +/// +/* + * @brief Split a string at the delimiters + * @param in String to split + * @param Delimiter criteria + * @return Tokens +*/ Tokens split(const std::string & tokenstring, char delimiter); } // namespace nav2_util diff --git a/nav2_util/package.xml b/nav2_util/package.xml index d53c1eca419..c4a163eed50 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.3 + 1.0.0 TODO Michael Jeronimo Mohammad Haghighipanah @@ -39,6 +39,7 @@ launch_testing_ament_cmake std_srvs action_msgs + ament_cmake_pytest ament_cmake diff --git a/nav2_util/src/costmap.cpp b/nav2_util/src/costmap.cpp index 7dd6a65c8ef..91f3b2fbc82 100644 --- a/nav2_util/src/costmap.cpp +++ b/nav2_util/src/costmap.cpp @@ -39,7 +39,7 @@ Costmap::Costmap( { if (lethal_threshold_ < 0. || lethal_threshold_ > 100.) { RCLCPP_WARN( - node_->get_logger(), "Costmap: Lethal threshold set to %.2f, it should be within" + node_->get_logger(), "Costmap: Lethal threshold set to %d, it should be within" " bounds 0-100. This could result in potential collisions!", lethal_threshold_); // lethal_threshold_ = std::max(std::min(lethal_threshold_, 100), 0); } diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index 889ecaa61fb..8948884ed18 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -42,9 +42,9 @@ namespace nav2_util LifecycleNode::LifecycleNode( const std::string & node_name, - const std::string & namespace_, bool use_rclcpp_node, + const std::string & ns, bool use_rclcpp_node, const rclcpp::NodeOptions & options) -: rclcpp_lifecycle::LifecycleNode(node_name, namespace_, options), +: rclcpp_lifecycle::LifecycleNode(node_name, ns, options), use_rclcpp_node_(use_rclcpp_node) { // server side never times out from lifecycle manager @@ -60,7 +60,7 @@ LifecycleNode::LifecycleNode( new_args.push_back(std::string("__node:=") + this->get_name() + "_rclcpp_node"); new_args.push_back("--"); rclcpp_node_ = std::make_shared( - "_", namespace_, rclcpp::NodeOptions(options).arguments(new_args)); + "_", ns, rclcpp::NodeOptions(options).arguments(new_args)); rclcpp_thread_ = std::make_unique(rclcpp_node_); } diff --git a/nav2_util/src/odometry_utils.cpp b/nav2_util/src/odometry_utils.cpp index 494b00e86c6..9bf585bdfe2 100644 --- a/nav2_util/src/odometry_utils.cpp +++ b/nav2_util/src/odometry_utils.cpp @@ -43,6 +43,26 @@ OdomSmoother::OdomSmoother( odom_cumulate_.twist.twist.angular.z = 0; } +OdomSmoother::OdomSmoother( + const nav2_util::LifecycleNode::WeakPtr & parent, + double filter_duration, + const std::string & odom_topic) +: odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) +{ + auto node = parent.lock(); + odom_sub_ = node->create_subscription( + odom_topic, + rclcpp::SystemDefaultsQoS(), + std::bind(&OdomSmoother::odomCallback, this, std::placeholders::_1)); + + odom_cumulate_.twist.twist.linear.x = 0; + odom_cumulate_.twist.twist.linear.y = 0; + odom_cumulate_.twist.twist.linear.z = 0; + odom_cumulate_.twist.twist.angular.x = 0; + odom_cumulate_.twist.twist.angular.y = 0; + odom_cumulate_.twist.twist.angular.z = 0; +} + void OdomSmoother::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { std::lock_guard lock(odom_mutex_); diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 6f735e6d25a..615366de29d 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -28,31 +28,39 @@ bool getCurrentPose( tf2_ros::Buffer & tf_buffer, const std::string global_frame, const std::string robot_frame, const double transform_timeout) { - static rclcpp::Logger logger = rclcpp::get_logger("getCurrentPose"); - geometry_msgs::msg::PoseStamped robot_pose; - tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose); - tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose); - robot_pose.header.frame_id = robot_frame; - robot_pose.header.stamp = rclcpp::Time(); + global_pose.header.frame_id = robot_frame; + global_pose.header.stamp = rclcpp::Time(); + + return transformPoseInTargetFrame( + global_pose, global_pose, tf_buffer, global_frame, transform_timeout); +} + +bool transformPoseInTargetFrame( + const geometry_msgs::msg::PoseStamped & input_pose, + geometry_msgs::msg::PoseStamped & transformed_pose, + tf2_ros::Buffer & tf_buffer, const std::string target_frame, + const double transform_timeout) +{ + static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame"); try { - global_pose = tf_buffer.transform( - robot_pose, global_frame, + transformed_pose = tf_buffer.transform( + input_pose, target_frame, tf2::durationFromSec(transform_timeout)); return true; } catch (tf2::LookupException & ex) { RCLCPP_ERROR( logger, - "No Transform available Error looking up robot pose: %s\n", ex.what()); + "No Transform available Error looking up target frame: %s\n", ex.what()); } catch (tf2::ConnectivityException & ex) { RCLCPP_ERROR( logger, - "Connectivity Error looking up robot pose: %s\n", ex.what()); + "Connectivity Error looking up target frame: %s\n", ex.what()); } catch (tf2::ExtrapolationException & ex) { RCLCPP_ERROR( logger, - "Extrapolation Error looking up robot pose: %s\n", ex.what()); + "Extrapolation Error looking up target frame: %s\n", ex.what()); } catch (tf2::TimeoutException & ex) { RCLCPP_ERROR( logger, @@ -60,7 +68,7 @@ bool getCurrentPose( } catch (tf2::TransformException & ex) { RCLCPP_ERROR( logger, "Failed to transform from %s to %s", - global_frame.c_str(), robot_frame.c_str()); + input_pose.header.frame_id.c_str(), target_frame.c_str()); } return false; diff --git a/nav2_util/test/test_dump_params/dump_params_md.txt b/nav2_util/test/test_dump_params/dump_params_md.txt index 25144298f8a..425ad44e296 100644 --- a/nav2_util/test/test_dump_params/dump_params_md.txt +++ b/nav2_util/test/test_dump_params/dump_params_md.txt @@ -2,7 +2,6 @@ |Parameter|Default Value| |---|---| |use_sim_time|False| -|param_not_set|NOT_SET| |param_bool|True| |param_int|1234| |param_double|3.14| diff --git a/nav2_util/test/test_dump_params/dump_params_md_verbose.txt b/nav2_util/test/test_dump_params/dump_params_md_verbose.txt index 929c8c93fed..4f086bc4e55 100644 --- a/nav2_util/test/test_dump_params/dump_params_md_verbose.txt +++ b/nav2_util/test/test_dump_params/dump_params_md_verbose.txt @@ -2,7 +2,6 @@ |Parameter|Default Value|Range|Description|Additional Constraints|Read-Only| |---|---|---|---|---|---| |use_sim_time|False|N/A|||False| -|param_not_set|NOT_SET|N/A|not set||False| |param_bool|True|N/A|boolean||True| |param_int|1234|-1000;10000;2||||False| |param_double|3.14|N/A|||False| diff --git a/nav2_util/test/test_dump_params/dump_params_multiple.txt b/nav2_util/test/test_dump_params/dump_params_multiple.txt index 5e790781556..df5b6314cfa 100644 --- a/nav2_util/test/test_dump_params/dump_params_multiple.txt +++ b/nav2_util/test/test_dump_params/dump_params_multiple.txt @@ -1,7 +1,6 @@ test_dump_params: ros__parameters: use_sim_time: False - param_not_set: NOT_SET param_bool: True param_int: 1234 param_double: 3.14 @@ -15,7 +14,6 @@ test_dump_params: test_dump_params_copy: ros__parameters: use_sim_time: False - param_not_set: NOT_SET param_bool: True param_int: 1234 param_double: 3.14 diff --git a/nav2_util/test/test_dump_params/dump_params_yaml.txt b/nav2_util/test/test_dump_params/dump_params_yaml.txt index 7c81e3fd5d3..ffb35a30e1c 100644 --- a/nav2_util/test/test_dump_params/dump_params_yaml.txt +++ b/nav2_util/test/test_dump_params/dump_params_yaml.txt @@ -1,7 +1,6 @@ test_dump_params: ros__parameters: use_sim_time: False - param_not_set: NOT_SET param_bool: True param_int: 1234 param_double: 3.14 diff --git a/nav2_util/test/test_dump_params/dump_params_yaml_verbose.txt b/nav2_util/test/test_dump_params/dump_params_yaml_verbose.txt index 2efd430a8a2..b49e53797c0 100644 --- a/nav2_util/test/test_dump_params/dump_params_yaml_verbose.txt +++ b/nav2_util/test/test_dump_params/dump_params_yaml_verbose.txt @@ -6,12 +6,6 @@ test_dump_params: # Additional constraints: # Read-only: False - param_not_set: NOT_SET - # Range: N/A - # Description: not set - # Additional constraints: - # Read-only: False - param_bool: True # Range: N/A # Description: boolean diff --git a/nav2_util/test/test_dump_params/test_dump_params_node.py b/nav2_util/test/test_dump_params/test_dump_params_node.py index 4e579df08d1..c1a0cdb814c 100755 --- a/nav2_util/test/test_dump_params/test_dump_params_node.py +++ b/nav2_util/test/test_dump_params/test_dump_params_node.py @@ -24,8 +24,6 @@ class TestDumpParamsNode(Node): def __init__(self, name): Node.__init__(self, name) - self.declare_parameter('param_not_set', - descriptor=ParameterDescriptor(description='not set')) self.declare_parameter('param_bool', True, ParameterDescriptor(description='boolean', read_only=True)) diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp index 9c379575bbd..52a2d4b457d 100644 --- a/nav2_util/test/test_geometry_utils.cpp +++ b/nav2_util/test/test_geometry_utils.cpp @@ -16,9 +16,11 @@ #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" +#include "nav_msgs/msg/path.hpp" #include "gtest/gtest.h" using nav2_util::geometry_utils::euclidean_distance; +using nav2_util::geometry_utils::calculate_path_length; TEST(GeometryUtils, euclidean_distance_point) { @@ -49,3 +51,50 @@ TEST(GeometryUtils, euclidean_distance_pose) ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.24695, 1e-5); } + +TEST(GeometryUtils, calculate_path_length) +{ + nav_msgs::msg::Path straight_line_path; + size_t nb_path_points = 10; + float distance_between_poses = 2.0; + float current_x_loc = 0.0; + + for (size_t i = 0; i < nb_path_points; ++i) { + geometry_msgs::msg::PoseStamped pose_stamped_msg; + pose_stamped_msg.pose.position.x = current_x_loc; + + straight_line_path.poses.push_back(pose_stamped_msg); + + current_x_loc += distance_between_poses; + } + + ASSERT_NEAR( + calculate_path_length(straight_line_path), + (nb_path_points - 1) * distance_between_poses, 1e-5); + + ASSERT_NEAR( + calculate_path_length(straight_line_path, straight_line_path.poses.size()), + 0.0, 1e-5); + + nav_msgs::msg::Path circle_path; + float polar_distance = 2.0; + uint32_t current_polar_angle_deg = 0; + constexpr float pi = 3.14159265358979; + + while (current_polar_angle_deg != 360) { + float x_loc = polar_distance * std::cos(current_polar_angle_deg * (pi / 180.0)); + float y_loc = polar_distance * std::sin(current_polar_angle_deg * (pi / 180.0)); + + geometry_msgs::msg::PoseStamped pose_stamped_msg; + pose_stamped_msg.pose.position.x = x_loc; + pose_stamped_msg.pose.position.y = y_loc; + + circle_path.poses.push_back(pose_stamped_msg); + + current_polar_angle_deg += 1; + } + + ASSERT_NEAR( + calculate_path_length(circle_path), + 2 * pi * polar_distance, 1e-1); +} diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp index a3530048a4b..bd277515ab0 100644 --- a/nav2_util/test/test_robot_utils.cpp +++ b/nav2_util/test/test_robot_utils.cpp @@ -29,4 +29,6 @@ TEST(RobotUtils, LookupExceptionError) geometry_msgs::msg::PoseStamped global_pose; tf2_ros::Buffer tf(node->get_clock()); ASSERT_FALSE(nav2_util::getCurrentPose(global_pose, tf, "map", "base_link", 0.1)); + global_pose.header.frame_id = "base_link"; + ASSERT_FALSE(nav2_util::transformPoseInTargetFrame(global_pose, global_pose, tf, "map", 0.1)); } diff --git a/nav2_util/test/test_service_client.cpp b/nav2_util/test/test_service_client.cpp index bc1b5518129..521d3f30568 100644 --- a/nav2_util/test/test_service_client.cpp +++ b/nav2_util/test/test_service_client.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include "nav2_util/service_client.hpp" #include "rclcpp/rclcpp.hpp" #include "std_srvs/srv/empty.hpp" +#include "std_msgs/msg/empty.hpp" #include "gtest/gtest.h" using nav2_util::ServiceClient; @@ -41,14 +43,6 @@ class TestServiceClient : public ServiceClient const rclcpp::Node::SharedPtr & getNode() {return node_;} }; -TEST(ServiceClient, are_non_alphanumerics_removed) -{ - TestServiceClient t("/foo/bar"); - string adjustedPrefix = "_foo_bar_Node_"; - ASSERT_EQ(t.name().length(), adjustedPrefix.length() + 8); - ASSERT_EQ(0, t.name().compare(0, adjustedPrefix.length(), adjustedPrefix)); -} - TEST(ServiceClient, can_ServiceClient_use_passed_in_node) { auto node = rclcpp::Node::make_shared("test_node"); @@ -56,3 +50,50 @@ TEST(ServiceClient, can_ServiceClient_use_passed_in_node) ASSERT_EQ(t.getNode(), node); ASSERT_EQ(t.name(), "test_node"); } + +TEST(ServiceClient, can_ServiceClient_invoke_in_callback) +{ + int a = 0; + auto service_node = rclcpp::Node::make_shared("service_node"); + auto service = service_node->create_service( + "empty_srv", + [&a](std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr) { + a = 1; + }); + auto srv_thread = std::thread([&]() {rclcpp::spin(service_node);}); + + auto pub_node = rclcpp::Node::make_shared("pub_node"); + auto pub = pub_node->create_publisher( + "empty_topic", + rclcpp::QoS(1).transient_local()); + auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);}); + + auto sub_node = rclcpp::Node::make_shared("sub_node"); + ServiceClient client("empty_srv", sub_node); + auto sub = sub_node->create_subscription( + "empty_topic", + rclcpp::QoS(1), + [&client](std_msgs::msg::Empty::SharedPtr) { + auto req = std::make_shared(); + auto res = client.invoke(req); + }); + + pub->publish(std_msgs::msg::Empty()); + rclcpp::spin_some(sub_node); + + rclcpp::shutdown(); + srv_thread.join(); + pub_thread.join(); + ASSERT_EQ(a, 1); +} + +TEST(ServiceClient, can_ServiceClient_timeout) +{ + rclcpp::init(0, nullptr); + auto node = rclcpp::Node::make_shared("test_node"); + TestServiceClient t("bar", node); + rclcpp::spin_some(node); + bool ready = t.wait_for_service(std::chrono::milliseconds(10)); + rclcpp::shutdown(); + ASSERT_EQ(ready, false); +} diff --git a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp index 0782111271e..de3e30716ff 100644 --- a/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp +++ b/nav2_voxel_grid/include/nav2_voxel_grid/voxel_grid.hpp @@ -200,12 +200,12 @@ class VoxelGrid unsigned int max_length = UINT_MAX); void clearVoxelLine( double x0, double y0, double z0, double x1, double y1, double z1, - unsigned int max_length = UINT_MAX); + unsigned int max_length = UINT_MAX, unsigned int min_length = 0); void clearVoxelLineInMap( double x0, double y0, double z0, double x1, double y1, double z1, unsigned char * map_2d, unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char free_cost = 0, unsigned char unknown_cost = 255, - unsigned int max_length = UINT_MAX); + unsigned int max_length = UINT_MAX, unsigned int min_length = 0); VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z); @@ -223,7 +223,8 @@ class VoxelGrid template inline void raytraceLine( ActionType at, double x0, double y0, double z0, - double x1, double y1, double z1, unsigned int max_length = UINT_MAX) + double x1, double y1, double z1, unsigned int max_length = UINT_MAX, + unsigned int min_length = 0) { int dx = int(x1) - int(x0); // NOLINT int dy = int(y1) - int(y0); // NOLINT @@ -237,25 +238,36 @@ class VoxelGrid int offset_dy = sign(dy) * size_x_; int offset_dz = sign(dz); - unsigned int z_mask = ((1 << 16) | 1) << (unsigned int)z0; - unsigned int offset = (unsigned int)y0 * size_x_ + (unsigned int)x0; - - GridOffset grid_off(offset); - ZOffset z_off(z_mask); - // we need to chose how much to scale our dominant dimension, based on the // maximum length of the line double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1) + (z0 - z1) * (z0 - z1)); + if ((unsigned int)(dist) < min_length) { + return; + } double scale = std::min(1.0, max_length / dist); + // Updating starting point to the point at distance min_length from the initial point + double min_x0 = x0 + dx / dist * min_length; + double min_y0 = y0 + dy / dist * min_length; + double min_z0 = z0 + dz / dist * min_length; + + + unsigned int z_mask = ((1 << 16) | 1) << (unsigned int)min_z0; + unsigned int offset = (unsigned int)min_y0 * size_x_ + (unsigned int)min_x0; + + GridOffset grid_off(offset); + ZOffset z_off(z_mask); + + unsigned int length = 0; // is x dominant if (abs_dx >= max(abs_dy, abs_dz)) { int error_y = abs_dx / 2; int error_z = abs_dx / 2; - + // Since initial point has been updated above, subtracting min_length from the total length + length = (unsigned int)(scale * abs_dx) - min_length; bresenham3D( at, grid_off, grid_off, z_off, abs_dx, abs_dy, abs_dz, error_y, error_z, - offset_dx, offset_dy, offset_dz, offset, z_mask, (unsigned int)(scale * abs_dx)); + offset_dx, offset_dy, offset_dz, offset, z_mask, length); return; } @@ -263,20 +275,23 @@ class VoxelGrid if (abs_dy >= abs_dz) { int error_x = abs_dy / 2; int error_z = abs_dy / 2; - + // Since initial point has been updated above, subtracting min_length from the total length + length = (unsigned int)(scale * abs_dy) - min_length; bresenham3D( at, grid_off, grid_off, z_off, abs_dy, abs_dx, abs_dz, error_x, error_z, - offset_dy, offset_dx, offset_dz, offset, z_mask, (unsigned int)(scale * abs_dy)); + offset_dy, offset_dx, offset_dz, offset, z_mask, length); return; } // otherwise, z is dominant int error_x = abs_dz / 2; int error_y = abs_dz / 2; + // Since initial point has been updated above, subtracting min_length from the total length + length = (unsigned int)(scale * abs_dz) - min_length; bresenham3D( at, z_off, grid_off, grid_off, abs_dz, abs_dx, abs_dy, error_x, error_y, offset_dz, - offset_dx, offset_dy, offset, z_mask, (unsigned int)(scale * abs_dz)); + offset_dx, offset_dy, offset, z_mask, length); } private: diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 68e8fe652cc..9390c2b8ef1 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.3 + 1.0.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_voxel_grid/src/voxel_grid.cpp b/nav2_voxel_grid/src/voxel_grid.cpp index c41bc40533c..44a63e2e31b 100644 --- a/nav2_voxel_grid/src/voxel_grid.cpp +++ b/nav2_voxel_grid/src/voxel_grid.cpp @@ -126,7 +126,7 @@ void VoxelGrid::markVoxelLine( void VoxelGrid::clearVoxelLine( double x0, double y0, double z0, double x1, double y1, double z1, - unsigned int max_length) + unsigned int max_length, unsigned int min_length) { if (x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1 >= size_x_ || y1 >= size_y_ || z1 >= size_z_) @@ -140,17 +140,17 @@ void VoxelGrid::clearVoxelLine( } ClearVoxel cv(data_); - raytraceLine(cv, x0, y0, z0, x1, y1, z1, max_length); + raytraceLine(cv, x0, y0, z0, x1, y1, z1, max_length, min_length); } void VoxelGrid::clearVoxelLineInMap( double x0, double y0, double z0, double x1, double y1, double z1, unsigned char * map_2d, unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char free_cost, - unsigned char unknown_cost, unsigned int max_length) + unsigned char unknown_cost, unsigned int max_length, unsigned int min_length) { costmap = map_2d; if (map_2d == NULL) { - clearVoxelLine(x0, y0, z0, x1, y1, z1, max_length); + clearVoxelLine(x0, y0, z0, x1, y1, z1, max_length, min_length); return; } @@ -166,7 +166,7 @@ void VoxelGrid::clearVoxelLineInMap( } ClearVoxelInMap cvm(data_, costmap, unknown_threshold, mark_threshold, free_cost, unknown_cost); - raytraceLine(cvm, x0, y0, z0, x1, y1, z1, max_length); + raytraceLine(cvm, x0, y0, z0, x1, y1, z1, max_length, min_length); } VoxelStatus VoxelGrid::getVoxel(unsigned int x, unsigned int y, unsigned int z) diff --git a/nav2_voxel_grid/test/voxel_grid_tests.cpp b/nav2_voxel_grid/test/voxel_grid_tests.cpp index cfc596f6ae2..92e0b0b46f8 100644 --- a/nav2_voxel_grid/test/voxel_grid_tests.cpp +++ b/nav2_voxel_grid/test/voxel_grid_tests.cpp @@ -172,6 +172,16 @@ TEST(voxel_grid, clearVoxelLineInMap) { vg.markVoxelInMap(0, 0, 5, 0); vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, nullptr, 16, 0); EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::FREE); + + // Testing for min range for raytrace clearing + vg.markVoxelInMap(0, 0, 5, 0); + vg.markVoxelInMap(0, 0, 7, 0); + vg.clearVoxelLineInMap( + 0, 0, 0, 0, 0, 9, nullptr, 16, 0, (unsigned char)'\000', + (unsigned char)'\377', UINT_MAX, 6); + EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::MARKED); + EXPECT_EQ(vg.getVoxel(0, 0, 7), nav2_voxel_grid::FREE); + delete[] map_2d; } diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 3253ab3439e..9407a6585c5 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(pluginlib REQUIRED) nav2_package() +link_libraries(stdc++fs) + include_directories( include ) @@ -96,7 +98,9 @@ install(DIRECTORY include/ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() + add_subdirectory(test) endif() ament_export_include_directories(include) diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index 5b8811c357d..49589b7dd6b 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -1,8 +1,9 @@ # Nav2 Waypoint Follower -The navigation2 waypoint follower is an example application of how to use the navigation action to complete some sort of orchestrated task. In this example, that task is to take a given set of waypoints and navigate to a set of positions in the order provided in the action request. The last waypoint in the waypoint array is the final position. +The Nav2 waypoint follower is an example application of how to use the navigation action to complete some sort of orchestrated task. In this example, that task is to take a given set of waypoints and navigate to a set of positions in the order provided in the action request. The last waypoint in the waypoint array is the final position. -The package exposes the `FollowWaypoints` action server of type `nav2_msgs/FollowWaypoints`. It is given an array of waypoints to visit, gives feedback about the current index of waypoint it is processing, and returns a list of waypoints it was unable to complete. +The package exposes the `follow_waypoints` action server of type `nav2_msgs/FollowWaypoints`. + It is given an array of waypoints to visit, gives feedback about the current index of waypoint it is processing, and returns a list of waypoints it was unable to complete. It also hosts a waypoint task executor plugin which can be used to perform custom behavior at a waypoint like waiting for user instruction, taking a picture, or picking up a box. @@ -12,7 +13,7 @@ There is a parameterization `stop_on_failure` whether to stop processing the way The ``nav2_waypoint_follower`` contains a waypoint following program with a plugin interface for specific task executors. This is useful if you need to go to a given location and complete a specific task like take a picture, pick up a box, or wait for user input. -It is a nice demo application for how to use navigation2 in a sample application. +It is a nice demo application for how to use Nav2 in a sample application. However, it could be used for more than just a sample application. There are 2 schools of thoughts for fleet managers / dispatchers. diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 37b9633ebaf..1abbdba75b3 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -68,7 +68,7 @@ class WaypointFollower : public nav2_util::LifecycleNode /** * @brief Configures member variables * - * Initializes action server for "FollowWaypoints" + * Initializes action server for "follow_waypoints" * @param state Reference to LifeCycle node state * @return SUCCESS or FAILURE */ diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index f4e28a6357a..669be9c732f 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.3 + 1.0.0 A waypoint follower navigation server Steve Macenski Apache-2.0 @@ -24,6 +24,8 @@ ament_lint_common ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest ament_cmake diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp index 598686a4392..7d1f4a9b1b4 100644 --- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp" #include #include #include +#include "nav2_util/node_utils.hpp" +#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp" namespace nav2_waypoint_follower { @@ -37,14 +38,18 @@ void PhotoAtWaypoint::initialize( curr_frame_msg_ = std::make_shared(); - node->declare_parameter(plugin_name + ".enabled", rclcpp::ParameterValue(true)); - node->declare_parameter( - plugin_name + ".image_topic", + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".enabled", + rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".image_topic", rclcpp::ParameterValue("/camera/color/image_raw")); - node->declare_parameter( - plugin_name + ".save_dir", + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".save_dir", rclcpp::ParameterValue("/tmp/waypoint_images")); - node->declare_parameter(plugin_name + ".image_format", rclcpp::ParameterValue("png")); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name + ".image_format", + rclcpp::ParameterValue("png")); std::string save_dir_as_string; node->get_parameter(plugin_name + ".enabled", is_enabled_); diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index ff81e7666dd..3dbb7d21f1e 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -72,7 +72,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "FollowWaypoints", std::bind(&WaypointFollower::followWaypoints, this)); + "follow_waypoints", std::bind(&WaypointFollower::followWaypoints, this)); try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( @@ -253,7 +253,7 @@ WaypointFollower::followWaypoints() new_goal = true; if (goal_index >= goal->poses.size()) { RCLCPP_INFO( - get_logger(), "Completed all %i waypoints requested.", + get_logger(), "Completed all %lu waypoints requested.", goal->poses.size()); result->missed_waypoints = failed_ids_; action_server_->succeeded_current(result); diff --git a/nav2_waypoint_follower/test/CMakeLists.txt b/nav2_waypoint_follower/test/CMakeLists.txt new file mode 100644 index 00000000000..64f54339047 --- /dev/null +++ b/nav2_waypoint_follower/test/CMakeLists.txt @@ -0,0 +1,10 @@ +# Test costmap downsampler +ament_add_gtest(test_task_executors + test_task_executors.cpp +) +ament_target_dependencies(test_task_executors + ${dependencies} +) +target_link_libraries(test_task_executors + ${library_name} wait_at_waypoint photo_at_waypoint input_at_waypoint +) diff --git a/nav2_waypoint_follower/test/test_task_executors.cpp b/nav2_waypoint_follower/test/test_task_executors.cpp new file mode 100644 index 00000000000..da0a0e99443 --- /dev/null +++ b/nav2_waypoint_follower/test/test_task_executors.cpp @@ -0,0 +1,164 @@ +// Copyright (c) 2021, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp" +#include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp" +#include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp" + + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(WaypointFollowerTest, WaitAtWaypoint) +{ + auto node = std::make_shared("testWaypointNode"); + + node->declare_parameter("WAW.waypoint_pause_duration", 50); + + std::unique_ptr waw( + new nav2_waypoint_follower::WaitAtWaypoint + ); + waw->initialize(node, std::string("WAW")); + + auto start_time = node->now(); + + // should wait 50ms + geometry_msgs::msg::PoseStamped pose; + waw->processAtWaypoint(pose, 0); + + auto end_time = node->now(); + + EXPECT_NEAR((end_time - start_time).seconds(), 0.05, 0.01); + + waw.reset(new nav2_waypoint_follower::WaitAtWaypoint); + node->set_parameter(rclcpp::Parameter("WAW.enabled", false)); + waw->initialize(node, std::string("WAW")); + + // plugin is not enabled, should exit + EXPECT_TRUE(waw->processAtWaypoint(pose, 0)); +} + +TEST(WaypointFollowerTest, InputAtWaypoint) +{ + auto node = std::make_shared("testWaypointNode"); + auto pub = node->create_publisher("input_at_waypoint/input", 1); + pub->on_activate(); + auto publish_message = + [&, this]() -> void + { + rclcpp::Rate(5).sleep(); + auto msg = std::make_unique(); + pub->publish(std::move(msg)); + rclcpp::spin_some(node->shared_from_this()->get_node_base_interface()); + }; + + std::unique_ptr iaw( + new nav2_waypoint_follower::InputAtWaypoint + ); + iaw->initialize(node, std::string("IAW")); + + auto start_time = node->now(); + + // no input, should timeout + geometry_msgs::msg::PoseStamped pose; + EXPECT_FALSE(iaw->processAtWaypoint(pose, 0)); + + auto end_time = node->now(); + + EXPECT_NEAR((end_time - start_time).seconds(), 10.0, 0.1); + + // has input now, should work + std::thread t1(publish_message); + EXPECT_TRUE(iaw->processAtWaypoint(pose, 0)); + t1.join(); + + iaw.reset(new nav2_waypoint_follower::InputAtWaypoint); + node->set_parameter(rclcpp::Parameter("IAW.enabled", false)); + iaw->initialize(node, std::string("IAW")); + + // plugin is not enabled, should exit + EXPECT_TRUE(iaw->processAtWaypoint(pose, 0)); +} + +TEST(WaypointFollowerTest, PhotoAtWaypoint) +{ + auto node = std::make_shared("testWaypointNode"); + auto pub = node->create_publisher("/camera/color/image_raw", 1); + pub->on_activate(); + std::condition_variable cv; + std::mutex mtx; + std::unique_lock lck(mtx, std::defer_lock); + bool data_published = false; + auto publish_message = + [&, this]() -> void + { + rclcpp::Rate(5).sleep(); + auto msg = std::make_unique(); + // fill image msg data. + msg->encoding = "rgb8"; + msg->height = 240; + msg->width = 320; + msg->step = 960; + auto size = msg->height * msg->width * 3; + msg->data.reserve(size); + int fake_data = 0; + for (size_t i = 0; i < size; i++) { + msg->data.push_back(fake_data++); + } + pub->publish(std::move(msg)); + rclcpp::spin_some(node->shared_from_this()->get_node_base_interface()); + lck.lock(); + data_published = true; + cv.notify_one(); + lck.unlock(); + }; + + std::unique_ptr paw( + new nav2_waypoint_follower::PhotoAtWaypoint + ); + paw->initialize(node, std::string("PAW")); + + // no images, throws because can't write + geometry_msgs::msg::PoseStamped pose; + EXPECT_FALSE(paw->processAtWaypoint(pose, 0)); + + std::thread t1(publish_message); + cv.wait(lck); + // has image now, since we force waiting until image is published + EXPECT_TRUE(paw->processAtWaypoint(pose, 0)); + t1.join(); + + paw.reset(new nav2_waypoint_follower::PhotoAtWaypoint); + node->set_parameter(rclcpp::Parameter("PAW.enabled", false)); + paw->initialize(node, std::string("PAW")); + + // plugin is not enabled, should exit + EXPECT_TRUE(paw->processAtWaypoint(pose, 0)); +} diff --git a/navigation2/CMakeLists.txt b/navigation2/CMakeLists.txt index 59ac3f63cae..763c95c1e96 100644 --- a/navigation2/CMakeLists.txt +++ b/navigation2/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(navigation2) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() find_package(ament_cmake REQUIRED) diff --git a/navigation2/package.xml b/navigation2/package.xml index 989ce6aa50b..3f2993e7e07 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.3 + 1.0.0 ROS2 Navigation Stack @@ -31,6 +31,7 @@ nav2_controller nav2_waypoint_follower nav2_smac_planner + nav2_regulated_pure_pursuit_controller ament_cmake diff --git a/tools/build_all.sh b/tools/build_all.sh index 4b2e3c7d45c..0e29d60b656 100755 --- a/tools/build_all.sh +++ b/tools/build_all.sh @@ -16,7 +16,7 @@ fi set -e -# so you can call from anywhere in the navigation2_ws, ros2_ws, or deps branches +# so you can call from anywhere in the nav2_ws, ros2_ws, or deps branches while [[ "$PWD" =~ ros2_ws ]]; do cd ../ done @@ -25,7 +25,7 @@ while [[ "$PWD" =~ ros2_nav_dependencies_ws ]]; do cd ../ done -while [[ "$PWD" =~ navigation2_ws ]]; do +while [[ "$PWD" =~ nav2_ws ]]; do cd ../ done @@ -54,7 +54,7 @@ export ROSDISTRO_INDEX_URL='https://raw.githubusercontent.com/ros2/rosdistro/ros colcon build --symlink-install) # Build our code -cd $CWD/navigation2_ws +cd $CWD/nav2_ws export ROSDISTRO_INDEX_URL='https://raw.githubusercontent.com/ros2/rosdistro/ros2/index.yaml' (. $ROS2_SETUP_FILE && . $CWD/ros2_nav_dependencies_ws/install/setup.bash && colcon build --symlink-install) @@ -62,7 +62,7 @@ export ROSDISTRO_INDEX_URL='https://raw.githubusercontent.com/ros2/rosdistro/ros # Update the ROS1 bridge if test "$ENABLE_ROS1" = true && test "$ENABLE_ROS2" = true ; then cd $CWD - . navigation2_ws/install/setup.bash + . nav2_ws/install/setup.bash cd $CWD/ros2_ws colcon build --symlink-install --packages-select ros1_bridge --cmake-force-configure fi diff --git a/tools/code_coverage_report.bash b/tools/code_coverage_report.bash index 4589ff3a44c..f4510238c65 100755 --- a/tools/code_coverage_report.bash +++ b/tools/code_coverage_report.bash @@ -36,61 +36,45 @@ for opt in "$@" ; do done set -o xtrace -mkdir -p $LCOVDIR +mkdir -p ${LCOVDIR} -# Generate initial zero-coverage data. -# This adds files that were otherwise not run to the report -lcov --capture --initial \ - --directory build \ - --output-file ${LCOVDIR}/initial_coverage.info \ - --rc lcov_branch_coverage=0 +# Ignore certain packages: +# - messages, which are auto generated files +# - system tests, which are themselves all test artifacts +# - rviz plugins, which are not used for real navigation +EXCLUDE_PACKAGES=$( + colcon list \ + --names-only \ + --packages-select-regex \ + ".*_msgs" \ + ".*_tests" \ + ".*_rviz.*" \ + | xargs) +INCLUDE_PACKAGES=$( + colcon list \ + --names-only \ + --packages-ignore \ + $EXCLUDE_PACKAGES \ + | xargs) # Capture executed code data. -lcov --capture \ - --directory build \ - --output-file ${LCOVDIR}/test_coverage.info \ - --rc lcov_branch_coverage=0 - -# Combine the initial zero-coverage report with the executed lines report. -lcov \ - --add-tracefile ${LCOVDIR}/initial_coverage.info \ - --add-tracefile ${LCOVDIR}/test_coverage.info \ - --output-file ${LCOVDIR}/full_coverage.info \ - --rc lcov_branch_coverage=0 - -# Only include files that are within this workspace. -# (eg filter out stdio.h etc) -lcov \ - --extract ${LCOVDIR}/full_coverage.info \ - "${PWD}/*" \ - --output-file ${LCOVDIR}/workspace_coverage.info \ - --rc lcov_branch_coverage=0 - -# Remove files in the build subdirectory. -# Those are generated files (like messages, services, etc) -# And system tests, which are themselves all test artifacts -# And rviz plugins, which are not used for real navigation -lcov \ - --remove ${LCOVDIR}/workspace_coverage.info \ - "${PWD}/build/*" \ - --remove ${LCOVDIR}/workspace_coverage.info \ - "${PWD}/*/dwb_msgs/*" \ - --remove ${LCOVDIR}/workspace_coverage.info \ - "${PWD}/*/nav2_msgs/*" \ - --remove ${LCOVDIR}/workspace_coverage.info \ - "${PWD}/*/nav_2d_msgs/*" \ - --remove ${LCOVDIR}/workspace_coverage.info \ - "${PWD}/*/nav2_system_tests/*" \ - --remove ${LCOVDIR}/workspace_coverage.info \ - "${PWD}/*/nav2_rviz_plugins/*" \ - --output-file ${LCOVDIR}/project_coverage.info \ - --rc lcov_branch_coverage=0 +fastcov --lcov \ + -d build \ + --exclude test/ $EXCLUDE_PACKAGES \ + --include $INCLUDE_PACKAGES \ + --process-gcno \ + --validate-sources \ + --dump-statistic \ + --output ${LCOVDIR}/total_coverage.info if [ $COVERAGE_REPORT_VIEW = codecovio ]; then - bash <(curl -s https://codecov.io/bash) \ - -f ${LCOVDIR}/project_coverage.info \ + curl -s https://codecov.io/bash > codecov + codecov_version=$(grep -o 'VERSION=\"[0-9\.]*\"' codecov | cut -d'"' -f2) + shasum -a 512 -c <(curl -s "https://raw.githubusercontent.com/codecov/codecov-bash/${codecov_version}/SHA512SUM" | grep -w "codecov") + bash codecov \ + -f ${LCOVDIR}/total_coverage.info \ -R src/navigation2 elif [ $COVERAGE_REPORT_VIEW = genhtml ]; then - genhtml ${LCOVDIR}/project_coverage.info \ + genhtml ${LCOVDIR}/total_coverage.info \ --output-directory ${LCOVDIR}/html fi diff --git a/tools/ctest_retry.bash b/tools/ctest_retry.bash index e65d6d2e267..c0756a070ab 100755 --- a/tools/ctest_retry.bash +++ b/tools/ctest_retry.bash @@ -43,7 +43,8 @@ done total=$RETRIES cd $TESTDIR -export RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED=1 +export RCUTILS_LOGGING_BUFFERED_STREAM=1 +export RCUTILS_LOGGING_USE_STDOUT=1 echo "Retrying Ctest up to " $total " times." for ((i=1;i<=total;i++)) diff --git a/tools/initial_ros_setup.sh b/tools/initial_ros_setup.sh index 4be3a5f0ab1..d53a1f66aea 100755 --- a/tools/initial_ros_setup.sh +++ b/tools/initial_ros_setup.sh @@ -41,8 +41,8 @@ return_to_root_dir() { download_navstack() { echo "Downloading the ROS 2 navstack" - mkdir -p navigation2_ws/src - cd navigation2_ws + mkdir -p nav2_ws/src + cd nav2_ws if [ -f "custom_nav2.repos" ]; then #override default location for testing vcs import src < custom_nav2.repos else @@ -65,7 +65,7 @@ download_ros2_dependencies() { echo "Downloading the dependencies workspace" mkdir -p ros2_nav_dependencies_ws/src cd ros2_nav_dependencies_ws - vcs import src < ${CWD}/navigation2_ws/src/navigation2/tools/underlay.repos + vcs import src < ${CWD}/nav2_ws/src/navigation2/tools/underlay.repos return_to_root_dir } @@ -106,7 +106,7 @@ if [ "$REPLY" = "y" ]; then download_all rosdep_install if [ "$ENABLE_BUILD" = true ]; then - $CWD/navigation2_ws/src/navigation2/tools/build_all.sh + $CWD/nav2_ws/src/navigation2/tools/build_all.sh fi cd ${CWD} @@ -115,9 +115,9 @@ if [ "$REPLY" = "y" ]; then echo "Everything downloaded and built successfully." echo "To use the navstack source the setup.bash in the install folder" echo - echo "> source navigation2/install/setup.bash" + echo "> source nav2_ws/install/setup.bash" echo echo "To build the navstack you can either" - echo "1. Run 'colcon build --symlink-install' from the navigation2 folder" - echo "2. or run 'make' from navigation2/build/ folder" + echo "1. Run 'colcon build --symlink-install' from the nav2_ws folder" + echo "2. or run 'make' from nav2_ws/build/ folder" fi diff --git a/tools/run_sanitizers b/tools/run_sanitizers index 7ded4aa3595..fb263eda27c 100755 --- a/tools/run_sanitizers +++ b/tools/run_sanitizers @@ -5,7 +5,7 @@ # To use this script, make sure you have the colcon plugins installed according # to the instructions above. Then build ros2_ws and navstack_dependencies_ws. -# Source the navstack_dependencies_ws and then cd to the navigation2_ws. +# Source the navstack_dependencies_ws and then cd to the nav2_ws. # # Run this script by invoking navigation2/tools/run_sanitizers # Afterwards, there should be two files in the root of the workspace: diff --git a/tools/underlay.repos b/tools/underlay.repos index 73d6b1f1e28..986e078866b 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -1,29 +1,25 @@ repositories: - BehaviorTree/BehaviorTree.CPP: - type: git - url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - version: master - ros/angles: - type: git - url: https://github.com/ros/angles.git - version: ros2 - ros-simulation/gazebo_ros_pkgs: - type: git - url: https://github.com/ros-simulation/gazebo_ros_pkgs.git - version: ros2 - ros-perception/image_common: - type: git - url: https://github.com/ros-perception/image_common.git - version: ros2 - ros-perception/vision_opencv: - type: git - url: https://github.com/ros-perception/vision_opencv.git - version: ros2 - ros/bond_core: - type: git - url: https://github.com/ros/bond_core.git - version: ros2 - ompl/ompl: - type: git - url: https://github.com/ompl/ompl.git - version: 1.5.0 + # BehaviorTree/BehaviorTree.CPP: + # type: git + # url: https://github.com/BehaviorTree/BehaviorTree.CPP.git + # version: master + # ros/angles: + # type: git + # url: https://github.com/ros/angles.git + # version: ros2 + # ros-simulation/gazebo_ros_pkgs: + # type: git + # url: https://github.com/ros-simulation/gazebo_ros_pkgs.git + # version: ros2 + # ros-perception/vision_opencv: + # type: git + # url: https://github.com/ros-perception/vision_opencv.git + # version: ros2 + # ros/bond_core: + # type: git + # url: https://github.com/ros/bond_core.git + # version: ros2 + # ompl/ompl: + # type: git + # url: https://github.com/ompl/ompl.git + # version: main diff --git a/tools/update_bt_diagrams.bash b/tools/update_bt_diagrams.bash index 94e5d00d0d7..f467fdc6e32 100755 --- a/tools/update_bt_diagrams.bash +++ b/tools/update_bt_diagrams.bash @@ -7,8 +7,8 @@ navigation2/tools/bt2img.py \ --image_out navigation2/nav2_bt_navigator/doc/simple_parallel \ --legend navigation2/nav2_bt_navigator/doc/legend navigation2/tools/bt2img.py \ - --behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml \ + --behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml \ --image_out navigation2/nav2_bt_navigator/doc/parallel_w_recovery navigation2/tools/bt2img.py \ - --behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml \ - --image_out navigation2/nav2_bt_navigator/doc/parallel_w_round_robin_recovery + --behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml \ + --image_out navigation2/nav2_bt_navigator/doc/parallel_through_poses_w_recovery